clc;
clear all;
close all;
% 添加函数路径
addpath('./DwaFunction');
% 添加数据路径
addpath('./InitializeData');
% 加载地图
load('./InitializeData/Environmental_simple');
% 加载色阶信息
load('./InitializeData/color');
%% 初始化参数设置
% 设置仿真步长
global dt;
dt = 0.1;
% 设置最长仿真时间200s
% 超出仿真时间路径依旧不收敛记为规划失败
End_Time = 200;
% 循环次数
cycles = End_Time/dt;
% 时间序列
Sequence = dt:dt:End_Time;
% 起点下标[y,x]
startpossub = [20,36];
% 目标点下标[y,x]
goalpossub= [15,5];
% 初始化参数
[field,obstacle,RM_parameters,Lidar] = DwaInitialization(field,startpossub);
%% 绘制选项
% 绘制雷达效果
USE_Lidar = 1;
% 绘制轨迹信息(绘制信息较多，打开后运行较慢)
USE_Traj = 0;
% 是否保存GIF
SAVE_GIF = 0;
%% 预处理
% 速度色阶
colorv = cmap(1+ceil(255*RM_parameters.x(3)/RM_parameters.KinematicLimit(1)),:);
Fig=figure;
filename = 'dynamic.gif';      % 输出路径+保存的文件名.gif
% 移动障碍索引
j = 1;
%% 仿真
for i = 1:cycles
    %% DWA过程
    % 计算动态窗口
    RM_parameters.DynamicWin = CalcDynamicWindow(RM_parameters.x,RM_parameters.KinematicLimit);
    % 获取局部障碍信息
    Lidar = LidarSearch(obstacle,Lidar);
    % 综合评估
    [RM_parameters.evalDB,RM_parameters.trajDB,Lidar] = DwaEvaluation(RM_parameters,Lidar,goalpossub);
    % 来不及制动停止运行
    if isempty(RM_parameters.evalDB)
        disp('制动失败!!');
        u=[0;0];return;
    end
    % 寻找最优控制输入
    u = FindOptimalSolution(RM_parameters.evalDB,RM_parameters.evalParam);
    % 计算下一时刻状态
    RM_parameters.x = Model(RM_parameters.x,u);
    % 记录状态信息
    RM_parameters.savex = [RM_parameters.savex,RM_parameters.x];
    % 是否到达目的地
    if norm(RM_parameters.x(1:2)-goalpossub')<1
        disp('到达目标!!');break;
    end
    %% 更新位置
    Lidar.x = RM_parameters.x(1);
    Lidar.y = RM_parameters.x(2);    
    %% 绘制动图
    % 计算速度色阶
    colorv = [colorv;cmap(1+ceil(254*RM_parameters.x(3)/RM_parameters.KinematicLimit(1)),:)];
    % 关闭绘制
    hold off;
    % 绘制栅格地图
    DrawMap(field,startpossub,goalpossub);
    % 绘制速度映射的色阶轨迹
    if USE_Traj
        for c =1:size(RM_parameters.savex,2)
            scatter(RM_parameters.savex(2,c),RM_parameters.savex(1,c),6,...
                'MarkerEdgeColor',colorv(c,:),...
                'MarkerFaceColor',colorv(c,:)...
                );hold on;
        end
    end
    
    % 绘制雷达仿真图像
    if USE_Lidar
       % 绘制圆形区域
       h = rectangle('Position',[Lidar.y - Lidar.r,Lidar.x - Lidar.r,2*Lidar.r,2*Lidar.r],'Curvature',[1,1], 'LineWidth', 1,'linestyle','-');
       % [R G B alpha]
       h.FaceColor = [0 1 0 0.1];
        for k = 1:size(Lidar.Data,1)
            % 最近距离使用红线绘制
            if k == Lidar.indmin
                plot([RM_parameters.x(2) RM_parameters.x(2)+Lidar.Data(k,1)*cos(Lidar.Data(k,2))], [RM_parameters.x(1) RM_parameters.x(1) + Lidar.Data(k,1)*sin(Lidar.Data(k,2))],'Color',[1,0,0],'LineWidth', 1,'LineStyle', '--');
            else
                plot([RM_parameters.x(2) RM_parameters.x(2)+Lidar.Data(k,1)*cos(Lidar.Data(k,2))], [RM_parameters.x(1) RM_parameters.x(1) + Lidar.Data(k,1)*sin(Lidar.Data(k,2))],'Color',[0.2,0.7,1],'LineWidth', 1,'LineStyle', '--');
            end
            hold on;
        end
    end
    % 机器人速度方向指示线长度
    ArrowLength=1;
    % 机器人
    quiver(RM_parameters.x(2),RM_parameters.x(1),ArrowLength*sin(RM_parameters.x(4)),ArrowLength*cos(RM_parameters.x(4)),'ok');hold on;
    % 动态窗口轨迹
    if ~isempty(RM_parameters.trajDB)
        for it=1:length(RM_parameters.trajDB(:,1))/5
            ind=1+(it-1)*5;
            plot(RM_parameters.trajDB(ind+1,:),RM_parameters.trajDB(ind,:),'-g');hold on;
        end
    end
    % 设置窗口大小和位置[x y L w]
    set(gcf, 'unit', 'centimeters', 'position', [10 5 15 15]);
    drawnow;
    % 保存GIF
    if SAVE_GIF
        frame = getframe(Fig);
        im = frame2im(frame);
        [imind,cm] = rgb2ind(im,256);
        if i == 1
            imwrite(imind,cm,filename,'gif','WriteMode','overwrite', 'Loopcount',inf);
            %Loopcount只是在i==1的时候才有用
        else
            imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',dt);
            %DelayTime:帧与帧之间的时间间隔
        end
    end
end
%% 绘制相关数据
% 全局速度
figure(2);
plot(Sequence(1:i),RM_parameters.savex(3,1:i),'Color',[1,0.3,0.6],'LineWidth',2, 'LineStyle', '-');hold on;
xtxt = xlabel('$$t$$/s','FontSize',15);
set(xtxt,'Interpreter','latex');
ytxt = ylabel('$$v$$/m/s','Interpreter','latex','FontSize',8);
set(ytxt,'Interpreter','latex');
set(gca,'FontName','Times New Roman','FontSize',8);
title("Robot linear speed");
% 距离障碍
figure(3);
plot(Sequence(1:i),Lidar.savemin(1:i),'Color',[0.1,0.3,0.2],'LineWidth',2, 'LineStyle', '-');hold on;
xtxt = xlabel('$$t$$/s','FontSize',15);
set(xtxt,'Interpreter','latex');
ytxt = ylabel('$$L$$/m','Interpreter','latex','FontSize',8);
set(ytxt,'Interpreter','latex');
set(gca,'FontName','Times New Roman','FontSize',8);
title("Nearest obstacle distance");