#ifndef MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
#define MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_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/controllerdefinitions/AcsCtrlDefinitions.h>

#include <cmath>
#include <iostream>

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

  void quest(acsctrl::SusDataProcessed *susData, acsctrl::MgmDataProcessed *mgmData,
             acsctrl::AttitudeEstimationData *attitudeEstimation);

 protected:
 private:
  AcsParameters *acsParameters;

  double qOld[4] = {0, 0, 0, 0};

  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
};

#endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */