#ifndef RECHARGE_HPP
#define RECHARGE_HPP

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
#include <interface/srv/scene_controls.hpp>
#include <interface/msg/empty.hpp>

#include <cmath>
#include <vector>
#include <memory>
#include <string>
#include <algorithm>
#include <deque>
#include <numeric>
#include <Eigen/Dense>

using namespace std::chrono_literals;

// 数据结构
struct IndexPoint
{
    double x;
    double y;
    int index;

    IndexPoint(double x = 0.0, double y = 0.0, int index = 0)
        : x(x), y(y), index(index) {}
};

struct StraightLine
{
    double k;
    double b;

    StraightLine(double k = 0.0, double b = 0.0) : k(k), b(b) {}
};

struct ScanFilterParam
{
    int min_index = 286;
    int max_index = 386;
    double min_intensities = 120.0;
    double max_intensities = 220.0;
    double min_distance = 0.28;
    double max_distance = 3.5;
    int min_count = 4;
    double dock_length = 0.42;
    int baselink_back_index = 337;
};

struct Result
{
    int frameid;
    sensor_msgs::msg::LaserScan::SharedPtr laserScanData;
    std::vector<IndexPoint> points;
    std::vector<IndexPoint> left_points;
    std::vector<IndexPoint> right_points;
    IndexPoint middle_point;
    StraightLine middle_line;
    StraightLine left_line;
    StraightLine right_line;
};

// 充电状态枚举
enum class ChargingState
{
    IDLE,
    STARTING,
    NAVIGATING,
    CHARGING,
    UNAVAILABLE,
    COMPLETED
};

// 工具类
class ToolClass
{
public:
    static double angleBetween(const IndexPoint &p1, const IndexPoint &p2);
    static double angleDiff(double a1, double a2);
    static double averageAngle(const std::vector<double> &angles);

    // 分段函数
    static std::vector<std::vector<IndexPoint>> splitPointsToSegments(
        const std::vector<IndexPoint> &points,
        double angle_threshold_deg = 15.0,
        int history_window_size = 3);
};

// 主节点类
class ScanFilterNode : public rclcpp::Node
{
public:
    ScanFilterNode();

private:
    void scanCallback1(const sensor_msgs::msg::LaserScan::SharedPtr msg, int begin_index, int end_index);
    void onPointPartition(const Result &result);
    void procData(Result &result);
    void rvizShowResult(const Result &result);
    sensor_msgs::msg::LaserScan::SharedPtr resetLaserData(sensor_msgs::msg::LaserScan::SharedPtr msg, int b_index, int e_index);
    Eigen::MatrixXd getPointList(const std::vector<IndexPoint> &points);
    std::vector<std::pair<int, int>> getSections(
        int begin_index, int end_index,
        const sensor_msgs::msg::LaserScan::SharedPtr msg,
        const std::map<int, std::pair<double, double>> &xy_list);

    std::map<int, std::tuple<int, int, double, double, double, double, double, int>> 
    getSegments(const std::map<int, std::pair<double, double>> &xy_points_map);

    std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
    filterSegments(const std::map<int, std::tuple<int, int, double, double, double, double, double, int>> &turn_segments);
    void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
    void onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg);
    void timerCallback();
    void publishFilteredScan(const sensor_msgs::msg::LaserScan::SharedPtr original_msg,
                             const std::vector<float> &filtered_ranges);

    // 激光数据处理函数
    std::map<int, std::pair<double, double>> laserScanToXY(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg);
    std::map<int, std::pair<double, double>> laserScanToXY1(int begin_index, int end_index,
                                                            const sensor_msgs::msg::LaserScan::SharedPtr scan_msg);

    // 过滤函数
    void filterScanModify0(sensor_msgs::msg::LaserScan::SharedPtr msg);
    void filterScanModify1(sensor_msgs::msg::LaserScan::SharedPtr msg);
    void filterScanModify2(sensor_msgs::msg::LaserScan::SharedPtr msg);
    void filterScanModify3(sensor_msgs::msg::LaserScan::SharedPtr msg);
    std::pair<int, int> filterScanModify4(sensor_msgs::msg::LaserScan::SharedPtr msg);

    // 直线拟合
    static std::pair<double, double> fitLineLeastSquares(const Eigen::MatrixXd &points);

    // 控制函数
    void ctrlProc(const std::vector<IndexPoint> &left_points,
                  const std::vector<IndexPoint> &right_points);
    static std::pair<double, double> computeDockingVelocity(double dx, double dy, double yaw_error);

    // 辅助函数
    static bool isValid(float value);
    int getFixedIndex(const sensor_msgs::msg::LaserScan::SharedPtr msg, int i);
    int getFixed();

    // ROS2成员
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_filtered_;
    rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub_line_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_ctrl_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_cmd_enable_;
    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_rviz_;
    rclcpp::TimerBase::SharedPtr timer_;

    // 状态变量
    bool cmd_enable_ = false;
    geometry_msgs::msg::Twist ctrl_twist_;
    rclcpp::Time ctrl_twist_time_;
    ScanFilterParam param_;
    double distance_threshold_;
    int fit_window_size_;
    double fit_residual_threshold_;
    int frame_index_ = 0;
    int counter_ = 0;
    rclcpp::Time detect_station_time_;
};

// 充电管理类
class ChargingManager : public rclcpp::Node
{
public:
    ChargingManager();

    void startCharging();
    void stopCharging();

private:
    void onCtrl(const std_msgs::msg::String::SharedPtr msg);
    void executeChargingControl();
    void monitorCharging();
    void changeState(ChargingState new_state);
    void reportState(ChargingState state);
    bool detectStation();
    bool detectChargingVoltage();
    std::pair<double, double> generateControlCommand();
    void applyControlCommand(double linear_vel, double angular_vel);

    // ROS2成员
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_ctrl_;

    // 状态变量
    ChargingState state_ = ChargingState::IDLE;
    rclcpp::Time last_station_detection_time_;
    rclcpp::Time last_voltage_detection_time_;
    double station_lost_timeout_ = 3.0;
    double control_interval_ = 0.1;
};

class RechargerRecv : public rclcpp::Node
{
public:
    RechargerRecv();
    ~RechargerRecv();

private:
    void onGetCharging(const std_msgs::msg::Int32::SharedPtr msg);
    void onModeChange(const std_msgs::msg::String::SharedPtr msg);
    void reportLoop();
    void callService(int start);
    void changeStatus(int status);

    // ROS2成员
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr getcharging_sub_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_mode_change_;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr pub_feed_;
    rclcpp::Client<interface::srv::SceneControls>::SharedPtr client_;

    // 状态变量
    int last_report_status_;
    int last_status_;
    bool running_;
    std::thread thread_;
};

// 全局函数声明
bool isCharging();
void setChargingVoltage(bool value);
bool getChargingVoltage();
void setMode(const std::string &mode);
std::string getMode();
std::pair<double, double> calculateAngleBisector(double k1, double b1, double k2, double b2);
#endif // RECHARGE_HPP