#include "Max31865EiveHandler.h"

#include <fsfw/datapool/PoolReadGuard.h>

Max31865EiveHandler::Max31865EiveHandler(object_id_t objectId, object_id_t comIF,
                                         CookieIF* comCookie)
    : DeviceHandlerBase(objectId, comIF, comCookie, nullptr),
      sensorDataset(this, EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
      debugDivider(5) {
  structLen = exchangeStruct.getSerializedSize();
}

void Max31865EiveHandler::doStartUp() {
  updatePeriodicReply(true, EiveMax31855::RtdCommands::EXCHANGE_SET_ID);
  if (state == InternalState::NONE or state == InternalState::INACTIVE) {
    if (instantNormal) {
      state = InternalState::ACTIVE;
    } else {
      state = InternalState::ON;
    }
    transitionOk = false;
  }
  if ((state == InternalState::ON or state == InternalState::ACTIVE) and transitionOk) {
    if (instantNormal) {
      setMode(MODE_NORMAL);
    } else {
      setMode(MODE_ON);
    }
  }
}

void Max31865EiveHandler::doShutDown() {
  if (state == InternalState::NONE or state == InternalState::ACTIVE or
      state == InternalState::ON) {
    state = InternalState::INACTIVE;
    transitionOk = false;
  }
  if (state == InternalState::INACTIVE and transitionOk) {
    updatePeriodicReply(false, EiveMax31855::RtdCommands::EXCHANGE_SET_ID);
    setMode(MODE_OFF);
  }
}

ReturnValue_t Max31865EiveHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  //*id = EiveMax31855::RtdCommands::EXCHANGE_SET_ID;
  return NOTHING_TO_SEND;
}

ReturnValue_t Max31865EiveHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  ReturnValue_t result = NOTHING_TO_SEND;
  if (state == InternalState::ON) {
    *id = EiveMax31855::RtdCommands::ON;
    result = buildCommandFromCommand(*id, nullptr, 0);
  }
  if (state == InternalState::ACTIVE) {
    *id = EiveMax31855::RtdCommands::ACTIVE;
    result = buildCommandFromCommand(*id, nullptr, 0);
  }
  if (state == InternalState::INACTIVE) {
    *id = EiveMax31855::RtdCommands::OFF;
    result = buildCommandFromCommand(*id, nullptr, 0);
  }
  return result;
}

ReturnValue_t Max31865EiveHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                           const uint8_t* commandData,
                                                           size_t commandDataLen) {
  auto cmdTyped = static_cast<EiveMax31855::RtdCommands>(deviceCommand);
  switch (cmdTyped) {
    case (EiveMax31855::RtdCommands::ON):
    case (EiveMax31855::RtdCommands::ACTIVE):
    case (EiveMax31855::RtdCommands::OFF):
    case (EiveMax31855::RtdCommands::CFG): {
      simpleCommand(cmdTyped);
      break;
    }
    case (EiveMax31855::RtdCommands::LOW_THRESHOLD):
    case (EiveMax31855::RtdCommands::HIGH_TRESHOLD): {
      break;
    }
    default:
      return NOTHING_TO_SEND;
  }
  return returnvalue::OK;
}

void Max31865EiveHandler::setInstantNormal(bool instantNormal) {
  this->instantNormal = instantNormal;
}

void Max31865EiveHandler::setDebugMode(bool enable, uint32_t divider) {
  this->debugMode = enable;
  debugDivider.setDivider(divider);
}

void Max31865EiveHandler::simpleCommand(EiveMax31855::RtdCommands cmd) {
  cmdBuf[0] = static_cast<uint8_t>(cmd);
  rawPacket = cmdBuf.data();
  rawPacketLen = 1;
}

void Max31865EiveHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
  if (getMode() == _MODE_TO_NORMAL) {
    if (state != InternalState::ACTIVE) {
      state = InternalState::ACTIVE;
      transitionOk = false;
    } else if (transitionOk) {
      setMode(MODE_NORMAL);
    }
  } else {
    DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
  }
}

void Max31865EiveHandler::fillCommandAndReplyMap() {
  insertInCommandMap(EiveMax31855::RtdCommands::ON);
  insertInCommandMap(EiveMax31855::RtdCommands::ACTIVE);
  insertInCommandMap(EiveMax31855::RtdCommands::OFF);
  insertInCommandMap(EiveMax31855::RtdCommands::CFG);
  insertInReplyMap(EiveMax31855::RtdCommands::EXCHANGE_SET_ID, 200, &sensorDataset, 0, true);
}

ReturnValue_t Max31865EiveHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                                DeviceCommandId_t* foundId, size_t* foundLen) {
  if (getMode() == _MODE_POWER_ON or getMode() == _MODE_WAIT_ON) {
    return IGNORE_FULL_PACKET;
  }
  if (remainingSize != structLen) {
    sif::error << "Invalid reply from RTD reader detected, reply size " << remainingSize
               << " not equal to exchange struct size " << structLen << std::endl;
    return DeviceHandlerIF::INVALID_DATA;
  }
  *foundId = EiveMax31855::RtdCommands::EXCHANGE_SET_ID;
  *foundLen = remainingSize;
  return returnvalue::OK;
}

ReturnValue_t Max31865EiveHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                        const uint8_t* packet) {
  size_t deserTmp = structLen;
  auto result = exchangeStruct.deSerialize(&packet, &deserTmp, SerializeIF::Endianness::MACHINE);
  if (result != returnvalue::OK) {
    return result;
  }
  if (getMode() == _MODE_TO_NORMAL and exchangeStruct.active and state == InternalState::ACTIVE) {
    transitionOk = true;
  }
  if (getMode() == _MODE_START_UP and exchangeStruct.configured and state == InternalState::ON) {
    transitionOk = true;
  }
  if (getMode() == _MODE_SHUT_DOWN and not exchangeStruct.active) {
    transitionOk = true;
    return returnvalue::OK;
  }

  // Calculate resistance
  float rtdValue = exchangeStruct.adcCode * EiveMax31855::RTD_RREF_PT1000 / INT16_MAX;
  // calculate approximation
  float approxTemp = exchangeStruct.adcCode / 32.0 - 256.0;

  PoolReadGuard pg(&sensorDataset);
  if (pg.getReadResult() != returnvalue::OK) {
    sif::warning << "Max31865EiveHandler: Failed to read sensor dataset" << std::endl;
    sensorDataset.temperatureCelcius.setValid(false);
    return returnvalue::OK;
  }
  sensorDataset.temperatureCelcius = approxTemp;
  sensorDataset.temperatureCelcius.setValid(true);

  if (debugMode) {
    if (debugDivider.checkAndIncrement()) {
      sif::info << "Max31865: " << std::setw(20) << std::left << locString << std::right
                << " | R[Ohm] " << rtdValue << " Ohms | Approx T[C]: " << approxTemp << std::endl;
    }
  }
  return returnvalue::OK;
}

uint32_t Max31865EiveHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 2000; }

ReturnValue_t Max31865EiveHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                           LocalDataPoolManager& poolManager) {
  using namespace MAX31865;
  localDataPoolMap.emplace(static_cast<lp_id_t>(PoolIds::RTD_VALUE), new PoolEntry<float>({0}));
  localDataPoolMap.emplace(static_cast<lp_id_t>(PoolIds::TEMPERATURE_C), new PoolEntry<float>({0}));
  localDataPoolMap.emplace(static_cast<lp_id_t>(PoolIds::LAST_FAULT_BYTE),
                           new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(static_cast<lp_id_t>(PoolIds::FAULT_BYTE), new PoolEntry<uint8_t>({0}));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(sensorDataset.getSid(), false, 30.0));
  return returnvalue::OK;
}

void Max31865EiveHandler::setDeviceInfo(uint8_t idx_, std::string location_) {
  idx = idx_;
  locString = std::move(location_);
}

ReturnValue_t Max31865EiveHandler::initialize() { return DeviceHandlerBase::initialize(); }