/*
 * PtgCtrl.h
 *
 *  Created on: 17 Jul 2022
 *      Author: Robin Marquardt
 *
 *      @brief: This class handles the pointing control mechanism. Calculation of an commanded
 * torque for the reaction wheels, and magnetic Field strength for magnetorques 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
 */

#ifndef PTGCTRL_H_
#define PTGCTRL_H_

#include <stdio.h>
#include <string.h>
#include <time.h>

#include "../AcsParameters.h"
#include "../SensorValues.h"
#include "../config/classIds.h"

class PtgCtrl {
 public:
  /* @brief: Constructor
   * @param: acsParameters_   Pointer to object which defines the ACS configuration parameters
   */
  PtgCtrl(AcsParameters *acsParameters_);
  virtual ~PtgCtrl();

  static const uint8_t INTERFACE_ID = CLASS_ID::PTG;
  static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);

  /* @brief: Load AcsParameters for this class
   * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
   */
  void loadAcsParameters(AcsParameters *acsParameters_);

  /* @brief: Calculates the needed torque for the pointing control mechanism
   * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
   */
  void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
              const double *deltaRate, const double *rwPseudoInv, double *torqueRws);

  void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
                       double *magFieldEst, bool magFieldEstValid, double *satRate,
                       int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
                       double *mgtDpDes);

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

  /* @brief: Commands the stiction torque in case wheel speed is to low
   * @param: rwAvailable 	Boolean Flag for all reaction wheels
   * 		 omegaRw		current wheel speed of reaction wheels
   * 		 torqueCommand  modified torque after antistiction
   */
  void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);

 private:
  AcsParameters::RwHandlingParameters *rwHandlingParameters;
  AcsParameters::InertiaEIVE *inertiaEIVE;
  AcsParameters::RwMatrices *rwMatrices;

  double torqueMemory[4] = {0, 0, 0, 0};
  double omegaMemory[4] = {0, 0, 0, 0};
};

#endif /* ACS_CONTROL_PTGCTRL_H_ */