#ifndef VISUALIZATION_MARKER_PUBLISHER_HPP
#define VISUALIZATION_MARKER_PUBLISHER_HPP

#include <istream>

#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>

#include "data_type.h"
#include "node_handle.hpp"

namespace mros {

//多个ID rviz会消耗大量资源
//#define VISUALIZATION_MARKER_TRACK_START_ID  100
//#define VISUALIZATION_MARKER_TRACK_ID_RANGE_MAX  100000

/**
 * @brief
 * @note
 * TODO: 当点数较多时传输带宽较大
 **/
class VisualizationMarkerPublisher{
public:

  explicit VisualizationMarkerPublisher( const ros::NodeHandlePtr nh ) : nh_( nh ) {
    ;
  }

  /**
   * @brief
   * @param  [in]   params   轨迹发布相关参数,其中
                    auto_publish_fq  定时器自动发送频率，小于零则不开定时器
   *                points_max：轨迹允许最大点数，设置为负数则不做限制
   * @param  [out]
   * @retval
   * @note
   **/
  void Init( const VisualizationMarkerPubParametersT& params ){
    if( params.points_max < 0 || params.points_max >= 100000 ){
      ROS_ERROR("points_max out of range, must in [0 - 99999] ");
      exit(0);
    }
    pub_params_ = params; //save params
    publisher_ = nh_->advertise<visualization_msgs::Marker>( params.topic , 30 );
    if( params.auto_publish_fq > 0){
      timer_ = nh_->createTimer( ros::Duration( 1.0 / double(params.auto_publish_fq) ),
                                  &mros::VisualizationMarkerPublisher::TimerCallBack, this );
    }
  }

  void AddMarker( const VisualizationMarkerParametersT& params ){
    ROS_INFO("marker base_id: %d ", params.base_id );
    if( params.base_id < 0 || params.base_id >= 1000 ){
      ROS_ERROR("base_id out of range, must in [0 - 999] ");
      exit(0);
    }
    visualization_msgs::Marker marker;
    MarkerDefaultSetting( marker ); //marker default base setting, must
    int32_t type = GetTypeValue( params.type );
    ColorT color = GetColorRbgValue( params.color ); //get color value
    double scale = GetScaleValue( params.scale );    //get scale value
    SetMarker( marker, pub_params_.frame_id , params.base_id , type, scale, color.r, color.g, color.b, color.a );
    markers_.push_back( marker );
  }


  void AddPoint( const uint marker_id, const geometry_msgs::Point  point ){
    assert( marker_id < markers_.size() );
    markers_[ marker_id ].points.push_back( point );
    if( pub_params_.points_max >= 0 && markers_[marker_id].points.size() > ulong( pub_params_.points_max ) ){
      markers_[ marker_id ].points.erase( markers_[ marker_id ].points.begin() );
   }
  }

  void AddPoints( const uint marker_id, const std::vector<geometry_msgs::Point> points ){
    for( auto point : points ){
      AddPoint( marker_id, point );
    }
  }

  void PublishPoint( const uint marker_id, const geometry_msgs::Point& point ){
    AddPoint( marker_id, point );
    //publish
    markers_[ marker_id ].header.stamp = ros::Time::now();
    publisher_.publish( markers_[ marker_id ] );
  }

  void PublishPoints( const uint marker_id, const std::vector<geometry_msgs::Point>& points ){
    AddPoints( marker_id, points );
    //publish
    markers_[ marker_id ].header.stamp = ros::Time::now();
    publisher_.publish( markers_[ marker_id ] );
  }

#if 0
  void SetColor( const VisualizationMarkerColorsL& opition ){
    ColorT color = GetColorRbgValue( opition ); //get color value
    marker_points_->color.r = float( color.r );
    marker_points_->color.g = float( color.g );
    marker_points_->color.b = float( color.b );
    marker_points_->color.a = float( color.a );
  }
#endif

  void SetMarker( visualization_msgs::Marker& marker,
                  const std::string frame_id, const int id ,
                  const int32_t type, const double scale,
                  const double r, const double g, const double b, const double a ){

    marker.header.frame_id = frame_id;
    marker.id = id;
    marker.type = type;

    marker.scale.x = scale;

    if( type == VISUALIZATION_MARKER_TYPE_LINE_LIST || type == VISUALIZATION_MARKER_TYPE_LINE_STRIP ){
      marker.scale.y = 0;
      marker.scale.z = 0;
    }
    else if( type == VISUALIZATION_MARKER_TYPE_POINTS ){
      marker.scale.y = scale;
      marker.scale.z = 0;
    }
    else {
      marker.scale.y = scale;
      marker.scale.z = scale;
    }
    //ROS_DEBUG("frame_id: %s id: %d color:(%f %f %f) scale: %f", //params.frame_id.c_str(), params.id, //color.r, color.g, color.b, scale );

    marker.color.r = float(r);
    marker.color.g = float(g);
    marker.color.b = float(b);
    marker.color.a = float(a);

  }

  // base_id: 基础参考id号,每个marker不能重复[0-999]

  void TimerCallBack( const ros::TimerEvent &event ){
    //ROS_DEBUG("visualization timer callback!");
    for( auto marker : markers_ ) {
      marker.header.stamp = ros::Time::now();
      publisher_.publish( marker );
    }
  }

private:
  ros::NodeHandlePtr nh_;
  ros::Timer timer_;
  ros::Publisher publisher_;
  VisualizationMarkerPubParametersT pub_params_;

  std::vector<visualization_msgs::Marker> markers_;

  //int points_max_;
  void MarkerDefaultSetting( visualization_msgs::Marker& marker ){
    marker.header.frame_id = "map";
    //marker_points_.type = visualization_msgs::Marker::CUBE_LIST;
    //marker_points_.type = visualization_msgs::Marker::LINE_LIST;
    marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration(); //保留一段时间 //marker_points_->lifetime = 0;  //持续保留
    marker.pose.orientation.w = 1;
    marker.pose.orientation.x = 0;
    marker.pose.orientation.y = 0;
    marker.pose.orientation.z = 0;
  }

  int32_t GetTypeValue( const VisualizationMarkerTypesL& opition ){
    int32_t type;
    switch ( opition ) {
      case VISUALIZATION_MARKER_TYPE_POINTS:
        type = visualization_msgs::Marker::POINTS;
        break;
      case VISUALIZATION_MARKER_TYPE_CUBE_LIST:
        type = visualization_msgs::Marker::CUBE_LIST;
        break;
      case VISUALIZATION_MARKER_TYPE_LINE_LIST:
        type = visualization_msgs::Marker::LINE_LIST;
        break;
      case VISUALIZATION_MARKER_TYPE_LINE_STRIP:
        type = visualization_msgs::Marker::LINE_STRIP;
        break;
    }
    return type;
  }
  double GetScaleValue( const VisualizationMarkerScaleL& opition ){
    double scale;
    double min_scale = 0.02;
    switch ( opition ) {
      case VISUALIZATION_MARKER_SCALE_01 :
        scale = min_scale;
        break;
      case VISUALIZATION_MARKER_SCALE_02 :
        scale = min_scale * 2;
        break;
      case VISUALIZATION_MARKER_SCALE_03 :
        scale = min_scale * 4;
        break;
      case VISUALIZATION_MARKER_SCALE_04 :
        scale = min_scale * 8;
        break;
      case VISUALIZATION_MARKER_SCALE_05 :
        scale = min_scale * 16;
        break;
      case VISUALIZATION_MARKER_SCALE_06 :
        scale = min_scale * 32;
        break;
    }
    return scale;
  }
  ColorT GetColorRbgValue( const VisualizationMarkerColorsL& opition ){
    ColorT color = Color(1,1,1);
    switch ( opition ) {
      case VISUALIZATION_MARKER_COLOR_WHITE :
        color.r = 1.0;
        color.g = 1.0;
        color.b = 1.0;
        break;
      case VISUALIZATION_MARKER_COLOR_BLACK :
        color.r = 0.0;
        color.g = 0.0;
        color.b = 0.0;
        break;
      case VISUALIZATION_MARKER_COLOR_RED :
        color.r = 1.0;
        color.g = 0.0;
        color.b = 0.0;
        break;
      case VISUALIZATION_MARKER_COLOR_ORANGE :
        color.r = 255/255.0;
        color.g = 97/255.0;
        color.b = 0.0;
        break;
      case VISUALIZATION_MARKER_COLOR_YELLOW :
        color.r = 1.0;
        color.g = 1.0;
        color.b = 0.0;
        break;
      case VISUALIZATION_MARKER_COLOR_GREEN :
        color.r = 0.0;
        color.g = 1.0;
        color.b = 0.0;
        break;
      case VISUALIZATION_MARKER_COLOR_BLUE :
        color.r = 0.0;
        color.g = 0.0;
        color.b = 1.0;
        break;
      case VISUALIZATION_MARKER_COLOR_CYAN :
        color.r = 34/255.0;
        color.g = 139/255.0;
        color.b = 34/255.0;
        break;
      case VISUALIZATION_MARKER_COLOR_VIOLET :
        color.r = 160/255.0;
        color.g = 32/255.0;
        color.b = 240/255.0;
        break;
    }
    return color;
  }

};  //end of VisualizationMarkerPublisher

}
#endif // VISUALIZATION_MARKER_PUBLISHER_HPP

#if 0
  void SetMarkerColor( const VisualizationMarkerColorsL& color_opition ){
    //get color value
    ColorT color = GetColorRbgValue( color_opition );
    marker_points_->color.r = float(color.r);
    marker_points_->color.g = float(color.g);
    marker_points_->color.b = float(color.b);
    marker_points_->color.a = float(color.a);
  }
#endif
#if 0
    marker_points_->id = 1;
    marker_points_->scale.x = 0.02;
    marker_points_->scale.y = 0.02;
    marker_points_->scale.z = 0.02;
    marker_points_->color.r = 0.9f;
    marker_points_->color.g = 0.9f;
    marker_points_->color.b = 0.9f;
    marker_points_->color.a = 1.0;
#endif
#if 0
  //ros timer, auto publish
  /timer_ = nh_->createTimer( ros::Duration( 1.0 / double(params.fq) ), &mros::VisualizationMarkerPublisher::TimerCallBack, this );
  void TimerCallBack( const ros::TimerEvent &event ){
    //ROS_DEBUG("visualization timer callback!");
    marker_points_->header.stamp = ros::Time::now();
    publisher_.publish( marker_points_ );
  }
#endif