%simulator PMSM pro PCRB s lepsi ocekavanou hodnotou E
% zatim zaver:
%   
clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   parametry stroje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Rs = 0.28;
Ls = 0.003465;
psipm = 0.1989;
B = 0;
TL = 0;
kp = 1.5;
pp = 4.0;
J = 0.04;
dt = 0.000125;

a = 0.9898;
b = 0.0072;
c = 0.0361;
d = 1.0;
e = 0.0149;

Lq = 1.0*Ls;
Ld = 0.9*Ls;

Q = diag([0.0013 0.0013 5.0e-6 1.0e-10]);
R = diag([0.0006 0.0006]);


T = 120000; %horizont

ref_ome = zeros(1, T); %referencni signal
% ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0];
ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0,0,-3, -6, -3];
% ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];

for k = 1:T,
       index = floor(k*dt);
       if(index>0)
           lower = ref_profile(index);
       else
           lower = 0;
       end
       if(index<T*dt)
           upper = ref_profile(index+1);
       else
           upper = 0;
       end
       ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt);
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   promenne stavu systemu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x_sys = zeros(4, T); %vnitrni stav systemu
u_ab = zeros(2, T); %rizeni systemu v alfa-beta

iJn = zeros(4,4,T);
iJn2 = zeros(4,4,T);
iJn3 = zeros(4,4,T);
iJn4 = zeros(4,4,T);
iJn5 = zeros(4,4,T);
Jj = inv(Q);
Jj2 = inv(Q);
Jj3 = inv(Q);
Jj4 = inv(Q);
Jj5 = inv(Q);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   promenne rizeni
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

u_dq = zeros(2, T);

%PI vektorove
% kon_pi = 3.0;
% kon_ii = 0.00375;
% kon_pu = 20.0;
% kon_iu = 0.05;
sum_iq = 0;
sum_ud = 0;
sum_uq = 0;

kon_pi = 30.0;
kon_ii = 0.0;
kon_pu = 20.0;
kon_iu = 0.0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   inicializace
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%alpha-beta, equal Ls
DF = zeros(4);
DF(1,1) = a;
DF(2,2) = a;
DF(3,3) = d;
DF(4,3) = dt;
DF(4,4) = 1.0;
DH = zeros(2,4);
DH(1,1) = 1.0;
DH(2,2) = 1.0;

%d-q red, equal Ls
DF2 = zeros(4);
DF2(1,1) = a;
DF2(2,2) = a;
DF2(2,3) = -b;
DF2(3,2) = e;
DF2(3,3) = d;
DF2(4,3) = dt;
DF2(4,4) = 1.0;
DH2 = DH;

%d-q full, equal Ls
DF3 = DF2;

%d-q, not equal Ld, Lq
DF4 = zeros(4);
DF4(1,1) = 1.0 - Rs*dt/Ld;
DF4(2,2) = 1.0 - Rs*dt/Lq;
DF4(3,3) = 1-B*dt/J;
DF4(4,3) = dt;
DF4(4,4) = 1.0;
    
%alpha-beta, not equal Ld, Lq
DF5 = zeros(4);

% figure;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   hlavni smycka
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for t = 1:T-1,
       
    
    %%%%% rizeni %%%% (estxt -> ut)
    ial = x_sys(1, t);
    ibe = x_sys(2, t);
    ome = x_sys(3, t);
    the = x_sys(4, t);  
    
    id = ial*cos(the) + ibe*sin(the);
    iq = ibe*cos(the) - ial*sin(the);
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %alpha-beta, equal Ls
%     DF(1,3) = b*sin(the);
%     DF(1,4) = b*ome*cos(the);
%     DF(2,3) = -b*cos(the);
%     DF(2,4) = b*ome*sin(the);
%     DF(3,1) = -e*sin(the);
%     DF(3,2) = e*cos(the);
%     DF(3,4) = -e*(ial*cos(the)+ibe*sin(the));
%     
%     D11 = DF'*inv(Q)*DF;
%     D12 = -DF'*inv(Q);
%     D21 = D12';
%     D22 = inv(Q) + DH'*inv(R)*DH;
%     
%     Jj = D22 - D21*inv(Jj + D11)*D12;
%         
%     iJn(:,:,t) = inv(Jj);
    
    %d-q red, equal Ls ... DF-OK
%     DH2(1,1) = cos(the);
%     DH2(1,2) = -sin(the);
%     DH2(1,4) = -id*ome*sin(the)-iq*ome*cos(the);
%     DH2(2,1) = sin(the);
%     DH2(2,2) = cos(the);
%     DH2(2,4) = id*ome*cos(the)-iq*ome*sin(the);
%     D11 = DF2'*inv(Q)*DF2;
%     D12 = -DF2'*inv(Q);
%     D21 = D12';
% %     D22 = inv(Q) + DH'*inv(R)*DH;
%     D22 = inv(Q) + DH2'*inv(R)*DH2;
%     
%     Jj2 = D22 - D21*inv(Jj2 + D11)*D12;
%         
%     iJn2(:,:,t) = inv(Jj2);
    
    %d-q full, equal Ls
%     DF3(1,2) = dt*ome;
%     DF3(1,3) = dt*iq;
%     DF3(2,1) = -dt*ome;
%     DF3(2,3) = -dt*id-b;
%     D11 = DF3'*inv(Q)*DF3; !!!!!!!!!!!!!!!!!BAD
    iQ = inv(Q);
    q11 = iQ(1,1);
    q12 = iQ(1,2); 
    q13  = iQ(1,3);
    q14  = iQ(1,4);
    q21  = iQ(2,1);
    q22  = iQ(2,2);
    q23  = iQ(2,3);
    q24  = iQ(2,4);
    q31  = iQ(3,1);
    q32  = iQ(3,2);
    q33  = iQ(3,3);
    q34  = iQ(3,4);
    q41  = iQ(4,1);
    q42  = iQ(4,2);
    q43  = iQ(4,3);
    q44 = iQ(4,4);
    om = ome;
    D11 = [[a*(a*iQ(1,1) - dt*om*iQ(2,1)) - dt*om*a*iQ(1,2) + dt^2*Q(3,3)*iQ(2,2), a*(a*iQ(1,2) - dt*om*iQ(2,2)) + e*(a*iQ(1,3) - dt*om*iQ(2,3)) + dt*om*a*iQ(1,1) - dt^2*Q(3,3)*iQ(2,1), d*(a*iQ(1,3) - dt*om*iQ(2,3)) - (a*iQ(1,2) - dt*om*iQ(2,2))*b - a*iQ(1,2)*dt*id - dt^2*Q(1,3)*iQ(2,2) + dt*(a*q14 - dt*om*q24) + dt*iq*a*q11 - dt^2*Q(2,3)*q21, a*q14 - dt*om*q24];...
            [ a*(a*q21 + e*q31 + dt*om*q11) - dt*om*(a*q22 + e*q32) - dt^2*Q(3,3)*q12, a*(a*q22 + e*q32 + dt*om*q12) + e*(a*q23 + e*q33 + dt*om*q13) + dt*om*(a*q21 + e*q31) + dt^2*Q(3,3)*q11, d*(a*q23 + e*q33 + dt*om*q13) + dt*(a*q24 + e*q34 + dt*om*q14) - b*(a*q22 + e*q32 + dt*om*q12) - dt*id*(a*q22 + e*q32) - dt^2*Q(1,3)*q12 + dt*iq*(a*q21 + e*q31) + dt^2*Q(2,3)*q11,a*q24 + e*q34 + dt*om*q14];...
            [ a*(d*q31 + dt*q41 - q21*(b + dt*id) + dt*iq*q11) - dt*om*(d*q32 + dt*q42 - q22*b) + dt*q22*(dt*Q(1,3)) - dt^2*Q(2,3)*q12, a*(d*q32 + dt*q42 - q22*(b + dt*id) + dt*iq*q12) + e*(d*q33 + dt*q43 - q23*(b + dt*id) + dt*iq*q13) + dt*om*(d*q31 + dt*q41 - q21*b) - q21*dt^2*Q(1,3) + dt^2*Q(2,3)*q11, d*(d*q33 + dt*q43 - q23*(b + dt*id) + dt*iq*q13) - (b + dt*id)*(d*q32 + dt*q42 - q22*b) + b*(q22*dt*id - dt*iq*q12) + dt*(q22*dt*Q(1,1) - dt*Q(1,2)*q12) + dt*(d*q34 + dt*q44 - q24*(b + dt*id) + dt*iq*q14) + dt*iq*(d*q31 + dt*q41 - q21*b) + dt*(- q21*dt*Q(1,2) + dt*Q(2,2)*q11), d*q34 + dt*q44 - q24*(b + dt*id) + dt*iq*q14];...
            [ a*q41 - dt*om*q42,  a*q42 + e*q43 + dt*om*q41,  d*q43 + dt*q44 - q42*(b + dt*id) + dt*iq*q41,   q44]];
 
    D12 = -DF3'*iQ;
    D21 = D12';
% %     D22 = inv(Q) + DH'*inv(R)*DH;
    D22 = inv(Q) + DH2'*inv(R)*DH2;
    
    Jj3 = D22 - D21*inv(Jj3 + D11)*D12;
        
    iJn3(:,:,t) = inv(Jj3);
    
    %d-q, not equal Ld, Lq
%     DF4(1,2) = Lq*dt/Ld*ome;
%     DF4(1,3) = Lq*dt/Ld*iq;
%     DF4(2,1) = -Ld*dt/Lq*ome;
%     DF4(2,3) = -Ld*dt/Lq*id-psipm*dt/Lq;
%     DF4(3,1) = kp*pp*pp*dt*(Ld-Lq)/J*iq;
%     DF4(3,2) = kp*pp*pp*dt/J*((Ld-Lq)*id+psipm);    
%     D11 = DF4'*inv(Q)*DF4;
%     D12 = -DF4'*inv(Q);
%     D21 = D12';
%     D22 = inv(Q) + DH'*inv(R)*DH;
% %     D22 = inv(Q) + DH2'*inv(R)*DH2;
%     
%     Jj4 = D22 - D21*inv(Jj4 + D11)*D12;
%         
%     iJn4(:,:,t) = inv(Jj4);
    
    %alpha-beta, not equal Ld, Lq
%     ia = ial;
%     ib = ibe;
%     psi = psipm;
%     kppj = kp*pp*pp/J;
%     DF5 = [[ (Lq - Rs*dt*sin(the)^2)/Lq - (dt*ome*sin(the)*Lq^2*cos(the) + Rs*dt*Lq*cos(the)^2)/(Ld*Lq) + (Ld*dt*ome*cos(the)*sin(the))/Lq,                                       (dt*(Ld - Lq)*(- Lq*ome*cos(the)^2 + Rs*cos(the)*sin(the) + Ld*ome*sin(the)^2))/(Ld*Lq),    dt*cos(the)*(ia*sin(the) - ib*cos(the) + (Lq*(ib*cos(the) - ia*sin(the)))/Ld) + dt*sin(the)*(psi/Lq - ia*cos(the) - ib*sin(the) + (Ld*(ia*cos(the) + ib*sin(the)))/Lq),                                               (dt*(ome*psi*cos(the) + Rs*ib*cos(2*the) - Rs*ia*sin(2*the)))/Lq + (Ld*dt*(ia*ome*cos(2*the) + ib*ome*sin(2*the)))/Lq - (dt*(Lq^2*ia*ome*cos(2*the) + Lq^2*ib*ome*sin(2*the) + Lq*Rs*ib*cos(2*the) - Lq*Rs*ia*sin(2*the)))/(Ld*Lq)];...
%             [                                       (dt*(Ld - Lq)*(- Ld*ome*cos(the)^2 + Rs*cos(the)*sin(the) + Lq*ome*sin(the)^2))/(Ld*Lq), (Lq - Rs*dt*cos(the)^2)/Lq - (Lq*Rs*dt*sin(the)^2 - Lq^2*dt*ome*cos(the)*sin(the))/(Ld*Lq) - (Ld*dt*ome*cos(the)*sin(the))/Lq, (dt*(Lq*ia - psi*cos(the)))/Lq + (dt*((Lq^2*ia*cos(2*the))/2 - (Lq^2*ia)/2 + (Lq^2*ib*sin(2*the))/2))/(Ld*Lq) - (Ld*dt*(ia/2 + (ia*cos(2*the))/2 + (ib*sin(2*the))/2))/Lq, (dt*ome*psi*sin(the) - Rs*dt*ia*(2*sin(the)^2 - 1) + Rs*dt*ib*sin(2*the))/Lq + (Ld*(dt*ib*ome*(2*sin(the)^2 - 1) + dt*ia*ome*sin(2*the)))/Lq - (Lq*Rs*dt*ib*sin(2*the) + Lq^2*dt*ib*ome*(2*sin(the)^2 - 1) + Lq^2*dt*ia*ome*sin(2*the) - Lq*Rs*dt*ia*(2*sin(the)^2 - 1))/(Ld*Lq)];...
%             [     -dt*kppj*(psi*sin(the) - cos(the)*(Ld - Lq)*(ib*cos(the) - ia*sin(the)) + sin(the)*(Ld - Lq)*(ia*cos(the) + ib*sin(the))),      dt*kppj*(psi*cos(the) + cos(the)*(Ld - Lq)*(ia*cos(the) + ib*sin(the)) + sin(the)*(Ld - Lq)*(ib*cos(the) - ia*sin(the))),                                                                                                                                                                         1.0,                                                                                                                                                   -dt*kppj*(psi*(ia*cos(the) + ib*sin(the)) + (Ld - Lq)*(ia*cos(the) + ib*sin(the))^2 - (Ld - Lq)*(ib*cos(the) - ia*sin(the))^2)];...
%             [                                                                                                                             0.0,                                                                                                                             0.0,                                                                                                                                                                        dt,                                                                                                                                                                                                                                                                                1.0]];
%     D11 = DF5'*inv(Q)*DF5;
%     D12 = -DF5'*inv(Q);
%     D21 = D12';
%     D22 = inv(Q) + DH'*inv(R)*DH;
%     
%     Jj5 = D22 - D21*inv(Jj5 + D11)*D12;
%         
%     iJn5(:,:,t) = inv(Jj5);
    
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%        
               
        
        sum_iq = sum_iq + ref_ome(t) - ome;
        ref_iq = kon_pi*(ref_ome(t) - ome) + kon_ii*sum_iq;
        sum_ud = sum_ud - id;
        u_dq(1, t) = kon_pu*(-id) + kon_iu*sum_ud;
        sum_uq = sum_uq + ref_iq - iq;
        u_dq(2, t) = kon_pu*(ref_iq - iq) + kon_iu*sum_uq;
        u_dq(1, t) = u_dq(1, t) - Ls*ome*ref_iq;
        u_dq(2, t) = u_dq(2, t) + psipm*ome;        
               
        
%         u_dq(2,t) = u_dq(2,t) + 10.0*cos(100*dt*t);   
        
%         ual = u_dq(1,t)*cos(the) - u_dq(2,t)*sin(the);
%         ube = u_dq(1,t)*sin(the) + u_dq(2,t)*cos(the);
% 
% %         ual = ual + 10.0*cos(100*dt*t);
% %         ube = ube + 10.0*sin(100*dt*t);
% 
% %         duab = sign(rand(2,1)-0.5)*10;
% %         ual = ual + duab(1);
% %         ube = ube + duab(2);
%         
%         ud = ual*cos(the) + ube*sin(the);
%         uq = ube*cos(the) - ual*sin(the);
    
%     u_dq(1,t) = 0.1*Ld/dt - (1.0*Ld/dt - Rs)*id - Lq*ome*iq;
    
    %%%% simulace %%% (xt + ut -> xt+1; xt+1 -> yt+1)
    idpl = (1.0 - Rs*dt/Ld)*id + Lq*dt/Ld*ome*iq + dt/Ld*u_dq(1,t);
    iqpl = (1.0 - Rs*dt/Lq)*iq - psipm*dt/Lq*ome - Ld*dt/Lq*ome*id + dt/Lq*u_dq(2,t);
    
    x_sys(1, t+1) = idpl*cos(the) - iqpl*sin(the);
    x_sys(2, t+1) = idpl*sin(the) + iqpl*cos(the);
    
    x_sys(3, t+1) = ref_ome(t);%(1.0-B*dt/J)*ome + kp*pp*pp*dt/J*((Ld-Lq)*id*iq + psipm*iq);
    x_sys(4, t+1) = the + dt*ome;
        
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   zaznam vysledku
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

xax = 1:T-1;
timex = (xax)*dt;

% subplot(2, 2, 1);
% plot(timex, x_sys(1, xax));
% subplot(2, 2, 2);
% plot(timex, x_sys(2, xax));
% subplot(2, 2, 3);
% plot(timex, x_sys(3, xax), timex, ref_ome(xax));
% subplot(2, 2, 4);
% plot(timex, atan2(sin(x_sys(4, xax)),cos(x_sys(4, xax))));

% subplot(2,1,1);
% plot(timex, x_sys(3, xax), timex, ref_ome(xax));
% subplot(2,1,2);
hold on;
plot(timex, squeeze(iJn3(4,4,xax)),'r');%,timex, squeeze(iJn5(4,4,xax)),timex, squeeze(iJn4(4,4,xax)));

% figure;
% plot(timex, x_sys(3, xax)-ref_ome(xax));