#include "UartTestClass.h"
#include "rpiConfig.h"

#include "fsfw/serviceinterface/ServiceInterface.h"

#include "lwgps/lwgps.h"

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

#define GPS_REPLY_WIRETAPPING   0

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

ReturnValue_t UartTestClass::initialize() {
#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
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t UartTestClass::performOneShotAction() {
#if RPI_TEST_GPS_DEVICE == 1
#endif
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t UartTestClass::performPeriodicAction() {
#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
    return HasReturnvaluesIF::RETURN_OK;
}