% ######################################################## %
% Herleiten der Bewegungsgleichungen des Einzylindermotors %
% aus den Aufgaben A12, B1, B2                             %
% ######################################################## %

syms r s b l lambda gamma Dgamma D2gamma m1 m2 m3 g Kr I1 I2 MA FG real
syms delta deltaStr delta2Str real
syms F1 F2 F3 F4 F5 F6 F7 F8 F9 F10 M1 M2 M3 M4 real

%% Bewegungsgleichung aus haendischer Herleitung (A12, B1, B2)

% ## Kinematik ##

% Kurbel
J_T1 = [-s*sin(gamma); ...
         s*cos(gamma); ...
         0];
a1q = [-s*cos(gamma); ...
       -s*sin(gamma); ...
        0]*Dgamma^2;
J_R1 = [0; ...
        0; ...
        1];
alpha1q = [0; ...
           0; ...
           0];
       
% Pleuel
J_T2 = [-r*sin(gamma)-b*deltaStr*sin(delta); ...
         r*cos(gamma)-b*deltaStr*cos(delta); ...
         0];   
a2q = [-b*delta2Str*lambda*sin(gamma)-(r+b*deltaStr*lambda)*cos(gamma); ...
       (r-b*lambda)*sin(gamma); ...
        0]*Dgamma^2;
J_R2 = [ 0; ...
         0; ...
        -deltaStr];
alpha2q = [ 0; ...
            0; ...
           -delta2Str*gamma];

% Kolben (Punktmasse)
J_T3 = [-r*sin(gamma)-l*deltaStr*sin(delta); ...
         0; ...
         0];     
a3q = [-l*delta2Str*lambda*sin(gamma)-(r+l*deltaStr*lambda)*cos(gamma); ...
        0; ...
        0]*Dgamma^2;

% globale Jacobi-Matrix J
J = [J_T1; 
     J_T2; 
     J_T3; 
     J_R1; 
     J_R2];

% ## Kinetik ##

% globale Massen-Matrix, bestehend aus Massen und Traegheitstensoren
I_1 = [Kr Kr 0;Kr Kr 0;0 0 I1];
I_2 = [Kr Kr 0;Kr Kr 0;0 0 I2];
M_qq = [diag([m1 m1 m1 m2 m2 m2 m3 m3 m3])       zeros(9,6); ...
        zeros(3,9)                         I_1   zeros(3,3); ...
        zeros(3,12)                                     I_2];

% Vektor der Coriolis- Zertrifugal-Kreisel-Kraefte
qc = [m1*a1q; ...
      m2*a2q; ...
      m3*a3q; ...
      I_1*alpha1q; ...
      I_2*alpha2q];

% eingepraegte Kraefte
qe = [-m1*g; ...
       0; ...
       0; ...
      -m2*g; ...
       0; ...
       0; ...
      -m3*g-FG; ...
       0; ...
       0; ...
       0; ...
       0; ...
      -MA; ...
       0; ...
       0; ...
       0];

%% D'Alembertsches Prinzip auf Newton-Euler-Gleichung anwenden

% Newton-Euler-Massen-Matrix M_quer berechnen
M_q = M_qq*J;

% Massenmatrix M resultiert aus Linksmultipli. mit Jq^T & Vereinfachung
M = simplify(J.'*M_q)

% Vektor der verallgemeinerten Coriolis- Zertrifugal- Kreisel-Kraefte
k = simplify(J.'*qc)

% Vektor der verallgemeinerten eingepraegten Kraefte
q = simplify(J.'*(qe))

%% Reaktionsgleichungen

% Reaktionskraefte und Momente
f1r = [F1-F4; F2-F5; F3-F6];
f2r = [F4-F7; F5-F8; F6];
f3r = [F7;    F8+F9; F10];
l1r = cross([-s*cos(gamma);-s*sin(gamma);0], [F1;F2;F3]) + ...
      cross([(r-s)*cos(gamma);(r-s)*sin(gamma);0], -[F4;F5;F6]) + ...
      [M1-M3; M2-M4; 0];
l2r = cross([-b*cos(delta);b*sin(delta);0], [F4;F5;F6]) + ...
      cross([(l-b)*cos(delta);-(l-b)*sin(delta);0], -[F7;F8;0]) + ...
      [M3; M4; 0];
qr  = [f1r;f2r;f3r;l1r;l2r];

% verallgemeinete Zwangskraefte
g_ = [F1;F2;F3;F4;F5;F6;F7;F8;F9;F10;M1;M2;M3;M4];

% Verteilungs-Matrix
Q = jacobian(qr,g_);

% Reaktionskraefte muessen verschwinden wegen Orthogonal.bed.: Jq^T*Qq = 0
test = simplify(J.'*Q);
test = subs(test, deltaStr ,r*cos(gamma)/(l*cos(delta)))

% Reaktions-Matrix (ab hier sollte besser numerisch weitergerechnet werden)
N = Q.'*(M\Q);

% Einfluss der eingepraegten Kraefte auf die Reaktionskraefte
q_hat = Q.'*(M\qe);

% Einfluss der Kreisekraefte
k_hat = Q.'*(M\qc);
