/**
 ******************************************************************************
 * @file           : test_diff_ctrl.cpp
 * @author         : wangyingjie
 * @brief          : None
 * @attention      : None
 * @date           : 2025/5/23
 ******************************************************************************
 */

#ifdef __cplusplus
extern "C"
{
#endif

#include "nzzn/diff_ctrl_c/heading_diff_ctrl_c.h"
#include "control/pid/studio_pid_c.h"
#include "geography/studio_proj_c.h"
#include "geometry/studio_geo_algo_c.h"

#ifdef __cplusplus
}
#endif

// 测试使用

#include "geometry/studio_geo_utils.h"
#include "geometry/studio_geo_algo.h"

// 测试使用结束

int main()
{
    double central_1 = floor(120.07158158 / 3.0) * 3.0;
    double central_2 = static_cast<int>(120.07158158 / 3) * 3;
    double central_3 = floor((120.07158158 + 1.5) / 3.0) * 3.0;
    printf("===================== %s =====================\n", __FILE__);

    silly::geo::utils::init_gdal_env();
    std::string path;
#ifdef IS_LINUX
    path = "/home/wyj/myself/2_data/2_geojson/multi_point/route_1.geojson";
#else
    path = "D:/9_data/2_readfile/geojson/multi_point/route_3.geojson";
#endif
    std::cout << "path: " << path << std::endl;

    std::vector<studio_geo_coll> res_collections;
    //studio_geo_coll temp_300;
    //temp_300.m_type = enum_geometry_type::egtLineString;
    //studio_geo_coll temp_100;
    //temp_100.m_type = enum_geometry_type::egtLineString;

    // 差速控制测试开始 ==================

    studio_point_c beg_p = studio_point_init(120.07158158, 36.15932277);
    studio_point_c end_p = studio_point_init(120.07193922, 36.15990152);
    studio_point_c change_1_p = studio_point_init(120.07232455, 36.15964342);
    studio_point_c change_2_p = studio_point_init(120.07215543, 36.15944710);

    // 测试记录 实际删去 --------beg--------
    studio_geo_coll beg_end_points;
    beg_end_points.m_type = enum_geometry_type::egtMultiPoint;
    beg_end_points.m_points.push_back(studio_point(beg_p.x, beg_p.y));
    beg_end_points.m_points.push_back(studio_point(end_p.x, end_p.y));
    beg_end_points.m_points.push_back(studio_point(change_1_p.x, change_1_p.y));
    beg_end_points.m_points.push_back(studio_point(change_2_p.x, change_2_p.y));
    //temp_300.m_line.push_back(studio_point(beg_p.x, beg_p.y));
    //temp_100.m_line.push_back(studio_point(beg_p.x, beg_p.y));
    // 测试记录 实际删去 --------end--------

    int count = 800;  // 最大迭代次数

    const int heads = 2;
    double current_heading[heads] = {300.0, 100.0};
    for (int i = 0; i < heads; ++i)
    {
        studio_geo_coll temp_line;
        temp_line.m_type = enum_geometry_type::egtLineString;
        temp_line.m_points.push_back(studio_point(beg_p.x, beg_p.y));

        diff_ctrl ctrl;
        init_diff_ctrl(&ctrl, &end_p, 0.5, 0.8, 0.01, 0.2);
        studio_point_c current_pos = beg_p;
        double current_heading_deg = current_heading[i];
        for (int j = 0; j < count; ++j)
        {
            double dist_sq = update_boat(&ctrl, &current_pos, current_heading_deg);

            printf("Step %02d: 航向=%.1f° L=%.2f R=%.2f 距离²=%.1fm²\n", j + 1, current_heading_deg, ctrl.left_throttle, ctrl.right_throttle, dist_sq);

            if (dist_sq < 4.0)
            {
                printf("到达目标！\n");
                break;
            }

            simulate_movement(&current_pos, &current_heading_deg, ctrl.left_throttle, ctrl.right_throttle);
            temp_line.m_line.push_back(studio_point(current_pos.x, current_pos.y));
        }

        // 前往先一个点
        reset_diff_ctrl_goal(&ctrl, &change_1_p);
        for (int j = 0; j < count; ++j)
        {
            double dist_sq = update_boat(&ctrl, &current_pos, current_heading_deg);
            
            printf("Step %02d: 航向=%.1f° L=%.2f R=%.2f 距离²=%.1fm²\n", j + 1, current_heading_deg, ctrl.left_throttle, ctrl.right_throttle, dist_sq);

            if (dist_sq < 4.0)
            {
                printf("到达目标！\n");
                break;
            }
            simulate_movement(&current_pos, &current_heading_deg, ctrl.left_throttle, ctrl.right_throttle);
            temp_line.m_line.push_back(studio_point(current_pos.x, current_pos.y));
        }

        // 前往第二个点
        reset_diff_ctrl_goal(&ctrl, &change_2_p);
        for (int j = 0; j < count; ++j)
        {
            double dist_sq = update_boat(&ctrl, &current_pos, current_heading_deg);

            printf("Step %02d: 航向=%.1f° L=%.2f R=%.2f 距离²=%.1fm²\n", j + 1, current_heading_deg, ctrl.left_throttle, ctrl.right_throttle, dist_sq);
            if (dist_sq < 4.0)
            {
                printf("到达目标！\n");
                break;
            }
            simulate_movement(&current_pos, &current_heading_deg, ctrl.left_throttle, ctrl.right_throttle);
            temp_line.m_line.push_back(studio_point(current_pos.x, current_pos.y));
        }
        res_collections.push_back(temp_line);
    }

    res_collections.push_back(beg_end_points);
    silly::geo::utils::write_geo_coll(path, res_collections);
    silly::geo::utils::destroy_gdal_env();

    return 0;
}