#ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_
#define MISSION_CONTROLLER_ACSCONTROLLER_H_

#include <commonObjects.h>
#include "controllerdefinitions/AcsCtrlDefinitions.h"
#include <fsfw/controller/ExtendedControllerBase.h>
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"

class AcsController : public ExtendedControllerBase {
 public:
  static constexpr dur_millis_t INIT_DELAY = 500;

  AcsController(object_id_t objectId);

 private:
  enum class InternalState { STARTUP, INITIAL_DELAY, READY };

  InternalState internalState = InternalState::STARTUP;

  ReturnValue_t handleCommandMessage(CommandMessage* message) override;
  void performControlOperation() override;

  ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                        LocalDataPoolManager& poolManager) override;
  LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;

  // Mode abstract functions
  ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
                                 uint32_t* msToReachTheMode) override;

  // MGMs
  acsctrl::MgmData mgmData;

  MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set = MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
  RM3100::Rm3100PrimaryDataset mgm1Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
  MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set = MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
  RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
  IMTQ::CalibratedMtmMeasurementSet imtqMgmSet = IMTQ::CalibratedMtmMeasurementSet(objects::IMTQ_HANDLER);

  PoolEntry<float> mgm0PoolVec = PoolEntry<float>(3);
  PoolEntry<float> mgm1PoolVec = PoolEntry<float>(3);
  PoolEntry<float> mgm2PoolVec = PoolEntry<float>(3);
  PoolEntry<float> mgm3PoolVec = PoolEntry<float>(3);
  PoolEntry<int32_t> imtqMgmPoolVec = PoolEntry<int32_t>(3);
  PoolEntry<uint8_t> imtqCalActStatus = PoolEntry<uint8_t>();
  void copyMgmData();

  // Initial delay to make sure all pool variables have been initialized their owners
  Countdown initialCountdown = Countdown(INIT_DELAY);
  // Sun Sensors
};

#endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */