/******************************************************************************
 * 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>
#include <cmath>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <fstream>

using namespace std;

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

int Localization::Init() {
  LOG(INFO)<<Name()<<" init start ...";
  if (!ros::param::get("car_input_mode", car_input_mode))
    car_input_mode = "pci";
  LOG(INFO)<< Name()<<": read param car_input_mode = " << car_input_mode;
//TODO dakai
  if (car_input_mode == "modbus") {
    std::string car_input_ip;
    int car_input_port;
    if (!ros::param::get("car_input_ip", car_input_ip)) {
        LOG(ERROR)<< Name()<<": read param car_input_ip error: not exist";
        return ROBOT_FAILTURE;
    }
    if (!ros::param::get("car_input_port", car_input_port)) {
        LOG(ERROR)<< Name()<<": read param car_input_port error: not exist";
        return ROBOT_FAILTURE;
    }
    LOG(INFO)<< Name() <<": read param car_input_ip:port = " << car_input_ip << ":" << car_input_port;
    modbus_socket = socket(AF_INET, SOCK_STREAM, 0);
    struct sockaddr_in serv_addr;
    memset(&serv_addr, 0, sizeof(serv_addr));  //每个字节都用0填充
    serv_addr.sin_family = AF_INET;  //使用IPv4地址
    serv_addr.sin_addr.s_addr = inet_addr(car_input_ip.c_str());
    serv_addr.sin_port = htons(car_input_port);  //端口
    int result = connect(modbus_socket, (struct sockaddr*)&serv_addr, sizeof(serv_addr));
    if (result == 0) {
      LOG(INFO) << Name() << ": " << car_input_ip << ":" << car_input_port << " connected";
    }
    else {
      LOG(ERROR) << Name() << ": connecting " << car_input_ip << ":" << car_input_port << " failed";
    }
  }
  else if (car_input_mode == "rtk") {
    try {
        serial_port = "/dev/ttyUSB0";
//        baudrate = 115200;
//        serial_port = "/dev/tty0";
        baudrate = 460800;
        timeout = 1000;//ms
    }
    catch (ros::Exception e)
    {
        LOG(ERROR)<<Name()<< " Unable to Init " << e.what();
        return ROBOT_FAILTURE;
    }
  }
  else if(car_input_mode == "pci")
  {
    // 第一步 创建设备对象
     hDevice = ACTS2110_DEV_Create(0, 0);
     if (hDevice == NULL)
     {
       LOG(ERROR)<<Name()<< "PCI DEV_Create Error";
       return ROBOT_FAILTURE;
     }
     memset(&AIParam, 0, sizeof(ACTS2110_AI_PARAM));
     // 通道参数
     AIParam.nSampChanCount = 1;
     for (nChannel = 0; nChannel < MainInfo.nAIChannelCount; nChannel++)
     {
         AIParam.CHParam[nChannel].nChannel = nChannel;
         AIParam.CHParam[nChannel].nSampleRange = ACTS2110_AI_SAMPRANGE_N10_P10V;
         AIParam.CHParam[nChannel].nRefGround = ACTS2110_AI_REFGND_RSE;
         AIParam.CHParam[nChannel].nReserved0 = 0;
         AIParam.CHParam[nChannel].nReserved1 = 0;
         AIParam.CHParam[nChannel].nReserved2 = 0;
     }
     AIParam.nSampleSignal = ACTS2110_AI_SAMPSIGNAL_2D5V;	// ACTS2110_AI_SAMPSIGNAL_0V

     // 时钟参数
     AIParam.fSampleRate = fSampleRate;
     AIParam.nSampleMode = ACTS2110_AI_SAMPMODE_CONTINUOUS;
     AIParam.nSampsPerChan = 1024;
     AIParam.nSampClkSource = ACTS2110_AIO_SAMPCLKSRC_LOCAL;
     AIParam.nClockOutput = ACTS2110_AIO_CLKOUT_DISABLE;
     AIParam.StartTrig.nSyncTSOut = ACTS2110_AIO_STSO_DISABLE;

     // 触发参数
     AIParam.StartTrig.nTriggerType = ACTS2110_AI_START_TRIGTYPE_NONE;
     AIParam.StartTrig.nTriggerSource = ACTS2110_AI_TRIG_SRC_FIRST;
     AIParam.StartTrig.nTriggerDir = ACTS2110_AI_TRIGDIR_FALLING;
     AIParam.StartTrig.fTriggerLevelTop = 1.0;
     AIParam.StartTrig.fTriggerLevelBtm = 0.0;
     AIParam.StartTrig.nTriggerSens = 0;
     AIParam.StartTrig.nDelaySamps = 0;
     AIParam.StartTrig.nReTrigger = 0;

     AIParam.PauseTrig.nTriggerType = ACTS2110_AI_START_TRIGTYPE_NONE;
     AIParam.PauseTrig.nTriggerSource = ACTS2110_AI_TRIG_SRC_FIRST;
     AIParam.PauseTrig.nTriggerDir = ACTS2110_AI_TRIGDIR_FALLING;
     AIParam.PauseTrig.fTriggerLevelTop = 1.0;
     AIParam.PauseTrig.fTriggerLevelBtm = 0.0;
     AIParam.PauseTrig.nTriggerSens = 0;


     // 其他参数
     AIParam.nReserved1 = 0;
     AIParam.nReserved2 = 0;
     AIParam.nReserved3 = 0;

     if (!ACTS2110_AI_VerifyParam(hDevice, &AIParam))
     {
       LOG(ERROR)<<Name()<< "PCI Wrong parameter, it has been adjusted to legal value，Please check the log file acts2110.log";
       return ROBOT_FAILTURE;
     }

     // 第三步 开始AI采集任务
      if (!ACTS2110_AI_StartTask(hDevice))
      {
          LOG(ERROR)<<Name()<< "PCI AI_StartTask Error";
          return ROBOT_FAILTURE;
      }
  }
    //TODO
  node_handle_.reset(new ros::NodeHandle());

  LOG(INFO)<<Name()<<" init end.";
  return ROBOT_SUCCESS;
}

int Localization::Start() {
  LOG(INFO)<<Name()<<" start start...";
  geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 1);
  //TODO  zhushidiao
//  string str = "/home/madesheng/taskuidata.txt";
//  read_txt(str);
//  speed_count = 0;
//      rack_test_timer_ = CreateTimer(ros::Duration(duration_rack), &Localization::RackTestOnTimer, this,false,false);
//      rack_test_timer_.start();
  //TODO dakai
if (car_input_mode == "modbus") {
    if (!ros::param::get("car_input_volspeed_max", car_input_volspeed_max))
        car_input_volspeed_max = 0;

    ros::Rate loop_rate(rate);
    while(ros::ok())
    {
      int send_length = write(modbus_socket, modbus_req, sizeof(modbus_req));
      if (send_length != sizeof(modbus_req)){
         LOG(ERROR) << Name() <<" modbus send error: " << send_length;
      }
      else {
        int recv_len = read(modbus_socket, modbus_recv,  sizeof(modbus_recv));
        if (recv_len != sizeof(modbus_recv)) {
          LOG(ERROR) << Name() <<" canbus recv read error: " << recv_len;
        }
        else {
          try{
              localization::localMsg localization;
              localization.Longitude =  0;
              localization.Latitude = 0;
              localization.Height = 0;
              localization.AngleDeviated = 0;
              int voltage = (unsigned int)modbus_recv[sizeof(modbus_recv)-2] * 256 + (unsigned int)modbus_recv[sizeof(modbus_recv)-1];
              int max = car_input_volspeed_max;
              localization.Speed = voltage / 65535.0 * max;
              if(localization.Speed < 0.4)
              {
              localization.Speed = 0;
              }
              geometry_pub.publish(localization);
              LOG(INFO) << Name() << " publish speed:" << std::setprecision(4) << localization.Speed;
          }
          catch (ros::Exception ex) {
              LOG(ERROR)<<Name()<< " publish speed error: " << ex.what();
          }
        }
      }
      ros::spinOnce();
      loop_rate.sleep();
    }
  }
  else if (car_input_mode == "rtk") {
    try {
       //设置串口属性，并打开串口
       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())
           {
             while(serial_gnss.available() >0)
             {
               std::string strSerialResult;
               strSerialResult = serial_gnss.readline();
             //  LOG(INFO)<<Name()<<"  Read: " << strSerialResult;
               NMEA0183 nmea;
               if(Analyse_ReadLine(strSerialResult,nmea) == ROBOT_SUCCESS)
               {
                 PublishData(nmea);
               }
             }
             //TODO
  //           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());
  //               strSerialResult = serial_gnss.readline(serial_gnss.available());
  //               LOG(INFO)<<Name()<<"  Read: " << strSerialResult;
  //               NMEA0183 nmea;
  //               if (Analyse(strSerialResult, nmea) == ROBOT_SUCCESS)
  //                   PublishData(nmea);
  //           }
             //TODO

             ros::spinOnce();
             loop_rate.sleep();
           }
       }
    }
    catch (serial::IOException& e) {
      LOG(ERROR)<<Name()<< " Unable to open port " << e.what();
      return ROBOT_FAILTURE;
    }
  }
else if(car_input_mode == "pci"){
  ros::Rate loop_rate(rate);
  while(ros::ok())
  {
    if (kbhit()) continue;
    if (!ACTS2110_AI_ReadAnalog(hDevice, fVoltArray, nReadSampsPerChan, &nSampsPerChanRead, &nReadableSamps, fTimeout))
    {
      LOG(ERROR) << Name() <<"PCI Read Data Timeout nSampsPerChanRead=" << nSampsPerChanRead;
    }
    else{
      if (nSampsPerChanRead <= 0)
      {
          continue;
      }
      LOG(INFO) << Name() << "PCI nSampsPerChanRead= " << nSampsPerChanRead;
      double voltage = 0;
      for (U32 nIndex = 0; nIndex < 1; nIndex++)
      {
          for (nChannel = 0; nChannel < AIParam.nSampChanCount; nChannel++)
          {
            voltage = fVoltArray[nChannel + nIndex * AIParam.nSampChanCount];
            printf("AI%02d=%.4f\t\n", nChannel, fVoltArray[nChannel + nIndex * AIParam.nSampChanCount]);
          }
      }
      try{
          localization::localMsg localization;
          localization.Longitude =  0;
          localization.Latitude = 0;
          localization.Height = 0;
          localization.AngleDeviated = 0;
          localization.Speed = voltage;
          if(localization.Speed < 0.4)
          {
          localization.Speed = 0;
          }
          geometry_pub.publish(localization);
          LOG(INFO) << Name() << " publish speed:" << std::setprecision(4) << localization.Speed;
      }
      catch (ros::Exception ex) {
          LOG(ERROR)<<Name()<< "PCI publish speed error: " << ex.what();
      }
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}
  //TODO
  LOG(INFO)<<Name()<<" start end.";
  return ROBOT_SUCCESS;
}

void Localization::RackTestOnTimer(const ros::TimerEvent &event)
{
  localization::localMsg localization;
  localization.Longitude =  0;
  localization.Latitude = 0;
  localization.Height =0;
  localization.AngleDeviated = 0;
  if(speed_count <= speed_number -1)
  {
    localization.Speed = speeds[speed_count];
  }
  else
  {
    localization.Speed = 0;
  }
  geometry_pub.publish(localization);
  speed_count ++;

}
int Localization::read_txt(string &path)
{
  //ifstream myfile("/home/madesheng/data.txt");
  ifstream myfile(path);
  if (!myfile.is_open())
  {
      LOG(ERROR)<<Name()<< "open src File  Error opening file" << endl;
      return ROBOT_FAILTURE;
  }
  for (int i = 0; i < 180000; i++)
  {
      if(myfile.eof())
      {
          speed_number = i;
          break;
      }
      myfile >> speeds[i];
      //myfile >> temp;
      //cout <<"char:"<< temp<< endl;
     //cout <<"angle:"<< angles[i] <<endl;
  }
  cout << "speed NUMBER:" <<speed_number<<endl;
   myfile.close();
   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...";
  if (car_input_mode == "modbus") {
     close(modbus_socket);
  }
  else if (car_input_mode == "rtk") {
    serial_gnss.close();
  }
  else if(car_input_mode == "pci")
  {
    // 第六步 停止AI采集任务
    if (!ACTS2110_AI_StopTask(hDevice))
    {
      LOG(ERROR)<<Name()<< "PCI AI_StopTask Error";
    }

    // 第七步 释放AI采集任务
    if (!ACTS2110_AI_ReleaseTask(hDevice))
    {
      LOG(ERROR)<<Name()<< "PCI AI_ReleaseTask Error";
    }

    // 第八步 释放设备对象
    if (!ACTS2110_DEV_Release(hDevice))
    {
      LOG(ERROR)<<Name()<< "PCI DEV_Release Error";
    }

  }
  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);
        int number = v_nmeaData.size();
              //TODO
//        if (v_nmeaData.size() !=3)
//            return ROBOT_FAILTURE;
              //TODO
        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.";
            }
            else if (type.compare("IFDYN") == 0)
            {
              IFDYN tempIFDYN;
              if(ParaIFDYN(*iter, tempIFDYN) != ROBOT_SUCCESS)
                return ROBOT_FAILTURE;
              nmea.ifdyn = tempIFDYN;
              nmea.ifdyn.print();
              LOG(INFO)<<Name()<<" para IFDYN end.";

            }
            else if(type.compare("GPFPD") == 0)
            {
              GPFPD tempGPFPD;
              if(ParaGPFPD(*iter, tempGPFPD) != ROBOT_SUCCESS)
                return ROBOT_FAILTURE;
              nmea.gpfpd = tempGPFPD;
              nmea.gpfpd.print();
              LOG(INFO)<<Name()<<" para GPFPD 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);
       // localization.Speed = to_string_with_precision(nmea.gprmc.Deviated, 4) * SPEED_FACTOR;
       // localization.Speed = to_string_with_precision(nmea.ifdyn.VelHori,4) * SPEED_FACTOR;
        double eastSpeed = to_string_with_precision(nmea.gpfpd.VelEast, 4) * SPEED_FACTOR;
        double northSpeed = to_string_with_precision(nmea.gpfpd.VelNorth, 4) * SPEED_FACTOR;
        double speeds = pow(eastSpeed, 2) + pow(northSpeed,2);
        localization.Speed = sqrt(speeds);
        geometry_pub.publish(localization);
        LOG(INFO)<< "Test Speed:"<<std::setprecision(4)<<localization.Speed;
    }
    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()
    };
}


int Localization::ParaIFDYN(std::string &strIFDYN, IFDYN &ifdyn)
{
  try
  {
      std::vector<std::string> nmea_ifdyn = Split(strIFDYN, ",");
      ifdyn.YawAngle = atof(nmea_ifdyn[1].c_str());
      ifdyn.RollAngle = atof(nmea_ifdyn[2].c_str());
      ifdyn.PitchAngle = atof(nmea_ifdyn[3].c_str());
      ifdyn.VelHori = atof(nmea_ifdyn[4].c_str());
      ifdyn.VelEast = atof(nmea_ifdyn[5].c_str());
      ifdyn.VelNorth = atof(nmea_ifdyn[6].c_str());
      ifdyn.VelD = atof(nmea_ifdyn[7].c_str());
      ifdyn.AccelX = atof(nmea_ifdyn[8].c_str());
      ifdyn.AccelY = atof(nmea_ifdyn[9].c_str());
      ifdyn.AccelZ = atof(nmea_ifdyn[10].c_str());
      ifdyn.AngleRateX = atof(nmea_ifdyn[11].c_str());
      ifdyn.AngleRateY = atof(nmea_ifdyn[12].c_str());
      ifdyn.AngleRateZ = atof(nmea_ifdyn[13].c_str());
      double test = ifdyn.VelHori * SPEED_FACTOR;
      LOG(INFO)<< "Test VelHori:"<<std::setprecision(4)<<test;

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

int Localization::ParaGPFPD(std::string &strGPFPD, GPFPD &gpfpd)
{
  try
  {

      std::vector<std::string> nmea_gpfpd = Split(strGPFPD, ",");
      if(nmea_gpfpd.size() < 16)
      {
        return ROBOT_FAILTURE;
      }
      gpfpd.GPSWeek = atoi(nmea_gpfpd[1].c_str());
      memset(&gpfpd.GPSWeekSecond, 0x00, sizeof(gpfpd.GPSWeekSecond));
      memcpy(&gpfpd.GPSWeekSecond, nmea_gpfpd[2].c_str(), 16);
      gpfpd.Heading = atof(nmea_gpfpd[3].c_str());
      gpfpd.Pitch = atof(nmea_gpfpd[4].c_str());
      gpfpd.Roll = atof(nmea_gpfpd[5].c_str());
      gpfpd.Latitude = atof(nmea_gpfpd[6].c_str());
      gpfpd.Longitude = atof(nmea_gpfpd[7].c_str());
      gpfpd.Alt = atof(nmea_gpfpd[8].c_str());
      gpfpd.VelEast = atof(nmea_gpfpd[9].c_str());
      gpfpd.VelNorth = atof(nmea_gpfpd[10].c_str());
      gpfpd.VelU = atof(nmea_gpfpd[11].c_str());
      gpfpd.BaseLine = atof(nmea_gpfpd[12].c_str());
      gpfpd.Sat1Num = atoi(nmea_gpfpd[13].c_str());
      gpfpd.Sat2Num = atoi(nmea_gpfpd[14].c_str());
      memset(&gpfpd.Status, 0x00, sizeof(gpfpd.Status));
      memcpy(&gpfpd.Status, nmea_gpfpd[15].c_str(), 4);
  }
  catch(ros::Exception ex)
  {
      LOG(ERROR)<<Name()<< " ParaGPFPD  error" << ex.what();
      return ROBOT_FAILTURE;
  }
  return ROBOT_SUCCESS;
}

int Localization::Analyse_ReadLine(std::string &strSerialResult,  NMEA0183 &nmea)
{
  //LOG(INFO)<<Name()<<" Analyse_ReadLine start.";
  try
  {
    std::string type = strSerialResult.substr(1,5);
    std::string *str = &strSerialResult;
  //  LOG(INFO)<<Name()<<" type:"<<type;
    if(type.compare("IFDYN") == 0)
    {
//      IFDYN tempIFDYN;
//      if(ParaIFDYN(*str,tempIFDYN) != ROBOT_SUCCESS)
//        return ROBOT_FAILTURE;
//      nmea.ifdyn = tempIFDYN;
//      nmea.ifdyn.print();
//      LOG(INFO)<<Name()<<" para IFDYN end.";
    }
    else if(type.compare("GPFPD") == 0)
    {
      GPFPD tempGPFPD;
      if(ParaGPFPD(*str, tempGPFPD) != ROBOT_SUCCESS)
        return ROBOT_FAILTURE;
      nmea.gpfpd = tempGPFPD;
     // nmea.gpfpd.print();
    //  LOG(INFO)<<Name()<<" para GPFPD end.";
    }
  }
  catch (ros::Exception ex)
  {
      LOG(ERROR)<<Name()<< " Analyse_ReadLine  error" << ex.what();
      return ROBOT_FAILTURE;
  }
 // LOG(INFO)<<Name()<<" Analyse_ReadLine end.";
  return ROBOT_SUCCESS;

}

}  // namespace localizationspace
}  // namespace robot
