#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 <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) : TestTask(objectId) { mode = TestModes::SCEX; }

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

ReturnValue_t UartTestClass::performOneShotAction() { return returnvalue::OK; }

ReturnValue_t UartTestClass::performPeriodicAction() {
  if (mode == TestModes::GPS) {
    gpsPeriodic();
  } else if (mode == TestModes::SCEX) {
    scexPeriodic();
  }
  return returnvalue::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 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);
}

void UartTestClass::scexPeriodic() {
  sif::info << "UartTestClass::scexInit: Sending ping command to SCEX" << std::endl;
  int result = prepareScexPing();
  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);
    }
  } while (bytesRead > 0);
}

int UartTestClass::prepareScexPing() {
  std::array<uint8_t, 128> tmpCmdBuf = {};
  // Send ping command
  tmpCmdBuf[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
  tmpCmdBuf[1] = 1;
  tmpCmdBuf[2] = 1;
  uint16_t userDataLen = 0;
  tmpCmdBuf[3] = (userDataLen >> 8) & 0xff;
  tmpCmdBuf[4] = userDataLen & 0xff;
  uint16_t crc = CRC::crc16ccitt(tmpCmdBuf.data(), 5);
  tmpCmdBuf[5] = (crc >> 8) & 0xff;
  tmpCmdBuf[6] = crc & 0xff;
  ReturnValue_t result =
      dleEncoder.encode(tmpCmdBuf.data(), 7, cmdBuf.data(), cmdBuf.size(), &encodedLen, true);
  if (result != returnvalue::OK) {
    sif::warning << "UartTestClass::scexInit: Encoding failed" << std::endl;
    return -1;
  }
  return 0;
}