#ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_
#define MISSION_CONTROLLER_ACSCONTROLLER_H_

#include <commonObjects.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>

#include "acs/ActuatorCmd.h"
#include "acs/Guidance.h"
#include "acs/Navigation.h"
#include "acs/OutputValues.h"
#include "acs/SensorProcessing.h"
#include "acs/control/Detumble.h"
#include "acs/control/PtgCtrl.h"
#include "acs/control/SafeCtrl.h"
#include "controllerdefinitions/AcsCtrlDefinitions.h"
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"

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

  AcsController(object_id_t objectId);

  static const Submode_t SUBMODE_SAFE = 2;
  static const Submode_t SUBMODE_DETUMBLE = 3;
  static const Submode_t SUBMODE_PTG_GS = 4;
  static const Submode_t SUBMODE_PTG_NADIR = 5;

 protected:
  void performSafe();
  void performDetumble();
  void performPointingCtrl();

 private:
  AcsParameters acsParameters;
  SensorProcessing sensorProcessing;
  Navigation navigation;
  ActuatorCmd actuatorCmd;
  Guidance guidance;

  SafeCtrl safeCtrl;
  Detumble detumble;
  PtgCtrl ptgCtrl;

  uint8_t detumbleCounter;

  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;
  void modeChanged(Mode_t mode, Submode_t submode);
  void announceMode(bool recursive);

  /* ACS Datasets */
  // MGMs
  acsctrl::MgmDataRaw mgmDataRaw;
  PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);
  PoolEntry<float> mgm1VecRaw = PoolEntry<float>(3);
  PoolEntry<float> mgm2VecRaw = PoolEntry<float>(3);
  PoolEntry<float> mgm3VecRaw = PoolEntry<float>(3);
  PoolEntry<float> imtqMgmVecRaw = PoolEntry<float>(3);
  PoolEntry<uint8_t> imtqCalActStatus = PoolEntry<uint8_t>();
  void copyMgmData();

  acsctrl::MgmDataProcessed mgmDataProcessed;
  PoolEntry<float> mgm0VecProc = PoolEntry<float>(3);
  PoolEntry<float> mgm1VecProc = PoolEntry<float>(3);
  PoolEntry<float> mgm2VecProc = PoolEntry<float>(3);
  PoolEntry<float> mgm3VecProc = PoolEntry<float>(3);
  PoolEntry<float> mgm4VecProc = PoolEntry<float>(3);
  PoolEntry<double> mgmVecTot = PoolEntry<double>(3);
  PoolEntry<float> mgmVecTotDer = PoolEntry<float>(3);
  PoolEntry<double> magIgrf = PoolEntry<double>(3);

  // SUSs
  acsctrl::SusDataRaw susDataRaw;
  PoolEntry<uint16_t> sus0ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus1ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus2ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus3ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus4ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus5ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus6ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus7ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus8ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus9ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus10ValRaw = PoolEntry<uint16_t>(6);
  PoolEntry<uint16_t> sus11ValRaw = PoolEntry<uint16_t>(6);
  void copySusData();

  acsctrl::SusDataProcessed susDataProcessed;
  PoolEntry<float> sus0VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus1VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus2VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus3VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus4VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus5VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus6VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus7VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus8VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus9VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus10VecProc = PoolEntry<float>(3);
  PoolEntry<float> sus11VecProc = PoolEntry<float>(3);
  PoolEntry<float> susVecTot = PoolEntry<float>(3);
  PoolEntry<float> susVecTotDer = PoolEntry<float>(3);
  PoolEntry<float> sunIjk = PoolEntry<float>(3);

  // GYRs
  acsctrl::GyrDataRaw gyrDataRaw;
  PoolEntry<double> gyr0VecRaw = PoolEntry<double>(3);
  PoolEntry<float> gyr1VecRaw = PoolEntry<float>(3);
  PoolEntry<double> gyr2VecRaw = PoolEntry<double>(3);
  PoolEntry<float> gyr3VecRaw = PoolEntry<float>(3);
  void copyGyrData();

  acsctrl::GyrDataProcessed gyrDataProcessed;
  PoolEntry<double> gyr0VecProc = PoolEntry<double>(3);
  PoolEntry<double> gyr1VecProc = PoolEntry<double>(3);
  PoolEntry<double> gyr2VecProc = PoolEntry<double>(3);
  PoolEntry<double> gyr3VecProc = PoolEntry<double>(3);
  PoolEntry<double> gyrVecTot = PoolEntry<double>(3);

  // GPS
  acsctrl::GpsDataProcessed gpsDataProcessed;
  PoolEntry<double> gcLatitude = PoolEntry<double>();
  PoolEntry<double> gdLongitude = PoolEntry<double>();

  // MEKF
  acsctrl::MekfData mekfData;
  PoolEntry<double> quatMekf = PoolEntry<double>(4);
  PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);

  // Actuator CMD
  acsctrl::ActuatorCmdData actuatorCmdData;
  PoolEntry<double> rwTargetTorque = PoolEntry<double>(4);
  PoolEntry<double> rwTargetSpeed = PoolEntry<double>(4);
  PoolEntry<double> mtqTargetDipole = PoolEntry<double>(3);

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

#endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */