function solveBVP3
run ../../parametry.m

global tf_su d1 d2 d3 d4 d5 d6 b1 b2 g sol
tf_su=1.59;
% 1.61;1.45
options = bvpset('RelTol',1e-8,'NMax',1000,'Stats','on');
solinit=bvpinit(linspace(0,tf_su,1000),@odeInit,[0 0 0 0]);
sol=bvp4c(@myODE, @odeBC, solinit,options)

%{
sol.th0(:,1)=linspace(0,tf_su,2000);
p=sol.parameters;
sol.th0(:,2) = -p(1)-p(3)-(p(2)+p(4))*cos(pi*sol.th0(:,1)/tf_su);
for i = 2:5
    sol.th0(:,2) = sol.th0(:,2) + p(i-1)*cos(i*pi*sol.th0(:,1)/tf_su);
end
%}

sol.th0=[sol.x;sol.y(1,:)]';
sol.th1=[sol.x;sol.y(2,:)]';
sol.th2=[sol.x;sol.y(3,:)]';
disp(sol.parameters);
close all
plot(sol.x,sol.y(1,:),sol.x,sol.y(2,:),sol.x,sol.y(3,:));

end

function dydt = myODE(t,y,p)
global tf_su d1 d2 d3 d4 d5 d6 b1 b2 g
%definuje diferenciální rovnici 1 řádu, která se bude řešit.

% y(1) ... cart
% y(2) ... arm1
% y(3) ... arm2
% y(4) ... dcart
% y(5) ... darm
% y(6) ... darm2

%{
% žádaná trajektorie

u=-p(1)-p(3)-(p(2)+p(4))*cos(pi*t/tf_su);
for i = 2:5
    u=u+p(i-1)*cos(i*pi*t/tf_su);
end
%}

%2. derivace žádané trajektorie
u=pi^2/tf_su^2*(p(2)+p(4))*cos(pi*t/tf_su);
for i = 2:5
    u=u-(i*pi/tf_su)^2*p(i-1)*cos(i*pi*t/tf_su);
end

    dydt = [y(4) y(5) y(6) u 0 0];
    A = d4;
    B = d5*cos(y(2)-y(3));
    C = - d2*cos(y(2))*u - (b1+b2)*y(5) + b2*y(6)- d5*sin(y(2)-y(3))*y(6)^2 + d2*g*sin(y(2));
    D = d6;
    E = - d3*cos(y(3))*u - b2*(y(6)-y(5)) + d5*sin(y(2)-y(3))*y(5)^2 + d3*g*sin(y(3));

    dydt(6) = (A*E-B*C) / (A*D-B^2);
    dydt(5) = (C*D-B*E) / (A*D-B^2);
end

function res = odeBC(ya, yb, p)
%{
res = [ya(1)
    ya(2)+pi 
    ya(3)+pi 
    ya(5)
    ya(6)
    ya(4)
    yb(2) 
    yb(3)
    yb(5)
    yb(6)];
%}
res = [ya(1)
    ya(2)+pi
    ya(3)+pi
    ya(4)
    ya(5)
    ya(6)
    yb(2)
    yb(3)
    yb(5)
    yb(6)
    ];
end

function v = odeInit(t)
global tf_su
    v=[0;
        -pi+pi*t/tf_su;
        -pi+pi*t/tf_su;
        0;
        pi/tf_su;
        pi/tf_su;
        ]';
end