#include "SyrlinksComHandler.h"

#include <fcntl.h>
#include <fsfw/ipc/MutexGuard.h>
#include <fsfw/tasks/SemaphoreFactory.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw_hal/linux/serial/SerialCookie.h>
#include <fsfw_hal/linux/serial/helper.h>
#include <unistd.h>

using namespace returnvalue;

SyrlinksComHandler::SyrlinksComHandler(object_id_t objectId)
    : SystemObject(objectId), ringBuf(2048, true) {
  lock = MutexFactory::instance()->createMutex();
  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
  semaphore->acquire();
}

ReturnValue_t SyrlinksComHandler::performOperation(uint8_t opCode) {
  while (true) {
    lock->lockMutex();
    state = State::SLEEPING;
    lock->unlockMutex();
    semaphore->acquire();
    readOneReply();
  }
  return returnvalue::OK;
}

ReturnValue_t SyrlinksComHandler::initializeInterface(CookieIF *cookie) {
  if (cookie == nullptr) {
    return returnvalue::FAILED;
  }
  SerialCookie *serCookie = dynamic_cast<SerialCookie *>(cookie);
  if (serCookie == nullptr) {
    return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
  }
  // comCookie = serCookie;
  std::string devname = serCookie->getDeviceFile();
  /* Get file descriptor */
  serialPort = open(devname.c_str(), O_RDWR);
  if (serialPort < 0) {
    sif::warning << "SyrlinksComHandler: open call failed with error [" << errno << ", "
                 << strerror(errno) << std::endl;
    return returnvalue::FAILED;
  }
  // Setting up UART parameters
  serial::setStopbits(tty, serCookie->getStopBits());
  serial::setParity(tty, serCookie->getParity());
  serial::setBitsPerWord(tty, BitsPerWord::BITS_8);
  tty.c_cflag &= ~CRTSCTS;  // Disable RTS/CTS hardware flow control
  serial::enableRead(tty);
  serial::ignoreCtrlLines(tty);

  // Use non-canonical mode and clear echo flag
  tty.c_lflag &= ~(ICANON | ECHO);

  // Non-blocking mode, use polling
  tty.c_cc[VTIME] = 0;
  tty.c_cc[VMIN] = 0;

  serial::setBaudrate(tty, serCookie->getBaudrate());
  if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
    sif::warning << "ScexUartReader::initializeInterface: tcsetattr call failed with error ["
                 << errno << ", " << strerror(errno) << std::endl;
  }
  // Flush received and unread data
  tcflush(serialPort, TCIOFLUSH);
  return returnvalue::OK;
}

ReturnValue_t SyrlinksComHandler::sendMessage(CookieIF *cookie, const uint8_t *sendData,
                                              size_t sendLen) {
  {
    MutexGuard mg(lock);
    if (state != State::SLEEPING) {
      return BUSY;
    }
  }
  serial::flushRxBuf(serialPort);

  ssize_t writtenBytes = write(serialPort, sendData, sendLen);
  if (writtenBytes != static_cast<ssize_t>(sendLen)) {
    sif::warning << "StrComHandler: Sending packet failed" << std::endl;
    return returnvalue::FAILED;
  }
  {
    MutexGuard mg(lock);
    state = State::ACTIVE;
  }
  semaphore->release();
  return returnvalue::OK;
}

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

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

ReturnValue_t SyrlinksComHandler::handleSerialReception() {
  ssize_t bytesRead = read(serialPort, reinterpret_cast<void *>(recBuf.data()),
                           static_cast<unsigned int>(recBuf.size()));
  if (bytesRead == 0) {
    return NO_SERIAL_DATA_READ;
  } else if (bytesRead < 0) {
    sif::warning << "SyrlinksComHandler: read call failed with error [" << errno << ", "
                 << strerror(errno) << "]" << std::endl;
    return FAILED;
  } else if (bytesRead >= static_cast<int>(recBuf.size())) {
    sif::error << "SyrlinksComHandler: Receive buffer too small for " << bytesRead << " bytes"
               << std::endl;
    return FAILED;
  } else if (bytesRead > 0) {
    // sif::info << "Received " << bytesRead << " bytes from the STR" << std::endl;
    // arrayprinter::print(recBuf.data(), bytesRead);
    ringBuf.writeData(recBuf.data(), bytesRead);
  }
  return OK;
}

ReturnValue_t SyrlinksComHandler::readReceivedMessage(CookieIF *cookie, uint8_t **buffer,
                                                      size_t *size) {
  MutexGuard mg(lock);
  if (replyResult != returnvalue::OK) {
    ReturnValue_t tmp = replyResult;
    replyResult = returnvalue::OK;
    return tmp;
  }
  if (replyLen == 0) {
    *size = 0;
    return returnvalue::OK;
  }
  *buffer = ipcBuf.data();
  *size = replyLen;
  replyLen = 0;
  return returnvalue::OK;
}

ReturnValue_t SyrlinksComHandler::readOneReply() {
  ReturnValue_t result;
  uint32_t nextDelayMs = 1;
  replyTimeout.resetTimer();
  while (true) {
    handleSerialReception();
    result = tryReadingOneSyrlinksReply();
    if (result == returnvalue::OK) {
      return returnvalue::OK;
    }
    if (replyTimeout.hasTimedOut()) {
      {
        MutexGuard mg(lock);
        replyResult = DeviceCommunicationIF::NO_REPLY_RECEIVED;
      }
      return returnvalue::FAILED;
    }
    TaskFactory::delayTask(nextDelayMs);
    if (nextDelayMs < 32) {
      nextDelayMs *= 2;
    }
  }
}
ReturnValue_t SyrlinksComHandler::tryReadingOneSyrlinksReply() {
  size_t bytesToRead = ringBuf.getAvailableReadData();
  if (bytesToRead == 0) {
    return returnvalue::OK;
  }
  bool startMarkerFound = false;
  size_t startIdx = 0;
  ringBuf.readData(recBuf.data(), bytesToRead);
  for (size_t idx = 0; idx < bytesToRead; idx++) {
    if (recBuf[idx] == START_MARKER) {
      if (startMarkerFound) {
        // Probably lost a packet. Discard broken packet.
        sif::warning << "SyrlinksComHandler: Detected 2 consecutive start markers" << std::endl;
        ringBuf.deleteData(idx);
      } else {
        startMarkerFound = true;
        startIdx = idx;
      }
    }
    if (recBuf[idx] == END_MARKER) {
      if (startMarkerFound) {
        {
          MutexGuard mg(lock);
          replyLen = idx - startIdx;
        }
        // Copy detected packet to IPC buffer so it can be passed back to the device handler.
        if (replyLen > ipcBuf.size()) {
          sif::error << "SyrlinksComHandler: Detected reply too large" << std::endl;
          ringBuf.deleteData(idx);
          return returnvalue::FAILED;
        }
        std::memcpy(ipcBuf.data(), recBuf.data() + startIdx, replyLen);
        ringBuf.deleteData(idx);
        return returnvalue::OK;
      } else {
        // Probably lost a packet. Discard broken packet.
        sif::warning << "SyrlinksComHandler: Detected 2 consecutive end markers" << std::endl;
        ringBuf.deleteData(idx);
      }
    }
  }
  return NO_PACKET_FOUND;
}