#include "TemperatureSensorInserter.h"

#include <fsfw/datapool/PoolReadGuard.h>
#include <objects/systemObjectList.h>

#include <cmath>
#include <cstdlib>
#include <utility>

TemperatureSensorInserter::TemperatureSensorInserter(object_id_t objectId,
                                                     Max31865DummyMap tempSensorDummies_,
                                                     Tmp1075DummyMap tempTmpSensorDummies_)
    : SystemObject(objectId),
      max31865DummyMap(std::move(tempSensorDummies_)),
      tmp1075DummyMap(std::move(tempTmpSensorDummies_)) {}

ReturnValue_t TemperatureSensorInserter::initialize() {
  testCase = TestCase::COOL_STR;
  return returnvalue::OK;
}

ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) {
  // TODO: deviceSensors
  if (not tempsWereInitialized) {
    for (auto& rtdDummy : max31865DummyMap) {
      rtdDummy.second->setTemperature(10, true);
    }
    for (auto& tmpDummy : tmp1075DummyMap) {
      tmpDummy.second->setTemperature(10, true);
    }
    tempsWereInitialized = true;
  }

  switch (testCase) {
    case (TestCase::NONE): {
      break;
    }
    case (TestCase::COOL_SYRLINKS): {
      // TODO: How do I insert this?
      // Does not work on EM, where a real syrlinks device is connected.
      if (cycles == 15) {
        lp_var_t<float> tempSyrlinksBasebandBoard =
            lp_var_t<float>(objects::SYRLINKS_HANDLER, syrlinks::TEMP_BASEBAND_BOARD);
        PoolReadGuard pg(&tempSyrlinksBasebandBoard);
        tempSyrlinksBasebandBoard.value = -50;
      }
      if (cycles == 30) {
        lp_var_t<float> tempSyrlinksBasebandBoard =
            lp_var_t<float>(objects::SYRLINKS_HANDLER, syrlinks::TEMP_BASEBAND_BOARD);
        PoolReadGuard pg(&tempSyrlinksBasebandBoard);
        tempSyrlinksBasebandBoard.value = 0;
      }
      break;
    }
    case (TestCase::COOL_HPA): {
      if (cycles == 15) {
        sif::debug << "Setting cold HPA temperature" << std::endl;
        max31865DummyMap[objects::RTD_9_IC12_HPA]->setTemperature(-60, true);
      }
      if (cycles == 30) {
        sif::debug << "Setting HPA temperature back to normal" << std::endl;
        max31865DummyMap[objects::RTD_9_IC12_HPA]->setTemperature(0, true);
      }
      break;
    }
    case (TestCase::COOL_MGT): {
      if (cycles == 15) {
        sif::debug << "Setting cold MGT temperature" << std::endl;
        max31865DummyMap[objects::RTD_15_IC18_IMTQ]->setTemperature(-60, true);
      }
      if (cycles == 30) {
        sif::debug << "Setting MGT temperature back to normal" << std::endl;
        max31865DummyMap[objects::RTD_15_IC18_IMTQ]->setTemperature(0, true);
      }
      break;
    }
    case(TestCase::COOL_STR): {
      if (cycles == 15) {
        sif::debug << "Setting cold STR temperature" << std::endl;
        max31865DummyMap[objects::RTD_4_IC7_STARTRACKER]->setTemperature(-40, true);
      }
      if (cycles == 30) {
        sif::debug << "Setting STR temperature back to normal" << std::endl;
        max31865DummyMap[objects::RTD_4_IC7_STARTRACKER]->setTemperature(0, true);
      }
      break;

    }
  }
  cycles++;
  return returnvalue::OK;
}
ReturnValue_t TemperatureSensorInserter::initializeAfterTaskCreation() { return returnvalue::OK; }