#include "PayloadPcduHandler.h"

#include <fsfw/src/fsfw/datapool/PoolReadGuard.h>

#ifdef FSFW_OSAL_LINUX
#include <fsfw_hal/linux/UnixFileGuard.h>
#include <fsfw_hal/linux/spi/SpiComIF.h>
#include <fsfw_hal/linux/spi/SpiCookie.h>
#include <fsfw_hal/linux/utility.h>
#include <sys/ioctl.h>
#endif

#include "devices/gpioIds.h"

PayloadPcduHandler::PayloadPcduHandler(object_id_t objectId, object_id_t comIF, CookieIF* cookie,
                                       GpioIF* gpioIF, bool periodicPrintout)
    : DeviceHandlerBase(objectId, comIF, cookie),
      adcSet(this),
      periodicPrintout(periodicPrintout),
      gpioIF(gpioIF) {}

void PayloadPcduHandler::doStartUp() {
  if ((state != States::PCDU_OFF) and (state != States::ON_TRANS_SSR)) {
    // Config error
    sif::error << "PayloadPcduHandler::doStartUp: Invalid state" << std::endl;
  }
  if (state == States::PCDU_OFF) {
    // Switch on relays here
    gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT0);
    gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT1);
    state = States::ON_TRANS_SSR;
    transitionOk = true;
  }
  if (state == States::ON_TRANS_SSR) {
    // If necessary, check whether a certain amount of time has elapsed
    if (transitionOk) {
      transitionOk = false;
      // We are now in ON mode
      startTransition(MODE_NORMAL, 0);
      adcState = AdcStates::BOOT_DELAY;
      // The ADC can now be read. If the values are not close to zero, we should not allow
      // transition
      monMode = MonitoringMode::CLOSE_TO_ZERO;
    }
  }
}

void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
  if (mode == _MODE_TO_NORMAL) {
    stateMachineToNormal();
  }
}

void PayloadPcduHandler::doShutDown() {}

ReturnValue_t PayloadPcduHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  switch (adcState) {
    case (AdcStates::SEND_SETUP): {
      *id = plpcdu::SETUP_CMD;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (AdcStates::NORMAL): {
      *id = plpcdu::READ_WITH_TEMP_EXT;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    default: {
      break;
    }
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t PayloadPcduHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  if (adcState == AdcStates::SEND_SETUP) {
    *id = plpcdu::SETUP_CMD;
    return buildCommandFromCommand(*id, nullptr, 0);
  }
  return NOTHING_TO_SEND;
}

void PayloadPcduHandler::fillCommandAndReplyMap() {
  insertInCommandAndReplyMap(plpcdu::READ_CMD, 2, &adcSet);
  insertInCommandAndReplyMap(plpcdu::READ_TEMP_EXT, 1, &adcSet);
  insertInCommandAndReplyMap(plpcdu::READ_WITH_TEMP_EXT, 1, &adcSet);
  insertInCommandAndReplyMap(plpcdu::SETUP_CMD, 1);
}

ReturnValue_t PayloadPcduHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                          const uint8_t* commandData,
                                                          size_t commandDataLen) {
  switch (deviceCommand) {
    case (plpcdu::SETUP_CMD): {
      cmdBuf[0] = plpcdu::SETUP_BYTE;
      rawPacket = cmdBuf.data();
      rawPacketLen = 1;
      break;
    }
    case (plpcdu::READ_CMD): {
      max1227::prepareExternallyClockedRead0ToN(cmdBuf.data(), plpcdu::CHANNEL_N, rawPacketLen);
      rawPacket = cmdBuf.data();
      break;
    }
    case (plpcdu::READ_TEMP_EXT): {
      max1227::prepareExternallyClockedTemperatureRead(cmdBuf.data(), rawPacketLen);
      rawPacket = cmdBuf.data();
      break;
    }
    case (plpcdu::READ_WITH_TEMP_EXT): {
      size_t sz = 0;
      max1227::prepareExternallyClockedRead0ToN(cmdBuf.data(), plpcdu::CHANNEL_N, sz);
      max1227::prepareExternallyClockedTemperatureRead(cmdBuf.data() + sz, sz);
      rawPacketLen = sz;
      rawPacket = cmdBuf.data();
      break;
    }
    default: {
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
  }
  return RETURN_OK;
}

ReturnValue_t PayloadPcduHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                               DeviceCommandId_t* foundId, size_t* foundLen) {
  // SPI is full duplex
  *foundId = getPendingCommand();
  *foundLen = remainingSize;
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                       const uint8_t* packet) {
  using namespace plpcdu;
  switch (id) {
    case (SETUP_CMD): {
      if (mode == _MODE_TO_NORMAL) {
        adcCmdExecuted = true;
      }
      break;
    }
    case (READ_TEMP_EXT): {
      uint8_t tempStartIdx = TEMP_REPLY_SIZE - 2;
      adcSet.tempC.value =
          max1227::getTemperature(packet[tempStartIdx] << 8 | packet[tempStartIdx + 1]);
      break;
    }
    case (READ_CMD): {
      PoolReadGuard pg(&adcSet);
      if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
        return pg.getReadResult();
      }
      handleExtConvRead(packet);
      handlePrintout();
      break;
    }
    case (READ_WITH_TEMP_EXT): {
      PoolReadGuard pg(&adcSet);
      if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
        return pg.getReadResult();
      }
      handleExtConvRead(packet);
      uint8_t tempStartIdx = ADC_REPLY_SIZE + TEMP_REPLY_SIZE - 2;
      adcSet.tempC.value =
          max1227::getTemperature(packet[tempStartIdx] << 8 | packet[tempStartIdx + 1]);
      handlePrintout();
      break;
    }
    default: {
      break;
    }
  }
  return HasReturnvaluesIF::RETURN_OK;
}

uint32_t PayloadPcduHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
  // 20 minutes transition delay is allowed
  return 20 * 60 * 60;
}

ReturnValue_t PayloadPcduHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                          LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::CHANNEL_VEC, &channelValues);
  localDataPoolMap.emplace(plpcdu::PlPcduPoolIds::TEMP, &tempC);
  return HasReturnvaluesIF::RETURN_OK;
}

void PayloadPcduHandler::setToGoToNormalModeImmediately(bool enable) {
  this->goToNormalMode = enable;
}

void PayloadPcduHandler::handleExtConvRead(const uint8_t* bufStart) {
  for (uint8_t idx = 0; idx < 12; idx++) {
    adcSet.channels[idx] = bufStart[idx * 2 + 1] << 8 | bufStart[idx * 2 + 2];
  }
}

void PayloadPcduHandler::handlePrintout() {
  if (periodicPrintout) {
    if (opDivider.checkAndIncrement()) {
      sif::info << "PL PCDU ADC hex [" << std::setfill('0') << std::hex;
      for (uint8_t idx = 0; idx < 12; idx++) {
        sif::info << std::setw(3) << adcSet.channels[idx];
        if (idx < 11) {
          sif::info << ",";
        }
      }
      sif::info << "] | T[C] " << std::dec << adcSet.tempC.value << std::endl;
    }
  }
}

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

void PayloadPcduHandler::stateMachineToNormal() {
  if (adcState == AdcStates::BOOT_DELAY) {
    if (adcCountdown.hasTimedOut()) {
      adcState = AdcStates::SEND_SETUP;
      adcCmdExecuted = false;
    }
  }
  if (adcState == AdcStates::SEND_SETUP) {
    if (adcCmdExecuted) {
      adcState = AdcStates::NORMAL;
      setMode(MODE_NORMAL, plpcdu::NORMAL_ADC_ONLY);
      adcCmdExecuted = false;
    }
  }
  if (submode == plpcdu::NORMAL_ALL_ON) {
    if (state == States::ON_TRANS_ADC_CLOSE_ZERO) {
      if (not commandExecuted) {
        countdown.resetTimer();
        commandExecuted = true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if (transitionOk and countdown.hasTimedOut()) {
        state = States::ON_TRANS_DRO;
        // Now start monitoring for negative voltages instead
        monMode = MonitoringMode::NEGATIVE;
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
    if (state == States::ON_TRANS_DRO) {
      if (not commandExecuted) {
        // Switch on DRO and start monitoring for negative voltagea
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_DRO);
        commandExecuted = true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if (transitionOk and countdown.hasTimedOut()) {
        state = States::ON_TRANS_X8;
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
    if (state == States::ON_TRANS_X8) {
      if (not commandExecuted) {
        // Switch on X8
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_X8);
        commandExecuted = true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if (transitionOk and countdown.hasTimedOut()) {
        state = States::ON_TRANS_TX;
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
    if (state == States::ON_TRANS_TX) {
      if (not commandExecuted) {
        // Switch on TX
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_TX);
        commandExecuted = true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if (transitionOk and countdown.hasTimedOut()) {
        state = States::ON_TRANS_MPA;
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
    if (state == States::ON_TRANS_MPA) {
      if (not commandExecuted) {
        // Switch on MPA
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_MPA);
        commandExecuted = true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if (transitionOk and countdown.hasTimedOut()) {
        state = States::ON_TRANS_HPA;
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
    if (state == States::ON_TRANS_HPA) {
      if (not commandExecuted) {
        // Switch on HPA
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_HPA);
        commandExecuted = true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if (transitionOk and countdown.hasTimedOut()) {
        state = States::PCDU_ON;
        setMode(MODE_NORMAL, plpcdu::NORMAL_ALL_ON);
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
  }
}

#ifdef FSFW_OSAL_LINUX
ReturnValue_t PayloadPcduHandler::extConvAsTwoCallback(SpiComIF* comIf, SpiCookie* cookie,
                                                       const uint8_t* sendData, size_t sendLen,
                                                       void* args) {
  auto handler = reinterpret_cast<PayloadPcduHandler*>(args);
  if (handler == nullptr) {
    sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
               << std::endl;
    return HasReturnvaluesIF::RETURN_FAILED;
  }
  DeviceCommandId_t currentCommand = handler->getPendingCommand();
  switch (currentCommand) {
    case (plpcdu::READ_WITH_TEMP_EXT): {
      return transferAsTwo(comIf, cookie, sendData, sendLen, false);
    }
    case (plpcdu::READ_TEMP_EXT): {
      return transferAsTwo(comIf, cookie, sendData, sendLen, true);
    }
    default: {
      return comIf->performRegularSendOperation(cookie, sendData, sendLen);
    }
  }
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t PayloadPcduHandler::transferAsTwo(SpiComIF* comIf, SpiCookie* cookie,
                                                const uint8_t* sendData, size_t sendLen,
                                                bool tempOnly) {
  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
  int retval = 0;
  // Prepare transfer
  int fileDescriptor = 0;
  std::string device = cookie->getSpiDevice();
  UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
  if (fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
    return SpiComIF::OPENING_FILE_FAILED;
  }
  spi::SpiModes spiMode = spi::SpiModes::MODE_0;
  uint32_t spiSpeed = 0;
  cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
  comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
  cookie->assignWriteBuffer(sendData);
  size_t transferLen = plpcdu::TEMP_REPLY_SIZE;
  if (not tempOnly) {
    transferLen += plpcdu::ADC_REPLY_SIZE;
  }
  cookie->setTransferSize(transferLen);

  gpioId_t gpioId = cookie->getChipSelectPin();
  GpioIF* gpioIF = comIf->getGpioInterface();
  MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
  uint32_t timeoutMs = 0;
  MutexIF* mutex = comIf->getMutex(&timeoutType, &timeoutMs);
  if (mutex == nullptr or gpioIF == nullptr) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::warning << "GyroADIS16507Handler::spiSendCallback: "
                    "Mutex or GPIO interface invalid"
                 << std::endl;
    return HasReturnvaluesIF::RETURN_FAILED;
#endif
  }

  if (gpioId != gpio::NO_GPIO) {
    result = mutex->lockMutex(timeoutType, timeoutMs);
    if (result != RETURN_OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
      sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
#endif
      return result;
    }
  }
  spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle();
  uint64_t origTx = transferStruct->tx_buf;
  uint64_t origRx = transferStruct->rx_buf;
  if (tempOnly) {
    transferLen = 1;
  } else {
    transferLen = plpcdu::ADC_REPLY_SIZE + 1;
  }
  transferStruct->len = transferLen;
  // Pull SPI CS low. For now, no support for active high given
  if (gpioId != gpio::NO_GPIO) {
    gpioIF->pullLow(gpioId);
  }

  // Execute transfer
  // Initiate a full duplex SPI transfer.
  retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
  if (retval < 0) {
    utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
    result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
  }
#if FSFW_HAL_SPI_WIRETAPPING == 1
  comIf->performSpiWiretapping(cookie);
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */

  if (gpioId != gpio::NO_GPIO) {
    gpioIF->pullHigh(gpioId);
  }

  transferStruct->tx_buf += transferLen;
  transferStruct->rx_buf += transferLen;
  transferStruct->len = plpcdu::TEMP_REPLY_SIZE - 1;
  if (gpioId != gpio::NO_GPIO) {
    gpioIF->pullLow(gpioId);
  }

  // Execute transfer
  // Initiate a full duplex SPI transfer.
  retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
  if (retval < 0) {
    utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
    result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
  }
#if FSFW_HAL_SPI_WIRETAPPING == 1
  comIf->performSpiWiretapping(cookie);
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */

  if (gpioId != gpio::NO_GPIO) {
    gpioIF->pullHigh(gpioId);
  }

  transferStruct->tx_buf = origTx;
  transferStruct->rx_buf = origRx;
  if (gpioId != gpio::NO_GPIO) {
    mutex->unlockMutex();
  }
  return HasReturnvaluesIF::RETURN_OK;
}
#endif