#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_

#include <fsfw/datapool/PoolReadGuard.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 FusedRotationEstimation {
 public:
  FusedRotationEstimation(AcsParameters *acsParameters_);

  void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed,
                                 acsctrl::MgmDataProcessed *mgmDataProcessed,
                                 acsctrl::GyrDataProcessed *gyrDataProcessed,
                                 ACS::SensorValues *sensorValues, const double timeDelta,
                                 acsctrl::FusedRotRateData *fusedRotRateData);

  void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
                                     acsctrl::MgmDataProcessed *mgmDataProcessed,
                                     acsctrl::GyrDataProcessed *gyrDataProcessed,
                                     acsctrl::FusedRotRateData *fusedRotRateData);

  void estimateFusedRotationRateStr(double *quatNew, const double timeDelta,
                                    acsctrl::FusedRotRateData *fusedRotRateData);

 protected:
 private:
  static constexpr double ZERO_VEC3[3] = {0, 0, 0};
  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};

  AcsParameters *acsParameters;
  double quatOld[3] = {0, 0, 0, 0};
  double rotRateOldB[3] = {0, 0, 0};

  void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
                                        acsctrl::FusedRotRateData *fusedRotRateData);
};

#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */