/*******************************
 *	EIVE Flight Software Framework (FSFW)
 * (c) 2022 IRS, Uni Stuttgart
 *******************************/
#ifndef SENSORPROCESSING_H_
#define SENSORPROCESSING_H_

#include <fsfw/returnvalues/returnvalue.h>
#include <stdint.h>  //uint8_t
#include <time.h>    /*purpose, timeval ?*/

#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "OutputValues.h"
#include "SensorValues.h"
#include "SusConverter.h"
#include "config/classIds.h"

/*Planned:
 * - Fusion of Sensor Measurements -
 * sunDirEst (mean value)
 * magField (mean value)
 * rmuSatRate (rmus, mean value)
 * - Models to get inertia values -
 * sunModelDir (input: time)
 * magModelField (input: position,time)
 * - Low Pass Filter maybe -
 * magField
 * SunDirEst*/

class SensorProcessing {
 public:
  void reset();

  SensorProcessing(AcsParameters *acsParameters_);
  virtual ~SensorProcessing();

  void process(acsctrl::SusDataRaw *susData, timeval now, ACS::SensorValues *sensorValues,
               ACS::OutputValues *outputValues,
               const AcsParameters *acsParameters);  // Will call protected functions
 private:
 protected:
  // short description needed for every function
  bool processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
                  const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
                  const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
                  const AcsParameters::MgmHandlingParameters *mgmParameters,
                  const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
                  bool gpsValid, double *magFieldEst, bool *outputValid, double *magFieldModel,
                  bool *magFieldModelValid, double *magneticFieldVectorDerivative,
                  bool *magneticFieldVectorDerivativeValid);  // Output

  void processSus(acsctrl::SusDataRaw *susData, timeval timeOfSusMeasurement,
                  const AcsParameters::SusHandlingParameters *susParameters,
                  const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst,
                  bool *sunDirEstValid, double *sunVectorInertial, bool *sunVectorInertialValid,
                  double *sunVectorDerivative, bool *sunVectorDerivativeValid);

  void processRmu(const double rmu0Value[], bool rmu0valid,  // processRmu
                  const double rmu1Value[], bool rmu1valid, const double rmu2Value[],
                  bool rmu2valid, timeval timeOfrmuMeasurement,
                  const AcsParameters::RmuHandlingParameters *rmuParameters, double *satRatEst,
                  bool *satRateEstValid);

  void processStr();

  void processGps(const double gps0latitude, const double gps0longitude, const bool validGps,
                  double *gcLatitude, double *gdLongitude);

  double savedMagFieldEst[3];
  timeval timeOfSavedMagFieldEst;
  double savedSunVector[3];
  timeval timeOfSavedSusDirEst;
  bool validMagField;
  bool validGcLatitude;

  SusConverter susConverter;
  AcsParameters acsParameters;
};

#endif /*SENSORPROCESSING_H_*/