#include "SusHandler.h"

#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/arrayprinter.h>

#include "OBSWConfig.h"

SusHandler::SusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF, CookieIF *comCookie)
    : DeviceHandlerBase(objectId, comIF, comCookie), divider(5), dataset(this), susIdx(susIdx) {}

SusHandler::~SusHandler() {}

void SusHandler::doStartUp() {
  if (comState == ComStates::IDLE) {
    comState = ComStates::WRITE_SETUP;
    commandExecuted = false;
  }
  if (comState == ComStates::WRITE_SETUP) {
    if (commandExecuted) {
      if (goToNormalModeImmediately) {
        setMode(MODE_NORMAL);
      } else {
        setMode(_MODE_TO_ON);
      }
      commandExecuted = false;
      if (clkMode == ClkModes::INT_CLOCKED) {
        comState = ComStates::START_INT_CLOCKED_CONVERSIONS;
      } else {
        comState = ComStates::EXT_CLOCKED_CONVERSIONS;
      }
    }
  }
}

void SusHandler::doShutDown() {
  setMode(_MODE_POWER_DOWN);
  comState = ComStates::IDLE;
}

ReturnValue_t SusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  switch (comState) {
    case (ComStates::IDLE): {
      break;
    }
    case (ComStates::WRITE_SETUP): {
      *id = SUS::WRITE_SETUP;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (ComStates::EXT_CLOCKED_CONVERSIONS): {
      *id = SUS::READ_EXT_TIMED_CONVERSIONS;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (ComStates::START_INT_CLOCKED_CONVERSIONS): {
      *id = SUS::START_INT_TIMED_CONVERSIONS;
      comState = ComStates::READ_INT_CLOCKED_CONVERSIONS;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (ComStates::READ_INT_CLOCKED_CONVERSIONS): {
      *id = SUS::READ_INT_TIMED_CONVERSIONS;
      comState = ComStates::START_INT_CLOCKED_CONVERSIONS;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (ComStates::EXT_CLOCKED_TEMP): {
      *id = SUS::READ_EXT_TIMED_TEMPS;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t SusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  if (comState == ComStates::WRITE_SETUP) {
    *id = SUS::WRITE_SETUP;
    return buildCommandFromCommand(*id, nullptr, 0);
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t SusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                  const uint8_t *commandData,
                                                  size_t commandDataLen) {
  using namespace max1227;
  switch (deviceCommand) {
    case (SUS::WRITE_SETUP): {
      if (clkMode == ClkModes::INT_CLOCKED) {
        cmdBuffer[0] = SUS::SETUP_INT_CLOKED;
      } else {
        cmdBuffer[0] = SUS::SETUP_EXT_CLOCKED;
      }

      rawPacket = cmdBuffer;
      rawPacketLen = 1;
      break;
    }
    case (SUS::START_INT_TIMED_CONVERSIONS): {
      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
      cmdBuffer[0] = max1227::buildResetByte(true);
      cmdBuffer[1] = SUS::CONVERSION;
      rawPacket = cmdBuffer;
      rawPacketLen = 2;
      break;
    }
    case (SUS::READ_INT_TIMED_CONVERSIONS): {
      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
      rawPacket = cmdBuffer;
      rawPacketLen = SUS::SIZE_READ_INT_CONVERSIONS;
      break;
    }
    case (SUS::READ_EXT_TIMED_CONVERSIONS): {
      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
      rawPacket = cmdBuffer;
      for (uint8_t idx = 0; idx < 6; idx++) {
        cmdBuffer[idx * 2] = buildConvByte(ScanModes::N_ONCE, idx, false);
        cmdBuffer[idx * 2 + 1] = 0;
      }
      cmdBuffer[12] = 0x00;
      rawPacketLen = SUS::SIZE_READ_EXT_CONVERSIONS;
      break;
    }
    case (SUS::READ_EXT_TIMED_TEMPS): {
      cmdBuffer[0] = buildConvByte(ScanModes::N_ONCE, 0, true);
      std::memset(cmdBuffer + 1, 0, 24);
      rawPacket = cmdBuffer;
      rawPacketLen = 25;
      break;
    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return HasReturnvaluesIF::RETURN_OK;
}

void SusHandler::fillCommandAndReplyMap() {
  insertInCommandAndReplyMap(SUS::WRITE_SETUP, 1);
  insertInCommandAndReplyMap(SUS::START_INT_TIMED_CONVERSIONS, 1);
  insertInCommandAndReplyMap(SUS::READ_INT_TIMED_CONVERSIONS, 1, &dataset,
                             SUS::SIZE_READ_INT_CONVERSIONS);
  insertInCommandAndReplyMap(SUS::READ_EXT_TIMED_CONVERSIONS, 1, &dataset,
                             SUS::SIZE_READ_EXT_CONVERSIONS);
  insertInCommandAndReplyMap(SUS::READ_EXT_TIMED_TEMPS, 1);
}

ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t remainingSize,
                                       DeviceCommandId_t *foundId, size_t *foundLen) {
  *foundId = this->getPendingCommand();
  *foundLen = remainingSize;
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
  switch (id) {
    case SUS::WRITE_SETUP: {
      if (mode == _MODE_START_UP) {
        commandExecuted = true;
      }
      return HasReturnvaluesIF::RETURN_OK;
    }
    case SUS::START_INT_TIMED_CONVERSIONS: {
      return HasReturnvaluesIF::RETURN_OK;
    }
    case SUS::READ_INT_TIMED_CONVERSIONS: {
      PoolReadGuard readSet(&dataset);
      dataset.temperatureCelcius = max1227::getTemperature(((packet[0] & 0x0f) << 8) | packet[1]);
      for (uint8_t idx = 0; idx < 6; idx++) {
        dataset.channels[idx] = packet[idx * 2 + 2] << 8 | packet[idx * 2 + 3];
      }
      dataset.setValidity(true, true);
      printDataset();
      break;
    }
    case (SUS::READ_EXT_TIMED_CONVERSIONS): {
      PoolReadGuard readSet(&dataset);
      for (uint8_t idx = 0; idx < 6; idx++) {
        dataset.channels[idx] = packet[idx * 2 + 1] << 8 | packet[idx * 2 + 2];
      }
      dataset.channels.setValid(true);
      // Read temperature in next read cycle
      if (clkMode == ClkModes::EXT_CLOCKED_WITH_TEMP) {
        comState = ComStates::EXT_CLOCKED_TEMP;
      }
      printDataset();
      break;
    }
    case (SUS::READ_EXT_TIMED_TEMPS): {
      PoolReadGuard readSet(&dataset);
      dataset.temperatureCelcius = max1227::getTemperature(((packet[23] & 0x0f) << 8) | packet[24]);
      dataset.temperatureCelcius.setValid(true);
      comState = ComStates::EXT_CLOCKED_CONVERSIONS;
      break;
    }
    default: {
      sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl;
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  return HasReturnvaluesIF::RETURN_OK;
}

uint32_t SusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 3000; }

ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                  LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(SUS::TEMPERATURE_C, &tempC);
  localDataPoolMap.emplace(SUS::CHANNEL_VEC, &channelVec);
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(dataset.getSid(), false, 5.0));
  return HasReturnvaluesIF::RETURN_OK;
}

void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalModeImmediately = enable; }

void SusHandler::printDataset() {
  if (periodicPrintout) {
    if (divider.checkAndIncrement()) {
      sif::info << "SUS ADC " << static_cast<int>(susIdx) << " hex [" << std::setfill('0')
                << std::hex;
      for (uint8_t idx = 0; idx < 6; idx++) {
        sif::info << std::setw(3) << dataset.channels[idx];
        if (idx < 6 - 1) {
          sif::info << ",";
        }
      }
      sif::info << "] | T[C] " << std::dec << dataset.temperatureCelcius.value << std::endl;
    }
  }
}

void SusHandler::enablePeriodicPrintout(bool enable, uint8_t divider) {
  this->periodicPrintout = enable;
  this->divider.setDivider(divider);
}