% 所有路径数据定义
paths = {
    [2,2; 5,5; 7,5; 11,9; 11,10; 10,11; 18,19; 19,19], 'A*';
    [2,2; 9,2; 9,6; 11,6; 11,10; 10,10; 10,12; 13,12; 13,13; 17,13; 17,14; 18,14; 18,18; 18,19; 19,19], 'Dijkstra';
    [2,2; 7,2; 10,6; 14,3; 18,7; 14,11; 10,13; 10,18; 15,17; 19,19], 'RTT_one';
    [2,2; 7,2; 10,6; 5,7; 3,11; 7,14; 11,12; 16,14; 16,19; 19,19], 'RTT_two';
    [2,2; 7,2; 10,6; 5,8; 2,12; 2,17; 7,17; 11,15; 16,16; 19,19], 'RTT_three';
    [2,2; 7,2; 10,6; 15,5; 11,9; 9,14; 13,17; 18,19; 19,19], 'RTT_four';
};

% 模拟参数
v_normal = 10;       % 直线速度 m/s
a_turn = -2;         % 拐弯减速加速度 m/s^2
dt = 0.1;            % 时间步长

% 遍历每一组路径
for k = 1:size(paths, 1)
    points = paths{k,1};
    group_name = paths{k,2};

    time = 0;
    time_series = [];
    speed_series = [];
    acc_series = [];
    total_distance = 0;
    total_time = 0;

    for i = 1:size(points, 1) - 1
        p1 = points(i, :);
        p2 = points(i + 1, :);
        segment_vector = p2 - p1;
        segment_distance = norm(segment_vector);
        total_distance = total_distance + segment_distance;

        if i > 1
            prev_vec = points(i, :) - points(i - 1, :);
            angle = acos(dot(prev_vec, segment_vector) / (norm(prev_vec) * norm(segment_vector)));
            delta_v = abs(v_normal * sin(angle / 2));
            
            if delta_v > 0
                t_decel = delta_v / abs(a_turn);
                t_accel = t_decel;

                % 减速段
                t = 0:dt:t_decel;
                v_decel = v_normal - abs(a_turn) * t;
                a_decel = -abs(a_turn) * ones(size(t));

                % 加速段
                t2 = 0:dt:t_accel;
                v_accel = v_normal - delta_v + abs(a_turn) * t2;
                a_accel = abs(a_turn) * ones(size(t2));

                % 拼接
                time_series = [time_series, time + t];
                speed_series = [speed_series, v_decel];
                acc_series = [acc_series, a_decel];
                time = time + t_decel;

                time_series = [time_series, time + t2];
                speed_series = [speed_series, v_accel];
                acc_series = [acc_series, a_accel];
                time = time + t_accel;

                total_time = total_time + t_decel + t_accel;
            end
        end

        % 匀速段
        t_segment = segment_distance / v_normal;
        t = 0:dt:t_segment;
        v_const = v_normal * ones(size(t));
        a_const = zeros(size(t));
        
        time_series = [time_series, time + t];
        speed_series = [speed_series, v_const];
        acc_series = [acc_series, a_const];
        time = time + t_segment;
        total_time = total_time + t_segment;
    end

    % 输出总路程与用时
    fprintf('%s：总路程 = %.2f 米，总用时 = %.2f 秒\n', group_name, total_distance, total_time);

    % 绘图
    figure('Name', group_name, 'NumberTitle', 'off');
    subplot(2,1,1);
    plot(time_series, speed_series, 'b', 'LineWidth', 2);
    xlabel('时间 (s)');
    ylabel('速度 (m/s)');
    title([group_name ' - 速度-时间图']);
    grid on;

    subplot(2,1,2);
    plot(time_series, acc_series, 'r', 'LineWidth', 2);
    xlabel('时间 (s)');
    ylabel('加速度 (m/s²)');
    title([group_name ' - 加速度-时间图']);
    grid on;
end