clc;
clear;
close all;

% x = [iG, phiG, wG, wP]
% Subskript Notation: GSM Subskript substituiert mit G
% Konstante aus Tabelle 2.1
LG  = 1.4e-3; % H
RG  = .46; % Ohm
kG  = .1; % N m A^-1
JG  = 12.4e-3; % kg m^2
dcG = .152; % N m
dvG = 1.8e-3; % N m s rad^-1
JP  = 32.5e-3; % kg m^2
dcP = .169; % N m
dvP = 2.7e-3; % N m s rad^-1
dqP = 1e-4; % N m s^2 rad^-2
cGP = .6822; % N m rad^-1
dGP = 1e-5; % N m s rad^-1
% Ruhelagenbedingungen (Abgabe)
Mext = 0;
uG = 5.6;
% Ruhelagengleichungen (erste Lösung aus Maple)
iGR1 = (-kG*sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (2*uG*dqP + kG*(dvG + dvP))*RG + kG^3)/(2*RG^2*dqP);
pGPR1 = ((-RG*dvG - kG^2)*sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-2*dcG*dqP + dvG^2 + dvG*dvP)*RG^2 + ((2*dvG + dvP)*kG^2 + 2*uG*dqP*kG)*RG + kG^4)/(2*RG^2*dqP*cGP);
wGR1 = (sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-dvG - dvP)*RG - kG^2)/(2*RG*dqP);
wPR1 = (sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-dvG - dvP)*RG - kG^2)/(2*RG*dqP);
% Ruhelagengleichungen (zweite Lösung aus Maple)
iGR2 = (kG*sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (2*uG*dqP + kG*(dvG + dvP))*RG + kG^3)/(2*RG^2*dqP);
pGPR2 = ((-RG*dvG - kG^2)*-sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-2*dcG*dqP + dvG^2 + dvG*dvP)*RG^2 + ((2*dvG + dvP)*kG^2 + 2*uG*dqP*kG)*RG + kG^4)/(2*RG^2*dqP*cGP);
wGR2 = (-sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-dvG - dvP)*RG - kG^2)/(2*RG*dqP);
wPR2 = (-sqrt(((-4*dcG - 4*dcP - 4*Mext)*dqP + (dvG + dvP)^2)*RG^2 + 2*kG*(2*uG*dqP + kG*(dvG + dvP))*RG + kG^4) + (-dvG - dvP)*RG - kG^2)/(2*RG*dqP);

% Matrizen
Al = [
    -RG/LG,      0,          -kG/LG,                            0;
    0,           0,               1,                           -1;
    kG/JG, -cGP/JG, (-dvG - dGP)/JG,                       dGP/JG;
    0,      cGP/JP,          dGP/JP, (-2*dqP*wPR1 - dGP - dvP)/JP
];
Bl = [
    1/LG,  0;
    0,     0;
    0,     0;
    0, -1/JP
];
cl = eye(4);
dl = zeros(4, 2);
x0 = [0; 0; 0; 0];

% reduziertes System
Aredl = [
    0,                               1,                           -1;
    -cGP/JG, -(kG^2/RG + dGP + dvG)/JG,                       dGP/JG;
     cGP/JP,                    dGP/JP, -(dGP + dvP + 2*dqP*wPR1)/JP
];
Bredl = [
    0,              0;
    kG/(JG*RG),     0;
    0,          -1/JP
];
credl = eye(3);
dredl = zeros(3, 2);
% Anfangswerte
xred0 = [0; 0; 0];

% Aliases
parGSM_MF.LG = LG;
parGSM_MF.RG = RG;
parGSM_MF.kG = kG;
parGSM_MF.JG = JG;
parGSM_MF.dcG = dcG;
parGSM_MF.dvG = dvG;
parGSM_MF.JP = JP;
parGSM_MF.dcP = dcP;
parGSM_MF.dvP = dvP;
parGSM_MF.dqP = dqP;
parGSM_MF.cGP = cGP;
parGSM_MF.dGP = dGP;
% Struktur mit Werte der Ruhelage
parGSM.iGSM_R = iGR1;
parGSM.phiGSMP_R = pGPR1;
parGSM.omegaGSM_R = wGR1;
parGSM.omegaP_R = wPR1;
parGSM.uGSM_R = uG;

Ta = 10e-3;
z = tf('z', Ta);
%Rz = tf((1.596e18*z^3 - 4.73e18*z^2 + 4.684e18*z - 1.55e18)/(z^3 + 1.407e16*z^2 + 3.791e16*z - 5.199e16))
