#ifndef SAFECTRL_H_
#define SAFECTRL_H_

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

#include "../AcsParameters.h"
#include "../SensorValues.h"
#include "eive/resultClassIds.h"

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

  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE;
  static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);

  ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
                         bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
                         double *satRateMekf, bool rateMekfValid, double *sunDirRef,
                         double *satRatRef,  // From Guidance (!)
                         double *outputAngle, double *outputMagMomB);

  ReturnValue_t safeNoMekf(const double *magneticFieldVector,
                           const double *magneticFieldVectorDerivative, const double *sunVector,
                           const double *sunvectorDerivative, double omegaRef,
                           double *torqueCommand, double *spinAxis);

  double estimateRotationAroundSun(const double *magneticFieldVector,
                                   const double *magneticFieldVectorDerivative,
                                   const double *sunvector, bool *updated);

 protected:
 private:
  AcsParameters *acsParameters;
  double gainMatrixInertia[3][3];

  double magFieldBState[3];
  timeval magFieldBStateTime;

  float lastCalculatedOmega = 0.0;
};

#endif /* ACS_CONTROL_SAFECTRL_H_ */