#include "ThermalController.h"

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

ReturnValue_t ThermalController::initialize() { return ControllerBase::initialize(); }

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;
  }

  componentTemperatures.rw = (sensorTemperatures.rw.value + sensorTemperatures.gps.value) / 2;

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

ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                         LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_RW, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_GPS, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(thermalControllerDefinitions::COMPONENT_RW, new PoolEntry<float>({0.0}));

  return RETURN_OK;
}
LocalPoolDataSetBase* ThermalController::getDataSetHandle(sid_t sid) { 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 MOVE_IS_INVALID;
  }
  return RETURN_OK;
}