%% Settings
clc
clear all
close all

global l;

Id_settled = zeros(1,16);
Rs_identified = zeros(1,16);
Ld_identified = zeros(1,16);

% Motor parameters
Rs_meas = 6.5; %ohm
ke = 0.14; %V.s/rad
pp = 3;
Ld = 12e-3; % H
Lq = 12e-3; % H
J = 1e-6;
F = 0;
Th0 = 0;
    
load('data_Ud.mat');                %Measured real data
load('data_for_PS.mat');            %Data for PowerStage transfer char.
load_system('model');               %Model of the drive

%% Identification
for j=1:16        
    if j<10
        s = ['data = step_Ud_0',num2str(j),';'];
    else
        s = ['data = step_Ud_',num2str(j),';'];
    end
    eval(s);
    
    %Data sorting
    Tvz = 62.5e-6;                              %sampling period
    t2 = (0:Tvz:(length(data(:,1))-1)*Tvz)';    %time vector
    l = length(t2);                             %length of the time vector
    Tend = (length(data(:,1))-1)*Tvz;           %end time
    Ud = data(:,2);                             %measured Ud
    Id = data(:,3);                             %measured Id
    Uq = data(:,4);                             %measured Uq
    Iq = data(:,5);                             %measured Iq
    
    Id_settled(j) = sum(Id(end-49:end))/50;
    Ud_step_height = sum(Ud(end-49:end))/50;
    set_param('model/Step1','After',mat2str(Ud_step_height/99));   %Step height
    
    a0 = [12e-3 12.78/2];

    result = fminsearch(@fkrit,a0,optimset('Display','iter','MaxFunEvals',10000,'MaxIter',10000,'TolFun',1e-2,'TolX',1e-2),Id);

    Ld = result(1);
    Rs = result(2);
    Rs_identified(j) = result(2);
    Ld_identified(j) = result(1);
        
    set_param('model/PMSM1','Ld',mat2str(Ld));
    set_param('model/PMSM1','Rs',mat2str(Rs));
    sim ('model');

% comparation plot
    figure;
    plot(t2,Id,'r','LineWidth',1);
    hold on;
    plot(t2,Id_model,'b','LineWidth',1);
    title('Id [mA]');
    legend('Id meas','Id model')
    grid on;
    xlabel('t(s)')
    ylabel('Id(A)')
end

%% Rs idenetified plot
    figure;
    
    plot(Rs_identified,'b');
    title('Rs identified');
    xlabel('Id[A]')
    ylabel('Rs[ohm]')    
    grid on;
    
%% Ld identified plot
    figure;

    plot(Ld_identified,'b');
    title('Ld identified');
    xlabel('Id[A]')
    ylabel('Ld[H]')    
    grid on;
    
%% Averaging
    
    Rs_avg = sum(Rs_identified(1:5))/10 + sum(Rs_identified(12:16))/10
    Ld_avg = sum(Ld_identified(1:5))/10 + sum(Ld_identified(12:16))/10