#include <sys/types.h>
#include <sys/stat.h>
#include <ros/ros.h>
#include <fcntl.h>
#include <stdio.h>
#include <termios.h>

#include <string>
#include <algorithm>
#include <stdlib.h>

#include <std_msgs/float.h>

using namespace std;

#define BAUDRATE 9600
#define SERIAL_DEVICE "/dev/ttyACM0"

bool SatIsCorrect(const std::string& input);
string ConvertToString(char* a, int size);

volatile int STOP=0;



int main(int argc, char **argv)
{
    ros::init(argc, argv, "serial_node");
    ros::NodeHandle n("~");
    ros::Rate loop_rate(50);
    ros::Publisher lon_lat_pub;
    lon_lat_pub = n.advertise<std_msgs::float32>("Longitude and latitude", 1);

    std::string lat_str = "";
    float lat_raw = 0;
    std::string lon_str = "";
    float lon_raw = 0;

    char buf[255];
    int serial_port,c,num_bytes_recieved;
    struct termios oldtio,newtio;

    serial_port = open(SERIAL_DEVICE, O_RDWR | O_NOCTTY);
    if (serial_port < 0){
        perror(SERIAL_DEVICE);
        exit(-1);
    }

    tcgetattr(serial_port, &oldtio);
    bzero(&newtio, sizeof(newtio));

    newtio.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;
    newtio.c_iflag = IGNPAR | ICRNL;
    newtio.c_oflag = 0;
    newtio.c_lflag = ICANON;

    tcflush(serial_port, TCIFLUSH);
    tcsetattr(serial_port, TCSANOW, &newtio);

    
    while (ros::ok())
    {
        
        num_bytes_recieved = read(serial_port, buf, sizeof(buf));
        buf[num_bytes_recieved] = 0;
        std::string nmea_sentence = ConvertToString(buf, num_bytes_recieved);

        if(SatIsCorrect(nmea_sentence)){
            int lat_index = nmea_sentence.find("N",6);
            if(lat_index > 0)lat_str = nmea_sentence.substr(lat_index-11,10);

            int lon_index = nmea_sentence.find("E",6);
            if(lon_index > 0)lon_str = nmea_sentence.substr(lon_index-11,10);
            
            lat_raw = std::stof(lat_str);
            lon_raw = std::stof(lon_str);

            float lat_whole = float(int(lat_raw/100));
            float lat_dec = (float(lat_raw) - lat_whole*100)/60;
            float latitude = lat_whole + lat_dec;

            float lon_whole = float(int(lon_raw/100));
            float lon_dec = (float(lon_raw) - lon_whole*100)/60;
            float longitude = lon_whole + lon_dec;

            // printf("lat_index = %i\n", lat_index);
            // printf("################\n");
            // printf("buf = %s\n", buf);
            // printf("################\n");
            // printf("lat_str = %s\n", lat_str.c_str());
            // puts("#####################");
            // printf("lat_raw = %f\n", lat_raw);
            // printf("nmea_sentence = %s\n", nmea_sentence.c_str());
            // printf("NEWNEWNEWNEWNEWNEW\n");
            printf("latitude = %f\n", latitude);
            printf("longitude = %f\n", longitude);
            puts("###########################");
        }

        
        ros::spinOnce();
        loop_rate.sleep();
    }
}


bool SatIsCorrect(const std::string& input){
    
    if(std::string::npos != input.find("GNGGA"))
        return true;
    
    if(std::string::npos != input.find("GNRMC"))
        return true;
    
    if(std::string::npos != input.find("GNGLL"))
        return true;

    return false;
}

std::string ConvertToString(char* a, int size){
    int i;
    std::string s = "";
    for(**0; i<size; i++){
        s = s + a[i];
    }
    return s;
}