/******************************************************************************
 * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
 * Author: madesheng
 * Licensed under the Apache License, Version 1.0 (the "License");
 *****************************************************************************/
#include <iterator>
#include <regex>
#include "localization.h"
#include "coordsconvertion.h"
#include <localization/localMsg.h>
#include "../common/message.h"
#include <iostream>




namespace  robot{
namespace localizationspace {
std::string Localization::Name() const { return "localization"; }

int Localization::Init() {
    try
    {
        LOG(INFO)<<Name()<<" init start ...";
        serial_port = "/dev/ttyUSB0";
        baudrate = 115200;
        timeout = 1000;
        node_handle_.reset(new ros::NodeHandle());
        LOG(INFO)<<Name()<<" init end.";
    }
    catch (ros::Exception e)
    {
        LOG(ERROR)<<Name()<< " Unable to Init " << e.what();
        return ROBOT_FAILTURE;
    }
    return ROBOT_SUCCESS;
}

int Localization::Start() {
 LOG(INFO)<<Name()<<" start start...";
 try
 {
     geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 1);
     //设置串口属性，并打开串口
     serial_gnss.setPort(serial_port);
     serial_gnss.setBaudrate(baudrate);
     serial::Timeout to = serial::Timeout::simpleTimeout(timeout);
     serial_gnss.setTimeout(to);
     serial_gnss.open();
     if (IsOpen())
     {
         //指定循环的频率
         ros::Rate loop_rate(rate);
         while(ros::ok())
         {

           if(serial_gnss.available() >200){
               LOG(INFO)<<Name()<<"  Reading from serial port. data size :"<<serial_gnss.available();
               std::string strSerialResult;
               strSerialResult= serial_gnss.read(serial_gnss.available());
               LOG(INFO)<<Name()<<"  Read: " << strSerialResult;
               NMEA0183 nmea;
               if (Analyse(strSerialResult, nmea) == ROBOT_SUCCESS)
                   PublishData(nmea);
           }
           ros::spinOnce();
           loop_rate.sleep();
         }
     }
 }
 catch (serial::IOException& e)
 {
     LOG(ERROR)<<Name()<< " Unable to open port " << e.what();
     return ROBOT_FAILTURE;
 }
 LOG(INFO)<<Name()<<" start end.";
 return ROBOT_SUCCESS;
}

bool Localization::IsOpen()
{
  //检测串口是否已经打开，并给出提示信息
  if(serial_gnss.isOpen())
  {
      LOG(INFO)<<Name()<<" Serial Port initialized.";
      return true;
  }
  else
  {
      LOG(ERROR)<<Name()<< "serial Port uninitialized.";
      return false;
  }
}


void Localization::Stop() {
  LOG(INFO)<<Name()<<" Stop start...";
  serial_gnss.close();
  LOG(INFO)<<Name()<<" Stop end.";
}


int Localization::Analyse(const std::string &strSerialResult,  NMEA0183 &nmea){

    //LOG(INFO)<<Name()<<" Analyse start.";
    try
    {
        std::string delim("\r\n");
        std::vector<std::string> v_nmeaData = Split(strSerialResult, delim);
        if (v_nmeaData.size() !=3)
            return ROBOT_FAILTURE;
        std::vector<std::string>::iterator iter_start = v_nmeaData.begin();
        std::vector<std::string>::iterator iter_end = v_nmeaData.end();
        std::vector<std::string>::iterator iter;
        for (iter =iter_start; iter != iter_end; iter++)
        {
            std::string type = (*iter).substr(1, 5);
            LOG(INFO)<<Name()<<" type:"<<type;
            if (type.compare("GPRMC") == 0)
            {
                GPRMC temGPRMC;
                if (ParaGPRMC(*iter, temGPRMC) != ROBOT_SUCCESS)
                     return ROBOT_FAILTURE;
                nmea.gprmc = temGPRMC;
                //nmea.gprmc.print();
                //LOG(INFO)<<Name()<<"para GPRMC end.";
            }

            else if (type.compare("GPGGA") == 0)
            {
                GPGGA tempGPGGA;
                if (ParaGPGGA(*iter, tempGPGGA) != ROBOT_SUCCESS)
                     return ROBOT_FAILTURE;
                nmea.gpgga = tempGPGGA;
                //nmea.gpgga.print();
                //LOG(INFO)<<Name()<<" para GPGGA end.";
            }
        }

    }
    catch (ros::Exception ex)
    {
        LOG(ERROR)<<Name()<< " Analyse  error" << ex.what();
        return ROBOT_FAILTURE;
    }
    //LOG(INFO)<<Name()<<" Analyse end.";
    return ROBOT_SUCCESS;

}


int Localization::ParaGPRMC(std::string &strGPRMC, GPRMC &gprmc){
    try
    {
        std::vector<std::string> nmea_gprmc = Split(strGPRMC, ",");
        gprmc.utcTime = atof(nmea_gprmc[1].c_str());
        if ( nmea_gprmc[2].compare("V") == 0)
             return ROBOT_FAILTURE;
        memset(&gprmc.locationStatus, 0x00, 2);
        memcpy(&gprmc.locationStatus, nmea_gprmc[2].c_str(), 1);
        gprmc. Latitude = StrToDouble(nmea_gprmc[3].c_str(), 2);
        memset(&gprmc.LatType, 0x00, 2);
        memcpy(&gprmc.LatType, nmea_gprmc[4].c_str(), 1);
        gprmc.Longitude = StrToDouble(nmea_gprmc[5].c_str(), 3);
        memset(&gprmc.LonType, 0x00, 2);
        memcpy(&gprmc.LonType, nmea_gprmc[6].c_str(), 1);
        gprmc.Speed = atof(nmea_gprmc[7].c_str());
        gprmc.Deviated = atof(nmea_gprmc[8].c_str());
        memset(&gprmc.Date, 0x00, 5);
        memcpy(&gprmc.Date, nmea_gprmc[9].c_str(), 4);
    }
    catch (ros::Exception ex)
    {
        LOG(ERROR)<<Name()<< " ParaGPRMC  error" << ex.what();
        return ROBOT_FAILTURE;
    }
     return ROBOT_SUCCESS;
}

int Localization::ParaGPGGA(std::string &strGPGGA, GPGGA &gpgga){
    try
    {
        std::vector<std::string> nmea_gpgga = Split(strGPGGA, ",");      
        gpgga.utcTime = atof(nmea_gpgga[1].c_str());
        if (nmea_gpgga[2].compare("") == 0)
             return ROBOT_FAILTURE;
        gpgga.Latitude = StrToDouble(nmea_gpgga[2].c_str(), 2);
        memset(&gpgga.LatType, 0x00, 2);
        memcpy(&gpgga.LatType, nmea_gpgga[3].c_str(), 1);
        gpgga.Longitude = StrToDouble(nmea_gpgga[4].c_str(), 3);
        memset(&gpgga.LonType, 0x00, 2);
        memcpy(&gpgga.LonType, nmea_gpgga[5].c_str(), 1);
        memset(&gpgga.GPSStatus, 0x00, 5);
        memcpy(&gpgga.GPSStatus, nmea_gpgga[6].c_str(), 4);
        gpgga.SatelliteNumber = atoi(nmea_gpgga[7].c_str());
        gpgga.Hdop = atof(nmea_gpgga[8].c_str());
        gpgga.Height = atof(nmea_gpgga[9].c_str());
        memset(&gpgga.HeightUint, 0x00, 2);
        memcpy(&gpgga.HeightUint, nmea_gpgga[10].c_str(), 1);
    }
    catch(ros::Exception ex)
    {
        LOG(ERROR)<<Name()<< " ParaGPGGA  error" << ex.what();
        return ROBOT_FAILTURE;
    }
    return ROBOT_SUCCESS;
}


int Localization::PublishData(NMEA0183 &nmea)
{
    LOG(INFO)<<Name()<<" PublishData start\n";
    try{

//        double northing, easting;
//        std::string zone;
//        CoordsConvertion::LLtoUTM(nmea.gprmc.Latitude, nmea.gprmc.Longitude, northing, easting, zone);
        localization::localMsg localization;
        localization.Longitude =  to_string_with_precision(nmea.gprmc.Longitude, 12);
        localization.Latitude = to_string_with_precision(nmea.gprmc.Latitude, 11);
        localization.Height = to_string_with_precision(nmea.gpgga.Height, 4);
        localization.AngleDeviated = to_string_with_precision(nmea.gprmc.Deviated, 4);
        geometry_pub.publish(localization);
    }
    catch (ros::Exception ex)
    {
        LOG(ERROR)<<Name()<< " PublishData  error" << ex.what();
        return ROBOT_FAILTURE;
    }
    LOG(INFO)<<Name()<<" PublishData end\n";
    return ROBOT_SUCCESS;

}
/* 用delim指定的正则表达式将字符串in分割，返回分割后的字符串数组 delim 分割字符串的正则表达式 */
std::vector<std::string> Localization::Split(const std::string& in, const std::string& delim) {
    std::regex re{ delim };
    // 构造函数,完成字符串分割
    return std::vector<std::string> {
        std::sregex_token_iterator(in.begin(), in.end(), re, -1),
        std::sregex_token_iterator()
    };
}



}  // namespace localizationspace
}  // namespace robot
