#ifndef SAFECTRL_H_
#define SAFECTRL_H_

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

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

  static constexpr uint8_t IF_SAFE_ID = CLASS_ID::ACS_SAFE;
  static constexpr ReturnValue_t SAFECTRL_NO_MAG_FIELD_FOR_CONTROL =
      returnvalue::makeCode(IF_SAFE_ID, 2);
  static constexpr ReturnValue_t SAFECTRL_USE_MEKF = returnvalue::makeCode(IF_SAFE_ID, 3);
  static constexpr ReturnValue_t SAFECTRL_USE_NONMEKF = returnvalue::makeCode(IF_SAFE_ID, 4);
  static constexpr ReturnValue_t SAFECTRL_USE_DAMPING = returnvalue::makeCode(IF_SAFE_ID, 5);
  static constexpr ReturnValue_t SAFECTRL_NO_SENSORS_FOR_CONTROL =
      returnvalue::makeCode(IF_SAFE_ID, 6);

  ReturnValue_t safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
                                 const bool satRotRateValid, const bool sunDirValid);

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

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

  void safeRateDamping(const double *magFieldB, const double *satRotRateB,
                       const double *satRotRateRefB, double *magMomB, double &errorAngle);

 protected:
 private:
  AcsParameters *acsParameters;
};

#endif /* ACS_CONTROL_SAFECTRL_H_ */