#include "TemperatureSensorInserter.h"

#include <objects/systemObjectList.h>

#include <cmath>
#include <cstdlib>

TemperatureSensorInserter::TemperatureSensorInserter(object_id_t objectId)
    : SystemObject(objects::THERMAL_TEMP_INSERTER),
      max31865PlocHeatspreaderSet(objects::RTD_0_IC3_PLOC_HEATSPREADER, MAX31865::MAX31865_SET_ID),
      max31865PlocMissionboardSet(objects::RTD_1_IC4_PLOC_MISSIONBOARD, MAX31865::MAX31865_SET_ID) {
}

ReturnValue_t TemperatureSensorInserter::initialize() {
  max31865PlocHeatspreaderSet.temperatureCelcius = 20.0;
  max31865PlocMissionboardSet.temperatureCelcius = 20.0;
  return returnvalue::OK;
}

ReturnValue_t TemperatureSensorInserter::performOperation(uint8_t opCode) {
  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();
  return returnvalue::OK;
}