/*
 * Guidance.h
 *
 *  Created on: 6 Jun 2022
 *      Author: Robin Marquardt
 */

#ifndef GUIDANCE_H_
#define GUIDANCE_H_


#include "AcsParameters.h"
#include "SensorValues.h"
#include "OutputValues.h"
#include <time.h>


class Guidance {
public:
	Guidance(AcsParameters *acsParameters_);
	virtual ~Guidance();

	void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);

	// Function to get the target quaternion and refence rotation rate from gps position and position of the ground station
	void targetQuatPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
			double targetQuat[4], double refSatRate[3]);
	void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
			double targetQuat[4], double refSatRate[3]);

	// Function to get the target quaternion and refence rotation rate for sun pointing after ground station
	void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
			double targetQuat[4], double refSatRate[3]);

	// Function to get the target quaternion and refence rotation rate from gps position for Nadir pointing
	void quatNadirPtgOldVersion(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
			double targetQuat[4], double refSatRate[3]);

	void quatNadirPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
			double targetQuat[4], double refSatRate[3]);

	// Function to get the target quaternion and refence rotation rate from parameters for inertial pointing
	void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);

	// @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired
	void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] );

//	@note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid reation wheel
//	maybe can be done in "commanding.h"
	void getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *rwPseudoInv);


private:
	AcsParameters acsParameters;
	bool strBlindAvoidFlag = false;
	timeval timeSavedQuaternionNadir;
	double savedQuaternionNadir[4] = {0, 0, 0, 0};
	double omegaRefSavedNadir[3] = {0, 0, 0};
};

#endif /* ACS_GUIDANCE_H_ */