#ifndef SAFECTRL_H_
#define SAFECTRL_H_

#include <eive/resultClassIds.h>
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <stdio.h>
#include <string.h>

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

  acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
                                         const bool satRotRateValid, const bool sunDirValid,
                                         const uint8_t mekfEnabled, const uint8_t gyrEnabled,
                                         const uint8_t dampingEnabled);

  void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
                const double *quatBI, const double *sunDirRefB, double *magMomB,
                double &errorAngle);

  void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
               const double *sunDirRefB, double *magMomB, double &errorAngle);

  void safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
                  const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB,
                  double *magMomB, double &errorAngle);

  void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
                          const double *sunDirRefB, double *magMomB, double &errorAngle);

  void safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB,
                             const double *sunDirRefB, double *magMomB, double &errorAngle);

  void splitRotationalRate(const double *satRotRateB, const double *sunDirB);

  void calculateRotationalRates(const double *magFieldB, const double *magRateB,
                                const double *sunDirB, const double *sunRateB,
                                double *fusedRotRate);

  void calculateRotationalRateTorque(const double *sunDirB, double &errorAngle,
                                     const double gainParallel, const double gainOrtho);

  void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
                                 const double gainAlign);

  void calculateMagneticMoment(double *magMomB);

 protected:
 private:
  AcsParameters *acsParameters;
  double magFieldBT[3] = {0, 0, 0};
  double satRotRateParallelB[3] = {0, 0, 0};
  double satRotRateOrthogonalB[3] = {0, 0, 0};
  double cmdParallel[3] = {0, 0, 0};
  double cmdOrtho[3] = {0, 0, 0};
  double cmdAlign[3] = {0, 0, 0};
  double cmdTorque[3] = {0, 0, 0};
};

#endif /* ACS_CONTROL_SAFECTRL_H_ */