#include "UartTestClass.h"
#if defined(RASPBERRY_PI)
#include "rpiConfig.h"
#elif defined(XIPHOS_Q7S)
#include "q7sConfig.h"
#endif

#include <errno.h>   // Error integer and strerror() function
#include <fcntl.h>   // Contains file controls like O_RDWR
#include <unistd.h>  // write(), read(), close()

#include "fsfw/serviceinterface/ServiceInterface.h"
#include "lwgps/lwgps.h"

#define GPS_REPLY_WIRETAPPING 0

UartTestClass::UartTestClass(object_id_t objectId) : TestTask(objectId) {}

ReturnValue_t UartTestClass::initialize() {
  if (mode == TestModes::GPS) {
    gpsInit();
  }

  return HasReturnvaluesIF::RETURN_OK;
}

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

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

void UartTestClass::gpsInit() {
#if RPI_TEST_GPS_DEVICE == 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_DEVICE == 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
}