% define synch gen values
clear all; close all; clc;
fq = 60; 
S = 659*10^6;
vline = 20*10^3;
vphi = vline/sqrt(3);
pf = 0.9;
theta = acos(pf);
w = 377;

% ---------- Wrong -------------------------
% Ia = S/(sqrt(3)*vphi)
% P = pf*S;
% R = P/(Ia^2)
% Q = S*sqrt(1-pf^2);
% Xl = Q/(Ia^2)
% L = Xl / w;
% 
% Vm = (20*10^(3)/sqrt(3))*sqrt(2)
% iaa = Vm/(sqrt(R^2 + Xl^2))
% ibb = Vm*cos(-2*pi/3)/(sqrt(R^2 + Xl^2))
% icc = Vm*cos(-4*pi/3)/(sqrt(R^2 + Xl^2))
% ------------------------------------------

Ia = S/(3*vphi);
iao = sqrt(2)*Ia*cos(-theta+pi);
ibo = sqrt(2)*Ia*cos(-theta+pi- 2*pi/3);
ico = sqrt(2)*Ia*cos(-theta+pi- 4*pi/3);
ifo = 338 / 0.0860;
ikdo = 0;
ikqo = 0;

iniCon = [iao;ibo;ico;ifo;ikdo;ikqo]
%% simulate first 10 seconds as normal
tspan1 = [0 10];
% thisOde=@(t,x) myOde(t,x,fault=0);
global fault
fault=0;
[t1,x1] = ode78(@myOde, tspan1, iniCon);

%% next 15 seconds are a fault
iniCon2 = x1(end,:);
tspan2 = [10 25];
fault=1;
[t2,x2] = ode78(@myOde, tspan2, iniCon2);

% fault clears
iniCon3 = x2(end,:);
tspan3 = [25 40];
fault=0;
[t3,x3] = ode78(@myOde, tspan3, iniCon3);

t = [t1;t2;t3]
x = [x1;x2;x3]

Ia = x(:,1);
Ib = x(:,2);
Ic = x(:,3);
plot(t,Ia,t,Ib,t,Ic);
legend('ia','ib','ic');

plot(t, x(:,4));
legend('if');

plot(t,x(:,5),t,x(:,6));
legend('ikd','ikq');

% ------------ Wrong --------------------
% plot(t,((x(:,1).*(3*Vm*pf))./(n*Xl)))
% legend('Torque')
% ---------------------------------------

va = vphi*sqrt(2)*cos(w*t);
vb = vphi*sqrt(2)*cos(w*t-2*pi/3);
vc = vphi*sqrt(2)*cos(w*t-4*pi/3);
p = va .* x (: ,1) + vb .* x (: ,2) + vc .* x (: ,3) ;
tem = (p) ./(w) ;
figure(4)
plot (t , tem/1e3 )
xlabel (" Time ")
ylabel (" Torque ( kNm ) ")
title (" Tem ")

function dx = myOde(t,x, fault)
    vf = 338;
    fq = 60;
    S = 659*10^6;

    global fault
    if fault
        vline = 0;
    else
        vline = 20*10^3;
    end
    
    pf = 0.9;
    theta_e = acos(pf);

    % ----------- wrong ------------
    % Ia = S/(sqrt(3) * vphi);
    % P = 0.9*S;
    % Rl = P/(Ia^2);
    % Q = S*sqrt(1-pf^2);
    % Xl = Q/(Ia^2);
    % L = Xl / w;
    % -----------------------------
    Zl = vline^2/S;
    R1 = Zl*cos(theta_e);
    L1 = Zl*sin(theta_e) / 377;


    % ------- Use rotor angle, not electrical----
    % theta = w*t;
    % -------------------------------------------
    nrpm = fq*120/4;
    n = nrpm * 0.10472;
    theta = n*t;           % rotor angle

    rs = 7.4e-4; rf = 0.0860;
    rkd = 1.58e-4; rkq = 1.227e-4;
    Lsv = 0.05e-3;Lmv = 0.05e-3;
    Lsa = 1.95e-3; Lff = 444e-3;
    Lma = 0.80e-3; Lakdm = 0.447e-3;
    Lafm = 26.1e-3; Lakqm = 0.670e-3;
    Lfkd = 5.05e-3; Lkdkd = 0.1254e-3;
    Lkdf = 6.29e-3; Lkqkq = 0.3762e-3;
    


    a = -2*pi/3;      % (2 theta - 2pi/3)
    b = -4*pi/3;      % (2 theta - 4pi/3)
    %------------------------------
    % Lss
    Laa = Lsa + Lsv*cos(2*theta);
    Lab = -Lma + Lmv*cos(2*theta + a);
    Lac = -Lma + Lmv*cos(2*theta + b);
    Lbb = Lsa + Lsv*cos(2*theta + b);
    Lbc = -Lma + Lmv*cos(2*theta);
    Lcc = Lsa + Lsv*cos(2*theta + a);

    %------------------------------
    % Lsr / Lrs
    Laf = Lafm*cos(2*theta);
    Lakd = Lakdm*cos(2*theta);
    Lakq = Lakqm*cos(2*theta + pi/2);       % ATTENTION : this value is positive and shifted by pi/2
    Lbf = Lafm*cos(2*theta + a);
    Lbkd = Lakdm*cos(2*theta + a);
    Lbkq = Lakqm*cos(2*theta + a + pi/2);   % ATTENTION : this value is positive and shifted by pi/2
    Lcf = Lafm*cos(2*theta + b);
    Lckd = Lakdm*cos(2*theta + b);
    Lckq = Lakqm*cos(2*theta + b+ pi/2);   % ATTENTION : this value is positive and shifted by pi/2


    % derivative values
    %------------------------------
    % dLss
    dLaa = -2*Lsv*sin(2*theta);
    dLab = -2*Lmv*sin(2*theta + a);
    dLac = -2*Lmv*sin(2*theta + b);
    dLbb = -2*Lsv*sin(2*theta + b);
    dLbc = -2*Lmv*sin(2*theta); 
    dLcc = -2*Lsv*sin(2*theta + a);
    %------------------------------
    % dLsr / dLrs
    dLaf = -2*Lafm*sin(2*theta);
    dLakd = -2*Lakdm*sin(2*theta);
    dLakq = -2*Lakqm*sin(2*theta + pi/2);
    dLbf = -2*Lafm*sin(2*theta + a);
    dLbkd = -2*Lakdm*sin(2*theta + a);
    dLbkq = -2*Lakqm*sin(2*theta + a + pi/2);
    dLcf = -2*Lafm*sin(2*theta + b);
    dLckd = -2*Lakdm*sin(2*theta + b);
    dLckq = -2*Lakqm*sin(2*theta + b + pi/2);

    
    % define inductance matrix and derivative inductance matrix
    R = diag([rs+R1, rs+R1, rs+R1, rf, rkd, rkq]);

    Lss = [Laa+L1, Lab, Lac; 
           Lab, Lbb+L1, Lbc; 
           Lac, Lbc, Lcc+L1];

    Lsr = [Laf, Lakd, Lakq; 
           Lbf, Lbkd, Lbkq; 
           Lcf, Lckd, Lckq];

    Lrs = Lsr';
    
    Lrr = [Lff, Lfkd, 0; 
           Lkdf, Lkdkd, 0; 
           0, 0, Lkqkq];

    L = [Lss, Lsr; Lrs, Lrr];
    
    dLss = [dLaa, dLab, dLac; 
            dLab, dLbb, dLbc; 
            dLac, dLbc, dLcc];

    dLsr = [dLaf, dLakd, dLakq; 
            dLbf, dLbkd, dLbkq; 
            dLcf, dLckd, dLckq];

    dLrs = dLsr';

    dLrr = zeros(3,3);

    dL = [dLss, dLsr; dLrs, dLrr];

    % V = RI + dL*I + L*dI
    % L*dI = V - RI - dL*I
    % dI = -invL(R + dL)*I + invL*V 
    
    A = -inv(L)*(R+n*dL);
    B = inv(L);

    % ------------ Wrong -----------------
    % Vm = (20*10^(3)/sqrt(3))*sqrt(2);
    % u = [Vm*sin(w.*t+pi/2), Vm*sin(w.*t + pi/2 - 2*pi/3), Vm*sin(w.*t + pi/2 - 4*pi/3), 338, 0, 0]';
    %u = [Vm;Vm;Vm;377;0;0];
    %u = K*sin(377.*t + pi/2);
    % -------------------------------------

    u = [0;0;0;vf;0;0];
    dx = A*x + B*u;
end


