#include <iostream>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include<boost/asio.hpp>
#include <boost/bind.hpp>

using namespace std;

int main(int argc, char* argv[])
{


    ros::init(argc, argv, "gps_serial_node");
    ros::NodeHandle nh;
    ros::NodeHandle nh_private_("~");
    ros::Publisher read_pub = nh.advertise<std_msgs::String>("gps_serial", 1000);
    ros::Rate loop_rate(50);
    try
    {
        boost::asio::io_service io;
        boost::asio::serial_port sp(io, "/dev/ttyUSB0");

        sp.set_option(boost::asio::serial_port::baud_rate(115200));
        sp.set_option(boost::asio::serial_port::flow_control());
        sp.set_option(boost::asio::serial_port::parity());
        sp.set_option(boost::asio::serial_port::stop_bits());
        sp.set_option(boost::asio::serial_port::character_size(8));

       // boost::asio::write(sp, boost::asio::buffer("\n", 1));

        char buf[401];
        std_msgs::String result;
        boost::system::error_code err;
        string read;
        while (ros::ok())
        {


            size_t ret = sp.read_some(boost::asio::buffer(buf,400), err);

            if (err)
            {
                cout << "read_some Error: " << err.message() << endl;
                break;
            }
            else
            {
                //buf[ret] = '\0';

                string str(buf);

                string foundFPD = "$GPFPD";
                string foundIMU=  "$GRIMU";
                read.append(str);

                if(read.size()>400)
                {
                    unsigned int loc1 = str.find(foundFPD, 0);
                    unsigned int loc2 = str.find(foundIMU, loc1 + 1);
                    unsigned int loc3 = str.find(foundFPD, loc2 + 1);
                    string gpfpd(str, loc1,loc2-loc1);
                    string gtimu(str, loc2, loc3 - loc2);
                    result.data =  gtimu;
                    read_pub.publish(result);
                    cout << "[result:] " << gtimu << endl << endl;

                }




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

        io.run();
    }
    catch (exception& err)
    {
        cout << "Exception Error: " << err.what() << endl;
    }


    //getchar();
    return 0;
}