#ifndef PTGCTRL_H_
#define PTGCTRL_H_

#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h>
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>

#include <cmath>

class PtgCtrl {
  /*
   * @brief: This class handles the pointing control mechanism. Calculation of an commanded
   * torque for the reaction wheels, and magnetic Field strength for magnetorquer 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
   */
 public:
  /* @brief: Constructor
   * @param: acsParameters_   Pointer to object which defines the ACS configuration parameters
   */
  PtgCtrl(AcsParameters *acsParameters_);
  virtual ~PtgCtrl();

  acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid,
                                                const bool strValid, const bool questValid,
                                                const bool fusedRateValid,
                                                const uint8_t rotRateSource,
                                                const uint8_t mekfEnabled);

  /* @brief: Calculates the needed torque for the pointing control mechanism
   */
  void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
              const double *deltaRate, const double *rwPseudoInv, double *torqueRws);

  void ptgNullspace(const bool allRwAvabilable,
                    AcsParameters::PointingLawParameters *pointingLawParameters,
                    const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
                    const int32_t speedRw3, double *rwTrqNs);

  void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail,
                       AcsParameters::PointingLawParameters *pointingLawParameters,
                       const double *magFieldB, const bool magFieldBValid, const int32_t speedRw0,
                       const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3,
                       double *mgtDpDes);

  /* @brief: Commands the stiction torque in case wheel speed is to low
   * 		 torqueCommand  modified torque after anti-stiction
   */
  void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);

 private:
  const AcsParameters *acsParameters;
  static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60;
};

#endif /* ACS_CONTROL_PTGCTRL_H_ */