close all
clc

angles = linspace(0, 2*pi, 500);
radius = 3;
CenterX = 0;
CenterY = 0;
x = radius * cos(angles) + CenterX;
y = radius * sin(angles) + CenterY;


c0_l = LaneMarkPosition_Lh_ME;
c1_l = LaneMarkHeadAngle_Lh_ME;
c2_l = LaneMarkModelA_Lh_ME;
c3_l = LaneMarkModelDerivA_Lh_ME;

c0_r = LaneMarkPosition_Rh_ME;
c1_r = LaneMarkHeadAngle_Rh_ME;
c2_r = LaneMarkModelA_Rh_ME;
c3_r = LaneMarkModelDerivA_Rh_ME;

x_temp = 0:0.1:60;


for i=70000:100:240000
    subplot(231)
    plot(x, y, 'b-', 'LineWidth', 2);
    hold on
    grid on
    plot(AccelForward(i), AccelLateral(i),'bd')
    plot(CenterX, CenterY, 'k+', 'LineWidth', 3, 'MarkerSize', 14);
    xlabel('X', 'FontSize', 14);
    ylabel('Y', 'FontSize', 14);    
    xlim([-4 4])
    ylim([-4 4])

    
    hold off
    drawnow


    subplot(234)
    plot(PosLon(70000:end),PosLat(70000:end))
    hold on
    grid on
    plot(PosLon(i),PosLat(i),'rd')
    

    hold off
    drawnow

    subplot(2,3,5:6)
    y_left = c0_l(i) + c1_l(i)*x_temp + c2_l(i)*x_temp.^2 + c3_l(i)*x_temp.^3;
    y_right = c0_r(i) + c1_r(i)*x_temp + c2_r(i)*x_temp.^2 + c3_r(i)*x_temp.^3;

    plot(x_temp,y_left)
    hold on
    grid on
    plot(x_temp,y_right)
    xlim([0 60])
    ylim([-10 10])
    draw_veh(0,0,-pi/2,2,4.5,'k',1);


    draw_veh(PosX01(i),PosY01(i),-pi/2,2,4.5,'r',1);
    draw_veh(PosX02(i),PosY02(i),-pi/2,2,4.5,'r',1);
    draw_veh(PosX03(i),PosY03(i),-pi/2,2,4.5,'r',1);
    draw_veh(PosX04(i),PosY04(i),-pi/2,2,4.5,'r',1);
    draw_veh(PosX05(i),PosY05(i),-pi/2,2,4.5,'r',1);
    draw_veh(PosX06(i),PosY06(i),-pi/2,2,4.5,'r',1);
    draw_veh(PosX07(i),PosY07(i),-pi/2,2,4.5,'r',1);
    draw_veh(PosX08(i),PosY08(i),-pi/2,2,4.5,'r',1);

    y_radar = (YAW_RATE(i)/180*pi / Speed2D(i) / 2)*x_temp.^2;
    plot(x_temp, y_radar,'g--')

    hold off
    drawnow
    
    subplot(2,3,2:3)
    plot(Speed2D(1:end))
    hold on
    grid on
    plot(i,Speed2D(i),'rd')
    % plot(i,,'md')

    hold off
    drawnow

end