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

#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[3]);

  // Function to get the target quaternion and refence rotation rate from gps position and
  // position of the ground station
  void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
                              acsctrl::GpsDataProcessed *gpsDataProcessed,
                              acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
                              double refSatRate[3]);
  void targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
                       acsctrl::SusDataProcessed *susDataProcessed,
                       acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
                       double targetQuat[4], double refSatRate[3]);
  void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
                               acsctrl::SusDataProcessed *susDataProcessed,
                               acsctrl::GpsDataProcessed *gpsDataProcessed, 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, acsctrl::MekfData *mekfData,
                  acsctrl::SusDataProcessed *susDataProcessed,
                  acsctrl::GpsDataProcessed *gpsDataProcessed, 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 quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
                             acsctrl::GpsDataProcessed *gpsDataProcessed,
                             acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
                             double refSatRate[3]);
  void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
                              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], acsctrl::MekfData *mekfData, double refSatRate[3],
                  double quatErrorComplete[4], double quatError[3], double deltaRate[3]);

  void refRotationRate(timeval now, double quatInertialTarget[4], double *refSatRate);

  //	@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};

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

#endif /* ACS_GUIDANCE_H_ */