clear all
clc
t0 = clock
syms t
syms alpha beta gamma delta_1 delta_2 delta_3 epsilon_1 epsilon_2 epsilon_3 phi nue psi;
syms alpha_d beta_d gamma_d delta_1_d delta_2_d delta_3_d epsilon_1_d epsilon_2_d epsilon_3_d phi_d nue_d psi_d;
syms alpha_dd beta_dd gamma_dd delta_1_dd delta_2_dd delta_3_dd epsilon_1_dd epsilon_2_dd epsilon_3_dd phi_dd nue_dd psi_dd;
syms i R r l x_W1 x_W2 x_W3 y_W1 y_W2 y_W3 z_W1 z_W2 z_W3 z_W x_M eta;
syms g m_K m_G m_W m_M J_K_xyz J_G_xx J_G_yy J_G_zz J_W_xx J_W_yy J_W_zz J_M_xx J_M_yy J_M_zz;
syms M_1 M_2 M_3;

% generalisierte Koordinaten
q = [alpha; beta; gamma; delta_1; delta_2; delta_3; phi; nue; psi];
q_d = [alpha_d; beta_d; gamma_d; delta_1_d; delta_2_d; delta_3_d; phi_d; nue_d; psi_d];
q_dd = [alpha_dd; beta_dd; gamma_dd; delta_1_dd; delta_2_dd; delta_3_dd; phi_dd; nue_dd; psi_dd];

% Eingang
u = [M_1; M_2; M_3];

% Koordinatentransformationen
arg = gamma;
A_x = [1 0         0;
       0 cos(arg)  -sin(arg);
       0 sin(arg)  cos(arg)];
   
arg = beta;
A_y = [cos(arg)  0 sin(arg);
       0         1 0;
       -sin(arg) 0 cos(arg)];

arg = alpha;
A_z = [cos(arg)  -sin(arg) 0;
       sin(arg)  cos(arg)  0;
       0         0         1];

A_GI = A_z * A_y * A_x;     % Trafo I -> G
   
arg = gamma;
A_x = [1 0         0;
       0 cos(arg)  sin(arg);
       0 -sin(arg) cos(arg)];
   
arg = beta;
A_y = [cos(arg) 0 -sin(arg);
       0        1 0;
       sin(arg) 0 cos(arg)];

arg = alpha;
A_z = [cos(arg)  sin(arg) 0;
       -sin(arg) cos(arg) 0;
       0         0        1];

A_IG = A_x * A_y * A_z;   % Trafo G -> I (= A_GI'!)

arg = psi;
A_x = [1 0         0;
       0 cos(arg)  sin(arg);
       0 -sin(arg) cos(arg)];
   
arg = nue;
A_y = [cos(arg) 0 -sin(arg);
       0        1 0;
       sin(arg) 0 cos(arg)];

arg = phi;
A_z = [cos(arg)  sin(arg) 0;
       -sin(arg) cos(arg) 0;
       0         0        1];

A_KI = A_z  * A_y  * A_x;   % Trafo I -> K

arg = psi;
A_x = [1 0         0;
       0 cos(arg)  -sin(arg);
       0 sin(arg) cos(arg)];
   
arg = nue;
A_y = [cos(arg) 0 sin(arg);
       0        1 0;
       -sin(arg) 0 cos(arg)];

arg = phi;
A_z = [cos(arg)  -sin(arg) 0;
       sin(arg) cos(arg) 0;
       0         0        1];

A_IK = A_x  * A_y  * A_z;   % Trafo K -> I

arg = 0;
A_x = [1 0         0;
       0 cos(arg)  sin(arg);
       0 -sin(arg) cos(arg)];
   
arg = eta*pi/180;
A_y = [cos(arg) 0 -sin(arg);
       0        1 0;
       sin(arg) 0 cos(arg)];

arg = 0;
A_z = [cos(arg)  sin(arg) 0;
       -sin(arg) cos(arg) 0;
       0         0        1];

A_W1G = A_z * A_y * A_x;    % Trafo G -> W1

arg = 0;
A_x = [1 0         0;
       0 cos(arg)  -sin(arg);
       0 sin(arg)  cos(arg)];
   
arg = eta*pi/180;
A_y = [cos(arg)  0 sin(arg);
       0         1 0;
       -sin(arg) 0 cos(arg)];

arg = 0;
A_z = [cos(arg)  -sin(arg) 0;
       sin(arg)  cos(arg)  0;
       0         0         1];

A_GW1 = A_x * A_y * A_z;    % Trafo W1 -> G

arg = 120*pi/180;
A_x = [1 0         0;
       0 cos(arg)  sin(arg);
       0 -sin(arg) cos(arg)];
   
arg = eta*pi/180;
A_y = [cos(arg) 0 -sin(arg);
       0        1 0;
       sin(arg) 0 cos(arg)];

arg = 0;
A_z = [cos(arg)  sin(arg) 0;
       -sin(arg) cos(arg) 0;
       0         0        1];

A_W2G = A_z * A_y * A_x;    % Trafo G -> W2

arg = 120*pi/180;
A_x = [1 0         0;
       0 cos(arg)  -sin(arg);
       0 sin(arg) cos(arg)];
   
arg = eta*pi/180;
A_y = [cos(arg) 0 sin(arg);
       0        1 0;
       -sin(arg) 0 cos(arg)];

arg = 0;
A_z = [cos(arg)  -sin(arg) 0;
       sin(arg) cos(arg) 0;
       0         0        1];

A_GW2 = A_x * A_y * A_z;    % Trafo W2 -> G

arg = 240*pi/180;
A_x = [1 0         0;
       0 cos(arg)  sin(arg);
       0 -sin(arg) cos(arg)];
   
arg = eta*pi/180;
A_y = [cos(arg) 0 -sin(arg);
       0        1 0;
       sin(arg) 0 cos(arg)];

arg = 0;
A_z = [cos(arg)  sin(arg) 0;
       -sin(arg) cos(arg) 0;
       0         0        1];
   
A_W3G = A_z * A_y * A_x;    % Trafo G -> W3

arg = 240*pi/180;
A_x = [1 0         0;
       0 cos(arg)  -sin(arg);
       0 sin(arg) cos(arg)];
   
arg = eta*pi/180;
A_y = [cos(arg) 0 sin(arg);
       0        1 0;
       -sin(arg) 0 cos(arg)];

arg = 0;
A_z = [cos(arg)  -sin(arg) 0;
       sin(arg) cos(arg) 0;
       0         0        1];

A_GW3 = A_x * A_y * A_z;    % Trafo W3 -> G
t1 = clock
% geometrische Beziehungen
x_W1 = (R+r)*sin(eta*pi/180);
x_W2 = -sin(30*pi/180)*x_W1;
x_W3 = x_W2;
y_W1 = 0;
y_W2 = cos(30*pi/180)*x_W1;
y_W3 = -y_W2;
z_W = -(l-(R+r)*cos(eta*pi/180));
z_W1 = z_W;
z_W2 = z_W;
z_W3 = z_W;

% % Beziehung zwischen delta_i und phi,nue,psi über Kontaktpunkt Q 
% % (noch unklarer Zusammenhang!!!)
r_OSW1 = A_IG * ([0; 0; l] + [x_W1; y_W1; z_W1]);
r_OSW2 = A_IG * ([0; 0; l] + [x_W2; y_W2; z_W2]);
r_OSW3 = A_IG * ([0; 0; l] + [x_W3; y_W3; z_W3]);
W1_v_Q1K = A_W1G*A_GI*cross([phi_d;nue_d;psi_d], (R/(sqrt(sum((r_OSW1).^2)))*(r_OSW1)));
W2_v_Q2K = A_W2G*A_GI*cross([phi_d;nue_d;psi_d], (R/(sqrt(sum((r_OSW2).^2)))*(r_OSW2)));
W3_v_Q3K = A_W3G*A_GI*cross([phi_d;nue_d;psi_d], (R/(sqrt(sum((r_OSW3).^2)))*(r_OSW3)));
% W1_v_Q1K = A_W1G*A_GI*cross([phi_d;nue_d;psi_d], [0;0;R]+A_IG*[x_W1;y_W1;z_W1+l]+A_IG*A_GW1*[0;0;-r]);
% W2_v_Q2K = A_W2G*A_GI*cross([phi_d;nue_d;psi_d], [0;0;R]+A_IG*[x_W2;y_W2;z_W2+l]+A_IG*A_GW2*[0;0;-r]);
% W3_v_Q3K = A_W3G*A_GI*cross([phi_d;nue_d;psi_d], [0;0;R]+A_IG*[x_W3;y_W3;z_W3+l]+A_IG*A_GW3*[0;0;-r]);
W1_v_Q1W = [0;r*delta_1_d;0] + A_W1G*A_GI*(jacobian([R*phi;R*nue;0]+A_IG*[x_W1;y_W1;z_W1+l],q)*q_d);
W2_v_Q2W = [0;r*delta_2_d;0] + A_W2G*A_GI*(jacobian([R*phi;R*nue;0]+A_IG*[x_W2;y_W2;z_W2+l],q)*q_d);
W3_v_Q3W = [0;r*delta_3_d;0] + A_W3G*A_GI*(jacobian([R*phi;R*nue;0]+A_IG*[x_W3;y_W3;z_W3+l],q)*q_d);

NLGS = [W1_v_Q1K - W1_v_Q1W; W2_v_Q2K - W2_v_Q2W; W3_v_Q3K - W3_v_Q3W];

% X = simplify(NLGS(2));
% Y = simplify(NLGS(5));
% Z = simplify(NLGS(8));
% S1 = simplify(solve(X,'phi_d'));  %phi_d=f(nue_d,psi_d)
% Y_neu = simplify(subs(Y,phi_d,S1));   %Y_neu=f(nue_d,psi_d)
% S2 = simplify(solve(Y_neu,'nue_d'));  %nue_d=f(psi_d)
% S1_neu = simplify(subs(S1,nue_d,S2));
% Z_neu = simplify(subs(Z,[phi_d, nue_d],[S1_neu, S2]));
% S3 = simplify(solve(Z_neu,'psi_d'));
% S2_neu = simplify(subs(S2,psi_d,S3));
% S1_neu_neu = simplify(subs(S1_neu,psi_d,S3));
% phi_d = S1_neu_neu;
% nue_d = S2_neu;
% psi_d = S3;

S = simplify(solve(NLGS(2), NLGS(5), NLGS(8), 'phi_d', 'nue_d', 'psi_d'));
phi_d = S.phi_d;
nue_d = S.nue_d;
psi_d = S.psi_d;

phi = subs(phi_d, [delta_1_d delta_2_d delta_3_d], [delta_1 delta_2 delta_3]);
nue = subs(nue_d, [delta_1_d delta_2_d delta_3_d], [delta_1 delta_2 delta_3]);
psi = subs(psi_d, [delta_1_d delta_2_d delta_3_d], [delta_1 delta_2 delta_3]);

% Neudefintion der Transformation I->K
arg = psi;
A_x = [1 0         0;
       0 cos(arg)  sin(arg);
       0 -sin(arg) cos(arg)];
   
arg = nue;
A_y = [cos(arg) 0 -sin(arg);
       0        1 0;
       sin(arg) 0 cos(arg)];

arg = phi;
A_z = [cos(arg)  sin(arg) 0;
       -sin(arg) cos(arg) 0;
       0         0        1];

A_KI = A_z  * A_y  * A_x;   % Trafo I -> K

% Beziehung zwischen delta_i und epsilon_i
epsilon_1_d = delta_1_d * i;
epsilon_2_d = delta_2_d * i;
epsilon_3_d = delta_3_d * i;
t2=clock
% Ortsvektoren der Schwerpunkte im Inertialsystem
r_OSK = [R*phi; R*nue; 0];
r_OSG = r_OSK + A_IG * [0; 0; l];
r_OSW1 = r_OSG + A_IG * [x_W1; y_W1; z_W1];
r_OSW2 = r_OSG + A_IG * [x_W2; y_W2; z_W2];
r_OSW3 = r_OSG + A_IG * [x_W3; y_W3; z_W3];
r_OSM1 = r_OSW1 + A_IG * A_GW1 * [x_M; 0; 0];
r_OSM2 = r_OSW2 + A_IG * A_GW2 * [x_M; 0; 0];
r_OSM3 = r_OSW3 + A_IG * A_GW3 * [x_M; 0; 0];

% absolute Winkelgeschwindigkeiten im körperfesten KOS
w_K = A_KI * [phi_d; nue_d; psi_d];
w_G = A_GI * [alpha_d; beta_d; gamma_d];
w_W1 = A_W1G * w_G + [delta_1_d; 0; 0];
w_W2 = A_W2G * w_G + [delta_2_d; 0; 0];
w_W3 = A_W3G * w_G + [delta_3_d; 0; 0];
w_M1 = A_W1G * w_G + [epsilon_1_d; 0; 0];
w_M2 = A_W2G * w_G + [epsilon_2_d; 0; 0];
w_M3 = A_W3G * w_G + [epsilon_3_d; 0; 0];

% Trägheitstensoren
J_K = diag(J_K_xyz);
J_G = diag([J_G_xx, J_G_yy, J_G_zz]);
J_W = diag([J_W_xx, J_W_yy, J_W_zz]);
J_M = diag([J_M_xx, J_M_yy, J_M_zz]);
t3=clock
% Kinetische Energie
T_K  = 0.5 * (m_K * sum((jacobian(r_OSK,q)*q_d).^2)  + [w_K(1) w_K(2) w_K(3)] * J_K * w_K);
T_G  = 0.5 * (m_G * sum((jacobian(r_OSG,q)*q_d).^2)  + [w_G(1) w_G(2) w_G(3)] * J_G * w_G);
T_W1 = 0.5 * (m_W * sum((jacobian(r_OSW1,q)*q_d).^2) + [w_W1(1) w_W1(2) w_W1(3)] * J_W * w_W1);
T_W2 = 0.5 * (m_W * sum((jacobian(r_OSW2,q)*q_d).^2) + [w_W2(1) w_W2(2) w_W2(3)] * J_W * w_W2);
T_W3 = 0.5 * (m_W * sum((jacobian(r_OSW3,q)*q_d).^2) + [w_W3(1) w_W3(2) w_W3(3)] * J_W * w_W3);
T_M1 = 0.5 * (m_M * sum((jacobian(r_OSM1,q)*q_d).^2) + [w_M1(1) w_M1(2) w_M1(3)] * J_M * w_M1);
T_M2 = 0.5 * (m_M * sum((jacobian(r_OSM2,q)*q_d).^2) + [w_M2(1) w_M2(2) w_M2(3)] * J_M * w_M2);
T_M3 = 0.5 * (m_M * sum((jacobian(r_OSM3,q)*q_d).^2) + [w_M3(1) w_M3(2) w_M3(3)] * J_M * w_M3);

T = T_K + T_G + T_W1 + T_W2 + T_W3 + T_M1 + T_M2 + T_M3;
t4=clock
% Potentielle Energie
V_K  = - m_K * [r_OSK(1) r_OSK(2) r_OSK(3)] * [0; 0; -g];
V_G  = - m_K * [r_OSG(1) r_OSG(2) r_OSG(3)]  * [0; 0; -g];
V_W1 = - m_W * [r_OSW1(1) r_OSW1(2) r_OSW1(3)] * [0; 0; -g];
V_W2 = - m_W * [r_OSW2(1) r_OSW2(2) r_OSW2(3)] * [0; 0; -g];
V_W3 = - m_W * [r_OSW3(1) r_OSW3(2) r_OSW3(3)] * [0; 0; -g];
V_M1 = - m_M * [r_OSM1(1) r_OSM1(2) r_OSM1(3)] * [0; 0; -g];
V_M2 = - m_M * [r_OSM2(1) r_OSM2(2) r_OSM2(3)] * [0; 0; -g];
V_M3 = - m_M * [r_OSM3(1) r_OSM3(2) r_OSM3(3)] * [0; 0; -g];

V = V_K + V_G + V_W1 + V_W2 + V_W3 + V_M1 + V_M2 + V_M3;
t5=clock
% Nicht-Konservative Kräfte
Q_1 = [M_1 0 0] * jacobian(w_W1,q_d);
Q_2 = [M_2 0 0] * jacobian(w_W2,q_d);
Q_3 = [M_3 0 0] * jacobian(w_W3,q_d);
Q = Q_1 + Q_2 + Q_3;
t6=clock

% nichtlineare Bewegungsgleichung
M = jacobian(jacobian(T,q_d),q_d);
t7=clock
k = jacobian(jacobian(T,q_d)*q_d,q) - jacobian(T,q) + diff(jacobian(T,q_d),t);
t8=clock
p = jacobian(V,q);
t9=clock
f = M * q_dd + k(:) + p(:) - Q(:);
t10=clock
save RT_NL_BWGL_3D

% Linearisierung um Arbeitspunkt
q_0 = zeros(length(q),1);
q_d_0 = zeros(length(q_d),1);
q_dd_0 = zeros(length(q_dd),1);
x_0 = [q_0; q_d_0; q_dd_0];
u_0 = [0; 0; 0];

p = conj([q; q_d; q_dd; u]');
j_Q = jacobian(f,u);
j_M = jacobian(f,q_dd);
j_D = jacobian(f,q_d);
t11=clock

Q_lin = subs(j_Q, p, [x_0; u_0]');
t12=clock
save check1
M_lin = subs(j_M, p, [x_0; u_0]');
t13=clock
save check2
D_lin = subs(j_D, p, [x_0; u_0]');
t14=clock
save check3
j_K = jacobian(f,q);
K_lin = subs(j_K, p, [x_0; u_0]');
t15=clock
save RT_LIN_BWGL_3D

% % Initialisierung der Parameter
% i = 15;
% g = 9.81;
% eta = 45;
% R = 0.15;
% r = 0.05;
% l = 0.5;
% m_K  = 1;
% m_G = 6;
% m_W = 0.3;
% m_M = 0.5;
% J_K_xyz = 0.4*m_K*R^2;
% J_G_xx = 0.5*m_G*(1.1*R)^2;
% J_G_yy = 0.25*m_G*(1.1*R)^2 + 1/12 * m_G * l^2;
% J_G_zz = 0.25*m_G*(1.1*R)^2 + 1/12 * m_G * l^2;
% b_W = 0.05;
% J_W_xx = 0.5*m_W*r^2;
% J_W_yy = 0.25*m_W*r^2 + 1/12 * m_W * b_W^2;
% J_W_zz = 0.25*m_W*r^2 + 1/12 * m_W * b_W^2;
% J_M_xx = 0.5*m_W*r^2;
% J_M_yy = 0.25*m_W*r^2 + 1/12 * m_W * b_W^2;
% J_M_zz = 0.25*m_W*r^2 + 1/12 * m_W * b_W^2;
% x_M = -80;

% % Parameter numerisch substituieren
% M_lin_num = subs(M_lin);
% D_lin_num = subs(D_lin);
% K_lin_num = subs(K_lin);
% Q_lin_num = subs(Q_lin);
% 
% % Zustandsraumdarstellung
% A = [zeros(length(M_lin_num),length(M_lin_num)) eye(length(M_lin_num)); M_lin_num\K_lin_num M_lin_num\D_lin_num];
% B = [zeros(length(M_lin_num), length(u)); M_lin_num\Q_lin_num];
% C = eye(length(A));
% D = zeros(length(A), length(u));
% sys = ss(A,B,C,D);
% 
% % Regelung
% Q = diag([5000 5000 5000 5000 5000 5000 5000 5000 5000 5000 5000 5000]);
% R = diag([300 300 300]);
% [Regler_opt,S,E] = lqr(sys,Q,R);