#include "rwSpiCallback.h"

#include <fsfw/timemanager/Stopwatch.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 {

namespace {
static bool MODE_SET = false;

ReturnValue_t openSpi(const std::string& devname, int flags, GpioIF* gpioIF, gpioId_t gpioId,
                      MutexIF* mutex, MutexIF::TimeoutType timeoutType, uint32_t timeoutMs,
                      int& fd);
/**
 * @brief   This function closes a spi session. Pulls the chip select to high an releases the
 *          mutex.
 * @param gpioId    Gpio ID of chip select
 * @param gpioIF    Pointer to gpio interface to drive the chip select
 * @param mutex     The spi mutex
 */
void closeSpi(int fd, gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex);
}  // namespace

ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sendData,
                          size_t sendLen, void* args) {
  // Stopwatch watch;
  ReturnValue_t result = returnvalue::OK;

  RwHandler* handler = reinterpret_cast<RwHandler*>(args);
  if (handler == nullptr) {
    sif::error << "rwSpiCallback::spiCallback: Pointer to handler is invalid" << std::endl;
    return returnvalue::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->getCsMutex();
  cookie->getMutexParams(timeoutType, timeoutMs);
  if (mutex == nullptr) {
    sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
    return returnvalue::FAILED;
  }

  int fileDescriptor = 0;
  const std::string& dev = comIf->getSpiDev();
  result = openSpi(dev, O_RDWR, &gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor);
  if (result != returnvalue::OK) {
    return result;
  }

  spi::SpiModes spiMode = spi::SpiModes::MODE_0;
  uint32_t spiSpeed = 0;
  cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
  // We are in protected section, so we can use the static variable here without issues.
  // We don't need to set the speed because a SPI core is used, but the mode has to be set once
  // correctly for all RWs
  if (not MODE_SET) {
    comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
    MODE_SET = true;
  }

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

  if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
    sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
    closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
    return rws::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(fileDescriptor, gpioId, &gpioIF, mutex);
      return rws::SPI_WRITE_FAILURE;
    }
    idx++;
  }

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

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

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

  size_t replyBufferSize = cookie->getMaxBufferSize();

  // There must be a delay of at least 20 ms after sending the command.
  // Delay for 70 ms here and release the SPI bus for that duration.
  closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
  usleep(rws::SPI_REPLY_DELAY);
  result = openSpi(dev, O_RDWR, &gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor);
  if (result != returnvalue::OK) {
    return result;
  }

  /**
   * 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(fileDescriptor, gpioId, &gpioIF, mutex);
      return rws::SPI_READ_FAILURE;
    }
    if (idx == 0) {
      if (byteRead != FLAG_BYTE) {
        sif::error << "Invalid data, expected start marker" << std::endl;
        closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
        return rws::NO_START_MARKER;
      }
    }

    if (byteRead != FLAG_BYTE) {
      break;
    }

    if (idx == 9) {
      sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl;
      closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);
      return rws::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 = rws::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 = rws::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(fileDescriptor, gpioId, &gpioIF, mutex);
        result = rws::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 = rws::SPI_READ_FAILURE;
        break;
      }
      if (byteRead != FLAG_BYTE) {
        sif::error << "rwSpiCallback::spiCallback: Missing end sign " << static_cast<int>(FLAG_BYTE)
                   << std::endl;
        decodedFrameLen--;
        result = rws::MISSING_END_SIGN;
        break;
      }
    }
    result = returnvalue::OK;
  }

  cookie->setTransferSize(decodedFrameLen);

  closeSpi(fileDescriptor, gpioId, &gpioIF, mutex);

  return result;
}

namespace {

ReturnValue_t openSpi(const std::string& devname, int flags, GpioIF* gpioIF, gpioId_t gpioId,
                      MutexIF* mutex, MutexIF::TimeoutType timeoutType, uint32_t timeoutMs,
                      int& fd) {
  ReturnValue_t result = mutex->lockMutex(timeoutType, timeoutMs);
  if (result != returnvalue::OK) {
    sif::debug << "rwSpiCallback::spiCallback: Failed to lock mutex" << std::endl;
    return result;
  }

  fd = open(devname.c_str(), flags);
  if (fd < 0) {
    sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl;
    return SpiComIF::OPENING_FILE_FAILED;
  }

  // Pull SPI CS low. For now, no support for active high given
  if (gpioId != gpio::NO_GPIO) {
    result = gpioIF->pullLow(gpioId);
    if (result != returnvalue::OK) {
      sif::error << "rwSpiCallback::spiCallback: Failed to pull chip select low" << std::endl;
      return result;
    }
  }
  return returnvalue::OK;
}
void closeSpi(int fd, gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex) {
  close(fd);
  if (gpioId != gpio::NO_GPIO) {
    if (gpioIF->pullHigh(gpioId) != returnvalue::OK) {
      sif::error << "closeSpi: Failed to pull chip select high" << std::endl;
    }
  }
  if (mutex->unlockMutex() != returnvalue::OK) {
    sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl;
    ;
  }
}

}  // namespace

}  // namespace rwSpiCallback