%% Maintance
% close all
% clear variables
% clc

%% Settings

% Display Input/Output data
    DispInOutData = false;
%     DispInOutData = true;

% Display Bode diagram using the matlab spectral analysis (function spa)
    DispMatlabSpa = false;  
%     DispMatlabSpa = true;
    
% Display Bode diagram using the spectral analysis without window
    DispMyBasicSpa = false; 
%     DispMyBasicSpa = true;
    
% Display Bode diagram using the spectral analysis with window
%     DispMyBarlettSpa = false;
    DispMyBarlettSpa = true;
    
% Display individual curves in figures
%   DispIndividCurv == false => each Bode diagrams of closed and open loop are displayed in separate figure;
%   DispIndividCurv == true => all Bode diagrams of closed loop from all spectral analysis methods are displayed in one figure and all Bode diagrams of open loop from all spectral analysis methods are displayed in one figure;
    DispIndividCurv = false;
%     DispIndividCurv = true;

% Unit of frequency axis
    FreqUnit = 'rad/s';
%     FreqUnit = 'Hz';

% Read data from file or tf    
%   bFromFile == TRUE => read data from file
%   bFromFile == FALSE => data from transfer function. Transfer function is defined on line 57.
% bFromFile = true;
bFromFile = false;

% Window size for spectral analysis with window
length_window_barlet = 8192; 
    
%% Read data

if bFromFile == true
% From file
    % Open LoadFile dialog
    [filename, pathname] = ...
         uigetfile({'*.identdat';'*.*'},'Input/Output measurement data');
     if filename == 0
         return
     end
    full_path = fullfile(pathname,filename);
    % Load data
    MeasurementData = ReadEstimationData( full_path );
else
%From matlab functions
    p = tf('p');
    F = 1/(0.1*p + 1);          % closed loop
%     F = 1/((0.01*p + 1)*(0.1*p + 1))  % closed loop
    Ts = 0.01;      % sampling time 
    N = 8192;     % count of samples
    % Type of input signal - is possible only 'prbs' or 'randn'
%     TypeInSignal = 'prbs';    
    TypeInSignal = 'randn';
    MeasurementData = GenerInterDataIdent(F, TypeInSignal , Ts, N );
end

% Display measured values
if DispInOutData == true
    figure
    t = 0:MeasurementData.BaseTime:(length(MeasurementData.allCommandValue)*MeasurementData.BaseTime) - MeasurementData.BaseTime;
    plot(t,MeasurementData.allCommandValue * MeasurementData.ScaleLoad,'b')
    hold on
    plot(t,MeasurementData.allFeedbackValue * MeasurementData.ScaleLoad,'r')
    legend('Command value','Feedback value')

    if strcmp(MeasurementData.ControlMode, 'PositionControl')
        ylabel('s[°]');
    elseif strcmp(MeasurementData.ControlMode, 'VelocityControl')
        ylabel('v[°/s]');
    elseif strcmp(MeasurementData.ControlMode, 'TorqueControl')
        ylabel('M[Nm]');
    end
    xlabel('t[s]');
end

%% Matlab method

if DispMatlabSpa == true
    % Interesting data - at the time of sampling PRBS
    dataspa = iddata(MeasurementData.FeedbackValueInterest(1,:)',MeasurementData.CommandValueInterest(1,:)',MeasurementData.PeriodInputSignal);

    % Matlab spectral analys
    freq_respon = spa(dataspa);
    [mag_bode_mat,phase_bode_mat,wout_bode_mat] = bode(freq_respon);    % Obtaining bode plot of closed loop

    [ F0_mag_mat, F0_phase_mat] = FwtoF0( reshape(mag_bode_mat,[1,size(mag_bode_mat,3)]), reshape(phase_bode_mat,[1,size(phase_bode_mat,3)])./(180/pi));  % Obtaining bode plot of open loop
    Fw_matlab = BodePlotData(reshape(mag_bode_mat,[1,size(mag_bode_mat,3)]), reshape(phase_bode_mat,[1,size(phase_bode_mat,3)]),wout_bode_mat'/2/pi);
    F0_matlab = BodePlotData(F0_mag_mat, F0_phase_mat.*(180/pi), wout_bode_mat'/2/pi);

    % Gw
    if DispIndividCurv == true
        if 0 == exist('hFig101','var')
            hFig101 = Fw_matlab.Show('FreqUnit',FreqUnit);
        else
            Fw_matlab.Show('FreqUnit',FreqUnit,'HandleFigure',hFig101);
        end

        if 0 == exist('LegendAllMethodsGw','var')
            LegendAllMethodsGw = cell(0,1);
        end
        
        LegendAllMethodsGw{size(LegendAllMethodsGw,1) + 1,1} = sprintf('Gw - Matlab - Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
        BodePlotData.SetLegend(hFig101,LegendAllMethodsGw)
        BodePlotData.SetTitle(hFig101,MeasurementData.CreateTitle('Closed loop'))
        
    else
        if 0 == exist('hFig1','var')
            hFig1 = Fw_matlab.Show('FreqUnit',FreqUnit);
        else
            Fw_matlab.Show('FreqUnit',FreqUnit,'HandleFigure',hFig1);
        end

        if 0 == exist('LegendFwMatlab','var')
            LegendFwMatlab = cell(0,1);
        end

        LegendFwMatlab{size(LegendFwMatlab,1)+1,1} = sprintf('Gw Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
        BodePlotData.SetLegend(hFig1,LegendFwMatlab)
        BodePlotData.SetTitle(hFig1,'Matlab method of spa')
        
    end
    % G0
    if DispIndividCurv == true
        if 0 == exist('hFig102','var')
            hFig102 = F0_matlab.Show('FreqUnit',FreqUnit);
        else
            F0_matlab.Show('FreqUnit',FreqUnit,'HandleFigure',hFig102);
        end

        if 0 == exist('LegendAllMethodsG0','var')
            LegendAllMethodsG0 = cell(0,1);
        end
        
        LegendAllMethodsG0{size(LegendAllMethodsG0,1) + 1,1} = sprintf('G0 - Matlab - Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
        BodePlotData.SetLegend(hFig102,LegendAllMethodsG0)
        BodePlotData.SetTitle(hFig102,MeasurementData.CreateTitle('Open loop'))
        
    else

        if 0 == exist('hFig2','var')
            hFig2 = F0_matlab.Show('FreqUnit',FreqUnit);
        else
            F0_matlab.Show('FreqUnit',FreqUnit,'HandleFigure',hFig2);
        end

        if 0 == exist('LegendF0Matlab','var')
            LegendF0Matlab = cell(0,1);
        end

        LegendF0Matlab{size(LegendF0Matlab,1)+1,1} = sprintf('G0 Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
        BodePlotData.SetLegend(hFig2,LegendF0Matlab)
        BodePlotData.SetTitle(hFig2,'Matlab method of spa')
    end     
end

%% My methods
    %% BASIC
        
    if DispMyBasicSpa == true

        % Compute bode diagram by spectral analysis without window
        [ Fw_mag, Fw_phase, F0_mag, F0_phase, f] = IdentSpectral( MeasurementData,'InterestingValues','Basic');

        Fw_basic = BodePlotData(Fw_mag, Fw_phase,f);
        F0_basic = BodePlotData(F0_mag, F0_phase,f);
    
        % Gw
        if DispIndividCurv == true
            if 0 == exist('hFig101','var')
                hFig101 = Fw_basic.Show('FreqUnit',FreqUnit);
            else
                Fw_basic.Show('FreqUnit',FreqUnit,'HandleFigure',hFig101);
            end

            if 0 == exist('LegendAllMethodsGw','var')
                LegendAllMethodsGw = cell(0,1);
            end

            LegendAllMethodsGw{size(LegendAllMethodsGw,1) + 1,1} = sprintf('Gw - Basic - Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
            BodePlotData.SetLegend(hFig101,LegendAllMethodsGw)
            BodePlotData.SetTitle(hFig101,MeasurementData.CreateTitle('Closed loop'))
        
        else
            if 0 == exist('hFig5','var')
                hFig5 = Fw_basic.Show('FreqUnit',FreqUnit);
            else
                Fw_basic.Show('FreqUnit',FreqUnit,'HandleFigure',hFig5);
            end
            
            if 0 == exist('LegendFwBasic','var')
                LegendFwBasic = cell(0,1);
            end
            
            LegendFwBasic{size(LegendFwBasic,1)+1,1} = sprintf('Gw Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
            BodePlotData.SetLegend(hFig5,LegendFwBasic)
            BodePlotData.SetTitle(hFig5,'Basic method of spa')
        end
        % G0
        if DispIndividCurv == true
            if 0 == exist('hFig102','var')
                hFig102 = F0_basic.Show('FreqUnit',FreqUnit);
            else
                F0_basic.Show('FreqUnit',FreqUnit,'HandleFigure',hFig102);
            end

            if 0 == exist('LegendAllMethodsG0','var')
                LegendAllMethodsG0 = cell(0,1);
            end

            LegendAllMethodsG0{size(LegendAllMethodsG0,1) + 1,1} = sprintf('G0 - Basic - Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
            BodePlotData.SetLegend(hFig102,LegendAllMethodsG0)
            BodePlotData.SetTitle(hFig102,MeasurementData.CreateTitle('Open loop'))
        
        else
            
            if 0 == exist('hFig6','var')
                hFig6 = F0_basic.Show('FreqUnit',FreqUnit);
            else
                F0_basic.Show('FreqUnit',FreqUnit,'HandleFigure',hFig6);
            end
            
            if 0 == exist('LegendF0Basic','var')
                LegendF0Basic = cell(0,1);
            end
            
            LegendF0Basic{size(LegendF0Basic,1)+1,1} = sprintf('G0 Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
            BodePlotData.SetLegend(hFig6,LegendF0Basic)
            BodePlotData.SetTitle(hFig6,'Basic method of spa')
        end     
    end


    %% Barlett

    % Compute bode diagram by spectral analysis with Bartlett window
    [ Fw_mag, Fw_phase, F0_mag, F0_phase, f] = IdentSpectral( MeasurementData,'InterestingValues','Barlett', length_window_barlet);
    
    Fw_barlett = BodePlotData(Fw_mag, Fw_phase,f);
    F0_barlett = BodePlotData(F0_mag, F0_phase,f);
    
    if DispMyBarlettSpa == true
        % Gw
        if DispIndividCurv == true
            if 0 == exist('hFig101','var')
                hFig101 = Fw_barlett.Show('FreqUnit',FreqUnit);
            else
                Fw_barlett.Show('FreqUnit',FreqUnit,'HandleFigure',hFig101);
            end

            if 0 == exist('LegendAllMethodsGw','var')
                LegendAllMethodsGw = cell(0,1);
            end

            LegendAllMethodsGw{size(LegendAllMethodsGw,1) + 1,1} = sprintf('Gw - Bartlett - Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
            BodePlotData.SetLegend(hFig101,LegendAllMethodsGw)
            BodePlotData.SetTitle(hFig101,MeasurementData.CreateTitle('Closed loop'))
            
        else
            if 0 == exist('hFig3','var')
                hFig3 = Fw_barlett.Show('FreqUnit',FreqUnit);
            else
                Fw_barlett.Show('FreqUnit',FreqUnit,'HandleFigure',hFig3);
            end
            
            if 0 == exist('LegendFwBarlett','var')
                LegendFwBarlett = cell(0,1);
            end
            
            LegendFwBarlett{size(LegendFwBarlett,1)+1,1} = sprintf('Gw Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
            BodePlotData.SetLegend(hFig3,LegendFwBarlett)
            BodePlotData.SetTitle(hFig3,'My Barlet method of spa')
        end
        % G0
        if DispIndividCurv == true
            if 0 == exist('hFig102','var')
                hFig102 = F0_barlett.Show('FreqUnit',FreqUnit);
            else
                F0_barlett.Show('FreqUnit',FreqUnit,'HandleFigure',hFig102);
            end

            if 0 == exist('LegendAllMethodsG0','var')
                LegendAllMethodsG0 = cell(0,1);
            end

            LegendAllMethodsG0{size(LegendAllMethodsG0,1) + 1,1} = sprintf('G0 - Bartlett - Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
            BodePlotData.SetLegend(hFig102,LegendAllMethodsG0)
            BodePlotData.SetTitle(hFig102,MeasurementData.CreateTitle('Open loop'))
            
        else
            
            if 0 == exist('hFig4','var')
                hFig4 = F0_barlett.Show('FreqUnit',FreqUnit);
            else
                F0_barlett.Show('FreqUnit',FreqUnit,'HandleFigure',hFig4);
            end
            
            if 0 == exist('LegendF0Barlett','var')
                LegendF0Barlett = cell(0,1);
            end
            
            LegendF0Barlett{size(LegendF0Barlett,1)+1,1} = sprintf('G0 Ts=%3.1fms Samples = %d',MeasurementData.PeriodInputSignal*1000, MeasurementData.LengthInputSignal);
            BodePlotData.SetLegend(hFig4,LegendF0Barlett)
            BodePlotData.SetTitle(hFig4,'My Barlet method of spa')
        end
        
    end
 
% Original tf    
if bFromFile == false
    
    [mag,phase,wout] = bode(F); % closed loop
    Fw_origin = BodePlotData(reshape(mag,[1,size(mag,3)]), reshape(phase,[1,size(phase,3)]),wout'/2/pi);
    
    F0 = F/(1 - F);     % open loop
    [mag,phase,wout] = bode(F0,{0.1 1e3});
    F0_origin = BodePlotData(reshape(mag,[1,size(mag,3)]), reshape(phase,[1,size(phase,3)]),wout'/2/pi);
    
    if DispIndividCurv == false
        % Basic SPA
        if DispMyBasicSpa == true


            Fw_origin.Show('FreqUnit',FreqUnit,'HandleFigure',hFig5);
            BodePlotData.SetLegend(hFig5,{'G_{w_{est}}';'G_{w_{org}}'})
            figure(hFig5)
            axis([0.1 1e4 -200 50])        


            F0_origin.Show('FreqUnit',FreqUnit,'HandleFigure',hFig6);
            BodePlotData.SetLegend(hFig6,{'G_{0_{est}}';'G_{0_{org}}'})
            figure(hFig6)
            axis([0.1 1e4 -200 0])
        end

        % Bartlett SPA
        if DispMyBarlettSpa == true
            Fw_origin.Show('FreqUnit',FreqUnit,'HandleFigure',hFig3);
            BodePlotData.SetLegend(hFig3,{'G_{w_{est}}';'G_{w_{org}}'})
            figure(hFig3)
            axis([0.1 1e4 -200 50])

            F0_origin.Show('FreqUnit',FreqUnit,'HandleFigure',hFig4);
            BodePlotData.SetLegend(hFig4,{'G_{0_{est}}';'G_{0_{org}}'})
            figure(hFig4)
            axis([0.1 1e4 -200 0])
        end
    end
end


