%% Aufstellen der Bewegungsgleichungen

% 5 Freiheitsgrade
% delta_x, delta_y, phi, gamma, theta


 clear all;
 close all;
 clc;

 format shortE;
% Variablendeklaration

 syms delta_x delta_y delta_z phi gamma theta ;
 syms delta_x_p delta_y_p delta_z_p phi_p gamma_p  theta_p ; 
 syms delta_x_pp delta_y_pp delta_z_pp phi_pp gamma_pp  theta_pp; 
 syms delta_sko delta_sku delta_sh1 delta_sh2 delta_sv1 delta_sv2;
 syms H1 H2 H3 H4 H5 HBu HBo;
 syms L1 L2 L3;
 syms B;
 syms Ixx Ixy Ixz Ixy Iyy Iyz Izx Izy Izz mt rk my;
 syms FMv_LB1 FMv_LB2 FMho_LB1 FMho_LB2 FMhu_LB1 FMhu_LB2 FA FAx FAy FAz 
 syms FF FD FN FR FRx;  
 syms sv0 c_hub d_hub;
 syms t;
%  syms io_v_LB1 io_v_LB2 io_ho_LB1 io_ho_LB2 io_hu_LB1 io_hu_LB2;
%  syms is_v_LB1 is_v_LB2 is_ho_LB1 is_ho_LB2 is_hu_LB1 is_hu_LB2;

% verallgemeinerte Koordinaten
 q = [delta_x; delta_y; delta_z; phi; gamma; theta]; % Lagevektor
 q_p = [delta_x_p; delta_y_p; delta_z_p; phi_p; gamma_p; theta_p]; % Geschwindigkeitsvektor
 q_pp = [delta_x_pp; delta_y_pp; delta_z_pp; phi_pp; gamma_pp; theta_pp]; %Beschleunigungsvektor

% Steuergröße
 u = [FMv_LB1; FMv_LB2; FMho_LB1; FMho_LB2; FMhu_LB1; FMhu_LB2];

% Initialisierung
% Werte_Initialisierung;
g=9.81;
%Trägheitstensor
Is =[Ixx,Ixy,Ixz;Ixy,Iyy,Iyz;Izx,Izy,Izz];

%Winkelgeschwindigkeit
 omega = [gamma_p;phi_p;theta_p];

% linearisierte Auslenkungen
 delta_sv1 = L2*phi;
 delta_sv2 = L3*phi;
 delta_sh1 = L2*theta;
 delta_sh2 = L3*theta;
 delta_sko = (H3+H1+H2+H5)*gamma;
 delta_sku = (H3-H4)*gamma;
 delta_swgamma = (H1+H3)*gamma;
 
% Ortsvektoren im I-System
 r_Mv_LB1 = [delta_x;delta_y;sv0+HBu+H1+H2+HBo-delta_sv1+delta_z];
 r_Mv_LB2 = [L2+L3+delta_x;delta_y;sv0+HBu+H1+H2+HBo+delta_sv2+delta_z];
 r_Mho_LB1 = [delta_x;-B/2+delta_sko-delta_sh1+delta_y;sv0+HBu+H1+H2+H5+delta_z];
 r_Mho_LB2 = [L2+L3+delta_x;-B/2+delta_sko+delta_sh2+delta_y;sv0+HBu+H1+H2+H5+delta_z];
 r_Mhu_LB1 = [delta_x;-B/2+delta_sku-delta_sh1+delta_y;sv0+HBu-H4+delta_z];
 r_Mhu_LB2 = [L2+L3+delta_x;-B/2+delta_sku+delta_sh2+delta_y;sv0+HBu-H4+delta_z]; 
 
 r_S =[L2+delta_x;delta_swgamma+delta_y; sv0+HBu+H1+delta_z];
 r_A = [L2-L1+delta_x;delta_swgamma-L1*theta+delta_y;sv0+HBu+H1+delta_z];
 r_K = [L2+delta_x;delta_y;sv0+HBu-H3+delta_z];
 r_SK = [0;-delta_swgamma;-H1-H3];
 r_KS = -r_SK;
 

% Kräfte
 %Magnetkraefte; %linearisierten Magnetkräfte
 
 FMv_LB1_v = [0;0;-FMv_LB1];
 FMv_LB2_v = [0;0;-FMv_LB2];
 FMhu_LB1_v = [0;FMhu_LB1;0];
 FMhu_LB2_v = [0;FMhu_LB2;0];
 FMho_LB1_v = [0;FMho_LB1;0];
 FMho_LB2_v = [0;FMho_LB2;0];
 FF_v = [0;0;c_hub*delta_z];
 FD_v= [0;0;d_hub * delta_z_p];
 FN_v = FF_v + FD_v;
 FA = [FAx;FAy;FAz];
 FR = [-FRx;0;0];

% FMv_LB1_v = [0;0;-2];
% FMv_LB2_v = [0;0;-3];
% FMhu_LB1_v = [0;5;0];
% FMhu_LB2_v = [0;5;0];
% FMho_LB1_v = [0;3;0];
% FMho_LB2_v = [0;5;0];
% FN_v = [0;0;1];

% Berechnung der Geschwindigkeit im I-System

 v_S = jacobian(r_S,q)*q_p;
 v_K = jacobian(r_K,q)*q_p;

%Berechnung des Drehimpulses

 Ls = Is*omega;

%kinetische Energie
 
 T = 1/2*mt*norm(v_S,1)^2+1/2*dot(omega,Ls);
 T = simple(T);

 %um den Kontaktpunkt
%  Ik = Is+mt*norm(r_SK)^2;
%  Lk = Ik*omega;
%  T1 = 1/2*mt*norm(v_K,1)^2+mt*dot((cross(v_K,omega)),r_SK)+1/2*dot(omega,Lk);
%  T1 = simplify(T1);
 
% potentielle Energie
 
 U = - 1/2*c_hub*(delta_z)^2; %potentielle Energie des Hubtische (Feder) 
 W = -mt*r_S.'*[0;0;-g]; % pot. Energie der Gewichtskraft(Nullpunkt I-System)
 V = U + W;
 V = simple(V);
 
% nicht konservative Kräfte
 
 Q_F= FMv_LB1_v'*jacobian(r_Mv_LB1,q)+FMv_LB2_v'*jacobian(r_Mv_LB2,q)...
    + FMhu_LB1_v'*jacobian(r_Mhu_LB1,q)+ FMhu_LB2_v'*jacobian(r_Mhu_LB2,q)...
    + FMho_LB1_v'*jacobian(r_Mho_LB1,q)+ FMho_LB2_v'*jacobian(r_Mho_LB2,q)...
    + FA'*jacobian(r_A,q) + FD_v'*jacobian(r_K,q)+FR'*jacobian(r_K,q);
 Q_F = simple(Q_F);

%Dissipationsfunktion (für Stokessche Reibung)

%  P = 1/2 * alpha *norm(v_A)^2; % alpha = 6 * pi *r * ny siehe
%  Q_R = -jacobian(P,q_p);
%  simplify(Q_R);

% nichtlineare Bewegungsgleichungen

 Q = Q_F; %+ Q_R;
 dTdv = simple(jacobian(T,q_p).');
 dTdq = simple(jacobian(T,q).');
 dVdq = simple(jacobian(V,q).');
 M = simple(jacobian(dTdv,q_p));
 k = simple(jacobian(dTdv,q)*q_p - dTdq + diff(dTdv,t));
 p = dVdq;


 f = simple(M * q_pp + k(:) + p(:) - Q(:));
 %f = collect(f,u); 
 f= simple(f);
 
 disp('System-Massenmatrix M')
 disp(M)
 disp('System-Vektorfunktion f')
 disp(f)

%  for kk=1:6
%    disp(collect(f(kk),q(kk)))  
%  end
 
% collect(f(1),q_pp(1)) 


% Definition des Zustandsvektors

 x = [delta_x; delta_y; phi; gamma; theta;
     delta_x_p; delta_y_p; phi_p; gamma_p;theta_p];