#include "RwPollingTask.h"

#include <fcntl.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/tasks/SemaphoreFactory.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>
#include <fsfw_hal/common/spi/spiCommon.h>
#include <fsfw_hal/linux/utility.h>
#include <sys/ioctl.h>
#include <unistd.h>

#include "devConf.h"
#include "mission/devices/devicedefinitions/rwHelpers.h"

RwPollingTask::RwPollingTask(object_id_t objectId, const char* spiDev, GpioIF& gpioIF)
    : SystemObject(objectId), spiDev(spiDev), gpioIF(gpioIF) {
  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
  semaphore->acquire();
  ipcLock = MutexFactory::instance()->createMutex();
  spiLock = MutexFactory::instance()->createMutex();
}

ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
  for (unsigned i = 0; i < 4; i++) {
    if (rwCookies[i] == nullptr) {
      sif::error << "Invalid RW cookie at index" << i << std::endl;
      return returnvalue::FAILED;
    }
  }
  while (true) {
    ipcLock->lockMutex();
    state = InternalState::IDLE;
    ipcLock->unlockMutex();
    semaphore->acquire();
    // This loop takes 50 ms on a debug build.
    // Stopwatch watch;
    TaskFactory::delayTask(5);
    int fd = 0;
    for (auto& skip : skipCommandingForRw) {
      skip = false;
    }
    setAllReadFlagsFalse();
    ReturnValue_t result = openSpi(O_RDWR, fd);
    if (result != returnvalue::OK) {
      continue;
    }
    for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
      if (rwCookies[idx]->specialRequest == rws::SpecialRwRequest::RESET_MCU) {
        prepareSimpleCommand(rws::RESET_MCU);
        // No point in commanding that specific RW for the cycle.
        skipCommandingForRw[idx] = true;
        writeOneRwCmd(idx, fd);
      } else if (rwCookies[idx]->setSpeed) {
        prepareSetSpeedCmd(idx);
        if (writeOneRwCmd(idx, fd) != returnvalue::OK) {
          continue;
        }
      }
    }
    closeSpi(fd);
    if (readAllRws(rws::SET_SPEED) != returnvalue::OK) {
      continue;
    }
    prepareSimpleCommand(rws::GET_LAST_RESET_STATUS);
    if (writeAndReadAllRws(rws::GET_LAST_RESET_STATUS) != returnvalue::OK) {
      continue;
    }
    prepareSimpleCommand(rws::GET_RW_STATUS);
    if (writeAndReadAllRws(rws::GET_RW_STATUS) != returnvalue::OK) {
      continue;
    }
    prepareSimpleCommand(rws::GET_TEMPERATURE);
    if (writeAndReadAllRws(rws::GET_TEMPERATURE) != returnvalue::OK) {
      continue;
    }
    prepareSimpleCommand(rws::CLEAR_LAST_RESET_STATUS);
    if (writeAndReadAllRws(rws::CLEAR_LAST_RESET_STATUS) != returnvalue::OK) {
      continue;
    }
    handleSpecialRequests();
  }

  return returnvalue::OK;
}

ReturnValue_t RwPollingTask::initialize() { return returnvalue::OK; }

ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) {
  // 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 modeAndSpeedWasSet) {
    int fd = open(spiDev, O_RDWR);
    if (fd < 0) {
      sif::error << "could not open RW SPI bus" << std::endl;
      return returnvalue::FAILED;
    }
    spi::SpiModes mode = spi::RW_MODE;
    int retval = ioctl(fd, SPI_IOC_WR_MODE, reinterpret_cast<uint8_t*>(&mode));
    if (retval != 0) {
      utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Setting SPI mode failed");
    }

    retval = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi::RW_SPEED);
    if (retval != 0) {
      utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Setting SPI speed failed");
    }
    close(fd);
    modeAndSpeedWasSet = true;
  }

  auto* rwCookie = dynamic_cast<RwCookie*>(cookie);
  if (rwCookie == nullptr) {
    sif::error << "RwPollingTask::initializeInterface: Wrong cookie" << std::endl;
    return returnvalue::FAILED;
  }
  rwCookies[rwCookie->rwIdx] = rwCookie;

  return returnvalue::OK;
}

ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
                                         size_t sendLen) {
  if (sendData == nullptr or sendLen < 8) {
    return DeviceHandlerIF::INVALID_DATA;
  }
  int32_t speed = 0;
  uint16_t rampTime = 0;
  const uint8_t* currentBuf = sendData;
  bool setSpeed = currentBuf[0];
  currentBuf += 1;
  sendLen -= 1;
  SerializeAdapter::deSerialize(&speed, &currentBuf, &sendLen, SerializeIF::Endianness::MACHINE);
  SerializeAdapter::deSerialize(&rampTime, &currentBuf, &sendLen, SerializeIF::Endianness::MACHINE);
  rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
  if (sendLen == 8 and sendData[7] < static_cast<uint8_t>(rws::SpecialRwRequest::NUM_REQUESTS)) {
    specialRequest = static_cast<rws::SpecialRwRequest>(sendData[7]);
  }
  RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
  if (rwCookie == nullptr) {
    return returnvalue::FAILED;
  }
  {
    MutexGuard mg(ipcLock);
    rwCookie->setSpeed = setSpeed;
    rwCookie->currentRwSpeed = speed;
    rwCookie->currentRampTime = rampTime;
    rwCookie->specialRequest = specialRequest;
    if (state == InternalState::IDLE) {
      state = InternalState::BUSY;
      semaphore->release();
    }
  }
  return returnvalue::OK;
}

ReturnValue_t RwPollingTask::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }

ReturnValue_t RwPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
  return returnvalue::OK;
}

ReturnValue_t RwPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) {
  RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
  if (rwCookie == nullptr or rwCookie->bufLock == nullptr) {
    return returnvalue::FAILED;
  }
  {
    MutexGuard mg(rwCookie->bufLock);
    memcpy(rwCookie->exchangeBuf.data(), rwCookie->replyBuf.data(), rwCookie->replyBuf.size());
  }
  *buffer = rwCookie->exchangeBuf.data();
  *size = rwCookie->exchangeBuf.size();
  return returnvalue::OK;
}

ReturnValue_t RwPollingTask::writeAndReadAllRws(DeviceCommandId_t id) {
  // Stopwatch watch;
  ReturnValue_t result = returnvalue::OK;

  int fd = 0;
  result = openSpi(O_RDWR, fd);
  if (result != returnvalue::OK) {
    return result;
  }
  for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
    if (skipCommandingForRw[idx]) {
      continue;
    }
    result = sendOneMessage(fd, *rwCookies[idx]);
    if (result != returnvalue::OK) {
      closeSpi(fd);
      return returnvalue::FAILED;
    }
  }

  closeSpi(fd);
  return readAllRws(id);
}

ReturnValue_t RwPollingTask::openSpi(int flags, int& fd) {
  fd = open(spiDev, flags);
  if (fd < 0) {
    sif::error << "RwPollingTask::openSpi: Failed to open device file" << std::endl;
    return SpiComIF::OPENING_FILE_FAILED;
  }

  return returnvalue::OK;
}

ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf,
                                           size_t maxReplyLen) {
  ReturnValue_t result = returnvalue::OK;
  int fd = 0;
  gpioId_t gpioId = rwCookie.getChipSelectPin();
  uint8_t byteRead = 0;
  result = openSpi(O_RDWR, fd);
  if (result != returnvalue::OK) {
    return result;
  }
  pullCsLow(gpioId, gpioIF);
  bool lastByteWasFrameMarker = false;
  Countdown cd(3000);
  size_t readIdx = 0;

  while (true) {
    lastByteWasFrameMarker = false;
    if (read(fd, &byteRead, 1) != 1) {
      sif::error << "RwPollingTask: Read failed. " << strerror(errno) << std::endl;
      pullCsHigh(gpioId, gpioIF);
      closeSpi(fd);
      return rws::SPI_READ_FAILURE;
    }
    if (byteRead == rws::FRAME_DELIMITER) {
      lastByteWasFrameMarker = true;
    }
    // Start of frame detected.
    if (byteRead != rws::FRAME_DELIMITER and not lastByteWasFrameMarker) {
      break;
    }

    if (readIdx % 100 == 0 && cd.hasTimedOut()) {
      pullCsHigh(gpioId, gpioIF);
      closeSpi(fd);
      return rws::SPI_READ_FAILURE;
    }
    readIdx++;
  }

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

  size_t decodedFrameLen = 0;
  MutexGuard mg(rwCookie.bufLock);

  while (decodedFrameLen < maxReplyLen) {
    // First byte already read in
    if (decodedFrameLen != 0) {
      byteRead = 0;
      if (read(fd, &byteRead, 1) != 1) {
        sif::error << "RwPollingTask: Read failed" << std::endl;
        result = rws::SPI_READ_FAILURE;
        break;
      }
    }

    if (byteRead == rws::FRAME_DELIMITER) {
      // Reached end of frame
      break;
    } else if (byteRead == 0x7D) {
      if (read(fd, &byteRead, 1) != 1) {
        sif::error << "RwPollingTask: Read failed" << std::endl;
        result = rws::SPI_READ_FAILURE;
        break;
      }
      if (byteRead == 0x5E) {
        *(replyBuf + decodedFrameLen) = 0x7E;
        decodedFrameLen++;
        continue;
      } else if (byteRead == 0x5D) {
        *(replyBuf + decodedFrameLen) = 0x7D;
        decodedFrameLen++;
        continue;
      } else {
        sif::error << "RwPollingTask: Invalid substitute" << std::endl;
        result = rws::INVALID_SUBSTITUTE;
        break;
      }
    } else {
      *(replyBuf + decodedFrameLen) = byteRead;
      decodedFrameLen++;
      continue;
    }

    // Check end marker.
    /**
     * There might be the unlikely case that each byte in a get-telemetry reply has been
     * replaced by its substitute. Then the next byte must correspond to the end sign 0x7E.
     * Otherwise there might be something wrong.
     */
    if (decodedFrameLen == maxReplyLen) {
      if (read(fd, &byteRead, 1) != 1) {
        sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl;
        result = rws::SPI_READ_FAILURE;
        break;
      }
      if (byteRead != rws::FRAME_DELIMITER) {
        sif::error << "rwSpiCallback::spiCallback: Missing end sign "
                   << static_cast<int>(rws::FRAME_DELIMITER) << std::endl;
        decodedFrameLen--;
        result = rws::MISSING_END_SIGN;
        break;
      }
    }
    result = returnvalue::OK;
  }

  pullCsHigh(gpioId, gpioIF);
  closeSpi(fd);
  return result;
}

ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) {
  ReturnValue_t result = sendOneMessage(fd, *rwCookies[rwIdx]);
  if (result != returnvalue::OK) {
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

ReturnValue_t RwPollingTask::readAllRws(DeviceCommandId_t id) {
  // SPI dev will be opened in readNextReply on demand.
  for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
    if (((id == rws::SET_SPEED) and !rwCookies[idx]->setSpeed) or skipCommandingForRw[idx]) {
      continue;
    }
    uint8_t* replyBuf;
    size_t maxReadLen = idAndIdxToReadBuffer(id, idx, &replyBuf);
    ReturnValue_t result = readNextReply(*rwCookies[idx], replyBuf + 1, maxReadLen);
    if (result == returnvalue::OK) {
      // The first byte is always a flag which shows whether the value was read
      // properly at least once.
      replyBuf[0] = true;
    }
  }
  // SPI is closed in readNextReply as well.
  return returnvalue::OK;
}

size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** ptr) {
  uint8_t* rawStart = rwCookies[rwIdx]->replyBuf.data();
  RwReplies replies(rawStart);
  switch (id) {
    case (rws::GET_RW_STATUS): {
      *ptr = replies.rwStatusReply;
      break;
    }
    case (rws::SET_SPEED): {
      *ptr = replies.setSpeedReply;
      break;
    }
    case (rws::CLEAR_LAST_RESET_STATUS): {
      *ptr = replies.clearLastResetStatusReply;
      break;
    }
    case (rws::GET_LAST_RESET_STATUS): {
      *ptr = replies.getLastResetStatusReply;
      break;
    }
    case (rws::GET_TEMPERATURE): {
      *ptr = replies.readTemperatureReply;
      break;
    }
    case (rws::GET_TM): {
      *ptr = replies.hkDataReply;
      break;
    }
    case (rws::INIT_RW_CONTROLLER): {
      *ptr = replies.initRwControllerReply;
      break;
    }
    default: {
      sif::error << "no reply buffer for rw command " << id << std::endl;
      *ptr = replies.dummyPointer;
      return 0;
    }
  }
  return rws::idToPacketLen(id);
}

void RwPollingTask::fillSpecialRequestArray() {
  for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
    if (skipCommandingForRw[idx]) {
      specialRequestIds[idx] = DeviceHandlerIF::NO_COMMAND_ID;
      continue;
    }
    switch (rwCookies[idx]->specialRequest) {
      case (rws::SpecialRwRequest::GET_TM): {
        specialRequestIds[idx] = rws::GET_TM;
        break;
      }
      case (rws::SpecialRwRequest::INIT_RW_CONTROLLER): {
        specialRequestIds[idx] = rws::INIT_RW_CONTROLLER;
        break;
      }
      default: {
        specialRequestIds[idx] = DeviceHandlerIF::NO_COMMAND_ID;
      }
    }
  }
}

void RwPollingTask::handleSpecialRequests() {
  int fd = 0;
  fillSpecialRequestArray();
  ReturnValue_t result = openSpi(O_RDWR, fd);
  if (result != returnvalue::OK) {
    return;
  }
  for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
    if (specialRequestIds[idx] == DeviceHandlerIF::NO_COMMAND_ID) {
      continue;
    }
    prepareSimpleCommand(specialRequestIds[idx]);
    writeOneRwCmd(idx, fd);
  }
  closeSpi(fd);
  usleep(rws::SPI_REPLY_DELAY);
  for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
    if (specialRequestIds[idx] == DeviceHandlerIF::NO_COMMAND_ID) {
      continue;
    }
    uint8_t* replyBuf;
    size_t maxReadLen = idAndIdxToReadBuffer(specialRequestIds[idx], idx, &replyBuf);
    result = readNextReply(*rwCookies[idx], replyBuf, maxReadLen);
    if (result == returnvalue::OK) {
      // The first byte is always a flag which shows whether the value was read
      // properly at least once.
      replyBuf[0] = true;
    }
  }
}

void RwPollingTask::setAllReadFlagsFalse() {
  for (auto& rwCookie : rwCookies) {
    RwReplies replies(rwCookie->replyBuf.data());
    replies.getLastResetStatusReply[0] = false;
    replies.clearLastResetStatusReply[0] = false;
    replies.hkDataReply[0] = false;
    replies.readTemperatureReply[0] = false;
    replies.rwStatusReply[0] = false;
    replies.setSpeedReply[0] = false;
    replies.initRwControllerReply[0] = false;
  }
}

// This closes the SPI
void RwPollingTask::closeSpi(int fd) {
  // This will perform the function to close the SPI
  close(fd);
  // The SPI is now closed.
}

ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) {
  gpioId_t gpioId = rwCookie.getChipSelectPin();
  if (spiLock == nullptr) {
    sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
    return returnvalue::FAILED;
  }
  // Add datalinklayer like specified in the datasheet.
  size_t lenToSend = 0;
  rws::encodeHdlc(writeBuffer.data(), writeLen, encodedBuffer.data(), lenToSend);
  pullCsLow(gpioId, gpioIF);
  if (write(fd, encodedBuffer.data(), lenToSend) != static_cast<ssize_t>(lenToSend)) {
    sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
    pullCsHigh(gpioId, gpioIF);
    return rws::SPI_WRITE_FAILURE;
  }
  pullCsHigh(gpioId, gpioIF);
  return returnvalue::OK;
}

ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, GpioIF& gpioIF) {
  ReturnValue_t result = spiLock->lockMutex(TIMEOUT_TYPE, TIMEOUT_MS);
  if (result != returnvalue::OK) {
    sif::debug << "RwPollingTask::pullCsLow: Failed to lock mutex" << std::endl;
    return result;
  }
  // 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 << "RwPollingTask::pullCsLow: Failed to pull chip select low" << std::endl;
      return result;
    }
  }
  return returnvalue::OK;
}

void RwPollingTask::pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF) {
  if (gpioId != gpio::NO_GPIO) {
    if (gpioIF.pullHigh(gpioId) != returnvalue::OK) {
      sif::error << "closeSpi: Failed to pull chip select high" << std::endl;
    }
  }
  if (spiLock->unlockMutex() != returnvalue::OK) {
    sif::error << "RwPollingTask::pullCsHigh: Failed to unlock mutex" << std::endl;
    ;
  }
}

void RwPollingTask::prepareSimpleCommand(DeviceCommandId_t id) {
  writeBuffer[0] = static_cast<uint8_t>(id);
  uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 1, 0xFFFF);
  writeBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
  writeBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
  writeLen = 3;
}

ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) {
  writeBuffer[0] = static_cast<uint8_t>(rws::SET_SPEED);
  uint8_t* serPtr = writeBuffer.data() + 1;
  int32_t speedToSet = 0;
  uint16_t rampTimeToSet = 10;
  {
    MutexGuard mg(ipcLock);
    speedToSet = rwCookies[rwIdx]->currentRwSpeed;
    rampTimeToSet = rwCookies[rwIdx]->currentRampTime;
  }
  size_t serLen = 1;
  SerializeAdapter::serialize(&speedToSet, &serPtr, &serLen, writeBuffer.size(),
                              SerializeIF::Endianness::LITTLE);
  SerializeAdapter::serialize(&rampTimeToSet, &serPtr, &serLen, writeBuffer.size(),
                              SerializeIF::Endianness::LITTLE);

  uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 7, 0xFFFF);
  writeBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
  writeBuffer[8] = static_cast<uint8_t>((crc >> 8) & 0xFF);
  writeLen = 9;
  return returnvalue::OK;
}