#include "ThermalController.h"

ThermalController::ThermalController(object_id_t objectId, object_id_t parentId)
    : ExtendedControllerBase(objectId, parentId),
      sensorTemperatures(this),
      componentTemperatures(this) {}

ReturnValue_t ThermalController::initialize() {
  auto result = ExtendedControllerBase::initialize();

  return result;
}

ReturnValue_t ThermalController::handleCommandMessage(CommandMessage* message) {
  return RETURN_FAILED;
}
void ThermalController::performControlOperation() {
  ReturnValue_t result = sensorTemperatures.read();
  if (result != RETURN_OK) {
    return;
  }
  result = componentTemperatures.read();
  if (result != RETURN_OK) {
    return;
  }

  sensorTemperatures.sensor_ploc_heatspreader =
      ((int)sensorTemperatures.sensor_ploc_heatspreader.value + 1) % 100;
  sensorTemperatures.sensor_ploc_missionboard =
      ((int)sensorTemperatures.sensor_ploc_missionboard.value + 2) % 100;
  sensorTemperatures.sensor_4k_camera = ((int)sensorTemperatures.sensor_4k_camera.value + 3) % 100;
  sensorTemperatures.sensor_dac_heatspreader =
      ((int)sensorTemperatures.sensor_dac_heatspreader.value + 4) % 100;
  sensorTemperatures.sensor_startracker =
      ((int)sensorTemperatures.sensor_startracker.value + 10) % 100;
  sensorTemperatures.sensor_rw1 = ((int)sensorTemperatures.sensor_rw1.value + 10) % 100;
  sensorTemperatures.sensor_dro = ((int)sensorTemperatures.sensor_dro.value + 10) % 100;
  sensorTemperatures.sensor_scex = ((int)sensorTemperatures.sensor_scex.value + 10) % 100;
  sensorTemperatures.sensor_x8 = ((int)sensorTemperatures.sensor_x8.value + 10) % 100;
  sensorTemperatures.sensor_hpa = ((int)sensorTemperatures.sensor_hpa.value + 10) % 100;
  sensorTemperatures.sensor_tx_modul = ((int)sensorTemperatures.sensor_tx_modul.value + 10) % 100;
  sensorTemperatures.sensor_mpa = ((int)sensorTemperatures.sensor_mpa.value + 10) % 100;
  sensorTemperatures.sensor_acu = ((int)sensorTemperatures.sensor_acu.value + 10) % 100;
  sensorTemperatures.sensor_plpcdu_heatspreader =
      ((int)sensorTemperatures.sensor_plpcdu_heatspreader.value + 10) % 100;
  sensorTemperatures.sensor_tcs_board = ((int)sensorTemperatures.sensor_tcs_board.value + 10) % 100;
  sensorTemperatures.sensor_magnettorquer =
      ((int)sensorTemperatures.sensor_magnettorquer.value + 10) % 100;

  sensorTemperatures.commit();
  componentTemperatures.commit();
}

ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                         LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLOC_HEATSPREADER,
                           new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLOC_MISSIONBOARD,
                           new PoolEntry<float>({1.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_4K_CAMERA,
                           new PoolEntry<float>({2.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_DAC_HEATSPREADER,
                           new PoolEntry<float>({3.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_STARTRACKER,
                           new PoolEntry<float>({4.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_RW1, new PoolEntry<float>({5.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_DRO, new PoolEntry<float>({6.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_SCEX, new PoolEntry<float>({7.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_X8, new PoolEntry<float>({8.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_HPA, new PoolEntry<float>({9.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TX_MODUL,
                           new PoolEntry<float>({10.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_MPA, new PoolEntry<float>({11.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_ACU, new PoolEntry<float>({12.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLPCDU_HEATSPREADER,
                           new PoolEntry<float>({13.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TCS_BOARD,
                           new PoolEntry<float>({14.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_MAGNETTORQUER,
                           new PoolEntry<float>({15.0}));

  localDataPoolMap.emplace(thermalControllerDefinitions::COMPONENT_RW, new PoolEntry<float>({0.0}));

  poolManager.subscribeForPeriodicPacket(sensorTemperatures.getSid(), true, 1.0, false);

  return RETURN_OK;
}

LocalPoolDataSetBase* ThermalController::getDataSetHandle(sid_t sid) {
  switch (sid.ownerSetId) {
    case thermalControllerDefinitions::SENSOR_TEMPERATURES:
      return &sensorTemperatures;
    case thermalControllerDefinitions::COMPONENT_TEMPERATURES:
      return &componentTemperatures;
    default:
      return nullptr;
  }
}

ReturnValue_t ThermalController::checkModeCommand(Mode_t mode, Submode_t submode,
                                                  uint32_t* msToReachTheMode) {
  if (submode != SUBMODE_NONE) {
    return INVALID_SUBMODE;
  }
  if ((mode != MODE_OFF) && (mode != MODE_ON) && (mode != MODE_NORMAL)) {
    return INVALID_MODE;
  }
  return RETURN_OK;
}