/*
 * PtgCtrl.h
 *
 *  Created on: 17 Jul 2022
 *      Author: Robin Marquardt
 *
 *      @brief: This class handles the pointing control mechanism. Calculation of an commanded torque
 *      for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
 *
 *      @note: A description of the used algorithms can be found in
 *      https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
 */

#ifndef PTGCTRL_H_
#define PTGCTRL_H_

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

class PtgCtrl{

public:
	/* @brief: Constructor
	 * @param: acsParameters_   Pointer to object which defines the ACS configuration parameters
	 */
	PtgCtrl(AcsParameters *acsParameters_);
	virtual ~PtgCtrl();

	static const uint8_t INTERFACE_ID = CLASS_ID::PTG;
	static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);

	/* @brief: Load AcsParameters für this class
	 * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
	 */
	void loadAcsParameters(AcsParameters *acsParameters_);

	/* @brief: Calculates the needed torque for the pointing control mechanism
	 * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
	 */
	void ptgGroundstation(const double mode,const double *qError,const  double *deltaRate,const double *rwPseudoInv,  double *torqueRws);

	void ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate, double *speedRw0,
			 double *speedRw1,  double *speedRw2,  double *speedRw3, double *mgtDpDes);

	void ptgNullspace(const double *speedRw0, const double *speedRw1, const double *speedRw2, const double *speedRw3, double *rwTrqNs);

private:
	AcsParameters::PointingModeControllerParameters* pointingModeControllerParameters;
	AcsParameters::RwHandlingParameters* rwHandlingParameters;
	AcsParameters::InertiaEIVE* inertiaEIVE;
	AcsParameters::RwMatrices* rwMatrices;
};

#endif /* ACS_CONTROL_PTGCTRL_H_ */