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];
q_d = [alpha_d; beta_d; gamma_d; delta_1_d; delta_2_d; delta_3_d];
q_dd = [alpha_dd; beta_dd; gamma_dd; delta_1_dd; delta_2_dd; delta_3_dd];

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

% Initialisierung der Parameter
% i = 18;
g = 9.81;
eta = 50;
R = 0.15;
r = 0.05;
l = 0.5;
m_K  = 0.6;
m_G = 7;
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 = -70;

% 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]);

% Koordinatentransformation
A_GI = simplify(kostrafo('z',gamma) * kostrafo('y',beta) * kostrafo('x',alpha));
A_IG = A_GI.';

A_W1G = kostrafo('y', eta*pi/180);
A_GW1 = A_W1G.';

A_W2G = kostrafo('y',eta*pi/180) * kostrafo('z',120*pi/180);
A_GW2 = A_W2G.';

A_W3G = kostrafo('y',eta*pi/180) * kostrafo('z',240*pi/180);
A_GW3 = A_W3G.';

A_1I = kostrafo('x',phi);
A_21 = kostrafo(A_1I*[0 1 0]',nue);
A_32 = kostrafo(A_21*A_1I*[0 0 1]',psi);
A_KI = simplify(A_32 * A_21 * A_1I);
A_IK = A_KI.';

% 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;

t1 = clock

% Beziehung zwischen delta_i und phi,nue,psi
% W1_v_Q1K = A_W1G * cross(A_GI * [phi_d;nue_d;psi_d], [R*sin(eta*pi/180); 0; -(l-R*cos(eta*pi/180))]);
% W2_v_Q2K = A_W2G * cross(A_GI * [phi_d;nue_d;psi_d], [-R*sin(eta*pi/180)*sin(30*pi/180); R*sin(eta*pi/180)*cos(30*pi/180); -(l-R*cos(eta*pi/180))]);
% W3_v_Q3K = A_W3G * cross(A_GI * [phi_d;nue_d;psi_d], [-R*sin(eta*pi/180)*sin(30*pi/180); -R*sin(eta*pi/180)*cos(30*pi/180); -(l-R*cos(eta*pi/180))]);
% 
% W1_v_Q1W1 = cross([delta_1_d;0;0],[0;0;-r]);
% W2_v_Q2W2 = cross([delta_2_d;0;0],[0;0;-r]);
% W3_v_Q3W3 = cross([delta_3_d;0;0],[0;0;-r]);

% % W1_v_Q1K = A_W1G * A_GI * cross([phi_d;nue_d;psi_d], A_IG * [R*sin(eta*pi/180); 0; -(l-R*cos(eta*pi/180))]);
% % W2_v_Q2K = A_W2G * A_GI * cross([phi_d;nue_d;psi_d], A_IG * [-R*sin(eta*pi/180)*sin(30*pi/180); R*sin(eta*pi/180)*cos(30*pi/180); -(l-R*cos(eta*pi/180))]);
% % W3_v_Q3K = A_W3G * A_GI * cross([phi_d;nue_d;psi_d], A_IG * [-R*sin(eta*pi/180)*sin(30*pi/180); -R*sin(eta*pi/180)*cos(30*pi/180); -(l-R*cos(eta*pi/180))]);
% % 
% % W1_v_Q1W1 = cross([delta_1_d;0;0],[0;0;-r]);
% % W2_v_Q2W2 = cross([delta_2_d;0;0],[0;0;-r]);
% % W3_v_Q3W3 = cross([delta_3_d;0;0],[0;0;-r]);

% % % W1_v_Q1K = cross(A_W1G * A_GI * [phi_d;nue_d;psi_d], [0; 0; R]);
% % % W2_v_Q2K = cross(A_W2G * A_GI * [phi_d;nue_d;psi_d], [0; 0; R]);
% % % W3_v_Q3K = cross(A_W3G * A_GI * [phi_d;nue_d;psi_d], [0; 0; R]);
% % % 
% % % W1_v_Q1W1 = cross([delta_1_d;0;0],[0;0;-r]);
% % % W2_v_Q2W2 = cross([delta_2_d;0;0],[0;0;-r]);
% % % W3_v_Q3W3 = cross([delta_3_d;0;0],[0;0;-r]);

W1_v_Q1K = A_W1G * A_GI * cross([phi_d;nue_d;psi_d], [0; 0; R] + 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 * A_GW2 * [0; 0; R]);
W3_v_Q3K = A_W3G * A_GI * cross([phi_d;nue_d;psi_d], [0; 0; R] + A_IG * A_GW3 * [0; 0; R]);

W1_v_Q1W1 = cross([delta_1_d;0;0],[0;0;-r]);
W2_v_Q2W2 = cross([delta_2_d;0;0],[0;0;-r]);
W3_v_Q3W3 = cross([delta_3_d;0;0],[0;0;-r]);


v1 = simplify(W1_v_Q1K - W1_v_Q1W1);
v2 = simplify(W2_v_Q2K - W2_v_Q2W2);
v3 = simplify(W3_v_Q3K - W3_v_Q3W3);

t2 = clock

S = solve(v1(2), v2(2), v3(2), 'phi_d', 'nue_d', 'psi_d');
phi_d = simplify(S.phi_d);
nue_d = simplify(S.nue_d);
psi_d = simplify(S.psi_d);

t3 = clock

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]);

t4 = clock

% Neudefintion der Transformation I->K
A_1I = kostrafo('x',phi);
A_21 = kostrafo(A_1I*[0 1 0]',nue);
A_32 = kostrafo(A_21*A_1I*[0 0 1]',psi);
A_KI = simplify(A_32 * A_21 * A_1I);
A_IK = A_KI.';

% 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;

% Schwerpunktsvektoren im I-System
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];

% Schwerpunktsgeschwindigkeiten im I-System
v_OSK = jacobian(r_OSK,q)*q_d;
v_OSG = jacobian(r_OSG,q)*q_d;
v_OSW1 = jacobian(r_OSW1,q)*q_d;
v_OSW2 = jacobian(r_OSW2,q)*q_d;
v_OSW3 = jacobian(r_OSW3,q)*q_d;
% v_OSM1 = jacobian(r_OSM1,q)*q_d;
% v_OSM2 = jacobian(r_OSM2,q)*q_d;
% v_OSM3 = jacobian(r_OSM3,q)*q_d;

% absolute Winkelgeschwindigkeiten im körperfesten KOS
w_K = A_KI * [phi_d; nue_d; psi_d];

A_x = kostrafo('x',alpha).';
A_y = kostrafo('y',beta).';
w_G = simplify(A_GI * ([alpha_d; 0; 0]+A_x*[0;beta_d;0]+A_x*A_y*[0;0;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];

t5 = clock

% Kinetische Energie 
T_K  = 0.5 * m_K * v_OSK.' * v_OSK + w_K.' * J_K * w_K;
T_G  = 0.5 * m_G * v_OSG.' * v_OSG  + w_G.' * J_G * w_G;
T_W1 = 0.5 * m_W * v_OSW1.' * v_OSW1 + w_W1.' * J_W * w_W1;
T_W2 = 0.5 * m_W * v_OSW2.' * v_OSW2 + w_W2.' * J_W * w_W2;
T_W3 = 0.5 * m_W * v_OSW3.' * v_OSW3 + w_W3.' * J_W * w_W3;
% T_M1 = 0.5 * v_OSM1.' * v_OSM1 + w_M1.' * J_M * w_M1;
% T_M2 = 0.5 * v_OSM2.' * v_OSM2 + w_M2.' * J_M * w_M2;
% T_M3 = 0.5 * v_OSM3.' * v_OSM3 + w_M3.' * J_M * w_M3;

t6 = clock

T = simplify(T_K + T_G + T_W1 + T_W2 + T_W3); % + T_M1 + T_M2 + T_M3);

t7 = clock

% Potentielle Energie
V_K  = - m_K * r_OSK.' * [0; 0; -g];
V_G  = - m_G * r_OSG.'  * [0; 0; -g];
V_W1 = - m_W * r_OSW1.' * [0; 0; -g];
V_W2 = - m_W * r_OSW2.' * [0; 0; -g];
V_W3 = - m_W * r_OSW3.' * [0; 0; -g];
% V_M1 = - m_M * r_OSM1.' * [0; 0; -g];
% V_M2 = - m_M * r_OSM2.' * [0; 0; -g];
% V_M3 = - m_M * r_OSM3.' * [0; 0; -g];

t8 = clock

V = simplify(V_K + V_G + V_W1 + V_W2 + V_W3); % + V_M1 + V_M2 + V_M3);

t9 = 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 = simplify(Q_1 + Q_2 + Q_3);

t10 = clock

% nichtlineare Bewegungsgleichung
m = jacobian(jacobian(T,q_d),q_d);

t11 = clock

k = jacobian(jacobian(T,q_d),q)*q_d - jacobian(T,q).' + diff(jacobian(T,q_d),t).';

t12 = clock

p = jacobian(V,q);

t13 = clock

f = simplify(m * q_dd + k(:) + p(:) - Q(:));

t14 = clock

save BWGL_NL

% Linearisierung um den 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];

j_Q = simplify(jacobian(f,u));

t15 = clock

j_M = simplify(jacobian(f,q_dd));

t16 = clock

% j_D = simplify(jacobian(f,q_d));
j_K = simplify(jacobian(f,q));


t17 = clock

Q_lin = subs(j_Q, [q; q_d; q_dd; u].', [x_0; u_0]');
Q_lin_num = double(Q_lin)

t18 = clock

M_lin = subs(j_M, [q; q_d; q_dd; u].', [x_0; u_0]');

t19 = clock

% D_lin = subs(j_D, [q; q_d; q_dd; u].', [x_0; u_0]');
D_lin = zeros(length(M_lin),length(M_lin));
% K_lin = subs(j_K, [q; q_d; q_dd; u].', [x_0; u_0]');

k_lin_11 = subs(j_K(1,1), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_11 = clock
save check4_11
k_lin_12 = subs(j_K(1,2), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_12 = clock
save check4_12
k_lin_13 = subs(j_K(1,3), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_13 = clock
save check4_13
k_lin_14 = subs(j_K(1,4), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_14 = clock
save check4_14
k_lin_15 = subs(j_K(1,5), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_15 = clock
save check4_15
k_lin_16 = subs(j_K(1,6), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_16 = clock
save check4_16

k_lin_21 = subs(j_K(2,1), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_21 = clock
save check4_21
k_lin_22 = subs(j_K(2,2), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_22 = clock
save check4_22
k_lin_23 = subs(j_K(2,3), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_23 = clock
save check4_23
k_lin_24 = subs(j_K(2,4), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_24 = clock
save check4_24
k_lin_25 = subs(j_K(2,5), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_25 = clock
save check4_25
k_lin_26 = subs(j_K(2,6), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_26 = clock
save check4_26

k_lin_31 = subs(j_K(3,1), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_31 = clock
save check4_31
k_lin_32 = subs(j_K(3,2), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_32 = clock
save check4_32
k_lin_33 = subs(j_K(3,3), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_33 = clock
save check4_33
k_lin_34 = subs(j_K(3,4), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_34 = clock
save check4_34
k_lin_35 = subs(j_K(3,5), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_35 = clock
save check4_35
k_lin_36 = subs(j_K(3,6), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_36 = clock
save check4_36

k_lin_41 = subs(j_K(4,1), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_41 = clock
save check4_41
k_lin_42 = subs(j_K(4,2), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_42 = clock
save check4_42
k_lin_43 = subs(j_K(4,3), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_43 = clock
save check4_43
k_lin_44 = subs(j_K(4,4), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_44 = clock
save check4_44
k_lin_45 = subs(j_K(4,5), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_45 = clock
save check4_45
k_lin_46 = subs(j_K(4,6), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_46 = clock
save check4_46

k_lin_51 = subs(j_K(5,1), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_51 = clock
save check4_51
k_lin_52 = subs(j_K(5,2), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_52 = clock
save check4_52
k_lin_53 = subs(j_K(5,3), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_53 = clock
save check4_53
k_lin_54 = subs(j_K(5,4), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_54 = clock
save check4_54
k_lin_55 = subs(j_K(5,5), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_55 = clock
save check4_55
k_lin_56 = subs(j_K(5,6), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_56 = clock
save check4_56

k_lin_61 = subs(j_K(6,1), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_61 = clock
save check4_61
k_lin_62 = subs(j_K(6,2), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_62 = clock
save check4_62
k_lin_63 = subs(j_K(6,3), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_63 = clock
save check4_63
k_lin_64 = subs(j_K(6,4), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_64 = clock
save check4_64
k_lin_65 = subs(j_K(6,5), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_65 = clock
save check4_65
k_lin_66 = subs(j_K(6,6), [q; q_d; q_dd; u].', [x_0; u_0]');
t19_66 = clock
save check4_66

K_lin = [k_lin_11 k_lin_12 k_lin_13 k_lin_14 k_lin_15 k_lin_16;
         k_lin_21 k_lin_22 k_lin_23 k_lin_24 k_lin_25 k_lin_26;
         k_lin_31 k_lin_32 k_lin_33 k_lin_34 k_lin_35 k_lin_36;
         k_lin_41 k_lin_42 k_lin_43 k_lin_44 k_lin_45 k_lin_46;
         k_lin_51 k_lin_52 k_lin_53 k_lin_54 k_lin_55 k_lin_56;
         k_lin_61 k_lin_62 k_lin_63 k_lin_64 k_lin_65 k_lin_66];

t20 = clock

Q_lin_num = double(Q_lin);
M_lin_num = double(M_lin);
D_lin_num = double(D_lin);
K_lin_num = double(K_lin);

save BWGL_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);

rg_QS = rank(ctrb(A,B));
rg_QB = rank(obsv(A,C));
lambda = eig(A);

save BWGL_SS

% Regelung
Q = diag([500000 500000 500000 500000 500000 500000 500000 500000 500000 500000 500000 500000]);
R = diag([3000 3000 3000]);
[Regler_opt,S,E] = lqr(sys,Q,R);

lambda_R = eig(A-B*Regler_opt);

% % Anfangs -und Endbedingung
% alpha0 = 0;
% beta0 = 20;
% gamma0 = 0;
% R = 0.15;
% % delta_10 = -gamma0*R/(r*sin(eta*pi/180))*pi/180;
% % delta_20 = -gamma0*R/(r*sin(eta*pi/180))*pi/180;
% % delta_30 = -gamma0*R/(r*sin(eta*pi/180))*pi/180;
% delta_10 = 0;
% delta_20 = beta0/(cos(eta*pi/180)*cos(30*pi/180))*pi/180;
% delta_30 = -beta0/(cos(eta*pi/180)*cos(30*pi/180))*pi/180;
% x0 = [alpha0*pi/180 beta0*pi/180 gamma0*pi/180 delta_10 delta_20 delta_30 0 0 0 0 0 0]';
% x_end=zeros(length(A),1);