#include "us_obstacles_manager.h"

#include <cstdlib>  //exit
#include <iostream>
#include <cstring>  //pour memmove
using namespace std;

#define TTY_DEVICE "/dev/ttyACM0"


UsObstaclesManager::UsObstaclesManager(uint8_t nbr_sensors, vector<uint8_t> sensors_positions, ros::NodeHandle *pNodeHandle, bool simulate_sensors)
{
    this->nbr_sensors=nbr_sensors;
    this->frame_size=1+1+nbr_sensors+1;
    this->time_last_valid_message=std::chrono::steady_clock::now() - std::chrono::hours(1);
    this->distances_US.resize(nbr_sensors);
    for(int i=0; i<nbr_sensors; i++)
        this->distances_US[i]=0;
    this->sensors_positions=sensors_positions;
    this->simulate_sensors=simulate_sensors;



    if(!simulate_sensors)
    {
        this->serial_port = open(TTY_DEVICE, O_RDWR);

        // Check for errors
        if (serial_port < 0) {
            printf("Error %i from open: %s\n", errno, strerror(errno));
            exit(-1);
        }


        // Create new termios struc, we call it 'tty' for convention
        struct termios tty;

        // Read in existing settings, and handle any error
        if(tcgetattr(serial_port, &tty) != 0) {
            printf("Error %i from tcgetattr: %s\n", errno, strerror(errno));
            exit(-1);
        }

        tty.c_cflag &= ~PARENB; // Clear parity bit, disabling parity (most common)
        tty.c_cflag &= ~CSTOPB; // Clear stop field, only one stop bit used in communication (most common)
        tty.c_cflag &= ~CSIZE; // Clear all bits that set the data size
        tty.c_cflag |= CS8; // 8 bits per byte (most common)
        tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control (most common)
        tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines (CLOCAL = 1)

        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
        // tty.c_oflag &= ~OXTABS; // Prevent conversion of tabs to spaces (NOT PRESENT ON LINUX)
        // tty.c_oflag &= ~ONOEOT; // Prevent removal of C-d chars (0x004) in output (NOT PRESENT ON LINUX)

        tty.c_cc[VTIME] = 0;    // VTIME=VMIN=0 -> read non bloquants
        tty.c_cc[VMIN] = 0;    // VTIME=VMIN=0 -> read non bloquants

        // Set in/out baud rate to be 9600
        cfsetispeed(&tty, B115200);   //possibilités : B0,  B50,  B75,  B110,  B134,  B150,  B200, B300, B600, B1200, B1800, B2400, B4800, B9600, B19200, B38400, B57600, B115200, B230400, B460800
        cfsetospeed(&tty, B115200);

        // Save tty settings, also checking for error
        if (tcsetattr(serial_port, TCSANOW, &tty) != 0) {
            printf("Error %i from tcsetattr: %s\n", errno, strerror(errno));
            exit(-1);
        }
    }
}
UsObstaclesManager::~UsObstaclesManager()
{
    if(!simulate_sensors)
        close(serial_port);
    for(int i=0; i<NBR_FILTERING_MEASUREMENTS; i++)
    {
        delete raw_distances_US_history[i];
    }
}
bool UsObstaclesManager::look_for_new_frame()
{
    bool valid_frame_detected=false;


    if(simulate_sensors)    //on simule les cpateurs
    {
    	//pas pertinent pour le bug
    }
    else    //cas normal : on lit les cpateurs
    {
        uint8_t free_bytes_in_buffer=BUFFER_SIZE-nbr_bytes_in_buffer;

        int num_bytes = read(serial_port, &(input_buffer[nbr_bytes_in_buffer]), free_bytes_in_buffer);




        // n is the number of bytes read. n may be 0 if no bytes were received, and can also be -1 to signal an error.
        if (num_bytes < 0)
        {
            printf("Error reading: %s", strerror(errno));
            //return 1;
        }
        nbr_bytes_in_buffer+=num_bytes;

        cout<<endl<<endl<<endl<<num_bytes<<" new bytes (total="<<(int)nbr_bytes_in_buffer<<"): buffer=";
        for(int i=0; i<nbr_bytes_in_buffer;i++)
            cout<<(int)(input_buffer[i])<<";";
        cout<<endl;

        uint8_t index=0;
        while(nbr_bytes_in_buffer-index>=frame_size)
        {
           //traitement des données reçues
        }
        nbr_bytes_in_buffer-=index;
        memmove(input_buffer, input_buffer+index, nbr_bytes_in_buffer);     //on déplace les octets restant vers le début du buffer. NB : emmove (de cstring) gère les zones de mémoires avec chevauchement
    }

    return valid_frame_detected;
}