clear all; clc; close all;

%% mapa 3

%% Parametry symulacji

dt=0.2; % krok całkowania
ts=20; % czas symulacji
t=0:dt:ts; %time span

%% Parametry fizyczne robota mobilnego (w zale�no�ci od promienia ko�a, jak i rozmiaru ramy, zale�e� b�dzie promie� skr�tu i inne parametry ruchu)
a=5;
d_w=20;
l_w=30; 

%% Warunki początkowe

x0=450;
y0=50;
psi0=pi/2;

eta0=[x0;y0;psi0];

eta(: , 1) = eta0; % przypisanie pierwszej kolumnie
                   % trzech warunków początkowych w kolejnych wierszach

%% czasy jazdy                 
t1=2;
t2=3; 
t3=5; 
t4=6; 
t5=8; 
t6=9; 
t7=11;
t8=12;
t9=14;
t10=15; 
t11=17;
t12=18;
t13=20;

for i=1:length(t)
    act_t=i*dt;
    psi = eta(3,i); %aktualna orientacja w rad
    
    %Jakobian macierzy:
    J_psi=[cos(psi),-sin(psi),0;
           sin(psi),cos(psi),0;
           0,0,1];

    if (act_t<=t1)
        omega_1=(40/(2*pi*a))*2*pi*t1;
        omega_2=(40/(2*pi*a))*2*pi*t1;
    elseif (act_t>t1) && (act_t<=t2) 
        omega_1=(d_w*pi/2)*2*pi/(2*pi*a)/(t2-t1);
        omega_2=-(d_w*pi/2)*2*pi/(2*pi*a)/(t2-t1);
    elseif (act_t>t2) && (act_t<=t3)
        omega_1=(150/(2*pi*a))*2*pi/(t3-t2);
        omega_2=(150/(2*pi*a))*2*pi/(t3-t2);
    elseif (act_t>t3) && (act_t<=t4)
        omega_1=-(d_w*pi/2)*2*pi/(2*pi*a)*(t4-t3);
        omega_2=(d_w*pi/2)*2*pi/(2*pi*a)*(t4-t3);
    elseif (act_t>t4) && (act_t<=t5)
        omega_1=(240/(2*pi*a))*2*pi/(t5-t4);
        omega_2=(240/(2*pi*a))*2*pi/(t5-t4);
    elseif (act_t>t5) && (act_t<=t6)
        omega_1=(d_w*pi/2)*2*pi/(2*pi*a)*(t6-t5);
        omega_2=-(d_w*pi/2)*2*pi/(2*pi*a)*(t6-t5);
    elseif (act_t>t6) && (act_t<=t7)
        omega_1=(100/(2*pi*a))*2*pi/(t7-t6);
        omega_2=(100/(2*pi*a))*2*pi/(t7-t6);
    elseif (act_t>t7) && (act_t<=t8)
        omega_1=(d_w*pi/2)*2*pi/(2*pi*a)*(t8-t7);
        omega_2=-(d_w*pi/2)*2*pi/(2*pi*a)*(t8-t7);
    elseif (act_t>t8) && (act_t<=t9)
        omega_1=(110/(2*pi*a))*2*pi/(t9-t8);
        omega_2=(110/(2*pi*a))*2*pi/(t9-t8);
    elseif (act_t>t9) && (act_t<=t10)
        omega_1=-(d_w*pi/2)*2*pi/(2*pi*a)*(t10-t9);
        omega_2=(d_w*pi/2)*2*pi/(2*pi*a)*(t10-t9);
    elseif (act_t>t10) && (act_t<=t11)
        omega_1=(150/(2*pi*a))*2*pi/(t11-t10);
        omega_2=(150/(2*pi*a))*2*pi/(t11-t10);
    elseif (act_t>t11) && (act_t<=t12)
        omega_1=-(d_w*pi/2)*2*pi/(2*pi*a)*(t12-t11);
        omega_2=(d_w*pi/2)*2*pi/(2*pi*a)*(t12-t11);
    elseif (act_t>t12) && (act_t<=t13)
        omega_1=(110/(2*pi*a))*2*pi/(t13-t12);
        omega_2=(110/(2*pi*a))*2*pi/(t13-t12);
    end

       omega=[omega_1; omega_2];

       % Macierz konfiguracji koła 

    W=[a/2 a/2; 0 0; -a/(2*d_w) a/(2*d_w)];

      %% Wektor prędkości wejściowych
    zeta(:,i)=W*omega;
       
    eta_dot(:,i) = J_psi*zeta(:,i); % Pochodna czasowa po wsp�rz�dnych uog�lnionych
       
    eta(:,i+1) = eta(:,i)+dt*eta_dot(:,i); % metoda Eulera
end

l=30; % długość platformy
w=20; %szerokość platformy

% Współrzędne opisujące kształ platformy
mr_co=[-l/2,l/2,l/2,-l/2,-l/2;
        -w/2,-w/2,w/2,w/2,-w/2];

figure
for i=1:length(t) % początek animacji
    x=[0 60 60 0];
    y=[120 120 140 140];
    fill(x,y,'w')
    hold on
    x=[135 155 155 135];
    y=[70 70 430 430];
    fill(x,y,'b')
    hold on
    x=[155 390 390 155];
    y=[240 240 260 260];
    fill(x,y,'b')
    hold on
    x=[280 300 300 280];
    y=[500 500 360 360];
    fill(x,y,'w')
    hold on
    x=[290 310 310 290];
    y=[0 0 160 160];
    fill(x,y,'w')
    hold on
    x=[360 500 500 360];
    y=[120 120 140 140];
    fill(x,y,'w')
    hold on

    psi= eta(3,i);
    R_psi=[cos(psi), -sin(psi);
           sin(psi),cos(psi)]; % macierz rotacji
    v_pos=R_psi*mr_co;

    fill(v_pos(1,:)+eta(1,i), v_pos(2,:)+eta(2,i),'g') % robot
    hold on, grid on; axis([0 500 0 500]), axis square

    plot(eta(1,1:i),eta(2,1:i),'b-'); % ścieżka ruchu

    xlabel('x[m]'); ylabel('y[m]');
    pause(0.1);
    hold off
end % koniec animacji