#ifndef GUIDANCE_H_
#define GUIDANCE_H_

#include <time.h>

#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "SensorValues.h"

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

  void getTargetParamsSafe(double sunTargetSafe[3], double &satRateRef, double inertiaEive[3][3]);
  ReturnValue_t solarArrayDeploymentComplete();

  // Function to get the target quaternion and refence rotation rate from gps position and
  // position of the ground station
  void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
                               double refDirB[3], double quatBI[4], double targetQuat[4],
                               double targetSatRotRate[3]);
  void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
                              double targetSatRotRate[3]);
  void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
                       double targetSatRotRate[3]);

  // Function to get the target quaternion and refence rotation rate for sun pointing after ground
  // station
  void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]);

  // Function to get the target quaternion and refence rotation rate from gps position for Nadir
  // pointing
  void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
                                    double targetQuat[4], double refDirB[3], double refSatRate[3]);
  void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
                                   double targetQuat[4], double refSatRate[3]);

  // @note: Calculates the error quaternion between the current orientation and the target
  // quaternion, considering a reference quaternion. Additionally the difference between the  actual
  // and a desired satellite rotational rate is calculated, again considering a reference rotational
  // rate. Lastly gives back the error angle of the error quaternion.
  void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                  double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
                  double errorQuat[4], double errorSatRotRate[3], double errorAngle);
  void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                  double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
                  double errorAngle);

  void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
                          double *targetSatRotRate);

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

 private:
  const AcsParameters *acsParameters;

  bool strBlindAvoidFlag = false;
  timeval timeSavedQuaternion;
  double savedQuaternion[4] = {0, 0, 0, 0};
  double omegaRefSaved[3] = {0, 0, 0};

  static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
  static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";
};

#endif /* ACS_GUIDANCE_H_ */