#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/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"
#include "mission/devices/devicedefinitions/SCEXDefinitions.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) {
  mode = TestModes::SCEX;
  scexMode = ScexModes::READER_TASK;
}

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) {
#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] = 1;   // 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, TCIFLUSH);
  } else {
#if defined(RASPBERRY_PI)
    std::string devname = "/dev/serial0";
#else
    std::string devname = "/dev/ul-scex";
#endif
    uartCookie =
        new UartCookie(this->getObjectId(), devname, UartModes::NON_CANONICAL, 57600, 4096);
    reader->setDebugMode(true);
    ReturnValue_t result = reader->initializeInterface(uartCookie);
  }
}

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

  if (scexMode == ScexModes::SIMPLE) {
    sif::info << "UartTestClass::scexInit: Sending ping command to SCEX" << std::endl;
    // reader->sendMessage(nullptr, nullptr, 0);
    uint8_t tmpCmdBuf[32] = {};
    size_t len = 0;
    prepareScexPing(tmpCmdBuf, &len);
    ReturnValue_t 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 ping command to solar experiment failed" << std::endl;
    }

    // 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 << "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) {
        sif::info << "Received " << bytesRead
                  << " bytes from the Solar Cell Experiment:" << std::endl;
        arrayprinter::print(recBuf.data(), bytesRead, OutputType::HEX, false);
        break;
      }
    } while (bytesRead > 0);
  } else {
    size_t len = 0;
    prepareScexPing(cmdBuf.data(), &len);
    reader->sendMessage(uartCookie, cmdBuf.data(), len);
  }
}

int UartTestClass::prepareScexPing(uint8_t* cmdBuf, size_t* len) {
  // Send ping command
  cmdBuf[0] = scex::CMD_PING;
  // 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;
}