#include "TemperatureSensorsDummy.h"

#include <objects/systemObjectList.h>

#include <cmath>
#include <cstdlib>

TemperatureSensorsDummy::TemperatureSensorsDummy()
    : ExtendedControllerBase(objects::RTD_0_IC3_PLOC_HEATSPREADER),
      max31865PlocHeatspreaderSet(objects::RTD_0_IC3_PLOC_HEATSPREADER, MAX31865::MAX31865_SET_ID),
      max31865PlocMissionboardSet(objects::RTD_1_IC4_PLOC_MISSIONBOARD, MAX31865::MAX31865_SET_ID) {
  ObjectManager::instance()->insert(objects::RTD_1_IC4_PLOC_MISSIONBOARD, this);
  ObjectManager::instance()->insert(objects::RTD_2_IC5_4K_CAMERA, this);
  ObjectManager::instance()->insert(objects::RTD_3_IC6_DAC_HEATSPREADER, this);
  ObjectManager::instance()->insert(objects::RTD_4_IC7_STARTRACKER, this);
  ObjectManager::instance()->insert(objects::RTD_5_IC8_RW1_MX_MY, this);
  ObjectManager::instance()->insert(objects::RTD_6_IC9_DRO, this);
  ObjectManager::instance()->insert(objects::RTD_7_IC10_SCEX, this);
  ObjectManager::instance()->insert(objects::RTD_8_IC11_X8, this);
  ObjectManager::instance()->insert(objects::RTD_9_IC12_HPA, this);
  ObjectManager::instance()->insert(objects::RTD_10_IC13_PL_TX, this);
  ObjectManager::instance()->insert(objects::RTD_11_IC14_MPA, this);
  ObjectManager::instance()->insert(objects::RTD_12_IC15_ACU, this);
  ObjectManager::instance()->insert(objects::RTD_13_IC16_PLPCDU_HEATSPREADER, this);
  ObjectManager::instance()->insert(objects::RTD_14_IC17_TCS_BOARD, this);
  ObjectManager::instance()->insert(objects::RTD_15_IC18_IMTQ, this);
  ObjectManager::instance()->insert(objects::TMP1075_HANDLER_TCS_0, this);
  ObjectManager::instance()->insert(objects::TMP1075_HANDLER_TCS_1, this);
}

ReturnValue_t TemperatureSensorsDummy::initialize() {
  static bool done = false;
  if (not done) {
    done = true;
    ReturnValue_t result = ExtendedControllerBase::initialize();
    if (result != returnvalue::OK) {
      return result;
    }
  }

  max31865PlocHeatspreaderSet.temperatureCelcius = 20.0;
  max31865PlocMissionboardSet.temperatureCelcius = 20.0;
  return returnvalue::OK;
}

ReturnValue_t TemperatureSensorsDummy::handleCommandMessage(CommandMessage* message) {
  return returnvalue::FAILED;
}

void TemperatureSensorsDummy::performControlOperation() {
  iteration++;
  value = sin(iteration / 80. * M_PI) * 10;

  ReturnValue_t result = max31865PlocHeatspreaderSet.read();
  if (result != returnvalue::OK) {
    sif::warning << "Failed to read temperature from MAX31865 dataset" << std::endl;
  }
  max31865PlocHeatspreaderSet.rtdValue = value - 5;
  max31865PlocHeatspreaderSet.temperatureCelcius = value;
  if ((iteration % 100) < 20) {
    max31865PlocHeatspreaderSet.setValidity(false, true);
  } else {
    max31865PlocHeatspreaderSet.setValidity(true, true);
  }
  max31865PlocHeatspreaderSet.commit();
}

ReturnValue_t TemperatureSensorsDummy::initializeLocalDataPool(
    localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(static_cast<lp_id_t>(MAX31865::PoolIds::RTD_VALUE),
                           new PoolEntry<float>({0}));
  localDataPoolMap.emplace(static_cast<lp_id_t>(MAX31865::PoolIds::TEMPERATURE_C),
                           new PoolEntry<float>({0}));
  localDataPoolMap.emplace(static_cast<lp_id_t>(MAX31865::PoolIds::LAST_FAULT_BYTE),
                           new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(static_cast<lp_id_t>(MAX31865::PoolIds::FAULT_BYTE),
                           new PoolEntry<uint8_t>({0}));

  return returnvalue::OK;
}

LocalPoolDataSetBase* TemperatureSensorsDummy::getDataSetHandle(sid_t sid) {
  sif::debug << "getHandle" << std::endl;
  if (sid.objectId == objects::RTD_0_IC3_PLOC_HEATSPREADER) {
    return &max31865PlocHeatspreaderSet;
  } else if (sid.objectId == objects::RTD_1_IC4_PLOC_MISSIONBOARD) {
    return &max31865PlocMissionboardSet;
  } else {
    return nullptr;
  }
}

ReturnValue_t TemperatureSensorsDummy::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 returnvalue::OK;
}