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

#include <stdint.h> //uint8_t
#include <sys/time.h> /*purpose, timeval ?*/
#include <config/classIds.h>
#include <fsfw/src/fsfw/returnvalues/returnvalue.h>
#include <AcsParameters.h>
#include <SensorValues.h>
#include <OutputValues.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(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(const float sus0Value[], bool sus0valid, const float sus1Value[], bool sus1valid,
			const float sus2Value[], bool sus2valid, const float sus3Value[], bool sus3valid,
			const float sus4Value[], bool sus4valid, const float sus5Value[], bool sus5valid,
			const float sus6Value[], bool sus6valid, const float sus7Value[], bool sus7valid,
			const float sus8Value[], bool sus8valid, const float sus9Value[], bool sus9valid,
			const float sus10Value[], bool sus10valid, const float sus11Value[], bool sus11valid,
			timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
			const AcsParameters::SunModelParameters *sunModelParameters, double *sunDirEst, bool *sunDirEstValid,
			double *sunVectorModel, bool *sunVectorModelValid,
			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;


};

#endif SENSORPROCESSING_H_