%simulator PMSM for PCRB, choice: Ls/Ldq & correct/incorrect expectation
clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%for experiments
    %ctrl.> 2,5,9,10
    %model> {1},{7},{4,6,10},{12} - {eq.} sort from worst

%   settings
    % ON/OFF  MODEL    IS CORR. E   INDUCT. L
model(1) = 1; % alpha-beta    nE      Ls
model(2) = 0; % d-q           nE      Ls
model(3) = 0; % 5sc           nE      Ls

model(4) = 0; % alpha-beta    E       Ls
model(5) = 0; % d-q           E       Ls
model(6) = 0; % 5sc           E       Ls

model(7) = 1; % alpha-beta    nE      Ldq
model(8) = 0; % d-q           nE      Ldq
model(9) = 0; % 5sc           nE      Ldq

model(10) = 0; % alpha-beta   E       Ldq
model(11) = 0; % d-q          E       Ldq
model(12) = 0; % 5sc          E       Ldq

    % CONTROL
    % 1 -   ome = ref_ome, the = integral(ome), ial = ibe = 0
    % 2 -   PI control    
    % 3 -   PI + injection (sin -> dq)
    % 4 -   PI + injection (rect. -> dq)
    % 5 -   PI + injection (const. -> ud) *J4 ~ 3e-6 for all exc. dq
    % 6 -   PI + rnd error (ref_ome) *J4: dq bad; ab,5sc Ls ~ 4e-4; ab,5sc Ldq ~5e-6  
    % 7 -   PI + injection (sin -> al-be)
    % 8 -   PI + injection (rect. -> al-be)
    % 9 -   PI + bicrit. (sign) *J4 ~ 1e-3 for all exc. dq
    % 10 -  PI + rnd bicrit. selection (5) 
    
% control = 2;
% control = 5;
% control = 8; %high frekq.
% control = 9;
control = 10;
    % param. amp*phi(injom*dt*t) //phi = sin,cos,sign(sin),sing(cos)
    
amp = 10.0;
injom = 5000;

    %cov Q, R matrices
%         %noise
% Q = diag([0.0013 0.0013 5.0e-6 1.0e-10]);
% R = diag([0.0006 0.0006]);
%         %EKF
% Q = diag([0.1 0.1 0.1 0.001]);
% R = diag([0.05 0.05]);
%         %test
Q = diag([0.1 0.1 0.1 10]);
R = diag([0.5 0.5]);

%horizont
T = 120000; 

%reference 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];
% ref_profile = ones(1,16);

[var_ome, var_th, var_th2] = fpcrb(model, control, amp, injom, Q, R, T, ref_profile);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   plot

%axis
xax = 1:T-1;
dt = 0.000125;
timex = (xax)*dt;
cmp = lines(sum(model));
for i=1:sum(model)
    figure(1);
    hold on;
    plot(timex, var_ome(xax,i),'Color',cmp(i,:));
    
    figure(2);
    hold on;
    plot(timex, var_th(xax,i),'Color',cmp(i,:));
    
    figure(3);
    hold on;
    plot(timex, var_th2(xax,i),'Color',cmp(i,:));
end

sound(chirp(0:0.001:2,0,1,150));