#include "IMU_Navdata.h"
#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <stdint.h>
#include <math.h>
#include <stdio.h>


int IMU_Navdata_init () {

        fd = open ("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK | O_NDELAY ) ;

        if (fd < 0) {
            printf("Failed to open serial port \n");
            perror("Failed to open serial port");
            return 0 ;
        }

                int flags = fcntl(fd, F_GETFL) ;

            fcntl(fd, F_SETFL, flags | O_NONBLOCK); //read calls are non blocking
            //set port options
            struct termios options;
            //Get the current options for the port
            tcgetattr(fd, &options);
            //Set the baud rates to 460800
            cfsetispeed(&options, 460800);
            cfsetospeed(&options, 460800);

            options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
            options.c_iflag = 0; //clear input options
            options.c_lflag = 0; //clear local options
            options.c_oflag &= ~OPOST; //clear output options (raw output)


            //Set the new options for the port
            tcsetattr(fd, TCSANOW, &options);

        // stop acquisition
        uint8_t cmd = 0x02;
        write (fd, &cmd, 1);

        // read some potential dirt (retry a lot of times)
        char tmp[100];
        for(int i = 0; i < 12; i++) {
            uint16_t dirt = read (fd, tmp, sizeof tmp);
            (void) dirt;

            usleep (1000);
        }


/******************************************************
 *  inline some stuff related to baro calibration
 *
 *******************************************************/
       /*
        uint8_t cmd_1=0x17; // send cmd 23
        write(fd,&cmd_1, 1);

           // wait 20ms to retrieve data
        for (int i=0;i<22;i++)
        {
            usleep(1000);
        }

                uint8_t calibBuffer[22];

                size_t baro_byte_count = 0;

          while(baro_byte_count < 22)
          {
            ssize_t n = read(fd, calibBuffer + baro_byte_count, 22 - baro_byte_count);
            if (n < 0)
            {
              if (errno == EAGAIN || errno == EWOULDBLOCK)
                continue;
              return n;
            }
            baro_byte_count += n;
          }
      */

/******************************************************
 * End of in-lining baro calibration
 *
 *******************************************************/
        // start acquisition
        cmd = 0x01;
        write (fd, &cmd, 1);


        port.checksum_errors = 0;
        port.bytesRead = 0;
        port.totalBytesRead = 0;
        port.packetsRead = 0;
        port.isInitialized = 1;


        isInitialized = 1 ;

        return 1 ;
 }

int IMU_Navdata_update(unsigned short *y) {

    static int last_checksum_wrong = 0 ;
    int checkSumOk = 1 ;

    Acquire_Navdata_Bytes();

    // while there is something interesting to do...
        while (port.bytesRead >= NAVDATA_PACKET_SIZE) {

            if (port.buffer[0] == NAVDATA_START_BYTE) {
                tcflush(fd,TCIOFLUSH);
                memcpy (&navdata, port.buffer, NAVDATA_PACKET_SIZE);

                // Calculating the checksum
                uint16_t checksum = 0;
                for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) {
                    checksum += port.buffer[i] + (port.buffer[i + 1] << 8);
                }

                // When checksum is incorrect
                if (navdata.chksum != checksum) {
                    printf ("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n", checksum, navdata.chksum, checksum - navdata.chksum);
                    port.checksum_errors++;
                                        checkSumOk = 0 ;
                }

                // When we already dropped a packet or checksum is correct
                if (last_checksum_wrong || navdata.chksum == checksum) {
                    // Invert byte order so that TELEMETRY works better
                    uint8_t tmp;
                    uint8_t* p = (uint8_t*) &(navdata.pressure);
                    tmp = p[0];
                    p[0] = p[1];
                    p[1] = tmp;
                    p = (uint8_t*) &(navdata.temperature_pressure);
                    tmp = p[0];
                    p[0] = p[1];
                    p[1] = tmp;

                    //baroUpdateLogic (); //removed this for now

                    //imuAvailable = 1 ;  // removed this for now
                    last_checksum_wrong = 0 ;
                    port.packetsRead++;
                }

                // Crop the buffer
                cropbuffer (NAVDATA_PACKET_SIZE);
                tcflush(fd,TCIOFLUSH);
            }
            else {
                // find start byte, copy all data from startbyte to buffer origin, update bytesread
                uint8_t * pint;
                pint = (uint8_t*) memchr (port.buffer, NAVDATA_START_BYTE, port.bytesRead);

                if (pint != NULL) {
                    cropbuffer (pint - port.buffer);
                }
                else {
                    // if the start byte was not found, it means there is junk in the buffer
                    port.bytesRead = 0;
                }
            }
        }



       *y = navdata.ultrasound;
return checkSumOk ;
}


void IMU_Navdata_stop() {

        isInitialized = 0 ;
        close (fd) ;



}



/*-----------------------------------------------------------------------------
*        Other functions
*-----------------------------------------------------------------------------*/

void Acquire_Navdata_Bytes () { //original function
        int newbytes = read (fd, port.buffer + port.bytesRead, NAVDATA_BUFFER_SIZE - port.bytesRead);

        // because non-blocking read returns -1 when no bytes available
        if (newbytes > 0) {
            port.bytesRead += newbytes;
            port.totalBytesRead += newbytes;
        }

}


 static void cropbuffer (int cropsize) {

        if (port.bytesRead - cropsize < 0) {
            // TODO think about why the amount of bytes read minus the cropsize gets below zero
            printf ("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", port.bytesRead, cropsize, port.buffer);
            return;
        }

        memmove (port.buffer, port.buffer + cropsize, NAVDATA_BUFFER_SIZE - cropsize);
        port.bytesRead -= cropsize;
}