close all
T=15;
n=200; % ekvidistantna mreža od n dijelova [0,T]
DeltaT=T/n; % jedinica vremena

m=15200; % masa u kg
G=m*9.81*0.166; % 16.6% težine na Zemlji

% poèetni uvjeti
x0=50;
v0=-10;
% cilj
xT=0;
vT=0;

% kreiranje matrica Ac i vektora bc (za Ac*w = bc)
% za uvjete tipa jednakosti
Ac=zeros(2*n,3*n-2);
bc=zeros(2*n,1);

Ac(1,1)=1;
Ac(2:n,1:n-1)=-eye(n-1)+diag(ones(n-2,1),1);
Ac(2:n,n:(2*n-2))=-DeltaT*eye(n-1);
Ac(n+1,n)=1;
Ac(n+2:2*n,n:(2*n-2))=-eye(n-1)+diag(ones(n-2,1),1);
Ac(n+1:2*n,(2*n-1):(3*n-2))=-DeltaT/m*eye(n);

bc(1)=x0+DeltaT*v0;
bc(n)=-xT;
bc((n+1):2*n)=-DeltaT/m*G*ones(n,1);
bc(n+1)=bc(n+1)+v0;
bc(2*n)=bc(2*n)-vT;

% funkcija cilja c
c=zeros(3*n-2,1);
c((2*n-1):(3*n-2))=1;

% kreiranje matrica A i vektora b (za A*w <= b)
% za uvjete tipa nejednakosti
A=zeros(3*n-1,3*n-2);
b=zeros(3*n-1,1);
b()
b((2*n):(3*n-1))=44000*ones(n,1); % ogranièenje na silu odozgo
A(1:n,(2*n-1):(3*n-2))=-eye(n);
A((n+1):(2*n-1),1:(n-1))=-eye(n-1);
A((2*n):(3*n-1),(2*n-1):(3*n-2))=eye(n);


x = linprog(c,A,b,Ac,bc);
%hold on;
polozaj=figure;
plot(DeltaT*(0:n),[x0,x(1:n-1)',xT],'g');
title('Graf položaja');
xlabel('vrijeme/s');
ylabel('visina/m') ;
brzina=figure;
plot(DeltaT*(0:n),[v0,x(n:(2*n-2))',vT],'b');
title('Graf brzine');
xlabel('vrijeme/s');
ylabel('brzina/ms^{-1}') ;
akceleracijaUpravljanje=figure;
plot(DeltaT*(0:(n-1)),x((2*n-1):(3*n-2))'/m,'r');
title('Funkcija upravljanja - u');
xlabel('vrijeme/s');
ylabel('akceleracija/ms^{-2}') ;

% vrijednost f-je cilja (pomnožiti s DeltaT, ako usporeðujemo za razlièite n)
sum(x((2*n-1):(3*n-2))'/m)
