#include "SusHandler.h"

#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw_hal/linux/spi/SpiComIF.h>

#include "OBSWConfig.h"

SusHandler::SusHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie,
                       LinuxLibgpioIF *gpioComIF, gpioId_t chipSelectId)
    : DeviceHandlerBase(objectId, comIF, comCookie), divider(5), dataset(this) {}

SusHandler::~SusHandler() {}

ReturnValue_t SusHandler::initialize() {
  ReturnValue_t result = RETURN_OK;
  result = DeviceHandlerBase::initialize();
  if (result != RETURN_OK) {
    return result;
  }
  return RETURN_OK;
}

void SusHandler::doStartUp() {
  if (comState == ComStates::IDLE) {
    comState = ComStates::WRITE_SETUP;
    commandExecuted = false;
  }
  if (comState == ComStates::WRITE_SETUP) {
    if (commandExecuted) {
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
      setMode(MODE_NORMAL);
#else
      setMode(_MODE_TO_ON);
#endif
      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); }

ReturnValue_t SusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  switch (comState) {
    case (ComStates::IDLE): {
      return NOTHING_TO_SEND;
    }
    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 HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                  const uint8_t *commandData,
                                                  size_t commandDataLen) {
  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;
      return RETURN_OK;
    }
    case (SUS::START_INT_TIMED_CONVERSIONS): {
      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
      cmdBuffer[0] = SUS::CONVERSION;
      rawPacket = cmdBuffer;
      rawPacketLen = 2;
      return RETURN_OK;
    }
    case (SUS::READ_INT_TIMED_CONVERSIONS): {
      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
      rawPacket = cmdBuffer;
      rawPacketLen = SUS::SIZE_READ_INT_CONVERSIONS;
      return RETURN_OK;
    }
    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, 0, false);
        cmdBuffer[idx * 2 + 1] = 0;
      }
      rawPacketLen = SUS::SIZE_READ_EXT_CONVERSIONS;
      return RETURN_OK;
    }
    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_FAILED;
}

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 = (*(packet) << 8 | *(packet + 1)) * 0.125;
      dataset.ain0 = (*(packet + 2) << 8 | *(packet + 3));
      dataset.ain1 = (*(packet + 4) << 8 | *(packet + 5));
      dataset.ain2 = (*(packet + 6) << 8 | *(packet + 7));
      dataset.ain3 = (*(packet + 8) << 8 | *(packet + 9));
      dataset.ain4 = (*(packet + 10) << 8 | *(packet + 11));
      dataset.ain5 = (*(packet + 12) << 8 | *(packet + 13));
      printDataset();
      break;
    }
    case (SUS::READ_EXT_TIMED_CONVERSIONS): {
      PoolReadGuard readSet(&dataset);
      dataset.ain0 = (packet[1] << 8) | packet[2];
      dataset.ain1 = (packet[3] << 8) | packet[4];
      dataset.ain2 = (packet[5] << 8) | packet[6];
      dataset.ain3 = (packet[7] << 8) | packet[8];
      dataset.ain4 = (packet[9] << 8) | packet[10];
      dataset.ain5 = (packet[11] << 8) | packet[12];
      // 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 = (packet[23] << 8) | packet[24];
      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 2000; }

ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                  LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(SUS::TEMPERATURE_C, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(SUS::AIN0, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SUS::AIN1, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SUS::AIN2, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SUS::AIN3, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SUS::AIN4, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SUS::AIN5, new PoolEntry<uint16_t>({0}));
  return HasReturnvaluesIF::RETURN_OK;
}

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

uint8_t SusHandler::buildConvByte(ScanModes scanMode, uint8_t channel, bool readTemp) {
  return (1 << 7) | (channel << 3) | (scanMode << 1) | readTemp;
}

void SusHandler::printDataset() {
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SUS == 1
  if (divider.checkAndIncrement()) {
    sif::info << "SUS Object ID 0x" << std::hex << this->getObjectId() << ":" << std::endl;
    sif::info << "Temperature: " << dataset.temperatureCelcius << " C" << std::endl;
    sif::info << "AIN0: " << std::dec << dataset.ain0 << std::endl;
    sif::info << "AIN1: " << std::dec << dataset.ain1 << std::endl;
    sif::info << "AIN2: " << std::dec << dataset.ain2 << std::endl;
    sif::info << "AIN3: " << std::dec << dataset.ain3 << std::endl;
    sif::info << "AIN4: " << std::dec << dataset.ain4 << std::endl;
    sif::info << "AIN5: " << std::dec << dataset.ain5 << std::endl;
  }
#endif
}