#include "ScexUartReader.h"

#include <fcntl.h>  // Contains file controls like O_RDWR
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/ipc/MutexFactory.h>
#include <fsfw/ipc/MutexGuard.h>
#include <fsfw/tasks/SemaphoreFactory.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw_hal/linux/uart/UartCookie.h>
#include <unistd.h>  // write(), read(), close()

#include <cerrno>  // Error integer and strerror() function
#include <iostream>

#include "OBSWConfig.h"

using namespace returnvalue;

ScexUartReader::ScexUartReader(object_id_t objectId)
    : SystemObject(objectId),
      decodeRingBuf(4096, true),
      ipcRingBuf(200 * 2048, true),
      ipcQueue(200),
      dleParser(decodeRingBuf, dleEncoder, {encodedBuf.data(), encodedBuf.size()},
                {decodedBuf.data(), decodedBuf.size()}) {
  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
  semaphore->acquire();
  lock = MutexFactory::instance()->createMutex();
}

ReturnValue_t ScexUartReader::performOperation(uint8_t operationCode) {
  lock->lockMutex();
  state = States::IDLE;
  lock->unlockMutex();
  while (true) {
    semaphore->acquire();
    int bytesRead = 0;
    // debugMode = true;
    while (true) {
      bytesRead = read(serialPort, reinterpret_cast<void *>(recBuf.data()),
                       static_cast<unsigned int>(recBuf.size()));
      if (bytesRead == 0) {
        {
          MutexGuard mg(lock);
          if (state == States::FINISH) {
            dleParser.reset();
            // Flush received and unread data
            tcflush(serialPort, TCIOFLUSH);
            state = States::IDLE;
            break;
          }
        }
        size_t bytesRead = 0;
        ReturnValue_t result = returnvalue::OK;
        // Can be used to read frame, parity and overrun errors
        // serial_icounter_struct icounter{};
        // uart::readCountersAndErrors(serialPort, icounter);
        while (result != DleParser::NO_PACKET_FOUND) {
          result = dleParser.parseRingBuf(bytesRead);
          if (result == returnvalue::OK) {
            // sif::info << "Found dle packet, read " << bytesRead << " bytes" << std::endl;
            // Packet found, advance read pointer.
            auto &decodedPacket = dleParser.getContext().decodedPacket;
            handleFoundDlePacket(decodedPacket.first, decodedPacket.second);
            dleParser.confirmBytesRead(bytesRead);
          } else if (result != DleParser::NO_PACKET_FOUND) {
            sif::warning << "ScexUartReader::performOperation: Possible packet loss" << std::endl;
            // Markers found at wrong place
            // which might be a hint for a possibly lost packet.
            dleParser.defaultErrorHandler();
            dleParser.confirmBytesRead(bytesRead);
          }
        }
        TaskFactory::delayTask(400);
      } else if (bytesRead < 0) {
        sif::warning << "ScexUartReader::performOperation: read call failed with error [" << errno
                     << ", " << strerror(errno) << "]" << std::endl;
        break;
      } else if (bytesRead >= static_cast<int>(recBuf.size())) {
        sif::error << "ScexUartReader::performOperation: Receive buffer too small for " << bytesRead
                   << " bytes" << std::endl;
      } else if (bytesRead > 0) {
        ReturnValue_t result = dleParser.passData(recBuf.data(), bytesRead);
        if (debugMode) {
          sif::info << "Received " << bytesRead
                    << " bytes from the Solar Cell Experiment:" << std::endl;
        }
        if (result != OK) {
          sif::warning << "ScexUartReader::performOperation: Passing data to DLE parser failed"
                       << std::endl;
        }
      }
    };
  }
  return OK;
}

ReturnValue_t ScexUartReader::initializeInterface(CookieIF *cookie) {
  UartCookie *uartCookie = dynamic_cast<UartCookie *>(cookie);
  if (uartCookie == nullptr) {
    return FAILED;
  }
  std::string devname = uartCookie->getDeviceFile();
  /* Get file descriptor */
  serialPort = open(devname.c_str(), O_RDWR);
  if (serialPort < 0) {
    sif::warning << "ScexUartReader::initializeInterface: open call failed with error [" << errno
                 << ", " << strerror(errno) << std::endl;
    return FAILED;
  }
  // Setting up UART parameters
  tty.c_cflag &= ~PARENB;  // Clear parity bit
  if (uartCookie->getStopBits() == StopBits::TWO_STOP_BITS) {
    // Use two stop bits
    tty.c_cflag |= CSTOPB;
  } else {
    // Clear stop field, only one stop bit used in communication
    tty.c_cflag &= ~CSTOPB;
  }

  tty.c_cflag &= ~CSIZE;          // Clear all the size bits
  tty.c_cflag |= CS8;             // 8 bits per byte
  tty.c_cflag &= ~CRTSCTS;        // Disable RTS/CTS hardware flow control
  tty.c_cflag |= CREAD | CLOCAL;  // Turn on READ & ignore ctrl lines (CLOCAL = 1)

  // 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;

  // The SCEX experiment has a fixed baud rate.
  if (cfsetispeed(&tty, B38400) != 0) {
    sif::warning << "ScexUartReader::initializeInterface: Setting baud rate failed" << std::endl;
  }
  //#endif
  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 OK;
}

ReturnValue_t ScexUartReader::sendMessage(CookieIF *cookie, const uint8_t *sendData,
                                          size_t sendLen) {
  if (sendData == nullptr or sendLen == 0) {
    return FAILED;
  }
  lock->lockMutex();
  if (state == States::NOT_READY or state == States::RUNNING) {
    lock->unlockMutex();
    return FAILED;
  }
  tcflush(serialPort, TCIOFLUSH);
  state = States::RUNNING;
  lock->unlockMutex();

  size_t encodedLen = 0;
  ReturnValue_t result =
      dleEncoder.encode(sendData, sendLen, cmdbuf.data(), cmdbuf.size(), &encodedLen, true);
  if (result != OK) {
    sif::warning << "ScexUartReader::sendMessage: Encoding failed" << std::endl;
    return FAILED;
  }
  size_t bytesWritten = write(serialPort, cmdbuf.data(), encodedLen);
  if (bytesWritten != encodedLen) {
    sif::warning << "ScexUartReader::sendMessage: Sending ping command to solar experiment failed"
                 << std::endl;
    return FAILED;
  }
  result = semaphore->release();
  if (result != OK) {
    std::cout << "ScexUartReader::sendMessage: Releasing semaphore failed" << std::endl;
  }
  return OK;
}

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

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

void ScexUartReader::setDebugMode(bool enable) { this->debugMode = enable; }

ReturnValue_t ScexUartReader::finish() {
  MutexGuard mg(lock);
  if (state == States::IDLE) {
    return FAILED;
  }
  state = States::FINISH;
  return OK;
}

void ScexUartReader::handleFoundDlePacket(uint8_t *packet, size_t len) {
  MutexGuard mg(lock);
  ReturnValue_t result = ipcQueue.insert(len);
  if (result != OK) {
    sif::warning << "ScexUartReader::handleFoundDlePacket: IPCQueue error" << std::endl;
  }
  result = ipcRingBuf.writeData(packet, len);
  if (result != OK) {
    sif::warning << "ScexUartReader::handleFoundDlePacket: IPCRingBuf error" << std::endl;
  }
}

ReturnValue_t ScexUartReader::readReceivedMessage(CookieIF *cookie, uint8_t **buffer,
                                                  size_t *size) {
  MutexGuard mg(lock);
  if (ipcQueue.empty()) {
    *size = 0;
    return OK;
  }
  ipcQueue.retrieve(size);
  *buffer = ipcBuffer.data();
  ReturnValue_t result = ipcRingBuf.readData(ipcBuffer.data(), *size, true);
  if (result != OK) {
    sif::warning << "ScexUartReader::readReceivedMessage: Reading RingBuffer failed" << std::endl;
  }
  return OK;
}