%simulator PMSM for PCRB, choice: Ls/Ldq & control
clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%   settings
    % ON/OFF  MODEL       INDUCT. L
model(1) = 1; % alpha-beta          Ls
model(2) = 0; % alpha-beta          Ldq
model(3) = 0; % reduced alpha-beta          Ls
model(4) = 0;%zatim nefunguje % reduced alpha-beta          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 = 0.001 -- 10;
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] = fpcrb2(model, control, amp, injom, Q, R, T, ref_profile, 100*eye(4));

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

p0 = 1;

%axis
xax = 1:T-1;
dt = 0.000125;
timex = (xax)*dt;
cmp = hsv(10);

[var_ome, var_th] = fpcrb2(model, control, 10, injom, Q, R, T, ref_profile, p0*eye(4));
figure(1);
hold on;
plot(timex, var_th(xax,1),'Color',cmp(1,:));

[var_ome, var_th] = fpcrb2(model, control, 1, injom, Q, R, T, ref_profile, p0*eye(4));
figure(1);
hold on;
plot(timex, var_th(xax,1),'Color',cmp(2,:));

[var_ome, var_th] = fpcrb2(model, control, 0.1, injom, Q, R, T, ref_profile, p0*eye(4));
figure(1);
hold on;
plot(timex, var_th(xax,1),'Color',cmp(3,:));

[var_ome, var_th] = fpcrb2(model, control, 0.02, injom, Q, R, T, ref_profile, p0*eye(4));
figure(1);
hold on;
plot(timex, var_th(xax,1),'Color',cmp(4,:));

[var_ome, var_th] = fpcrb2(model, control, 0.01, injom, Q, R, T, ref_profile, p0*eye(4));
figure(1);
hold on;
plot(timex, var_th(xax,1),'Color',cmp(5,:));

[var_ome, var_th] = fpcrb2(model, control, 0.005, injom, Q, R, T, ref_profile, p0*eye(4));
figure(1);
hold on;
plot(timex, var_th(xax,1),'Color',cmp(6,:));

[var_ome, var_th] = fpcrb2(model, control, 0.003, injom, Q, R, T, ref_profile, p0*eye(4));
figure(1);
hold on;
plot(timex, var_th(xax,1),'Color',cmp(7,:));

[var_ome, var_th] = fpcrb2(model, control, 0.002, injom, Q, R, T, ref_profile, p0*eye(4));
figure(1);
hold on;
plot(timex, var_th(xax,1),'Color',cmp(8,:));

[var_ome, var_th] = fpcrb2(model, control, 0.0015, injom, Q, R, T, ref_profile, p0*eye(4));
figure(1);
hold on;
plot(timex, var_th(xax,1),'Color',cmp(9,:));

[var_ome, var_th] = fpcrb2(model, control, 0.001, injom, Q, R, T, ref_profile, p0*eye(4));
figure(1);
hold on;
plot(timex, var_th(xax,1),'Color',cmp(10,:));
legend('10','1','0.1','0.02','0.01','0.005','0.003','0.002','0.0015','0.001','Location','NorthEastOutside');

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