#ifndef MISSION_CONTROLLER_THERMALCONTROLLER_H_
#define MISSION_CONTROLLER_THERMALCONTROLLER_H_

#include <bsp_q7s/core/CoreDefinitions.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/devicehandlers/DeviceHandlerThermalSet.h>
#include <fsfw/timemanager/Countdown.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
#include <mission/acs/gyroAdisHelpers.h>
#include <mission/acs/imtqHelpers.h>
#include <mission/acs/rwHelpers.h>
#include <mission/acs/str/strHelpers.h>
#include <mission/acs/susMax1227Helpers.h>
#include <mission/com/syrlinksDefs.h>
#include <mission/controller/tcsDefs.h>
#include <mission/payload/payloadPcduDefinitions.h>
#include <mission/power/bpxBattDefs.h>
#include <mission/power/gsDefs.h>
#include <mission/tcs/HeaterHandler.h>
#include <mission/tcs/Max31865Definitions.h>
#include <mission/tcs/Tmp1075Definitions.h>
#include <mission/utility/trace.h>

#include <list>

/**
 * NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit
 * is exceeded.
 * OP Limit: Soft limit. Device should be switched off or TCS controller should take action if the
 * limit is exceeded to avoid reaching NOP limit
 */
struct TempLimits {
  TempLimits(float nopLowerLimit, float opLowerLimit, float cutOffLimit, float opUpperLimit,
             float nopUpperLimit)
      : opLowerLimit(opLowerLimit),
        opUpperLimit(opUpperLimit),
        cutOffLimit(cutOffLimit),
        nopLowerLimit(nopLowerLimit),
        nopUpperLimit(nopUpperLimit) {}
  float opLowerLimit;
  float opUpperLimit;
  float cutOffLimit;
  float nopLowerLimit;
  float nopUpperLimit;
};

struct ThermalState {
  uint8_t errorCounter;
  // Is heating on for that thermal module?
  bool heating = false;
  heater::Switch heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES;
  // Heater start time and end times as UNIX seconds. Please note that these times will be updated
  // when a switch command is sent, with no guarantess that the heater actually went on.
  uint32_t heaterStartTime = 0;
  uint32_t heaterEndTime = 0;
};

struct HeaterState {
  bool switchTransition;
  HeaterHandler::SwitchState target;
  uint8_t heaterSwitchControlCycles;
};

using HeaterSwitchStates = std::array<HeaterHandler::SwitchState, heater::NUMBER_OF_SWITCHES>;

enum ThermalComponents : uint8_t {
  NONE = 0,
  ACS_BOARD = 1,
  MGT = 2,
  RW = 3,
  STR = 4,
  IF_BOARD = 5,
  TCS_BOARD = 6,
  OBC = 7,
  OBCIF_BOARD = 8,
  SBAND_TRANSCEIVER = 9,
  PCDUP60_BOARD = 10,
  PCDUACU = 11,
  PCDUPDU = 12,
  PLPCDU_BOARD = 13,
  PLOCMISSION_BOARD = 14,
  PLOCPROCESSING_BOARD = 15,
  DAC = 16,
  CAMERA = 17,
  DRO = 18,
  X8 = 19,
  HPA = 20,
  TX = 21,
  MPA = 22,
  SCEX_BOARD = 23,
  NUM_ENTRIES
};

class ThermalController : public ExtendedControllerBase {
 public:
  static constexpr uint8_t SUBMODE_NO_HEATER_CTRL = 1;

  static const uint16_t INVALID_TEMPERATURE = 999;
  static const uint8_t NUMBER_OF_SENSORS = 16;
  static constexpr int16_t SANITY_LIMIT_LOWER_TEMP = -80;
  static constexpr int16_t SANITY_LIMIT_UPPER_TEMP = 160;

  ThermalController(object_id_t objectId, HeaterHandler& heater);

  ReturnValue_t initialize() override;

 protected:
  struct HeaterContext {
   public:
    HeaterContext(heater::Switch switchNr, heater::Switch redundantSwitchNr,
                  const TempLimits& tempLimit)
        : switchNr(switchNr), redSwitchNr(redundantSwitchNr), tempLimit(tempLimit) {}
    bool doHeaterHandling = true;
    heater::Switch switchNr;
    HeaterHandler::SwitchState switchState = HeaterHandler::SwitchState::OFF;
    heater::Switch redSwitchNr;
    const TempLimits& tempLimit;
  };

  void performThermalModuleCtrl(const HeaterSwitchStates& heaterSwitchStates);
  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;

 private:
  static const uint32_t INIT_DELAY = 1500;

  static const uint32_t TEMP_OFFSET = 5;

  enum class InternalState { STARTUP, INITIAL_DELAY, READY };

  InternalState internalState = InternalState::STARTUP;

  HeaterHandler& heaterHandler;

  tcsCtrl::SensorTemperatures sensorTemperatures;
  tcsCtrl::SusTemperatures susTemperatures;
  tcsCtrl::DeviceTemperatures deviceTemperatures;
  tcsCtrl::HeaterInfo heaterInfo;
  lp_vec_t<int16_t, 9> currentVecPdu2 =
      lp_vec_t<int16_t, 9>(gp_id_t(objects::PDU2_HANDLER, PDU::pool::PDU_CURRENTS));

  DeviceHandlerThermalSet imtqThermalSet;

  // Temperature Sensors
  MAX31865::PrimarySet maxSet0PlocHspd;
  MAX31865::PrimarySet maxSet1PlocMissionBrd;
  MAX31865::PrimarySet maxSet2PlCam;
  MAX31865::PrimarySet maxSet3DacHspd;
  MAX31865::PrimarySet maxSet4Str;
  MAX31865::PrimarySet maxSet5Rw1MxMy;
  MAX31865::PrimarySet maxSet6Dro;
  MAX31865::PrimarySet maxSet7Scex;
  MAX31865::PrimarySet maxSet8X8;
  MAX31865::PrimarySet maxSet9Hpa;
  MAX31865::PrimarySet maxSet10EbandTx;
  MAX31865::PrimarySet maxSet11Mpa;
  MAX31865::PrimarySet maxSet31865Set12;
  MAX31865::PrimarySet maxSet13PlPcduHspd;
  MAX31865::PrimarySet maxSet14TcsBrd;
  MAX31865::PrimarySet maxSet15Imtq;

  TMP1075::Tmp1075Dataset tmp1075SetTcs0;
  TMP1075::Tmp1075Dataset tmp1075SetTcs1;
  TMP1075::Tmp1075Dataset tmp1075SetPlPcdu0;
  // damaged
  // TMP1075::Tmp1075Dataset tmp1075SetPlPcdu1;
  TMP1075::Tmp1075Dataset tmp1075SetIfBoard;

  // SUS
  susMax1227::SusDataset susSet0;
  susMax1227::SusDataset susSet1;
  susMax1227::SusDataset susSet2;
  susMax1227::SusDataset susSet3;
  susMax1227::SusDataset susSet4;
  susMax1227::SusDataset susSet5;
  susMax1227::SusDataset susSet6;
  susMax1227::SusDataset susSet7;
  susMax1227::SusDataset susSet8;
  susMax1227::SusDataset susSet9;
  susMax1227::SusDataset susSet10;
  susMax1227::SusDataset susSet11;

  lp_var_t<float> tempQ7s = lp_var_t<float>(objects::CORE_CONTROLLER, core::PoolIds::TEMPERATURE);
  lp_var_t<int16_t> battTemp1 =
      lp_var_t<int16_t>(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_1);
  lp_var_t<int16_t> battTemp2 =
      lp_var_t<int16_t>(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_2);
  lp_var_t<int16_t> battTemp3 =
      lp_var_t<int16_t>(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_3);
  lp_var_t<int16_t> battTemp4 =
      lp_var_t<int16_t>(objects::BPX_BATT_HANDLER, BpxBattery::BATT_TEMP_4);
  lp_var_t<int32_t> tempRw1 = lp_var_t<int32_t>(objects::RW1, rws::TEMPERATURE_C);
  lp_var_t<int32_t> tempRw2 = lp_var_t<int32_t>(objects::RW2, rws::TEMPERATURE_C);
  lp_var_t<int32_t> tempRw3 = lp_var_t<int32_t>(objects::RW3, rws::TEMPERATURE_C);
  lp_var_t<int32_t> tempRw4 = lp_var_t<int32_t>(objects::RW4, rws::TEMPERATURE_C);
  lp_var_t<float> tempStartracker =
      lp_var_t<float>(objects::STAR_TRACKER, startracker::MCU_TEMPERATURE);
  lp_var_t<float> tempSyrlinksPowerAmplifier =
      lp_var_t<float>(objects::SYRLINKS_HANDLER, syrlinks::TEMP_POWER_AMPLIFIER);
  lp_var_t<float> tempSyrlinksBasebandBoard =
      lp_var_t<float>(objects::SYRLINKS_HANDLER, syrlinks::TEMP_BASEBAND_BOARD);
  lp_var_t<int16_t> tempMgt = lp_var_t<int16_t>(objects::IMTQ_HANDLER, imtq::MCU_TEMPERATURE);
  lp_vec_t<float, 3> tempAcu =
      lp_vec_t<float, 3>(objects::ACU_HANDLER, ACU::pool::ACU_TEMPERATURES);
  lp_var_t<float> tempPdu1 = lp_var_t<float>(objects::PDU1_HANDLER, PDU::pool::PDU_TEMPERATURE);
  lp_var_t<float> tempPdu2 = lp_var_t<float>(objects::PDU2_HANDLER, PDU::pool::PDU_TEMPERATURE);
  lp_var_t<float> temp1P60dock =
      lp_var_t<float>(objects::P60DOCK_HANDLER, P60Dock::pool::P60DOCK_TEMPERATURE_1);
  lp_var_t<float> temp2P60dock =
      lp_var_t<float>(objects::P60DOCK_HANDLER, P60Dock::pool::P60DOCK_TEMPERATURE_2);
  lp_var_t<float> tempGyro0 = lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, adis1650x::TEMPERATURE);
  lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, l3gd20h::TEMPERATURE);
  lp_var_t<float> tempGyro2 = lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, adis1650x::TEMPERATURE);
  lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, l3gd20h::TEMPERATURE);
  lp_var_t<float> tempMgm0 =
      lp_var_t<float>(objects::MGM_0_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
  lp_var_t<float> tempMgm2 =
      lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
  lp_var_t<float> tempAdcPayloadPcdu = lp_var_t<float>(objects::PLPCDU_HANDLER, plpcdu::TEMP);

  // TempLimits
  TempLimits acsBoardLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);
  TempLimits mgtLimits = TempLimits(-40.0, -40.0, 65.0, 70.0, 70.0);
  TempLimits rwLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);
  TempLimits strLimits = TempLimits(-30.0, -20.0, 65.0, 70.0, 80.0);
  TempLimits ifBoardLimits = TempLimits(-65.0, -40.0, 80.0, 85.0, 150.0);
  TempLimits tcsBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 130.0);
  TempLimits obcLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);
  TempLimits obcIfBoardLimits = TempLimits(-65.0, -40.0, 80.0, 85.0, 125.0);
  TempLimits sBandTransceiverLimits = TempLimits(-40.0, -25.0, 35.0, 40.0, 65.0);
  TempLimits pcduP60BoardLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0);
  TempLimits pcduAcuLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0);
  TempLimits pcduPduLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0);
  TempLimits plPcduBoardLimits = TempLimits(-55.0, -40.0, 80.0, 85.0, 125.0);
  TempLimits plocMissionBoardLimits = TempLimits(-30.0, -10.0, 40.0, 45.0, 60);
  TempLimits plocProcessingBoardLimits = TempLimits(-30.0, -10.0, 40.0, 45.0, 60.0);
  TempLimits dacLimits = TempLimits(-65.0, -40.0, 113.0, 118.0, 150.0);
  TempLimits cameraLimits = TempLimits(-40.0, -30.0, 60.0, 65.0, 85.0);
  TempLimits droLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
  TempLimits x8Limits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
  TempLimits hpaLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
  TempLimits txLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
  TempLimits mpaLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
  TempLimits scexBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 150.0);

  double sensorTemp = INVALID_TEMPERATURE;
  ThermalComponents thermalComponent = NONE;
  bool redSwitchNrInUse = false;
  MessageQueueId_t camId = MessageQueueIF::NO_QUEUE;
  bool componentAboveCutOffLimit = false;
  bool componentAboveUpperLimit = false;
  Event overHeatEventToTrigger;
  bool eBandTooHotFlag = false;
  bool camTooHotOneShotFlag = false;
  bool scexTooHotFlag = false;
  bool plocTooHotFlag = false;
  bool pcduSystemTooHotFlag = false;
  bool syrlinksTooHotFlag = false;
  bool obcTooHotFlag = false;
  bool strTooHotFlag = false;
  bool rwTooHotFlag = false;

  bool transitionWhenHeatersOff = false;
  uint32_t transitionWhenHeatersOffCycles = 0;
  Mode_t targetMode = MODE_OFF;
  Submode_t targetSubmode = SUBMODE_NONE;
  uint32_t cycles = 0;
  std::array<ThermalState, 30> thermalStates{};
  std::array<HeaterState, 7> heaterStates{};

  // Initial delay to make sure all pool variables have been initialized their owners.
  // Also, wait for system initialization to complete.
  Countdown initialCountdown = Countdown(INIT_DELAY);

#if OBSW_THREAD_TRACING == 1
  uint32_t opCounter = 0;
#endif

  std::array<std::pair<bool, double>, 5> sensors;
  uint8_t numSensors = 0;

  PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>({10.0});
  PoolEntry<float> tmp1075Tcs1 = PoolEntry<float>({10.0});
  PoolEntry<float> tmp1075PlPcdu0 = PoolEntry<float>({10.0});
  PoolEntry<float> tmp1075PlPcdu1 = PoolEntry<float>({10.0});
  PoolEntry<float> tmp1075IfBrd = PoolEntry<float>({10.0});
  PoolEntry<uint8_t> heaterSwitchStates = PoolEntry<uint8_t>(heater::NUMBER_OF_SWITCHES);
  PoolEntry<int16_t> heaterCurrent = PoolEntry<int16_t>();

  static constexpr dur_millis_t MUTEX_TIMEOUT = 50;

  void startTransition(Mode_t mode, Submode_t submode) override;

  bool heaterCtrlAllowed() const;
  void resetThermalStates();

  void resetSensorsArray();
  void copySensors();
  void copySus();
  void copyDevices();

  void ctrlComponentTemperature(HeaterContext& heaterContext);
  void checkLimitsAndCtrlHeater(HeaterContext& heaterContext);
  bool heaterCtrlCheckUpperLimits(HeaterContext& heaterContext);
  void heaterCtrlTempTooHighHandler(HeaterContext& heaterContext, const char* whatLimit);

  bool chooseHeater(heater::Switch& switchNr, heater::Switch redSwitchNr);
  bool selectAndReadSensorTemp(HeaterContext& htrCtx);

  void heaterSwitchHelper(heater::Switch switchNr, HeaterHandler::SwitchState state,
                          unsigned componentIdx);

  void ctrlAcsBoard();
  void ctrlMgt();
  void ctrlRw();
  void ctrlStr();
  void ctrlIfBoard();
  void ctrlTcsBoard();
  void ctrlObc();
  void ctrlObcIfBoard();
  void ctrlSBandTransceiver();
  void ctrlPcduP60Board();
  void ctrlPcduAcu();
  void ctrlPcduPdu();
  void ctrlPlPcduBoard();
  void ctrlPlocMissionBoard();
  void ctrlPlocProcessingBoard();
  void ctrlDac();
  void ctrlCameraBody();
  void ctrlDro();
  void ctrlX8();
  void ctrlHpa();
  void ctrlTx();
  void ctrlMpa();
  void ctrlScexBoard();
  void heaterTransitionControl(const HeaterSwitchStates& currentHeaterStates);
  void setMode(Mode_t mode, Submode_t submode);
  uint32_t tempFloatToU32() const;
  bool tooHotHandler(object_id_t object, bool& oneShotFlag);
  void tooHotHandlerWhichClearsOneShotFlag(object_id_t object, bool& oneShotFlag);
};

#endif /* MISSION_CONTROLLER_THERMALCONTROLLER_H_ */