#ifndef MULTIPLICATIVEKALMANFILTER_H_
#define MULTIPLICATIVEKALMANFILTER_H_

#include <stdint.h>

#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "eive/resultClassIds.h"

class MultiplicativeKalmanFilter {
  /* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
   *           means of the spacecraft attitude sensors
   *
   * @note: A description of the used algorithms can be found in the bachelor thesis of Robin
   *        Marquardt
   *        https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811
   */
 public:
  /* @brief: Constructor
   * @param: acsParameters_   Pointer to object which defines the ACS configuration parameters
   */
  MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
  virtual ~MultiplicativeKalmanFilter();

  ReturnValue_t reset(acsctrl::MekfData *mekfData);

  /* @brief: init() - This function initializes the Kalman Filter and will provide the first
   * quaternion through the QUEST algorithm
   * @param:  magneticField_  magnetic field vector in the body frame
   * 		  sunDir_ 		  sun direction vector in the body frame
   * 		  sunDirJ 		  sun direction vector in the ECI frame
   * 		  magFieldJ 	  magnetic field vector in the ECI frame
   */
  ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
                     const bool validSS, const double *sunDirJ, const bool validSSModel,
                     const double *magFieldJ, const bool validMagModel,
                     acsctrl::MekfData *mekfData);

  /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
   * for the current step after the initalization
   * @param:  quaternionSTR   Star Tracker Quaternion between from body to ECI frame
   * 			rateGYRs_ 		Estimated satellite rotation rate from the
   * Gyroscopes [rad/s] magneticField_  magnetic field vector in the body frame sunDir_
   * sun direction vector in the body frame sunDirJ 		sun direction vector in the ECI
   * frame magFieldJ 		magnetic field vector in the ECI frame
   * 			outputQuat 		Stores the calculated quaternion
   * 			outputSatRate 	Stores the adjusted satellite rate
   * @return 	ReturnValue_t	Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from
   * the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model
   * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
   * 							RETURN_OK else
   */
  ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
                        const bool validGYRs_, const double *magneticField_,
                        const bool validMagField_, const double *sunDir_, const bool validSS,
                        const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
                        const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);

  enum MekfStatus : uint8_t {
    UNINITIALIZED = 0,
    NO_GYR_DATA = 1,
    NO_MODEL_VECTORS = 2,
    NO_SUS_MGM_STR_DATA = 3,
    COVARIANCE_INVERSION_FAILED = 4,
    INITIALIZED = 10,
    RUNNING = 11,
  };

  // resetting Mekf
  static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
  static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);
  static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
  static constexpr ReturnValue_t MEKF_NO_MODEL_VECTORS = returnvalue::makeCode(IF_MEKF_ID, 4);
  static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
  static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
      returnvalue::makeCode(IF_MEKF_ID, 6);
  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);

 private:
  /*Parameters*/
  AcsParameters::InertiaEIVE *inertiaEIVE;
  AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
  double quaternion_STR_SB[4];
  bool validInit;

  /*States*/
  double initialQuaternion[4];          /*after reset?QUEST*/
  double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
  double propagatedQuaternion[4];       /*Filter Quaternion for next step*/
  uint8_t sensorsAvail;

  /*Outputs*/
  double quatBJ[4];  /* Output Quaternion */
  double biasGYR[3]; /*Between measured and estimated sat Rate*/
                     /*Parameter INIT*/
  /*Functions*/
  void loadAcsParameters(AcsParameters *acsParameters_);
  void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
  void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
                     double satRotRate[3]);
};

#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */