#include "negotiator_drivers/flippers/serialPort.h"

#include <cstdio>
#include <string>

#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <sys/ioctl.h>

#include <ros/ros.h>

#define MAX_ATTEMPTS 4 // Maximum number of retires to read requested number of data
using namespace Comms;
using std::string;

/*! The constructor
  * \param path serial port path
  * \param speed the speed at which the port will be connected.
  * This value is taken from termios.h e.g. B576000 << Needs to be improved in future
  */
SerialPort::SerialPort(string path, speed_t speed){
   _fd = -1;
   _path = path;
   _speed = speed;
}

/*! Destructor */
SerialPort::~SerialPort(){
    /* Make sure we exit gracefully */
    if(_fd !=-1)
        closePort();
}

std::string SerialPort::getPort() {
    return _path;
}

/*! Opens the port, and sets the communication link parameters
  * \returns false if it cannot open the port
  */
bool SerialPort::openPort(){

    /* Open the port in Read Write mode */
    _fd = open(_path.c_str(), O_RDWR | O_NONBLOCK);
    if(_fd ==-1)
         return false;


    /* If we are here then we have opened the port sucessfully */
    struct termios port_options;

    /* Get the port options from the device */
    tcgetattr(_fd ,&port_options);
    port_options_atStart = port_options;

    /* Set 8 bit no parity 1 stop sign (8N1) */
    port_options.c_cflag &= ~PARENB; /* No parity */
    port_options.c_cflag &= ~CSTOPB; /* 1 Stop bit */
    port_options.c_cflag &= ~CSIZE;
    port_options.c_cflag |= CS8;     /* 8 bit char size */

    /* Other options */
    port_options.c_cflag     |= (CLOCAL | CREAD); /* Enable to read the port, and do not make owner of the port */
    port_options.c_lflag     &= ~(ICANON | ECHO | ECHOE | ISIG); /* Raw input, do not process the inputt */
    port_options.c_oflag     &= ~OPOST; /* Raw output, do not process the output */
    port_options.c_iflag     |= IGNBRK; /* Ignore the break condition */
    port_options.c_iflag     &= ~(ICRNL|IGNCR|INLCR|IXOFF|IXON); /* Do not process input, raw input */


    /* Timeout options */
    port_options.c_cc[VMIN] = 0; /* No minimum characters*/
    port_options.c_cc[VTIME]= 0; /* No wait time */

    /* Set the io baud rate */
    cfsetispeed(&port_options , _speed);
    cfsetospeed(&port_options , _speed);
    tcflush(_fd , TCIOFLUSH); /* Flush the input and output buffers */

    /* Set the port options TCSANOW=> set it now without any delay */
    tcsetattr(_fd, TCSANOW, &port_options);

    return true;
}

/*! Closes the port */
void SerialPort::closePort(){
    /* Reset the port options */
    tcsetattr(_fd, TCSANOW, &port_options_atStart);
    close(_fd);
    _fd = -1;
}


/*! Sends data over the serial port
  * \param data pointer to the data
  * \param size the number of bytes to be sent
  */
ssize_t SerialPort::sendData(const void *data, size_t size){
   return(write(_fd, data, size));
}

/*! Reads data from the serial port
  * \param buffer the buffer which will hold the data
  * \param size number of bytes that needs to be read
  * \param sec a positive integer value representing number of seconds to wait for data to become available.
  * \param usec a positive integer value representing number of micro seconds to wait for data to become available.
  * \returns the number of bytes actually read. Retuns -1 if it times out, -2 on any other error.
  */
ssize_t SerialPort::readData(void *buffer, size_t size, unsigned int sec, unsigned int usec){

    ssize_t ret;
    /* Wait for some data to appear on the port */
    ret = dataAvailable(sec, usec);
    if(ret > 0)
        return(read(_fd, buffer, size));
    return ret;
}

/*! Reads data from the serial port. It will block until data becomes available
  * \param buffer the buffer which will hold the data
  * \param size number of bytes that needs to be read
  * \returns the number of bytes actually read. Returns negative integer if there is an error
  */
ssize_t SerialPort::readDataBlocking(void *buffer, size_t size){

    ssize_t ret;
    ret = dataAvailableBlocking();

    if(ret > 0)
        return(read(_fd, buffer, size));

    return ret;

}

/*! Check for number of bytes available at the port. It uses the select statement.
  * The method waits indifinitely until there is data on the port
  * \param sec positive integer. Number of seconds to wait for data to become available.
  * \param usec positive integer. Number of micor seconds to wait for data to become available.
  * \returns Number of bytes available at the port. Returns -2 for errors.
  */
ssize_t SerialPort::dataAvailableBlocking(){

    int         n;
    unsigned int   max_fd;
    fd_set         input;


    /* Initialize the input set */
    FD_ZERO(&input);
    FD_SET(_fd, &input);

    max_fd = _fd + 1;

    /* Do the select */
    n = select(max_fd, &input, NULL, NULL, NULL);

    /* See if there was an error */
    if (n < 0){
        ROS_ERROR("SerialPort::DataAvailableBlocking => Select statement failed\n");
        return -2;
    }
    else{
        /* We have input */
        if (FD_ISSET(_fd, &input))
            return n;
    }
    return -2;
}


/*! Check for number of bytes available at the port. It uses the select statement
  * \param sec positive integer. Number of seconds to wait for data to become available.
  * \param usec positive integer. Number of micor seconds to wait for data to become available.
  * \returns Number of bytes available at the port. It it times out it will return -1. -2 for
  *          other errors.
  */
ssize_t SerialPort::dataAvailable(unsigned int sec, unsigned int usec){

    int         n;
    unsigned int   max_fd;
    fd_set         input;
    struct timeval timeout;
    /* Initialize the input set */
    FD_ZERO(&input);
    FD_SET(_fd, &input);

    max_fd = _fd + 1;

    /* Initialize the timeout structure */
    timeout.tv_sec  = sec;
    timeout.tv_usec = usec;

    /* Do the select */
    n = select(max_fd, &input, NULL, NULL, &timeout);

    /* See if there was an error */
    if (n < 0){
        ROS_ERROR("SerialPort::DataAvailable => Select statement failed\n");
        return -2;
    }

    else if (n == 0){
        ROS_ERROR("SerialPort::DataAvailable => Select statement timed out\n");
        return -1;
    }
    else{
        /* We have input */
        if (FD_ISSET(_fd, &input)){
            ioctl(_fd, FIONREAD, &n);
            return n;
        }
    }
    return -2;
}

/*!
  * Changes the value of RTS line
  * \param state true will set it to high. False will set it to low
  */
void SerialPort::setRTS(bool state){

    int status;
    ioctl(_fd, TIOCMGET, &status);
    if(state)
        status |= TIOCM_RTS;
    else
        status &= ~TIOCM_RTS;

    ioctl(_fd, TIOCMSET, &status);
}

/*!
  * Changes the value of DTR line
  * \param state true will set it to high. False will set it to low
  */
void SerialPort::setDTR(bool state){

    int status;
    ioctl(_fd, TIOCMGET, &status);
    if(state)
        status |= TIOCM_DTR;
    else
        status &= ~TIOCM_DTR;

    ioctl(_fd, TIOCMSET, &status);

}

/*! Flush the Input buffer of the port */
void SerialPort::flushInput(){
    tcflush(_fd , TCIFLUSH); /* Flush the input and output buffers */
}

/*! Makes multiple attempts to read N bytes of data from the port
  * \param buffer storage for the data to be read
  * \param nBytes number of bytes to be read
  * \param sec number of seconds to wait for data to become available
  * \param usec number of micro seconds to wait for the data to appear on the port.
  * \returns true if it can read all of the data requested
  */
ssize_t SerialPort::nReadData(unsigned char* buffer, size_t nBytes, unsigned int sec, unsigned int usec){

    size_t bytesSofar =0;
    int attempts = 0;

    while(attempts < MAX_ATTEMPTS){
        if (dataAvailable(sec,usec) < 0) {
            ROS_ERROR("SerialPort::nReadData => Error checking for available data on port.\n");
            break;
        }
        bytesSofar += read(_fd, &buffer[bytesSofar], nBytes-bytesSofar);
        if(bytesSofar == nBytes)
            return bytesSofar;
        attempts++;
    }

    ROS_ERROR("SerialPort::nReadData => Failed to read %Zu Bytes in %d attempts\n", nBytes, attempts);
    return bytesSofar;

}
