#ifndef GUIDANCE_H_
#define GUIDANCE_H_

#include <fsfw/coordinates/CoordinateTransformations.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <time.h>

#include <cmath>
#include <filesystem>
#include <string>

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

  void getTargetParamsSafe(double sunTargetSafe[3]);
  ReturnValue_t solarArrayDeploymentComplete();
  void resetValues();

  void targetQuatPtgIdle(const double timeDelta, double sunDirI[3], double targetQuat[4],
                         double targetSatRotRate[3]);
  void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
                           double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
  void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
                       double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
  void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3],
                          double velSatF[3], double targetQuat[4], double refSatRate[3]);

  void targetRotationRate(const double timeDelta, double quatInertialTarget[4],
                          double *targetSatRotRate);

  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);

  ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);

 private:
  const AcsParameters *acsParameters;

  static constexpr double ZERO_VEC3[3] = {0, 0, 0};
  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};

  bool strBlindAvoidFlag = false;
  double quatIXprev[4] = {0, 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_ */