#ifndef MULTIPLICATIVEKALMANFILTER_H_
#define MULTIPLICATIVEKALMANFILTER_H_

#include <common/config/eive/resultClassIds.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>

class MultiplicativeKalmanFilter {
  /* @brief: This class handles the calculation of an estimated quaternion and the gyroscope 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
   */
  MultiplicativeKalmanFilter(AcsParameters *acsParameters);
  virtual ~MultiplicativeKalmanFilter();

  ReturnValue_t reset(acsctrl::AttitudeEstimationData *attitudeEstimationData);

  ReturnValue_t init(const acsctrl::SusDataProcessed *susData,
                     const acsctrl::MgmDataProcessed *mgmData,
                     const acsctrl::GyrDataProcessed *gyrData,
                     acsctrl::AttitudeEstimationData *attitudeEstimationData);

  ReturnValue_t mekfEst(const acsctrl::SusDataProcessed *susData,
                        const acsctrl::MgmDataProcessed *mgmData,
                        const acsctrl::GyrDataProcessed *gyrData, const double timeDelta,
                        acsctrl::AttitudeEstimationData *attitudeEstimationData);

  void updateStandardDeviations(const AcsParameters *acsParameters);

  void setStrData(const double qX, const double qY, const double qZ, const double qW,
                  const bool valid, const bool allowStr);

  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_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7);
  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8);
  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);

 private:
  static constexpr double ZERO_VEC3[3] = {0, 0, 0};
  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
  static constexpr double ZERO_MAT66[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                              {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                              {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
  static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
  static constexpr double EYE3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
  static constexpr double EYE6[6][6] = {{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0},
                                        {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};

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

  enum SensorAvailability : uint8_t {
    NONE = 0,
    SUS_MGM_STR = 1,
    SUS_MGM = 2,
    SUS_STR = 3,
    MGM_STR = 4,
    SUS = 5,
    MGM = 6,
    STR = 7,
  };

  MekfStatus mekfStatus = MekfStatus::UNINITIALIZED;

  struct StrData {
    struct StrQuat {
      double value[4] = {0, 0, 0, 0};
      bool valid = false;
    } strQuat;
  } strData;

  // Standard Deviations
  double sigmaSus = 0;
  double sigmaMgm = 0;
  double sigmaStr = 0;
  double sigmaGyr = 0;
  // sigmaV
  double sigmaGyrArw = 0;
  // sigmaU
  double sigmaGyrBs = 0;

  // Covariance Matrices
  double covSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double covMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double covStr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

  double covAposteriori[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                 {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};

  // Sensor Availability
  SensorAvailability sensorsAvailable = SensorAvailability::NONE;
  uint8_t matrixDimensionFactor = 0;

  // Estimated States
  double estimatedQuaternionBI[4] = {0, 0, 0, 1};
  double estimatedBiasGyr[3] = {0, 0, 0};
  double estimatedRotRate[3] = {0, 0, 0};
  double estimatedCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                            {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                            {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};

  // Functions
  ReturnValue_t checkAvailableSensors(const acsctrl::SusDataProcessed *susData,
                                      const acsctrl::MgmDataProcessed *mgmData,
                                      const acsctrl::GyrDataProcessed *gyrData,
                                      acsctrl::AttitudeEstimationData *attitudeEstimationData);

  void kfUpdate(const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
                double *measSensMatrix, double *measCovMatrix, double *measVec, double *measEstVec);
  ReturnValue_t kfGain(double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
                       acsctrl::AttitudeEstimationData *attitudeEstimationData);
  void kfCovAposteriori(double *kalmanGain, double *measSensMatrix);
  void kfStateAposteriori(double *kalmanGain, double *measVec, double *estVec);
  void kfPropagate(const acsctrl::GyrDataProcessed *gyrData, const double timeDiff);

  ReturnValue_t kfFiniteCheck(acsctrl::AttitudeEstimationData *attitudeEstimationData);

  void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *attitudeEstimationData);
  void updateDataSet(acsctrl::AttitudeEstimationData *attitudeEstimationData);
};

#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */