/******************************************************************************
 * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
 * Author: madesheng
 * Licensed under the Apache License, Version 1.0 (the "License");
 *****************************************************************************/

/**
 * @file
 */

#ifndef ROBOT_ROBOT_LOCALIZATION_H_
#define ROBOT_ROBOT_LOCALIZATION_H_

#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <sstream>
#include <serial/serial.h>  //ROS已经内置了的串口包
#include "../common/robotapp.h"
#include "ros/ros.h"
#include <stdlib.h>
#include <math.h>

#include "localizationdata.h"

/**
 * @namespace robot::localization
 * @brief robot::localization
 */
namespace robot {
namespace localizationspace {

/**
* @class localization
*
* @brief localization module main class.
* It processes the control data to send protocol messages to can bus.
*/
class Localization : public robot::common::RobotApp{
 public:
  Localization()  {}
  virtual~Localization() {}

  /**
  * localization总线节点名称
  */
  std::string Name() const override;

  /**
  * @brief module initialization function
  * @return initialization status
  */
  int Init() override;

  /**
  * @brief module start function
  * @return start status
  */
  int Start() override;

  /**
  * @brief module stop function
  */
  void Stop() override;

 private:

  bool IsOpen();

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

  std::vector<std::string> Split(const std::string& in, const std::string& delim);

  int ParaGPRMC(std::string &nmea_gprmc, GPRMC &gprmc);

  int ParaGPGGA(std::string &nmea_gpgga, GPGGA &gpgga);
  /**
   * @brief string localization ddmm convert to double dd type
   * @param strResult
   * @param pos
   * @return dd type
   */
  inline double StrToDouble(const std::string &strResult, int pos)
  {
      return atof(strResult.substr(0, pos).c_str()) + atof(strResult.substr(pos, strResult.length() - pos).c_str())/60.0;
  }


  inline double to_string_with_precision(double a_value, int n){
      std::ostringstream out;
      out<<std::setprecision(n)<<a_value;
      return atof(out.str().c_str());
  }

  /**
   * @brief publish data message to other nodes
   * @param nmea
   * @return ture or false
   */
  int PublishData(NMEA0183 &nmea);
private:
  int64_t last_timestamp_ = 0;

  ros::Timer timer_;

  std::string serial_port ;

  uint32_t baudrate = 0;

  uint32_t timeout = 50;

  serial::Serial serial_gnss; //声明串口对象

  ros::Publisher geometry_pub;

  const double rate = 20;  //20HZ

};


}  // namespace localization
}  // namespace robot

#endif  // ROBOT_ROBOT_LOCALIZATION_H_
