clc;clear variables;format compact;
warning off
syms t  x3(t) x6(t) y1(t) y2(t) dy1(t) dy2(t) ddy1(t) ddy2(t) dddy1(t) dddy2(t) l1 l2
% 0.Ableitung
x6=atan(dy2/dy1)
% 1.Ableitung
dx6=diff(x6,t);
dx6=simplify(dx6);
dx6=subs(dx6,diff(dy1,t),ddy1);
dx6=subs(dx6,diff(dy2,t),ddy2);

dx6str=char(dx6);
dx6str=strrep(dx6str,'(t)','');

% 2.Ableitung
ddx6=diff(dx6);
ddx6=simplify(ddx6);

ddx6=subs(ddx6,diff(dy1,t),ddy1);
ddx6=subs(ddx6,diff(dy2,t),ddy2);
ddx6=subs(ddx6,diff(ddy1,t),dddy1);
ddx6=subs(ddx6,diff(ddy2,t),dddy2);

ddx6str=char(ddx6);
ddx6str=strrep(ddx6str,'(t)','');

% clear t y1 y2 dy1 dy2 ddy1 ddy2 dddy1 dddy2 l2 x6
% syms t x1(t) y1(t) y2(t) dy1(t) dy2(t) ddy1(t) ddy2(t) dddy1(t) dddy2(t) l2 x6(t)

%% x1,dx1,ddx1 das Auto ist rechts vom Anhänger
x1=y1+l2*cos(x6);

dx1=diff(x1,t);
dx1=simplify(dx1);
dx1=subs(dx1,diff(y1,t),dy1);
dx1=subs(dx1,diff(y2,t),dy2);
dx1=subs(dx1,diff(dy1,t),ddy1);
dx1=subs(dx1,diff(dy2,t),ddy2);


ddx1=diff(dx1,t);
ddx1=simplify(ddx1);
ddx1=subs(ddx1,diff(y1,t),dy1);
ddx1=subs(ddx1,diff(y2,t),dy2);
ddx1=subs(ddx1,diff(dy1,t),ddy1);
ddx1=subs(ddx1,diff(dy2,t),ddy2);
ddx1=subs(ddx1,diff(ddy1,t),dddy1);
ddx1=subs(ddx1,diff(ddy2,t),dddy2);
%% x2,dx2,ddx2
x2=y2+l2*sin(x6);

dx2=diff(x2,t);
dx2=simplify(dx2);
dx2=subs(dx2,diff(y1,t),dy1);
dx2=subs(dx2,diff(y2,t),dy2);
dx2=subs(dx2,diff(dy1,t),ddy1);
dx2=subs(dx2,diff(dy2,t),ddy2);


ddx2=diff(dx2,t);
ddx2=simplify(ddx2);
ddx2=subs(ddx2,diff(y2,t),dy2);
ddx2=subs(ddx2,diff(y2,t),dy2);
ddx2=subs(ddx2,diff(dy1,t),ddy1);
ddx2=subs(ddx2,diff(dy2,t),ddy2);
ddx2=subs(ddx2,diff(ddy1,t),dddy1);
ddx2=subs(ddx2,diff(ddy2,t),dddy2);


%% u1,u2
u1=dx1/(cos(atan(dx2/dx1)))
x3=acos(dx1/u1);
u2=atan(diff(x3,t)*l1/u1)
u2=subs(u2,diff(y2,t),dy2);
u2=subs(u2,diff(y2,t),dy2);
u2=subs(u2,diff(dy1,t),ddy1);
u2=subs(u2,diff(dy2,t),ddy2);
u2=subs(u2,diff(ddy1,t),dddy1);
u2=subs(u2,diff(ddy2,t),dddy2);

%% zu String und Sym konvertieren ohne (t)
clear t y1(t) y2(t) dy1(t) dy2(t) ddy1(t) ddy2(t) dddy1(t) dddy2(t) l2 x6(t)
syms t y1 y2 dy1 dy2 ddy1 ddy2 dddy1 dddy2 l2 x6
% Strings:
dx1str=char(dx1);
dx1str=strrep(dx1str,'(t)','');
ddx1str=char(ddx1);
ddx1str=strrep(ddx1str,'(t)','');
dx2str=char(dx2);
dx2str=strrep(dx2str,'(t)','');
ddx2str=char(ddx2);
ddx2str=strrep(ddx2str,'(t)','');
u1str=char(u1);
u1str=strrep(u1str,'(t)','');
u2str=char(u2);
u2str=strrep(u2str,'(t)','');
u2str=strrep(u2str,'(t)','')


% Syms:
dx1=simplify(sym(dx1str))
ddx1=simplify(sym(ddx1str))
dx2=simplify(sym(dx2str))
ddx2=simplify(sym(ddx2str))
u1=simplify(sym(u1str))
u2=simplify(sym(u2str))