#include "UartTestClass.h"

#include <errno.h>  // Error integer and strerror() function
#include <fcntl.h>  // Contains file controls like O_RDWR
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw_hal/linux/uart/UartCookie.h>
#include <linux/devices/ScexDleParser.h>
#include <linux/devices/ScexUartReader.h>
#include <unistd.h>  // write(), read(), close()

#include "OBSWConfig.h"
#include "fsfw/globalfunctions/CRC.h"
#include "fsfw/globalfunctions/DleEncoder.h"
#include "fsfw/globalfunctions/arrayprinter.h"
#include "fsfw/serviceinterface.h"

#define GPS_REPLY_WIRETAPPING 0

#ifndef RPI_TEST_GPS_HANDLER
#define RPI_TEST_GPS_HANDLER 0
#endif

UartTestClass::UartTestClass(object_id_t objectId, ScexUartReader* reader)
    : TestTask(objectId), reader(reader), decodeRingBuf(4096, true) {
  mode = TestModes::SCEX;
  scexMode = ScexModes::SIMPLE;
  currCmd = scex::ScexCmds::FRAM;
  if (mode == TestModes::SCEX) {
    dleParser =
        new ScexDleParser(decodeRingBuf, dleEncoder, {encodedBuf.data(), encodedBuf.size()},
                          {decodedBuf.data(), decodedBuf.size()}, &foundDlePacketHandler, this);
  }
}

ReturnValue_t UartTestClass::initialize() {
  if (mode == TestModes::GPS) {
    gpsInit();
  } else if (mode == TestModes::SCEX) {
    scexInit();
  }
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t UartTestClass::performOneShotAction() { return HasReturnvaluesIF::RETURN_OK; }

ReturnValue_t UartTestClass::performPeriodicAction() {
  if (mode == TestModes::GPS) {
    gpsPeriodic();
  } else if (mode == TestModes::SCEX) {
    scexPeriodic();
  }
  return HasReturnvaluesIF::RETURN_OK;
}

void UartTestClass::gpsInit() {
#if RPI_TEST_GPS_HANDLER == 1
  int result = lwgps_init(&gpsData);
  if (result == 0) {
    sif::warning << "lwgps_init error: " << result << std::endl;
  }

  /* Get file descriptor */
  serialPort = open("/dev/serial0", O_RDWR);
  if (serialPort < 0) {
    sif::warning << "open call failed with error [" << errno << ", " << strerror(errno)
                 << std::endl;
  }
  /* Setting up UART parameters */
  tty.c_cflag &= ~PARENB;         // Clear parity bit
  tty.c_cflag &= ~CSTOPB;         // Clear stop field, only one stop bit used in communication
  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 canonical mode for GPS device
  tty.c_lflag |= ICANON;
  tty.c_lflag &= ~ECHO;                    // Disable echo
  tty.c_lflag &= ~ECHOE;                   // Disable erasure
  tty.c_lflag &= ~ECHONL;                  // Disable new-line echo
  tty.c_lflag &= ~ISIG;                    // Disable interpretation of INTR, QUIT and SUSP
  tty.c_iflag &= ~(IXON | IXOFF | IXANY);  // Turn off s/w flow ctrl
  tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR |
                   ICRNL);  // Disable any special handling of received bytes
  tty.c_oflag &= ~OPOST;    // Prevent special interpretation of output bytes (e.g. newline chars)
  tty.c_oflag &= ~ONLCR;    // Prevent conversion of newline to carriage return/line feed

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

  cfsetispeed(&tty, B9600);
  cfsetospeed(&tty, B9600);
  if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
    sif::warning << "tcsetattr call failed with error [" << errno << ", " << strerror(errno)
                 << std::endl;
    ;
  }
  // Flush received and unread data. Those are old NMEA strings which are not relevant anymore
  tcflush(serialPort, TCIFLUSH);
#endif
}

void UartTestClass::gpsPeriodic() {
#if RPI_TEST_GPS_HANDLER == 1
  int bytesRead = 0;
  do {
    bytesRead = read(serialPort, reinterpret_cast<void*>(recBuf.data()),
                     static_cast<unsigned int>(recBuf.size()));
    if (bytesRead < 0) {
      sif::warning << "UartTestClass::performPeriodicAction: read call failed with error [" << errno
                   << ", " << strerror(errno) << "]" << std::endl;
      break;
    } else if (bytesRead >= static_cast<int>(recBuf.size())) {
      sif::debug << "UartTestClass::performPeriodicAction: "
                    "recv buffer might not be large enough"
                 << std::endl;
    } else if (bytesRead > 0) {
      // pass data to lwgps for processing
#if GPS_REPLY_WIRETAPPING == 1
      sif::info << recBuf.data() << std::endl;
#endif
      int result = lwgps_process(&gpsData, recBuf.data(), bytesRead);
      if (result == 0) {
        sif::warning << "UartTestClass::performPeriodicAction: lwgps_process error" << std::endl;
      }
      recvCnt++;
      if (recvCnt == 6) {
        recvCnt = 0;
        sif::info << "GPS Data" << std::endl;
        // Print messages
        printf("Valid status: %d\n", gpsData.is_valid);
        printf("Latitude: %f degrees\n", gpsData.latitude);
        printf("Longitude: %f degrees\n", gpsData.longitude);
        printf("Altitude: %f meters\n", gpsData.altitude);
      }
    }
  } while (bytesRead > 0);
#endif
}

void UartTestClass::scexInit() {
  if (reader == nullptr) {
    sif::warning << "UartTestClass::scexInit: Reader invalid" << std::endl;
    return;
  }
  if (scexMode == ScexModes::SIMPLE) {
    scexSimpleInit();
  } else {
#if defined(RASPBERRY_PI)
    std::string devname = "/dev/serial0";
#else
    std::string devname = "/dev/ul-scex";
#endif
    uartCookie = new UartCookie(this->getObjectId(), devname, UartBaudRate::RATE_57600, 4096);
    reader->setDebugMode(true);
    ReturnValue_t result = reader->initializeInterface(uartCookie);
    if (result != HasReturnvaluesIF::RETURN_OK) {
      sif::warning << "UartTestClass::gpsPeriodic: Initializing SCEX reader "
                      "UART IF failed"
                   << std::endl;
    }
  }
}

void UartTestClass::scexPeriodic() {
  if (reader == nullptr) {
    return;
  }

  if (scexMode == ScexModes::SIMPLE) {
    scexSimplePeriodic();
  } else {
    size_t len = 0;
    prepareScexCmd(scex::ScexCmds::PING, false, cmdBuf.data(), &len);
    reader->sendMessage(uartCookie, cmdBuf.data(), len);
  }
}

void UartTestClass::scexSimpleInit() {
#if defined(RASPBERRY_PI)
  std::string devname = "/dev/serial0";
#else
  std::string devname = "/dev/ul-scex";
#endif
  /* Get file descriptor */
  serialPort = open(devname.c_str(), O_RDWR);
  if (serialPort < 0) {
    sif::warning << "open call failed with error [" << errno << ", " << strerror(errno)
                 << std::endl;
    return;
  }
  // Setting up UART parameters
  tty.c_cflag &= ~PARENB;         // Clear parity bit
  tty.c_cflag &= ~CSTOPB;         // Clear stop field, only one stop bit used in communication
  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, read until either line is 0.1 second idle or maximum of 255 bytes are
  // received in one go
  tty.c_cc[VTIME] = 10;  // In units of 0.1 seconds
  tty.c_cc[VMIN] = 255;  // Read up to 255 bytes

  // Q7S UART Lite has fixed baud rate. For other linux systems, set baud rate here.
#if !defined(XIPHOS_Q7S)
  if (cfsetispeed(&tty, B57600) != 0) {
    sif::warning << "UartTestClass::scexInit: Setting baud rate failed" << std::endl;
  }
#endif

  if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
    sif::warning << "tcsetattr call failed with error [" << errno << ", " << strerror(errno)
                 << std::endl;
  }
  // Flush received and unread data
  tcflush(serialPort, TCIOFLUSH);
}

void UartTestClass::scexSimplePeriodic() {
  using namespace scex;
  ReturnValue_t result = RETURN_OK;
  if (not cmdSent) {
    // Flush received and unread data
    tcflush(serialPort, TCIFLUSH);
    uint8_t tmpCmdBuf[32] = {};
    size_t len = 0;
    sif::info << "UartTestClass::scexSimplePeriodic: Sending command to SCEX" << std::endl;
    prepareScexCmd(currCmd, false, tmpCmdBuf, &len);
    result = dleEncoder.encode(tmpCmdBuf, len, cmdBuf.data(), cmdBuf.size(), &encodedLen, true);
    if (result != HasReturnvaluesIF::RETURN_OK) {
      sif::warning << "UartTestClass::scexInit: Encoding failed" << std::endl;
      return;
    }
    if (result != 0) {
      return;
    };
    size_t bytesWritten = write(serialPort, cmdBuf.data(), encodedLen);
    if (bytesWritten != encodedLen) {
      sif::warning << "Sending command to solar experiment failed" << std::endl;
    }
    cmdSent = true;
    cmdDone = false;
  }
  if (not cmdDone) {
    // Read back reply immediately
    int bytesRead = 0;
    do {
      bytesRead = read(serialPort, reinterpret_cast<void*>(recBuf.data()),
                       static_cast<unsigned int>(recBuf.size()));
      if (bytesRead == 0) {
        sif::warning << "Reading SCEX: Timeout or no bytes read" << std::endl;
      } else if (bytesRead < 0) {
        sif::warning << "UartTestClass::performPeriodicAction: read call failed with error ["
                     << errno << ", " << strerror(errno) << "]" << std::endl;
        break;
      } else if (bytesRead >= static_cast<int>(recBuf.size())) {
        sif::debug << "UartTestClass::performPeriodicAction: recv buffer might not be large enough"
                   << std::endl;
      } else if (bytesRead > 0) {
        dleParser->passData(recBuf.data(), bytesRead);
        if (currCmd == ScexCmds::PING) {
          cmdDone = true;
          cmdSent = false;
        }
      }
    } while (bytesRead > 0);
  }
}

int UartTestClass::prepareScexCmd(scex::ScexCmds cmd, bool tempCheck, uint8_t* cmdBuf,
                                  size_t* len) {
  using namespace scex;
  // Send ping command
  cmdBuf[0] = scex::createCmdByte(cmd, false);
  // These two fields are the packet counter and the total packet count. Those are 1 and 1 for each
  // telecommand so far
  cmdBuf[1] = 1;
  cmdBuf[2] = 1;
  uint16_t userDataLen = 0;
  cmdBuf[3] = (userDataLen >> 8) & 0xff;
  cmdBuf[4] = userDataLen & 0xff;
  uint16_t crc = CRC::crc16ccitt(cmdBuf, 5);
  cmdBuf[5] = (crc >> 8) & 0xff;
  cmdBuf[6] = crc & 0xff;
  *len = 7;
  return 0;
}

void UartTestClass::foundDlePacketHandler(uint8_t* packet, size_t len, void* args) {
  UartTestClass* obj = reinterpret_cast<UartTestClass*>(args);
  obj->handleFoundDlePacket(packet, len);
}

void UartTestClass::handleFoundDlePacket(uint8_t* packet, size_t len) {
  sif::info << "Detected DLE encoded packet with decoded size " << len << std::endl;
}