% M4 Project Synch Gen with Park Transformation & 3 Phase Fault
close all; clear all; clc;

S = 659*10^6;
p = 4;
pf = 0.9;
Vline = 20*10^3;
vphi = Vline/sqrt(3);
phi = acosd(pf);
nrpm = 60*120/4;
n = nrpm*0.10472; % rad/s
w = 377;

Ia = S/(3*vphi);
ia = -Ia*pf - 1i*Ia*sind(phi);

iao = sqrt(2)*Ia*cosd(-phi + 180);
ibo = sqrt(2)*Ia*cosd(-phi - 120 + 180);
ico = sqrt(2)*Ia*cosd(-phi - 240 + 180);
Iabc = [iao; ibo; ico];

ikd1o = 0;
ikd2o = 0;
ikq1o = 0;
ikq2o = 0;

% -------------------------------
va = (Vline / sqrt(3))*sqrt(2);
d2 = 0.4158e-3;
d1 = 0.1254e-3;
Xd = 377 * (d1+d2);
q2 = 0.1375e-3;
q1 = 0.3762e-3;
Xq = 377 * (q1+q2);
Lafm = 26.1e-3;
% ---------------------------------------------------------
delta = atand((Xq*Ia*cosd(phi))/(va - Xq*Ia*sind(phi)));
sigma = delta*pi/180 + 3*pi/2;
Eq = (va - Xq*ia*sind(phi)) + 1i*(Xq*ia*cosd(phi));
Eaf = Eq + (Xd-Xq) * ia*sind(delta + phi);
If = sqrt(2)*Eaf/(377*Lafm);
ifo = sqrt(real(If)^2 + imag(If)^2);
vfo = ifo*0.0860;
% ----------------------------------------------------------
% 
T = (2/3) * [cos(sigma),    cos(sigma-2*pi/3),   cos(sigma-4*pi/3);
     -sin(sigma),   -sin(sigma-2*pi/3),  -sin(sigma-4*pi/3);
     1/2, 1/2,       1/2];

va = vphi*sqrt(2)*cos(0);
vb = vphi*sqrt(2)*cos(-2*pi/3);
vc = vphi*sqrt(2)*cos(-4*pi/3);
vdqo = T*[va;vb;vc];

Ifkdkq = [ifo;0;0;0;0];

iniCon = [Iabc; Ifkdkq]
tspan1 = [0 10/377];
% 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/377 25/377];
fault=1;
[t2,x2] = ode78(@myode, tspan2, iniCon2);

% fault clears
iniCon3 = x2(end,:);
tspan3 = [25/377 30/377];
fault=0;
[t3,x3] = ode78(@myode, tspan3, iniCon3);

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

Id = x(:,1);
Iq = x(:,2);
Io = x(:,3);
If = x(:,4);
Ikd1 = x(:,5);
Ikd2 = x(:,6);
Ikq1 = x(:,7);
Ikq2 = x(:,8);

plot(t,Id,t,Iq,t,Io);
legend('id','iq','io', "Location","southeast");
xlabel("Time [s]");
ylabel("Current [A]")
title("DQO Currents")

plot(t,If);
xlabel("Time [S]")
ylabel("Current [A]")
title("Field Current")

plot(t,Ikd1,t,Ikd2,t,Ikq1,t,Ikq2);
legend('idq','id2','iq1','iq2',"Location","northeast")
xlabel("Time [S]")
ylabel("Current [A]")
title("Damping Circuit")

p = vdqo(1) .* x (: ,1) + vdqo(2) .* x (: ,2) + vdqo(3) .* x (: ,3) ;
tem = (p) ./(w) ;
figure(4)
plot (t , tem/1e3 )
xlabel (" Time ")
ylabel (" Torque ( kNm ) ")
title ("Tem")


function dx = myode(t, x, fault)

    global fault
    if fault
        vline = 0;
    else
        vline = 20*10^3;
    end
    
    S = 659e6;
    pf = 0.9;
    theta_e = acos(pf);
    Zl = vline^2 / S;
    Rl = Zl*cos(theta_e);
    Ll = Zl*sin(theta_e) / 377;

    nrpm = 60*120/4;
    n = nrpm*0.10472; % rad/s
    sigma = n*t;
 
    rs = 7.4e-4; rf = 0.0860;
    rkd1 = 1.58e-4; rkd2 = 0.0130;
    rkq1 = 1.227e-4;  rkq2 = 0.0174;
    
    Lsa = 1.95e-3; Lsv = 0.05e-3;
    Lma = 0.80e-3; Lmv = 0.05e-3;
    Lafm = 26.1e-3; Lff = 444e-3;
    Lfkd1 = 5.05e-3; Lfkd2 = 12.38e-3;
    Lkd1f = 6.29e-3; Lkd2f = 12.38e-3;
    Lakdm1 = 0.447e-3; Lakdm2 = 0.79e-3;
    Lakqm1 = 0.670e-3; Lakqm2 = 0.378e-3;
    Lkd1kd1 = 0.1254e-3; Lkd1kd2 = 0.19e-3;
    Lkd2kd1 = 0.148e-3; Lkd2kd2 = 0.4158e-3;
    Lkq1kq1 = 0.3762e-3; Lkq1kq2 = 0.159e-3;
    Lkq2kq1 = 0.160e-3; Lkq2kq2 = 0.1375e-3;


    %Rss = diag([rs rs rs]);
    Rss = diag([rs+Rl rs+Rl rs+Rl]);
    Rrr = diag([rf rkd1, rkd2, rkq1, rkq2]);
    R = [Rss, zeros(3,5); zeros(5,3), Rrr];

    
    Ld = Lsa + Lma + (3/2)*(Lsv);
    Lq = Lsa + Lma - (3/2)*(Lsv);
    Lo = Lsa - 2*Lma;
    
    Lss1 = [0, -1*Lq, 0; 
            Ld, 0,    0;
            0,  0,    0];
    
    Lss2 = [0,    0,     -Lakqm1, 0,      -Lakqm2;
            Lafm, Lakdm1, 0,      Lakdm2, 0;
            0,    0,      0,      0,      0];
    
    L1 = [Lss1,       Lss2; 
          zeros(5,3), zeros(5,5)];
    
    Lss3 = diag([Ld+Ll Lq+Ll Lo+Ll]);
    
    Lss4 = [Lafm, Lakdm1, 0,      Lakdm2, 0;
            0,    0,      Lakqm1, 0,      Lakqm2;
            0,    0,      0,      0,      0];
    
    Lss5 = (3/2).*Lss4';
    
    Lss6 = [Lff,   Lfkd1,   0,        Lfkd2,   0;
           Lkd1f, Lkd1kd1, 0,        Lkd1kd2, 0;
           0,     0,       Lkd2kd2,  0,       Lkq1kq2;
           Lkd2f, Lkd2kd1, 0,        Lkq1kq1, 0;
           0,     0,       Lkq2kq1,  0,       Lkq2kq2];
    
    L2 = [Lss3, Lss4;
          Lss5, Lss6];


    T = 2/3 * [cos(sigma),  cos(sigma -2*pi/3),   cos(sigma -4*pi/3);
               -sin(sigma), -sin(sigma -2*pi/3),  -sin(sigma-4*pi/3);
               1/2,   1/2,            1/2];

    % V = RI + w L1 I + L2 Idot
    % L2 idot = V - (R+wL1)I
    % idot = -invL2(R+wL1)I + invL2V

    va = (vline/sqrt(3))*sqrt(2)*cos(n*t);
    vb = (vline/sqrt(3))*sqrt(2)*cos(n*t - 2*pi/3);
    vc = (vline/sqrt(3))*sqrt(2)*cos(n*t - 4*pi/3);
    vf = 237.9676;
    vdqo = T*[va;vb;vc];

    A = -inv(L2)*(R + n*L1);
    B = inv(L2);
    u = [vdqo;vf;0;0;0;0];
    dx = A*x + B*u;

end

