#include "rwSpiCallback.h"

#include "devices/gpioIds.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw_hal/linux/UnixFileGuard.h"
#include "fsfw_hal/linux/spi/SpiCookie.h"
#include "mission/devices/RwHandler.h"

namespace rwSpiCallback {

ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sendData,
                          size_t sendLen, void* args) {
  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;

  RwHandler* handler = reinterpret_cast<RwHandler*>(args);
  if (handler == nullptr) {
    sif::error << "rwSpiCallback::spiCallback: Pointer to handler is invalid" << std::endl;
    return HasReturnvaluesIF::RETURN_FAILED;
  }

  uint8_t writeBuffer[2];
  uint8_t writeSize = 0;

  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) {
    sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
    return HasReturnvaluesIF::RETURN_FAILED;
  }

  int fileDescriptor = 0;
  std::string device = cookie->getSpiDevice();
  UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "rwSpiCallback::spiCallback");
  if (fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
    sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl;
    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);

  result = mutex->lockMutex(timeoutType, timeoutMs);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    sif::debug << "rwSpiCallback::spiCallback: Failed to lock mutex" << std::endl;
    return result;
  }

  /** Sending frame start sign */
  writeBuffer[0] = 0x7E;
  writeSize = 1;

  // Pull SPI CS low. For now, no support for active high given
  if (gpioId != gpio::NO_GPIO) {
    if (gpioIF->pullLow(gpioId) != HasReturnvaluesIF::RETURN_OK) {
      sif::error << "rwSpiCallback::spiCallback: Failed to pull chip select low" << std::endl;
    }
  }

  if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
    sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
    closeSpi(gpioId, gpioIF, mutex);
    return RwHandler::SPI_WRITE_FAILURE;
  }

  /** Encoding and sending command */
  size_t idx = 0;
  while (idx < sendLen) {
    switch (*(sendData + idx)) {
      case 0x7E:
        writeBuffer[0] = 0x7D;
        writeBuffer[1] = 0x5E;
        writeSize = 2;
        break;
      case 0x7D:
        writeBuffer[0] = 0x7D;
        writeBuffer[1] = 0x5D;
        writeSize = 2;
        break;
      default:
        writeBuffer[0] = *(sendData + idx);
        writeSize = 1;
        break;
    }
    if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
      sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
      closeSpi(gpioId, gpioIF, mutex);
      return RwHandler::SPI_WRITE_FAILURE;
    }
    idx++;
  }

  /** Sending frame end sign */
  writeBuffer[0] = 0x7E;
  writeSize = 1;

  if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
    sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
    closeSpi(gpioId, gpioIF, mutex);
    return RwHandler::SPI_WRITE_FAILURE;
  }

  uint8_t* rxBuf = nullptr;
  result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    closeSpi(gpioId, gpioIF, mutex);
    return result;
  }

  size_t replyBufferSize = cookie->getMaxBufferSize();

  /** There must be a delay of at least 20 ms after sending the command */
  usleep(RwDefinitions::SPI_REPLY_DELAY);

  /**
   * The reaction wheel responds with empty frames while preparing the reply data.
   * However, receiving more than 5 empty frames will be interpreted as an error.
   */
  uint8_t byteRead = 0;
  for (int idx = 0; idx < 10; idx++) {
    if (read(fileDescriptor, &byteRead, 1) != 1) {
      sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
      closeSpi(gpioId, gpioIF, mutex);
      return RwHandler::SPI_READ_FAILURE;
    }
    if (idx == 0) {
      if (byteRead != FLAG_BYTE) {
        sif::error << "Invalid data, expected start marker" << std::endl;
        closeSpi(gpioId, gpioIF, mutex);
        return RwHandler::NO_START_MARKER;
      }
    }

    if (byteRead != FLAG_BYTE) {
      break;
    }

    if (idx == 9) {
      sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl;
      closeSpi(gpioId, gpioIF, mutex);
      return RwHandler::NO_REPLY;
    }
  }

#if FSFW_HAL_SPI_WIRETAPPING == 1
  sif::info << "RW start marker detected" << std::endl;
#endif

  size_t decodedFrameLen = 0;
  while (decodedFrameLen < replyBufferSize) {
    /** First byte already read in */
    if (decodedFrameLen != 0) {
      byteRead = 0;
      if (read(fileDescriptor, &byteRead, 1) != 1) {
        sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
        result = RwHandler::SPI_READ_FAILURE;
        break;
      }
    }

    if (byteRead == FLAG_BYTE) {
      /** Reached end of frame */
      break;
    } else if (byteRead == 0x7D) {
      if (read(fileDescriptor, &byteRead, 1) != 1) {
        sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl;
        result = RwHandler::SPI_READ_FAILURE;
        break;
      }
      if (byteRead == 0x5E) {
        *(rxBuf + decodedFrameLen) = 0x7E;
        decodedFrameLen++;
        continue;
      } else if (byteRead == 0x5D) {
        *(rxBuf + decodedFrameLen) = 0x7D;
        decodedFrameLen++;
        continue;
      } else {
        sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl;
        closeSpi(gpioId, gpioIF, mutex);
        result = RwHandler::INVALID_SUBSTITUTE;
        break;
      }
    } else {
      *(rxBuf + decodedFrameLen) = byteRead;
      decodedFrameLen++;
      continue;
    }

    /**
     * There might be the unlikely case that each byte in a get-telemetry reply has been
     * replaced by its substitute. Than the next byte must correspond to the end sign 0x7E.
     * Otherwise there might be something wrong.
     */
    if (decodedFrameLen == replyBufferSize) {
      if (read(fileDescriptor, &byteRead, 1) != 1) {
        sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl;
        result = RwHandler::SPI_READ_FAILURE;
        break;
      }
      if (byteRead != 0x7E) {
        sif::error << "rwSpiCallback::spiCallback: Missing end sign 0x7E" << std::endl;
        decodedFrameLen--;
        result = RwHandler::MISSING_END_SIGN;
        break;
      }
    }
    result = HasReturnvaluesIF::RETURN_OK;
  }

  cookie->setTransferSize(decodedFrameLen);

  closeSpi(gpioId, gpioIF, mutex);

  return result;
}

void closeSpi(gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex) {
  if (gpioId != gpio::NO_GPIO) {
    if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
      sif::error << "closeSpi: Failed to pull chip select high" << std::endl;
    }
  }
  if (mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
    sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl;
    ;
  }
}
}  // namespace rwSpiCallback