#ifndef PATH_VISUALIZER_HPP_
#define PATH_VISUALIZER_HPP_

#include <memory>
#include <curl/curl.h>
#include <nlohmann/json.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <algorithm>
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include <nav_msgs/msg/path.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>

class PathVisualizer : public rclcpp::Node
{
public:
    PathVisualizer(const std::string& file_name);
private:
    void localozation_callback(const geometry_msgs::msg::Pose::SharedPtr msg);
    void control();
    double quaternion_to_yaw(double x,
                             double y,
                             double z,
                             double w);
    double normalize_angle(double angle);
    bool downloadPathJson(const std::string &url,std::string &json_data);
    bool loadPathFromJson(const std::string &json_data);

    double gps_to_x(double lat,double lon);
    double gps_to_y(double lat,double lon);

    int search_nearest_point();
    int findLookAheadPoint();
    int findClosestPoint(int forward_index_limit);
    // pub rviz info
    void publishPathRviz();
    void publishVehiclePoseRviz();
    void publishVehicleTFRviz();
    void publishHistoryPathRviz();
    void publishVehicleInfoRviz();
    void transformPathToVehicleStart();
    void densifyPath(double interval);
    void publishLocalPose();
    void publishOdomPath();

private:
    // topic pub
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr local_pose_pub_;
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr odom_path_pub_;
    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vehicle_pub_;
    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr path_pub_;
    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr history_pose_pub_;
    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vehicle_info_pub_;
    
    // topic sub
    rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr odom_sub_;
    rclcpp::TimerBase::SharedPtr timer_;
    
    // 坐标转换broadcaster
    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; // odom -> baselink

private:
    // odom path info 
    std::vector<double> path_x_;
    std::vector<double> path_y_;
    std::vector<double> path_yaw_;
    // odom history pose
    std::vector<double> history_x_;
    std::vector<double> history_y_;
    std::vector<double> history_yaw_;

    int target_index_;
    // localization info
    double x_;
    double y_;
    double yaw_;
    bool has_pose_;
    // contorl paramss
    double max_speed_ = 0.8;
    double max_yaw_rate_ = 2.0;
    double lookahead_distance_ =0.6;
    double origin_lat_;
    double origin_lon_;
    double origin_yaw_;
    int forward_index_limit_=100;


    bool origin_set_= false;
    bool path_transed = false;
    std::string file_name_;
    std::string path_url_ ;
    std::string json_data_;

};

#endif