/*
 * safeCtrl.h
 *
 *  Created on: 19 Apr 2022
 *      Author: rooob
 */

#ifndef SAFECTRL_H_
#define SAFECTRL_H_

#include "../SensorValues.h"
#include "../OutputValues.h"
#include "../AcsParameters.h"
#include "../config/classIds.h"
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <fsfw/returnvalues/HasReturnvaluesIF.h>


class SafeCtrl{

public:

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

	static const uint8_t INTERFACE_ID = CLASS_ID::SAFE;
	static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);

	void loadAcsParameters(AcsParameters *acsParameters_);

	ReturnValue_t safeMekf(timeval now, double *quatBJ, bool *quatBJValid,
			double *magFieldModel, bool *magFieldModelValid,
			double *sunDirModel, bool *sunDirModelValid,
			double *satRateMekf, bool *rateMekfValid,
			double *sunDirRef, double *satRatRef, // From Guidance (!)
			double *outputMagMomB, bool *outputValid);

	void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid,
			double *sunRateB, bool *sunRateBValid,
			double *magFieldB, bool *magFieldBValid,
			double *magRateB, bool *magRateBValid,
			double *sunDirRef, double *satRateRef,
			double *outputMagMomB, bool *outputValid);

	void idleSunPointing(); // with reaction wheels

protected:

private:
	AcsParameters::SafeModeControllerParameters* safeModeControllerParameters;
	AcsParameters::InertiaEIVE* inertiaEIVE;
	double gainMatrixInertia[3][3];

	double magFieldBState[3];
	timeval magFieldBStateTime;

};



#endif /* ACS_CONTROL_SAFECTRL_H_ */