#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,
                                 acsctrl::AttitudeEstimationData *attitudeEstimationData,
                                 const double timeDelta,
                                 acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
                                 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 quatOldQuest[4] = {0, 0, 0, 0};
  double quatOldStr[4] = {0, 0, 0, 0};
  double rotRateOldB[3] = {0, 0, 0};

  void estimateFusedRotationRateSusMgm(acsctrl::SusDataProcessed *susDataProcessed,
                                       acsctrl::MgmDataProcessed *mgmDataProcessed,
                                       acsctrl::GyrDataProcessed *gyrDataProcessed,
                                       acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
  void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
                                        acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
  void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData,
                                      const double timeDelta,
                                      acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
  void estimateFusedRotationRateStr(ACS::SensorValues *sensorValues, const double timeDelta,
                                    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
};

#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */