/*
 * safeCtrl.h
 *
 *  Created on: 19 Apr 2022
 *      Author: rooob
 */

#ifndef SAFECTRL_H_
#define SAFECTRL_H_

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

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

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

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

  void loadAcsParameters(AcsParameters *acsParameters_);

  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 *outputMagMomB, bool *outputValid);

  void safeNoMekf(timeval now, double *susDirB, bool *susDirBValid, double *sunRateB,
                  bool *sunRateBValid, double *magFieldB, bool *magFieldBValid, double *magRateB,
                  bool *magRateBValid, double *sunDirRef, double *satRateRef, double *outputMagMomB,
                  bool *outputValid);

  void idleSunPointing();  // with reaction wheels

 protected:
 private:
  AcsParameters::SafeModeControllerParameters *safeModeControllerParameters;
  AcsParameters::InertiaEIVE *inertiaEIVE;
  double gainMatrixInertia[3][3];

  double magFieldBState[3];
  timeval magFieldBStateTime;
};

#endif /* ACS_CONTROL_SAFECTRL_H_ */