function [flag] = postProcessing(robot, benchmarkSettings, experimentDataStruct)

% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot, Gordon Cheng
%%

% We start by loading the desired experiment data in proper data structures: 
initL2Error = zeros(1, benchmarkSettings.numberOfInitialEstimates);
finalL2Error = zeros(benchmarkSettings.numberOfInitialEstimates, benchmarkSettings.numberOfExperimentsPerInitialPoint, benchmarkSettings.nbAlg);
finalTime = zeros(benchmarkSettings.numberOfInitialEstimates, benchmarkSettings.numberOfExperimentsPerInitialPoint, benchmarkSettings.nbAlg);
iterConv = ones(benchmarkSettings.numberOfInitialEstimates, benchmarkSettings.numberOfExperimentsPerInitialPoint, benchmarkSettings.nbAlg);
modelRecalcConv = ones(benchmarkSettings.numberOfInitialEstimates, benchmarkSettings.numberOfExperimentsPerInitialPoint, benchmarkSettings.nbAlg);
paramError = zeros(benchmarkSettings.numberOfInitialEstimates, benchmarkSettings.numberOfExperimentsPerInitialPoint, benchmarkSettings.nbAlg, robot.paramVectorSize);
paramValue = zeros(benchmarkSettings.numberOfInitialEstimates, benchmarkSettings.numberOfExperimentsPerInitialPoint, benchmarkSettings.nbAlg, robot.paramVectorSize);
paramInitValue = zeros(benchmarkSettings.numberOfInitialEstimates, benchmarkSettings.numberOfExperimentsPerInitialPoint, robot.paramVectorSize);
decimRate = zeros(benchmarkSettings.nbAlg, 1);
averageTime = zeros(1, benchmarkSettings.nbAlg);
stdTime = zeros(1, benchmarkSettings.nbAlg);
averageIter = zeros(1, benchmarkSettings.nbAlg);
stdIter = zeros(1, benchmarkSettings.nbAlg);
averageError = zeros(1, benchmarkSettings.nbAlg);
convergence  = zeros(benchmarkSettings.numberOfInitialEstimates, benchmarkSettings.numberOfExperimentsPerInitialPoint, benchmarkSettings.nbAlg);
averageParamError = zeros(robot.paramVectorSize, benchmarkSettings.nbAlg);
stdParamError = zeros(robot.paramVectorSize, benchmarkSettings.nbAlg);
stdError = zeros(1, benchmarkSettings.nbAlg);

resFlag = false;
for index=1:benchmarkSettings.nbAlg
    % Loading data:
    if benchmarkSettings.identificationMethods.showResults(index)==1 % If the method is selected
        resFlag = true;
        resultFile = sprintf('%s_%s/decim_%d/results_%s_%s.mat', robot.name, benchmarkSettings.noiseLevel, benchmarkSettings.decimRate, benchmarkSettings.identificationMethods.algName{index}, robot.name)
        resultVariable = sprintf('results_%s',benchmarkSettings.identificationMethods.algName{index});
        if exist(fullfile(resultFile),'file') == 2
            resultDataStruct{index} = load(resultFile);
            Beta_obj = eval(sprintf('resultDataStruct{index}.%s.benchmarkSettings.Beta_obj',resultVariable));
            for i=1:benchmarkSettings.numberOfInitialEstimates
                
                initL2Error(i) = norm(eval(sprintf('resultDataStruct{index}.%s.benchmarkSettings.Initial_Beta(:, i)',resultVariable))-Beta_obj);
                for j=1:benchmarkSettings.numberOfExperimentsPerInitialPoint
                    theta = reshape(eval(sprintf('resultDataStruct{index}.%s.Betas(i, j, :)',resultVariable)), robot.paramVectorSize, 1);
                    finalTime(i, j, index) = eval(sprintf('resultDataStruct{index}.%s.times(i, j)',resultVariable));
                    
                    error = norm(Beta_obj-theta);
                    if (error>50*norm(Beta_obj-benchmarkSettings.Initial_Beta(:,i))) && ~strcmp(benchmarkSettings.identificationMethods.algName{index}, 'LS') % If final error is greater than the initial error
                        convergence(i,j,index) = 0;
                        finalL2Error(i, j, index) = 0;
                        paramError(i, j, index, :) = 0*(Beta_obj-theta);
                        paramValue(i, j, index, :) = theta;
                    else
                        convergence(i,j,index) = 1;
                        finalL2Error(i, j, index) = error;
                        paramError(i, j, index, :) = Beta_obj-theta;
                        paramValue(i, j, index, :) = theta;
                    end
                    paramInitValue(i, j, :) = eval(sprintf('resultDataStruct{index}.%s.benchmarkSettings.Initial_Beta(:, i)',resultVariable));
                    decimRate(index) = eval(sprintf('resultDataStruct{index}.%s.benchmarkSettings.decimRate',resultVariable));
                    if strcmp(benchmarkSettings.identificationMethods.algName{index}, 'DIDIM') || strcmp(benchmarkSettings.identificationMethods.algName{index}, 'IV')
                        iterConv(i, j, index) = eval(sprintf('resultDataStruct{index}.%s.iteration(i, j)',resultVariable));
                        modelRecalcConv(i, j, index) = eval(sprintf('resultDataStruct{index}.%s.iteration(i, j)',resultVariable));
                    end
                    if strcmp(benchmarkSettings.identificationMethods.algName{index}, 'PC_DIDIM') || strcmp(benchmarkSettings.identificationMethods.algName{index}, 'PC_IV')
                        iterConv(i, j, index) = eval(sprintf('resultDataStruct{index}.%s.iteration(i, j)',resultVariable));
                        modelRecalcConv(i, j, index) = eval(sprintf('resultDataStruct{index}.%s.iteration(i, j)',resultVariable));
                    end
                    if strcmp(benchmarkSettings.identificationMethods.algName{index}, 'CLOE') || strcmp(benchmarkSettings.identificationMethods.algName{index}, 'CLIE')
                        modelRecalcConv(i, j, index) = eval(sprintf('resultDataStruct{index}.%s.iteration(i, j).funcCount',resultVariable));
                        iterConv(i, j, index) = eval(sprintf('resultDataStruct{index}.%s.iteration(i, j).iterations',resultVariable));
                    end
                end
                for k=1:robot.paramVectorSize
                    stdParamError(k,index) =  stdParamError(k,index)+std(paramError(i, find(convergence(i,:,index)), index,k));
                end
            end
            
            
            
            %             if strcmp(benchmarkSettings.identificationMethods.algName{index}, 'EKF') || strcmp(benchmarkSettings.identificationMethods.algName{index}, 'UKF') || strcmp(benchmarkSettings.identificationMethods.algName{index}, 'SRUKF')|| strcmp(benchmarkSettings.identificationMethods.algName{index}, 'CDKF') || strcmp(benchmarkSettings.identificationMethods.algName{index}, 'SRCDKF')
            %                 paramEvolution(:,:,:,index) = eval(sprintf('resultDataStruct{index}.%s.Betas_iteration',resultVariable));
            %             end
            convIndex = find(convergence(:,:,index)); % index of the points which converged.
            fE = reshape(finalL2Error(:,:, index), benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1);
            fT(:,index) = reshape(finalTime(:,:, index), benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1);
            fI(:,index) = reshape(iterConv(:,:, index), benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1);
            fR(:,index) = reshape(modelRecalcConv(:,:, index), benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1);
            for i = 1:robot.paramVectorSize
                fEP(:,i, index) = reshape(paramError(:,:, index,i), benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1);
                fP(:,i, index) = reshape(paramValue(:,:, index,i), benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1);
            end
            
            averageIter(index) = mean(fI(convIndex));
            averageTime(index) = mean(fT(convIndex, index));
            averageError(index) = mean(fE(convIndex));
            stdError(index) = std(reshape(fE(convIndex), numel(fE(convIndex)), 1));
            stdTime(index) = std(reshape(finalTime(:, :, index), numel(finalTime(:, :, index)), 1));
            stdIter(index) = std(reshape(iterConv(:, :, index), numel(iterConv(:, :, index)), 1));
            
            averageParamError(:,index) = mean(fEP(convIndex,:, index),1);
            averageParam(:,index) = mean(fP(convIndex,:, index),1);
            %             errorFunction(robot, benchmarkSettings, experimentDataStruct, averageParam(:,index), index, i)
            for i = 1:robot.paramVectorSize
                ss = reshape(eval(sprintf('resultDataStruct{index}.%s.Betas(1:benchmarkSettings.numberOfInitialEstimates, 1:benchmarkSettings.numberOfExperimentsPerInitialPoint, i)',resultVariable)),benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1);
                stdParamError(i,index) =  std(ss(convIndex));
            end
            %             if strcmp(benchmarkSettings.identificationMethods.algName{index}, 'EKF') || strcmp(benchmarkSettings.identificationMethods.algName{index}, 'UKF') || strcmp(benchmarkSettings.identificationMethods.algName{index}, 'SRUKF')|| strcmp(benchmarkSettings.identificationMethods.algName{index}, 'CDKF') || strcmp(benchmarkSettings.identificationMethods.algName{index}, 'SRCDKF')
            %                 getCovarianceVideo(eval(sprintf('resultDataStruct{index}.%s.Covariance_iteration',resultVariable)), benchmarkSettings.identificationMethods.algName{index}, 100, false)
            %             end
        else
            resultDataStruct{index} = 0;
        end
    end
end




%% Run result analysis tools:
if resFlag == true
    methods = categorical(benchmarkSettings.identificationMethods.algName);
    methods = reordercats(methods,benchmarkSettings.identificationMethods.algName);
    
    
    % generation of a tabular with average and std parameter errors for latex
% generateLaTeXparamTab(robot,  benchmarkSettings, resultDataStruct, averageParamError, stdParamError)
% generateLaTeXparamTabPercent(robot,  benchmarkSettings, resultDataStruct, averageParamError, stdParamError)
% generateLaTeXparamTabPercent2(robot,  benchmarkSettings, resultDataStruct, averageParamError, stdParamError)
% % % % % generateLaTeXFOMTab(robot,  benchmarkSettings, experimentDataStruct, fP, fEP, fT, fI, fR, averageParam, stdParamError, averageTime, averageIter)
%generateLaTeXFOMTabTot(robot,  benchmarkSettings, experimentDataStruct, fP, fEP, fT, fI, fR, averageParam, stdParamError, averageTime, averageIter)
% generateReport(robot,  benchmarkSettings, experimentDataStruct, fP, fEP, fT, fI, fR, averageParam, averageParamError, stdParamError, averageTime, averageIter);
    %     plotRecalculatedClosedLoopDDM(robot, benchmarkSettings, resultDataStruct, experimentDataStruct);
    %     plotRawResults(benchmarkSettings, fEP, fT);
    %     plotRawResultsHistogram(benchmarkSettings, fEP, fT);
    %     plotRawResultsParamwise(robot, benchmarkSettings, fEP, fT)
    %     plot3DMeanResults(robot, benchmarkSettings, averageParamError);
    %         plotSummary(benchmarkSettings, averageTime, averageIter, averageError, stdError, methods);
%             plotConvergenceStatus(robot, benchmarkSettings, convergence, decimRate);
    %         plotParamwiseHoriz(robot, benchmarkSettings, averageParamError, stdParamError);
%             plotParamwiseVert(robot, benchmarkSettings, averageParamError, stdParamError);

%     plotRecalculatedTorquesFull(robot, benchmarkSettings, experimentDataStruct, averageParam, false);
    
    
% % % % % % % %         plotParamwiseVertNew(robot, benchmarkSettings, averageParamError, stdParamError, decimRate);
    %
    %     for i=1:robot.nbDOF
    %         plotRecalculatedTorques(robot, benchmarkSettings, Q, Qp, Qpp, Tau, t, averageParam, i);
    %     end
    %
%             plotErrorHistogram(robot, benchmarkSettings, experimentDataStruct, decimRate, averageParam, stdParamError);
    %
%         checkStatHypotheses(robot, benchmarkSettings, experimentDataStruct, decimRate, averageParam, stdParamError);
    
plotParamwiseMatrix(robot, benchmarkSettings, averageParamError, stdParamError, decimRate);
    
    %
    %     %% Parameter Convergence Kalman
    %     cc=parula(robot.paramVectorSize);
    %     index = find(benchmarkSettings.identificationMethods.showResults);
    %     counter = 0;
    %     indexK = [];
    %     for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    %         if strcmp(benchmarkSettings.identificationMethods.algName{index(i)}, 'EKF') || strcmp(benchmarkSettings.identificationMethods.algName{index(i)}, 'UKF') || strcmp(benchmarkSettings.identificationMethods.algName{index(i)}, 'SRUKF')|| strcmp(benchmarkSettings.identificationMethods.algName{index(i)}, 'CDKF') || strcmp(benchmarkSettings.identificationMethods.algName{index(i)}, 'SRCDKF')
    %             counter = counter+1;
    %             indexK(counter) = index(i);
    %         end
    %         if counter == 1
    %             figure('Name','Parameter Convergence')
    %         end
    %         for i=1:counter
    %             subplot(counter,1,i)
    %             for j=1:robot.paramVectorSize
    %                 plot(paramEvolution(j,:,1200,indexK(i))','LineWidth',2 ,'color',cc(j,:))
    %                 hold on
    %                 plot(repmat(benchmarkSettings.Beta_obj(j),1,size(paramEvolution,2))','--','LineWidth',2,'color',cc(j,:))
    %             end
    %             title(sprintf('\\textbf{Convergence} \\boldmath{$%s$}', benchmarkSettings.identificationMethods.algName{indexK(i)}), 'Interpreter', 'LaTeX')
    %         end
    %     end
    %
end
end


%%%%%%%%%%%%%%%%%%%%%%%
%% generateReport:
%%%%%%%%%%%%%%%%%%%%%%%


% mkdir(sprintf('%s/%s_%s/decim%d/paramError',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i))))

function []=generateReport(robot,  benchmarkSettings, experimentDataStruct, fP, fEP, fT, fI, fR, averageParam, averageParamError, stdParamError, averageTime, averageIter)

if strcmp(robot.name, 'TX40_uncoupled')
    name = 'TX40';
else
    name = robot.name;
end


% Import the DOM and Report API packages so you do not have to use long class names.
import mlreportgen.report.*;
import mlreportgen.dom.*;

imgStyle = {ScaleToFit(true)};

% Create a container to hold the report content.
% To create a Word report, change the output type from "pdf" to "docx".
% rpt = mlreportgen.report.Report("PortraitAndLandscapeReport", "pdf");
rpt = Report(sprintf('%s_%s_decim_%d',robot.name, benchmarkSettings.noiseLevel, benchmarkSettings.decimRate),"pdf");
open(rpt);

% Specify the page orientation, height, and width.
pageLayoutObj.PageSize.Orientation = "portrait";
pageLayoutObj.PageSize.Height = "297mm";
pageLayoutObj.PageSize.Width = "210mm";

% Specify the page margins.
pageLayoutObj.PageMargins.Top = "0cm";
pageLayoutObj.PageMargins.Bottom = "0cm";
pageLayoutObj.PageMargins.Left = "0cm";
pageLayoutObj.PageMargins.Right = "0cm";

pageLayoutObj.PageMargins.Header = "0cm";
pageLayoutObj.PageMargins.Footer = "0cm";

% Add the page layout object to the report.
add(rpt,pageLayoutObj);



% Genrate a table in LaTeX format, containing Figures of Merit (FOM) for
% the set of considered identification methods
N_fom = 5;
options.filter = 'butterworth';
Tau_decim = [];
Qpp_decim = [];
Qp_decim = [];
Q_decim = [];

for expNb = 1:1%benchmarkSettings.numberOfExperimentsPerInitialPoint % Concatenating the experiment data
    %     [~, ~, ~, ~, ~, ~, Tau_decim_exp, Qpp_decim_exp, Qp_decim_exp, Q_decim_exp, ~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb,[]);
    [~, Tau_decim_exp, Qpp_decim_exp, Qp_decim_exp, Q_decim_exp, ~, ~,~,~,~,~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb,[]);
    Tau_decim = [Tau_decim Tau_decim_exp];
    Qpp_decim = [Qpp_decim Qpp_decim_exp];
    Qp_decim = [Qp_decim Qp_decim_exp];
    Q_decim = [Q_decim Q_decim_exp];
end

% Generation of the tabularcontatinning the average error and standard deviation of the error.
index = find(benchmarkSettings.identificationMethods.showResults);
% file = fopen(sprintf('%s/%s_%s/decim%d/LaTeX/FOMtab.tex',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, benchmarkSettings.decimRate(index(1))), 'w');


%
% fprintf(file, "\\cellcolor{green!45}\\textbf{%s, MCS, decim %d} & \\cellcolor{gray!25}$E(d_{\\boldsymbol{q}}), \\sigma_{\\boldsymbol{q}}$ & \\cellcolor{gray!25}$E(d_{\\boldsymbol{\\tau}}), \\sigma_{\\boldsymbol{\\tau}}$& \\cellcolor{gray!25}$E(d_{t}), \\sigma_{t}$  &\\cellcolor{gray!25} $E(d_{N_{it}}), \\sigma_{N_{it}}$ & \\cellcolor{gray!25}$E(d_{N_{sim}}), \\sigma_{N_{sim}}$ \\\\ \n",name ,benchmarkSettings.decimRate(index(1)));
% fprintf(file, "\\midrule\n");

for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    nameAlgo = getNameId(benchmarkSettings,index,i)
    % Compute relative torque error:
    [~,nbSamples]=size(Tau_decim);
    W = observationMatrixOrdered(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q_decim, Qp_decim, Qpp_decim);
    %     Y_tau_ref = W*benchmarkSettings.Beta_obj;          % Recomputed torque with real parameter values
    Y_tau_ref = torqueVectorOrdered(Tau_decim);
    Y_tau_recomputed = W*averageParam(:,index(i));
    
    Y_error_tau = Y_tau_ref - Y_tau_recomputed;   % Recomputed torque with mean value of the estimated parameters
    
    Y_error_tau_joint=zeros(nbSamples,robot.nbDOF);
    Y_tau_joint=zeros(nbSamples,robot.nbDOF);
    Y_tau_recomputed_joint=zeros(nbSamples,robot.nbDOF);
    for jointNb = 1:robot.nbDOF
        Y_error_tau_joint(:,jointNb) = Y_error_tau((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        Y_tau_joint(:,jointNb) = Y_tau_ref((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        Y_tau_recomputed_joint(:,jointNb) = Y_tau_recomputed((jointNb-1)*nbSamples+1:jointNb*nbSamples);
    end
    
    Y_error_tau_med = sqrt(sum(Y_error_tau_joint.^2,2));
    Y_tau_med = sqrt(sum(Y_tau_joint.^2,2));
    d_tau(i,1) = 100*mean(Y_error_tau_med./Y_tau_med);
    sigma_d_tau(i,1) = 100*std(Y_error_tau_med./Y_tau_med);
    
    % Compute relative joint position error:
    t_control = linspace(benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples);  % Control epochs
    augmentedDesiredState = benchmarkSettings.trajectoryData.getTrajectoryData(t_control, benchmarkSettings.interpolationAlgorithm); % augmentedState = [Qpp; Qp; Q];
    [~, stateVector, ~] = integrateClosedLoopDynamics_mex(augmentedDesiredState, averageParam(:,index(i)), robot.name, robot.numericalParameters.Geometry, ...
        robot.numericalParameters.Gravity, benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples, benchmarkSettings.nbSamples, ...
        robot.controlParameters.Kp, robot.controlParameters.Ki, robot.controlParameters.Kd, robot.controlParameters.Ktau, robot.controlParameters.antiWindup, ...
        robot.physicalConstraints.limQ_L, robot.physicalConstraints.limQ_U, robot.physicalConstraints.limQp_L, robot.physicalConstraints.limQp_U, ...
        robot.physicalConstraints.limQpp_L, robot.physicalConstraints.limQpp_U, robot.physicalConstraints.limTau_L, robot.physicalConstraints.limTau_U, benchmarkSettings.integrationAlgorithm);
    Q_sim = stateVector(robot.nbDOF+1:2*robot.nbDOF,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder);
    Qp_sim = stateVector(1:robot.nbDOF,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder);
    
    t_sample = linspace(benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbSamples);  % Control epochs
    stateVectorDesired = benchmarkSettings.trajectoryData.getTrajectoryData(t_sample, benchmarkSettings.interpolationAlgorithm); % augmentedState = [Qpp; Qp; Q];
    Q_sim_decim = [];
    Qp_sim_decim = [];
    Qd_decim = [];
    Qpd_decim = [];

    for jointNb = 1:robot.nbDOF
        Q_sim_decim(jointNb,:) = decimate(Q_sim(jointNb,:),benchmarkSettings.decimRate/benchmarkSettings.decimRate);
        Qp_sim_decim(jointNb,:) = decimate(Qp_sim(jointNb,:),benchmarkSettings.decimRate/benchmarkSettings.decimRate);
        Qd_decim(jointNb,:) = decimate(stateVectorDesired(2*robot.nbDOF + jointNb,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder),benchmarkSettings.decimRate/benchmarkSettings.decimRate);
        Qpd_decim(jointNb,:) = decimate(stateVectorDesired(robot.nbDOF + jointNb,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder),benchmarkSettings.decimRate/benchmarkSettings.decimRate);
    end
    
    
    
    Y_error_Q = torqueVectorOrdered(Q_decim-Q_sim_decim);
    Y_Q_ref = torqueVectorOrdered(Q_decim);
    for jointNb = 1:robot.nbDOF
        Y_error_Q_joint(:,jointNb) = Y_error_Q((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        Y_Q_joint(:,jointNb) = Y_Q_ref((jointNb-1)*nbSamples+1:jointNb*nbSamples);
    end
    Y_error_Q_med = sqrt(sum(Y_error_Q_joint.^2,2));
    Y_Q_med = sqrt(sum(Y_Q_joint.^2,2));
    d_q(i,1) = 100*mean(Y_error_Q_med./Y_Q_med);
    sigma_d_q(i,1) = 100*std(Y_error_Q_med./Y_Q_med);
    
    % Compute average parameter error:
    paramRef = repmat(benchmarkSettings.Beta_obj.',benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1);
    relativeAverageParamErrorsMC = (2/robot.paramVectorSize)*sum(abs(fEP(:,:,index(i)))./(abs(paramRef)+abs(fP(:,:,index(i)))),2); % series of d_\beta for each MC experiment
    d_beta(i,1) = 100*mean(relativeAverageParamErrorsMC);
    sigma_d_beta(i,1) = 100*std(relativeAverageParamErrorsMC);
    
    % Get the mean time required to compute one parameter estimate:
    d_t(i,1) = mean(fT(:,index(i)));
    sigma_d_t(i,1) = 100*std(fT(:,index(i)))/mean(fT(:,index(i)));
    
    % If applicable, get the number of iterations required to compute one
    % parameter estimate:
    d_i(i,1) = mean(fI(:,index(i)));
    sigma_d_i(i,1) = 100*std(fI(:,index(i)))/mean(fI(:,index(i)));
    
    % If applicable, get the number of model recalculations required to compute one
    % parameter estimate:
    d_r(i,1) = mean(fR(:,index(i)));
    if mean(fR(:,index(i)))~=0
        sigma_d_r(i,1) = 100*std(fR(:,index(i)))/mean(fR(:,index(i)));
    else
        sigma_d_r(i,1) = -1;
    end
    
    %     mean_fom(1,i) = d_beta(i);
    mean_fom(1,i) = d_q(i);
    mean_fom(2,i) = d_tau(i);
    mean_fom(3,i) = d_t(i);
    mean_fom(4,i) = d_i(i);
    mean_fom(5,i) = d_r(i);
    
    %     unit{1} = '\%';
    unit{1} = '\%';
    unit{2} = '\%';
    unit{3} = 's';
    unit{4} = '';
    unit{5} = '';
    
    %     sigma_fom(1,i) = sigma_d_beta(i);
    sigma_fom(1,i) = sigma_d_q(i);
    sigma_fom(2,i) = sigma_d_tau(i);
    sigma_fom(3,i) = sigma_d_t(i);
    sigma_fom(4,i) = sigma_d_i(i);
    sigma_fom(5,i) = sigma_d_r(i);
    
    Algorithm{i,1} = getNameId(benchmarkSettings,index,i);
    
    
    %     fprintf(file, "\\textbf{%s}", nameAlg);
    %
    %     for j = 1:N_fom
    %         if j == N_fom
    %             if ~strcmp(nameAlg,'CLIE') && ~strcmp(nameAlg,'CLOE') ~= ~strcmp(nameAlg,'DIDIM') ~= ~strcmp(nameAlg,'IDIM-IV') ~= ~strcmp(nameAlg,'PC-IDIM-IV') ~= ~strcmp(nameAlg,'PC-DIDIM')
    %                 fprintf(file, " & N.A.");
    %             else
    %                 fprintf(file, " & %5.2f%s (%1.2f\\%%)", mean_fom(j,i), unit{j}, sigma_fom(j,i));
    %             end
    %         elseif j == 1
    %             fprintf(file, " & %5.3f%s (%1.2f\\%%)", mean_fom(j,i), unit{j}, sigma_fom(j,i));
    %         else
    %             fprintf(file, " & %5.2f%s (%1.2f\\%%)", mean_fom(j,i), unit{j}, sigma_fom(j,i));
    %         end
    %
    %     end
    
    %     fprintf(file, "\\\\  \n");
end

% % mltable = table(sigma_d_q,sigma_d_tau,sigma_d_t, sigma_d_i, sigma_d_r);
% mltable = table(Algorithm, d_q, d_tau, d_t, d_i, d_r);
% 
% mltableObj = MATLABTable(mltable);
% tbodyObj = mltableObj.Body;
% tbodyObj.TableEntriesStyle = {Color('blue')};
% tbODYObj.TableEntriesHAlign = 'center';
% 
% add(chapter,mltableObj)


% fclose(file);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

options.filter = 'butterworth';
set(0, 'DefaultFigureRenderer', 'painters');

for i=1:sum(benchmarkSettings.identificationMethods.showResults)

    chapter = Chapter("Title", sprintf('%s, %s, %s, decimation rate %d',name, benchmarkSettings.identificationMethods.algName{index(i)}, benchmarkSettings.noiseLevel, benchmarkSettings.decimRate));
    section = Section("Title", "Parameter Errors");
    
    barh(1:robot.paramVectorSize, averageParamError(:,index(i)), 'FaceColor', [0 .39 .74], 'EdgeColor', [0 .59 .94], 'FaceAlpha',0.75);
    hold on
    errorbar(averageParamError(:,index(i)),1:robot.paramVectorSize, stdParamError(:,index(i)),'horizontal','r.','LineWidth',0.5)
    hold off
    grid on
    grid minor
    ylabel('Index');
    xlabel('Parameter Error');
    xlim([-0.5 0.5])
    x=0.5;
    nameAlgo = getNameId(benchmarkSettings,index,i);
    text(0.4*(-ones(1,robot.paramVectorSize)).^(0:robot.paramVectorSize-1), linspace(1-x,robot.paramVectorSize-x,robot.paramVectorSize),strcat('err(',strcat(num2str(linspace(1,robot.paramVectorSize,robot.paramVectorSize).','%d\n'),strcat(')=',strcat(strcat(num2str(averageParamError(:,index(i)),'%5.3f\n'),'±'),num2str(stdParamError(:,index(i)),'%5.3f\n'))))),'vert','bottom','horiz','center','Rotation',0,'FontSize', 8);
    title(sprintf('Parameter Identification %s %s', name, nameAlgo))
    legend({'mean', 'std'},'Location','southoutside','Orientation','horizontal')
    set(gcf,'OuterPosition',[0 0 960 540]);
    pbaspect([2 1 1])
    figMeanErrorVar=Figure();
    figMeanErrorVarImg = Image(getSnapshotImage(figMeanErrorVar, rpt));
    figMeanErrorVarImg.Style = imgStyle;
    
    % Add the table to the chapter
    add(section,figMeanErrorVarImg)
    add(chapter, section)
    
    
    Tau_ndecim = [];
    Qpp_ndecim = [];
    Qp_ndecim = [];
    Q_ndecim = [];
    
    for expNb = 1:1%benchmarkSettings.numberOfExperimentsPerInitialPoint % Concatenating the experiment data
        [t_ndecim, Tau_ndecim_exp, Qpp_ndecim_exp, Qp_ndecim_exp, Q_ndecim_exp, ~, ~, ~, ~, ~, ~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb,[]);
        Tau_ndecim = [Tau_ndecim Tau_ndecim_exp];
        Qpp_ndecim = [Qpp_ndecim Qpp_ndecim_exp];
        Qp_ndecim = [Qp_ndecim Qp_ndecim_exp];
        Q_ndecim = [Q_ndecim Q_ndecim_exp];
    end
    
    % Compute the observation matrix:
    W = observationMatrix(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q_ndecim, Qp_ndecim, Qpp_ndecim);
    %     num = [   1,    0.0781,    0.2344;... % Crimson Red
    %      0,    0.9000,         0.1;... % Real Green
    %          0.1172,    0.5625,    1.0000;... % Deep Sky Blue
    %          1.0000,         0,    1.0000;... % Magenta
    %          0,    1.0000,    1.0000;... % Cyan
    %          0.8000,    0.2695,         0;... % Orange
    %          0,    1.0000,    0.25000;... % Light Green
    %          0,         0,    0.5000;... % Navy Blue
    %          0.5000,         0,    0.5000;... % Purple
    %          0.5391,    0.1680,    0.8828;... % BlueViolet
    %          0,         0,    1.0000;... % Blue
    %          0.5000,    0.5000,    0.5000];   % Gray
    %
    %      set(groot,'defaultAxesColorOrder',num)
    section = Section("Title", "Recomputed torques");
    for jointNb = 1:robot.nbDOF
        referenceTau = W*benchmarkSettings.Beta_obj;
        graph = plot(t_ndecim, Tau_ndecim(jointNb,:),'Color',[0,         0,    0.5000],'LineWidth',1);
        graph.Color(4)=0.1;
        hold on
        plot(t_ndecim,referenceTau(jointNb:robot.nbDOF:end),'Color',[0,    0.9000,         0.1], 'LineWidth',2);
        hold on
        recomputedTau = W*averageParam(:,index(i));
        plot(t_ndecim, recomputedTau(jointNb:robot.nbDOF:end),'Color',[1,    0.0781,    0.2344],'LineWidth',1);
        hold off
        xlabel('Time [s]');
        ylabel(sprintf('Torque [N.m]'));
        title(sprintf('Recomputed torque %s %s, joint %d',name, nameAlgo, jointNb))
        grid on
        grid minor
        if benchmarkSettings.experimentOnTrueRobot == true
            legend({'Real','CAD Parameters',sprintf('%s',nameAlgo)},'Location','southoutside','Orientation','horizontal')
        else
            legend({'Real','Reference',sprintf('%s',nameAlgo)},'Location','southoutside','Orientation','horizontal')
        end
        set(gcf,'OuterPosition',[0 0 960 540]);
        
        figTorqueError=Figure();
        figTorqueErrorImg = Image(getSnapshotImage(figTorqueError, rpt));
        figTorqueErrorImg.Style = imgStyle;
        % Add the figure to the chapter
        add(section,figTorqueErrorImg)
        
    end
    add(chapter,section)
%     section = Section("Title", "Recomputed velocities");
%     for jointNb = 1:robot.nbDOF
%         plot(t_decim, Qpd_decim(jointNb,:),'--','Color',[0.1172, 0.5625, 1.0000],'LineWidth',2);
%         hold on
%         plot(t_decim, Qp_decim(jointNb,:),'Color',[0, 0,    0.5000],'LineWidth',1.5);
%         hold on
%         plot(t_decim,Qp_sim_decim(jointNb,:),'Color',[0,    0.9000,         0.1], 'LineWidth',1);
%         hold on
%         plot(t_decim,100*(Qp_decim(jointNb,:)-Qp_sim_decim(jointNb,:)),'Color',[1,    0.0781,    0.2344], 'LineWidth',1);
%         hold off
%         xlabel('Time [s]');
%         ylabel(sprintf('Joint velocity [rad/s]'));
%         title(sprintf('Joint velocity %s joint %d in closed loop with the %s estimates',name, jointNb, nameAlgo))
%         grid on
%         grid minor
%         legend({'Desired','Real',sprintf('Recomputed %s',nameAlgo), 'Error: 100 x (Real - Recomputed)'},'Location','southoutside','Orientation','horizontal')
%         set(gcf,'OuterPosition',[0 0 960 540]);
%         
%         figQpError=Figure();
%         figQpErrorImg = Image(getSnapshotImage(figQpError, rpt));
%         figQpErrorImg.Style = imgStyle;
%         % Add the figure to the chapter
%         add(section,figQpErrorImg)
%     end
%     add(chapter,section)
%     section = Section("Title", "Recomputed positions");
%     for jointNb = 1:robot.nbDOF
%         plot(t_decim, Qd_decim(jointNb,:),'--','Color',[0.1172, 0.5625, 1.0000],'LineWidth',2);
%         hold on
%         plot(t_decim, Q_decim(jointNb,:),'Color',[0, 0,    0.5000],'LineWidth',1.5);
%         hold on
%         plot(t_decim,Q_sim_decim(jointNb,:),'Color',[0,    0.9000,         0.1], 'LineWidth',1);
%         hold on
%         plot(t_decim,100*(Q_decim(jointNb,:)-Q_sim_decim(jointNb,:)),'Color',[1,    0.0781,    0.2344], 'LineWidth',1);
%         hold off
%         xlabel('Time [s]');
%         ylabel(sprintf('Joint position [rad]'));
%         title(sprintf('Joint position %s joint %d in closed loop with the %s estimates',name, jointNb, nameAlgo))
%         grid on
%         grid minor
%         legend({'Desired','Real',sprintf('Recomputed %s',nameAlgo), 'Error: 100 x (Real - Recomputed)'},'Location','southoutside','Orientation','horizontal')
%         set(gcf,'OuterPosition',[0 0 960 540]);
%         
%         figQpError=Figure();
%         figQpErrorImg = Image(getSnapshotImage(figQpError, rpt));
%         figQpErrorImg.Style = imgStyle;
%         % Add the figure to the chapter
%         add(section,figQpErrorImg)
%     end
%     add(chapter,section)
   
    
    % Add the chapter to the report
    add(rpt, chapter);
    delete(gcf);
end

% Generate and display the report
close(rpt);
rptview(rpt);

end

%%%%%%%%%%%%%%%%%%%%%%%
%% generateLaTeXFOMTab:
%%%%%%%%%%%%%%%%%%%%%%%

function []=generateLaTeXFOMTab(robot,  benchmarkSettings, experimentDataStruct, fP, fEP, fT, fI, fR, averageParam, stdParamError, averageTime, averageIter)

% Genrate a table in LaTeX format, containing Figures of Merit (FOM) for
% the set of considered identification methods
N_fom = 5;
options.filter = 'butterworth';
Tau_decim = [];
Qpp_decim = [];
Qp_decim = [];
Q_decim = [];

for expNb = 1:1%benchmarkSettings.numberOfExperimentsPerInitialPoint % Concatenating the experiment data
    %     [~, ~, ~, ~, ~, ~, Tau_decim_exp, Qpp_decim_exp, Qp_decim_exp, Q_decim_exp, ~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb,[]);
    [~, Tau_decim_exp, Qpp_decim_exp, Qp_decim_exp, Q_decim_exp, ~, ~,~,~,~,~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb,[]);
    Tau_decim = [Tau_decim Tau_decim_exp];
    Qpp_decim = [Qpp_decim Qpp_decim_exp];
    Qp_decim = [Qp_decim Qp_decim_exp];
    Q_decim = [Q_decim Q_decim_exp];
end

% Generation of the tabularcontatinning the average error and standard deviation of the error.
index = find(benchmarkSettings.identificationMethods.showResults);
file = fopen(sprintf('%s/%s_%s/decim%d/LaTeX/FOMtab.tex',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, benchmarkSettings.decimRate(index(1))), 'w');


if strcmp(robot.name, "TX40_uncoupled")
    name = "TX40";
else
    name = robot.name;
end

fprintf(file, "\\cellcolor{green!45}\\textbf{%s, MCS, decim %d} & \\cellcolor{gray!25}$E(d_{\\boldsymbol{q}}), \\sigma_{\\boldsymbol{q}}$ & \\cellcolor{gray!25}$E(d_{\\boldsymbol{\\tau}}), \\sigma_{\\boldsymbol{\\tau}}$& \\cellcolor{gray!25}$E(d_{t}), \\sigma_{t}$  &\\cellcolor{gray!25} $E(d_{N_{it}}), \\sigma_{N_{it}}$ & \\cellcolor{gray!25}$E(d_{N_{sim}}), \\sigma_{N_{sim}}$ \\\\ \n",name ,benchmarkSettings.decimRate(index(1)));
fprintf(file, "\\midrule\n");
            
for i=1:sum(benchmarkSettings.identificationMethods.showResults)

    % Compute relative torque error:
    [~,nbSamples]=size(Tau_decim);
    W = observationMatrixOrdered(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q_decim, Qp_decim, Qpp_decim);
%     Y_tau_ref = W*benchmarkSettings.Beta_obj;          % Recomputed torque with real parameter values
    Y_tau_ref = torqueVectorOrdered(Tau_decim);
    Y_tau_recomputed = W*averageParam(:,index(i));  
    
    Y_error_tau = Y_tau_ref - Y_tau_recomputed;   % Recomputed torque with mean value of the estimated parameters
      
    Y_error_tau_joint=zeros(nbSamples,robot.nbDOF);    
    Y_tau_joint=zeros(nbSamples,robot.nbDOF);
    Y_tau_recomputed_joint=zeros(nbSamples,robot.nbDOF);
    for jointNb = 1:robot.nbDOF
        Y_error_tau_joint(:,jointNb) = Y_error_tau((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        Y_tau_joint(:,jointNb) = Y_tau_ref((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        Y_tau_recomputed_joint(:,jointNb) = Y_tau_recomputed((jointNb-1)*nbSamples+1:jointNb*nbSamples);
    end
    figure()
    plot(Y_tau_joint(:,1),'r')
    hold on
    plot(Y_tau_recomputed_joint(:,1),'b')
    
    figure()
    plot(Y_tau_joint(:,2),'r')
    hold on
    plot(Y_tau_recomputed_joint(:,2),'b')
    
    figure()
    plot(Y_tau_joint(:,3),'r')
    hold on
    plot(Y_tau_recomputed_joint(:,3),'b')
    
    figure()
    plot(Y_tau_joint(:,4),'r')
    hold on
    plot(Y_tau_recomputed_joint(:,4),'b')
    
    figure()
    plot(Y_tau_joint(:,5),'r')
    hold on
    plot(Y_tau_recomputed_joint(:,5),'b')
    
    figure()
    plot(Y_tau_joint(:,6),'r')
    hold on
    plot(Y_tau_recomputed_joint(:,6),'b')
    
    pause
    
    Y_error_tau_med = sqrt(sum(Y_error_tau_joint.^2,2));
    Y_tau_med = sqrt(sum(Y_tau_joint.^2,2));
    d_tau(i) = 100*mean(Y_error_tau_med./Y_tau_med);
    sigma_d_tau(i) = 100*std(Y_error_tau_med./Y_tau_med);
    
    % Compute relative joint position error:
    t_control = linspace(benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples);  % Control epochs
    augmentedDesiredState = benchmarkSettings.trajectoryData.getTrajectoryData(t_control, benchmarkSettings.interpolationAlgorithm); % augmentedState = [Qpp; Qp; Q];
    [~, stateVector, ~] = integrateClosedLoopDynamics_mex(augmentedDesiredState, averageParam(:,index(i)), robot.name, robot.numericalParameters.Geometry, ...
        robot.numericalParameters.Gravity, benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples, benchmarkSettings.nbSamples, ...
        robot.controlParameters.Kp, robot.controlParameters.Ki, robot.controlParameters.Kd, robot.controlParameters.Ktau, robot.controlParameters.antiWindup, ...
        robot.physicalConstraints.limQ_L, robot.physicalConstraints.limQ_U, robot.physicalConstraints.limQp_L, robot.physicalConstraints.limQp_U, ...
        robot.physicalConstraints.limQpp_L, robot.physicalConstraints.limQpp_U, robot.physicalConstraints.limTau_L, robot.physicalConstraints.limTau_U, benchmarkSettings.integrationAlgorithm);
    Q_sim = stateVector(robot.nbDOF+1:2*robot.nbDOF,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder);
    Qp_sim = stateVector(1:robot.nbDOF,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder);
    
    Q_sim_decim = [];
    Qp_sim_decim = [];
    for jointNb = 1:robot.nbDOF
        Q_sim_decim(jointNb,:) = decimate(Q_sim(jointNb,:),benchmarkSettings.decimRate/benchmarkSettings.decimRate);
        Qp_sim_decim(jointNb,:) = decimate(Qp_sim(jointNb,:),benchmarkSettings.decimRate/benchmarkSettings.decimRate);
    end
    
%     figure()
%     plot(Q_sim_decim.','b')
%     hold on
%     plot(Q_decim.','r')
%     
%     figure()
%     plot(Qp_sim_decim.','b')
%     hold on
%     plot(Qp_decim.','r')
    
    Y_error_Q = torqueVectorOrdered(Q_decim-Q_sim_decim);
    Y_Q_ref = torqueVectorOrdered(Q_decim);
    for jointNb = 1:robot.nbDOF
        Y_error_Q_joint(:,jointNb) = Y_error_Q((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        Y_Q_joint(:,jointNb) = Y_Q_ref((jointNb-1)*nbSamples+1:jointNb*nbSamples);
    end
    Y_error_Q_med = sqrt(sum(Y_error_Q_joint.^2,2));
    Y_Q_med = sqrt(sum(Y_Q_joint.^2,2));
    d_q(i) = 100*mean(Y_error_Q_med./Y_Q_med);
    sigma_d_q(i) = 100*std(Y_error_Q_med./Y_Q_med);
    
    % Compute average parameter error:
    paramRef = repmat(benchmarkSettings.Beta_obj.',benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1);
    relativeAverageParamErrorsMC = (2/robot.paramVectorSize)*sum(abs(fEP(:,:,index(i)))./(abs(paramRef)+abs(fP(:,:,index(i)))),2); % series of d_\beta for each MC experiment
    d_beta(i) = 100*mean(relativeAverageParamErrorsMC);
    sigma_d_beta(i) = 100*std(relativeAverageParamErrorsMC);
    
    % Get the mean time required to compute one parameter estimate:
    d_t(i) = mean(fT(:,index(i)));
    sigma_d_t(i) = 100*std(fT(:,index(i)))/mean(fT(:,index(i)));
    
    % If applicable, get the number of iterations required to compute one
    % parameter estimate:
    d_i(i) = mean(fI(:,index(i)));
    sigma_d_i(i) = 100*std(fI(:,index(i)))/mean(fI(:,index(i)));
    
    % If applicable, get the number of model recalculations required to compute one
    % parameter estimate:
    d_r(i) = mean(fR(:,index(i)));
    if mean(fR(:,index(i)))~=0
        sigma_d_r(i) = 100*std(fR(:,index(i)))/mean(fR(:,index(i)));
    else
        sigma_d_r(i) = -1;
    end
    
%     mean_fom(1,i) = d_beta(i);
    mean_fom(1,i) = d_q(i);
    mean_fom(2,i) = d_tau(i);
    mean_fom(3,i) = d_t(i);
    mean_fom(4,i) = d_i(i);
    mean_fom(5,i) = d_r(i);
    
%     unit{1} = '\%';
    unit{1} = '\%';
    unit{2} = '\%';
    unit{3} = 's';
    unit{4} = '';
    unit{5} = '';
    
%     sigma_fom(1,i) = sigma_d_beta(i);
    sigma_fom(1,i) = sigma_d_q(i);
    sigma_fom(2,i) = sigma_d_tau(i);
    sigma_fom(3,i) = sigma_d_t(i);
    sigma_fom(4,i) = sigma_d_i(i);
    sigma_fom(5,i) = sigma_d_r(i);
    
    nameAlg = getNameId(benchmarkSettings,index,i);
    
    
    fprintf(file, "\\textbf{%s}", nameAlg);
    
    for j = 1:N_fom
        if j == N_fom
            if ~strcmp(nameAlg,'CLIE') && ~strcmp(nameAlg,'CLOE') ~= ~strcmp(nameAlg,'DIDIM') ~= ~strcmp(nameAlg,'IDIM-IV') ~= ~strcmp(nameAlg,'PC-IDIM-IV') ~= ~strcmp(nameAlg,'PC-DIDIM')
                fprintf(file, " & N.A.");
            else
                fprintf(file, " & %5.2f%s (%1.2f\\%%)", mean_fom(j,i), unit{j}, sigma_fom(j,i));
            end
        elseif j == 1
            fprintf(file, " & %5.3f%s (%1.2f\\%%)", mean_fom(j,i), unit{j}, sigma_fom(j,i));
        else
            fprintf(file, " & %5.2f%s (%1.2f\\%%)", mean_fom(j,i), unit{j}, sigma_fom(j,i));
        end
        
    end
    
    fprintf(file, "\\\\  \n");
end

fclose(file);

% replace the format E of matlab with \cdot10^{ of latex using a unix
% command line -> DO NOT WORK IN WINDOWS
% if isunix
%     unix(sprintf("sed -i 's/E/\\cdot10^{/g' FOMtab_%s.txt", robot.name));
% end

end


function []=generateLaTeXFOMTabTot(robot,  benchmarkSettings, experimentDataStruct, fP, fEP, fT, fI, fR, averageParam, stdParamError, averageTime, averageIter)

if strcmp(robot.name, "TX40_uncoupled")
    name = "TX40";
    nameTot = "Staubli TX40";
elseif strcmp(robot.name, "TX40")
    name = robot.name;
    nameTot = "Staubli TX40";
elseif strcmp(robot.name, "RV2SQ")
    name = robot.name;
    nameTot = "Mitsubishi RV2SQ";
else
    name = robot.name;
end

% Genrate a table in LaTeX format, containing Figures of Merit (FOM) for
% the set of considered identification methods
N_fom = 5;
options.filter = 'butterworth';
Tau_decim = [];
Qpp_decim = [];
Qp_decim = [];
Q_decim = [];

for expNb = 1:1%benchmarkSettings.numberOfExperimentsPerInitialPoint % Concatenating the experiment data
    %     [~, ~, ~, ~, ~, ~, Tau_decim_exp, Qpp_decim_exp, Qp_decim_exp, Q_decim_exp, ~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb,[]);
    [~, Tau_decim_exp, Qpp_decim_exp, Qp_decim_exp, Q_decim_exp, ~, ~,~,~,~,~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb,[]);
    Tau_decim = [Tau_decim Tau_decim_exp];
    Qpp_decim = [Qpp_decim Qpp_decim_exp];
    Qp_decim = [Qp_decim Qp_decim_exp];
    Q_decim = [Q_decim Q_decim_exp];
end
% Generation of the tabularcontatinning the average error and standard deviation of the error.
index = find(benchmarkSettings.identificationMethods.showResults);
folderPath = sprintf('Benchmark/Robot_Identification_Results/LaTeX/%s_%s/decim%d/LaTeX', robot.name, benchmarkSettings.noiseLevel, benchmarkSettings.decimRate(index(1)))
templateFolderPath = 'Benchmark/Robot_Identification_Results/LaTeX/Template';
filePath = sprintf('Benchmark/Robot_Identification_Results/LaTeX/%s_%s/decim%d/LaTeX/FOMtab.tex', robot.name, benchmarkSettings.noiseLevel, benchmarkSettings.decimRate(index(1)));
robotHasSymbolicModel = exist(folderPath, 'dir');

if robotHasSymbolicModel ==  0 % If the resut folder does not exist
    disp('The LaTeX report folder was not detected: creating it...');
    mkdir(folderPath)
    copyfile(templateFolderPath, folderPath)
    addpath(genpath('Benchmark'));  % Add the newly created directories to the matlab path
end
% 
% 
% if isunix
%     unix(sprintf("mkdir -p %s/%s_%s/decim%d/LaTeX/", benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, benchmarkSettings.decimRate(index(1))));
%     unix(sprintf("touch %s/%s_%s/decim%d/LaTeX/FOMtab.tex", benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, benchmarkSettings.decimRate(index(1))));
% end

file = fopen(filePath, 'w');



fprintf(file, "\\documentclass[letterpaper, 10 pt, conference]{ieeeconf} \n");
fprintf(file, "\n");
fprintf(file, "\\IEEEoverridecommandlockouts\n");
fprintf(file, "\n");  
fprintf(file, "\\overrideIEEEmargins\n");
fprintf(file, "\n");  
fprintf(file, "\\usepackage{cite}\n");
fprintf(file, "\\usepackage{graphics} \n");
fprintf(file, "\\usepackage[table, dvipsnames]{xcolor} \n");
fprintf(file, "\\usepackage{epsfig} \n");
fprintf(file, "\\usepackage{times} \n");
fprintf(file, "\\usepackage{amsmath} \n");
fprintf(file, "\\usepackage{amssymb}  \n");
fprintf(file, "\\usepackage{color} \n");
fprintf(file, "\\usepackage{subfig} \n");
fprintf(file, "\\usepackage{tikz} \n");
fprintf(file, "\\usepackage{url} \n");
fprintf(file, "\\usepackage{algorithm2e} \n");
fprintf(file, "\\usepackage{threeparttable} \n");
fprintf(file, "\\usepackage{booktabs, dcolumn} \n");
fprintf(file, "\\newcommand\\mc[1]{\\multicolumn{1}{c}{#1}} \n");
fprintf(file, "\\usepackage{tabularx} \n");
fprintf(file, "\\newcolumntype{d}[1]{D{.}{.}{\\#1}} \n");
fprintf(file, "\\usepackage{makecell} \n");
fprintf(file, "\\usepackage{stfloats} \n");
fprintf(file, "\\makeatletter\n");
fprintf(file, "\\def\\endfigure{\\end@float} \n");
fprintf(file, "\\def\\endtable{\\end@float} \n");
fprintf(file, "\\makeatother \n");
fprintf(file, "\\usepackage{ulem} \n");
fprintf(file, "\\usepackage{stfloats} \n");
fprintf(file, "\\usepackage{geometry} \n");
fprintf(file, "\\geometry{ a4paper, total={170mm,257mm}, left=20mm, top=20mm }  \n");
fprintf(file, "\n");
fprintf(file, "\\begin{document} \n");
fprintf(file, "	\\title{\\LARGE \\bf Experiment Report %s Monte Carlo Simulation %d Samples, %s} \n", name, benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint, benchmarkSettings.noiseLevel);
fprintf(file, "	\\author{Quentin~Leboutet, Julien~Roux, Alexandre~Janot,\\\\ \n");
fprintf(file, "	 J.~Rogelio~Guadarrama-Olvera, and~Gordon~Cheng}  \n");
fprintf(file, "	\\maketitle \n");
fprintf(file, "\\noindent \\textbf{Experiment conditions: } \n");
fprintf(file, "\\begin{itemize} \n");
fprintf(file, "	\\item Robot: \\textbf{%s}, %d-DoF manipulator \n",nameTot, robot.nbDOF);
fprintf(file, "	\\item $%d\\times %d = %d$ standard parameters (no $\\boldsymbol{\\tau}_{off}$) \n", robot.nbDOF, robot.numericalParameters.numParam(1), robot.nbDOF*robot.numericalParameters.numParam(1) );
fprintf(file, "	\\item $%d$ base parameters \n", robot.paramVectorSize);
fprintf(file, "	\\item Control frequency: %d Hz \n",benchmarkSettings.f_ctrl);
fprintf(file, "	\\item Sampling frequency: %d Hz \n",benchmarkSettings.f);
fprintf(file, "	\\item Decimation frequency: %d Hz \n",benchmarkSettings.fdecim);
fprintf(file, "	\\item Decimation ratio: %d \n",benchmarkSettings.decimRate);
fprintf(file, "	\\item Butterworth filter cutoff frequency: %d Hz \n",benchmarkSettings.filter.butterworth.freq_fil);
fprintf(file, "	\\item Experiment duration: %d s \n",benchmarkSettings.t_f-benchmarkSettings.t_i);
fprintf(file, "	\\item Noise levels: \\textbf{%s} levels \n",benchmarkSettings.noiseLevel);
fprintf(file, "	\\begin{itemize} \n");
fprintf(file, "		\\item Torque noise standard deviation: $\\boldsymbol{\\sigma_\\tau} = %f N.m $ \n",robot.numericalParameters.sd_tau(1));
fprintf(file, "		\\item Position noise standard deviation: $\\boldsymbol{\\sigma_q} = %f deg $  \n",robot.numericalParameters.sd_q(1));
fprintf(file, "	\\end{itemize} \n");
fprintf(file, "	\\item \\textbf{No regularization} in general, except for PC-OLS-Euclidean, PC-OLS-Entropic, and PC-OLS-ConstPullback. In these cases, the regularization weight is set to $10^{-2}$. \n");
fprintf(file, "\\end{itemize} \n");
fprintf(file, "\\begin{table*}[t!] \n");
fprintf(file, "	\\small \n");
fprintf(file, "	\\centering \n");
fprintf(file, "	\\begin{threeparttable} \n");
fprintf(file, "		\\begin{tabular}{| l | c | c | c | c | c | c |} \n");
fprintf(file, "			\\toprule \n");
fprintf(file, "\\cellcolor{green!45}\\textbf{%s, MCS, decim %d} & \\cellcolor{gray!25}$E(d_{\\boldsymbol{q}}), \\sigma_{\\boldsymbol{q}}$ & \\cellcolor{gray!25}$E(d_{\\boldsymbol{\\tau}}), \\sigma_{\\boldsymbol{\\tau}}$& \\cellcolor{gray!25}$E(d_{t}), \\sigma_{t}$  &\\cellcolor{gray!25} $E(d_{N_{it}}), \\sigma_{N_{it}}$ & \\cellcolor{gray!25}$E(d_{N_{sim}}), \\sigma_{N_{sim}}$ \\\\ \n",name ,benchmarkSettings.decimRate(index(1)));
fprintf(file, "\\midrule\n");
            
for i=1:sum(benchmarkSettings.identificationMethods.showResults)

    % Compute relative torque error:
    [~,nbSamples]=size(Tau_decim);
    W = observationMatrixOrdered(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q_decim, Qp_decim, Qpp_decim);
%     Y_tau_ref = W*benchmarkSettings.Beta_obj;          % Recomputed torque with real parameter values
    Y_tau_ref = torqueVectorOrdered(Tau_decim);
    Y_tau_recomputed = W*averageParam(:,index(i));  
    
    Y_error_tau = Y_tau_ref - Y_tau_recomputed;   % Recomputed torque with mean value of the estimated parameters
      
    Y_error_tau_joint=zeros(nbSamples,robot.nbDOF);    
    Y_tau_joint=zeros(nbSamples,robot.nbDOF);
    Y_tau_recomputed_joint=zeros(nbSamples,robot.nbDOF);
    for jointNb = 1:robot.nbDOF
        Y_error_tau_joint(:,jointNb) = Y_error_tau((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        Y_tau_joint(:,jointNb) = Y_tau_ref((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        Y_tau_recomputed_joint(:,jointNb) = Y_tau_recomputed((jointNb-1)*nbSamples+1:jointNb*nbSamples);
    end
%     figure()
%     plot(Y_tau_joint(:,1),'r')
%     hold on
%     plot(Y_tau_recomputed_joint(:,1),'b')
%     
%     figure()
%     plot(Y_tau_joint(:,2),'r')
%     hold on
%     plot(Y_tau_recomputed_joint(:,2),'b')
%     
%     figure()
%     plot(Y_tau_joint(:,3),'r')
%     hold on
%     plot(Y_tau_recomputed_joint(:,3),'b')
%     
%     figure()
%     plot(Y_tau_joint(:,4),'r')
%     hold on
%     plot(Y_tau_recomputed_joint(:,4),'b')
%     
%     figure()
%     plot(Y_tau_joint(:,5),'r')
%     hold on
%     plot(Y_tau_recomputed_joint(:,5),'b')
%     
%     figure()
%     plot(Y_tau_joint(:,6),'r')
%     hold on
%     plot(Y_tau_recomputed_joint(:,6),'b')
%     
%     pause
    
    Y_error_tau_med = sqrt(sum(Y_error_tau_joint.^2,2));
    Y_tau_med = sqrt(sum(Y_tau_joint.^2,2));
    d_tau(i) = 100*mean(Y_error_tau_med./Y_tau_med);
    sigma_d_tau(i) = 100*std(Y_error_tau_med./Y_tau_med);
    
    % Compute relative joint position error:
    t_control = linspace(benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples);  % Control epochs
    augmentedDesiredState = benchmarkSettings.trajectoryData.getTrajectoryData(t_control, benchmarkSettings.interpolationAlgorithm); % augmentedState = [Qpp; Qp; Q];
    [~, stateVector, ~] = integrateClosedLoopDynamics_mex(augmentedDesiredState, averageParam(:,index(i)), robot.name, robot.numericalParameters.Geometry, ...
        robot.numericalParameters.Gravity, benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples, benchmarkSettings.nbSamples, ...
        robot.controlParameters.Kp, robot.controlParameters.Ki, robot.controlParameters.Kd, robot.controlParameters.Ktau, robot.controlParameters.antiWindup, ...
        robot.physicalConstraints.limQ_L, robot.physicalConstraints.limQ_U, robot.physicalConstraints.limQp_L, robot.physicalConstraints.limQp_U, ...
        robot.physicalConstraints.limQpp_L, robot.physicalConstraints.limQpp_U, robot.physicalConstraints.limTau_L, robot.physicalConstraints.limTau_U, benchmarkSettings.integrationAlgorithm);
    Q_sim = stateVector(robot.nbDOF+1:2*robot.nbDOF,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder);
    Qp_sim = stateVector(1:robot.nbDOF,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder);
    
    Q_sim_decim = [];
    Qp_sim_decim = [];
    for jointNb = 1:robot.nbDOF
        Q_sim_decim(jointNb,:) = decimate(Q_sim(jointNb,:),benchmarkSettings.decimRate/benchmarkSettings.decimRate);
        Qp_sim_decim(jointNb,:) = decimate(Qp_sim(jointNb,:),benchmarkSettings.decimRate/benchmarkSettings.decimRate);
    end
    
% figure('Name', 'Q')
%     plot(Q_sim_decim.','b')
%     hold on
%     plot(Q_decim.','r')
%     
%     figure('Name', 'Qp')
%     plot(Qp_sim_decim.','b')
%     hold on
%     plot(Qp_decim.','r')
    
    Y_error_Q = torqueVectorOrdered(Q_decim-Q_sim_decim);
    Y_Q_ref = torqueVectorOrdered(Q_decim);
    for jointNb = 1:robot.nbDOF
        Y_error_Q_joint(:,jointNb) = Y_error_Q((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        Y_Q_joint(:,jointNb) = Y_Q_ref((jointNb-1)*nbSamples+1:jointNb*nbSamples);
    end
    Y_error_Q_med = sqrt(sum(Y_error_Q_joint.^2,2));
    Y_Q_med = sqrt(sum(Y_Q_joint.^2,2));
    d_q(i) = 100*mean(Y_error_Q_med./Y_Q_med);
    sigma_d_q(i) = 100*std(Y_error_Q_med./Y_Q_med);
    
    % Compute average parameter error:
    paramRef = repmat(benchmarkSettings.Beta_obj.',benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1);
    relativeAverageParamErrorsMC = (2/robot.paramVectorSize)*sum(abs(fEP(:,:,index(i)))./(abs(paramRef)+abs(fP(:,:,index(i)))),2); % series of d_\beta for each MC experiment
    d_beta(i) = 100*mean(relativeAverageParamErrorsMC);
    sigma_d_beta(i) = 100*std(relativeAverageParamErrorsMC);
    
    % Get the mean time required to compute one parameter estimate:
    d_t(i) = mean(fT(:,index(i)));
    sigma_d_t(i) = 100*std(fT(:,index(i)))/mean(fT(:,index(i)));
    
    % If applicable, get the number of iterations required to compute one
    % parameter estimate:
    d_i(i) = mean(fI(:,index(i)));
    sigma_d_i(i) = 100*std(fI(:,index(i)))/mean(fI(:,index(i)));
    
    % If applicable, get the number of model recalculations required to compute one
    % parameter estimate:
    d_r(i) = mean(fR(:,index(i)));
    if mean(fR(:,index(i)))~=0
        sigma_d_r(i) = 100*std(fR(:,index(i)))/mean(fR(:,index(i)));
    else
        sigma_d_r(i) = -1;
    end
    
%     mean_fom(1,i) = d_beta(i);
    mean_fom(1,i) = d_q(i);
    mean_fom(2,i) = d_tau(i);
    mean_fom(3,i) = d_t(i);
    mean_fom(4,i) = d_i(i);
    mean_fom(5,i) = d_r(i);
    
%     unit{1} = '\%';
    unit{1} = '\%';
    unit{2} = '\%';
    unit{3} = 's';
    unit{4} = '';
    unit{5} = '';
    
%     sigma_fom(1,i) = sigma_d_beta(i);
    sigma_fom(1,i) = sigma_d_q(i);
    sigma_fom(2,i) = sigma_d_tau(i);
    sigma_fom(3,i) = sigma_d_t(i);
    sigma_fom(4,i) = sigma_d_i(i);
    sigma_fom(5,i) = sigma_d_r(i);
    
    nameAlg = getNameId(benchmarkSettings,index,i);
    
    
    fprintf(file, "\\textbf{%s}", nameAlg);
    
    for j = 1:N_fom
        if j == N_fom
            if ~strcmp(nameAlg,'CLIE') && ~strcmp(nameAlg,'CLOE') ~= ~strcmp(nameAlg,'DIDIM') ~= ~strcmp(nameAlg,'IDIM-IV') ~= ~strcmp(nameAlg,'PC-IDIM-IV') ~= ~strcmp(nameAlg,'PC-DIDIM')
                fprintf(file, " & N.A.");
            else
                fprintf(file, " & %5.2f%s (%1.2f\\%%)", mean_fom(j,i), unit{j}, sigma_fom(j,i));
            end
        elseif j == 1
            fprintf(file, " & %5.3f%s (%1.2f\\%%)", mean_fom(j,i), unit{j}, sigma_fom(j,i));
        else
            fprintf(file, " & %5.2f%s (%1.2f\\%%)", mean_fom(j,i), unit{j}, sigma_fom(j,i));
        end
        
    end
    
    fprintf(file, "\\\\  \n");
end
fprintf(file, "\\bottomrule\n");
fprintf(file, "	\\end{tabular} \n");
fprintf(file, "	\\end{threeparttable} \n");
fprintf(file, "	\\caption{Various figures of merit computed for the %s model, with a sampling frequency $f = %d Hz$ and a decimation rate of %d. These results are obtained using the average values $E(d)$ and standard deviations $\\sigma$ of the %d parameter estimates obtained during the MCS. } \n", nameTot, benchmarkSettings.f, benchmarkSettings.decimRate, benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint);
fprintf(file, "\\end{table*} \n");

fprintf(file, "\\end{document} \n");

fclose(file);

% replace the format E of matlab with \cdot10^{ of latex using a unix
% command line -> DO NOT WORK IN WINDOWS
% if isunix
%     unix(sprintf("sed -i 's/E/\\cdot10^{/g' FOMtab_%s.txt", robot.name));
% end

end

%%%%%%%%%%%%%%%%%%%%%%%
%% generateLaTeXparamTab:
%%%%%%%%%%%%%%%%%%%%%%%

function []=generateLaTeXparamTab(robot,  benchmarkSettings, resultDataStruct, averageParamError, stdParamError)

N_param = robot.paramVectorSize;
N_methode = benchmarkSettings.nbAlg;


% Generation of the tabularcontatinning the average error and standard deviation of the error.
file = fopen('tab.txt', 'w');

for i = 1:N_param
    fprintf(file, "\\beta_{%d}= % 5.4f", i, benchmarkSettings.Beta_obj(i));
    
    for j = 1:N_methode
        if benchmarkSettings.identificationMethods.showResults(j)==1
            fprintf(file, " & %5.4f + %.0E}", averageParamError(i, j), stdParamError(i, j));
        end
        
    end
    fprintf(file, "\\\\  \n");
end

fclose(file);

% replace the format E of matlab with \cdot10^{ of latex using a unix
% command line -> DO NOT WORK IN WINDOWS
if isunix
    unix("sed -i 's/E/\\cdot10^{/g' tab.txt");
end

% Generation of the tabular containing the average identified parameters and there standard deviation
averageParameters = zeros(N_param, N_methode);
stdParameters = zeros(N_param, N_methode);

for i=1:N_methode
    if benchmarkSettings.identificationMethods.showResults(i)==1
        Betas = eval(sprintf('resultDataStruct{i}.results_%s.Betas',benchmarkSettings.identificationMethods.algName{i}));
        averageParameters(:,i) = reshape(mean(reshape(Betas(1:benchmarkSettings.numberOfInitialEstimates, 1:benchmarkSettings.numberOfExperimentsPerInitialPoint,:), benchmarkSettings.numberOfInitialEstimates*robot.paramVectorSize,robot.paramVectorSize)), robot.paramVectorSize, 1);
        stdParameters(:, i) = reshape(std(reshape(Betas(1:benchmarkSettings.numberOfInitialEstimates, 1:benchmarkSettings.numberOfExperimentsPerInitialPoint,:), benchmarkSettings.numberOfInitialEstimates*robot.paramVectorSize,robot.paramVectorSize)), robot.paramVectorSize,1); % maybe replace 0 by 1 to weight by N-1 instead of N iyswIm
    end
end

file_bis = fopen('tab_identified_param.txt', 'w');

for i = 1:N_param
    fprintf(file_bis, "\\beta_{%d}= % 5.4f", i, benchmarkSettings.Beta_obj(i));
    indexMethods = find(benchmarkSettings.identificationMethods.showResults(:));
    
    [~, indexMin(i)] = min(abs(averageParamError(i,indexMethods)));
    counter = 0;
    
    for j = indexMethods(1):indexMethods(end)
        counter = counter +1;
        if benchmarkSettings.identificationMethods.showResults(j)==1
            if counter == indexMin(i) % Best estimate
                fprintf(file_bis, " & \\color{blue}%5.4f \\color{black}+ %.0E}", averageParameters(i, j), stdParameters(i, j));
            else
                fprintf(file_bis, " & %5.4f + %.0E}", averageParameters(i, j), stdParameters(i, j));
            end
        end
    end
    
    fprintf(file_bis, "\\\\  \n");
end

fclose(file_bis);

% replace the format E of matlab with \cdot10^{ of latex using a unix
% command line -> DO NOT WORK IN WINDOWS
if isunix
    unix("sed -i 's/E/\\cdot10^{/g' tab_identified_param.txt");
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% generateLaTeXparamTabPercent:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function []=generateLaTeXparamTabPercent(robot,  benchmarkSettings, resultDataStruct, averageParamError, stdParamError)

N_param = robot.paramVectorSize;
N_methode = benchmarkSettings.nbAlg;

% Generation of the tabular containing the average identified parameters and there standard deviation
averageParameters = zeros(N_param, N_methode);
stdParameters = zeros(N_param, N_methode);

for i=1:N_methode
    if benchmarkSettings.identificationMethods.showResults(i)==1
        Betas = eval(sprintf('resultDataStruct{i}.results_%s.Betas',benchmarkSettings.identificationMethods.algName{i}));
        averageParameters(:,i) = reshape(mean(reshape(Betas(1:benchmarkSettings.numberOfInitialEstimates, 1:benchmarkSettings.numberOfExperimentsPerInitialPoint,:), benchmarkSettings.numberOfInitialEstimates*robot.paramVectorSize,robot.paramVectorSize)), robot.paramVectorSize, 1);
        stdParameters(:, i) = reshape(std(reshape(Betas(1:benchmarkSettings.numberOfInitialEstimates, 1:benchmarkSettings.numberOfExperimentsPerInitialPoint,:), benchmarkSettings.numberOfInitialEstimates*robot.paramVectorSize,robot.paramVectorSize)), robot.paramVectorSize,1); % maybe replace 0 by 1 to weight by N-1 instead of N iyswIm
    end
end


file = fopen('tab_identified_param_percent.txt', 'w');

for i = 1:N_param
    fprintf(file, "$\\beta_{%d}$=\\textbf{%1.4f}", i, benchmarkSettings.Beta_obj(i));
    indexMethods = find(benchmarkSettings.identificationMethods.showResults(:));
    
    %     [~, indexMin(i)] = min(abs(averageParamError(i,indexMethods)));
    
    [~,indexMin(i,:)]=sort(abs(averageParamError(i,indexMethods)));
    counter = 0;
    
    for j = indexMethods(1):indexMethods(end) % For all the possible methods
        if benchmarkSettings.identificationMethods.showResults(j)==1
            counter = counter +1;
            if counter == indexMin(i,1) % Best estimate
                fprintf(file, "&\\cellcolor{blue!25}\\textbf{%1.4f}", averageParameters(i, j));
                %               fprintf(file, " & \\color{blue}%1.2f \\color{black} (%1.1f\\%%)", averageParameters(i, j), sqrt(stdParameters(i, j))/abs(averageParameters(i, j)));
            elseif counter == indexMin(i,2) % Second best estimate
                fprintf(file, "&\\cellcolor{cyan!25}\\textbf{%1.4f}", averageParameters(i, j));
            elseif counter == indexMin(i,3) % Third best estimate
                fprintf(file, "&\\cellcolor{green!25}\\textbf{%1.4f}", averageParameters(i, j));
            elseif counter == indexMin(i,end) % Worst estimate
                fprintf(file, "&\\cellcolor{red!25}\\textbf{%1.4f}", averageParameters(i, j));
            elseif counter == indexMin(i,end-1) % Second worst estimate
                fprintf(file, "&\\cellcolor{orange!25}\\textbf{%1.4f}", averageParameters(i, j));
            elseif counter == indexMin(i,end-2) % Third worst estimate
                fprintf(file, "&\\cellcolor{yellow!25}\\textbf{%1.4f}", averageParameters(i, j));
            else
                fprintf(file, "&%1.4f", averageParameters(i, j));
                %               fprintf(file, " & %1.2f (%1.1f\\%%)", averageParameters(i, j), sqrt(stdParameters(i, j))/abs(averageParameters(i, j)));
            end
        end
    end
    fprintf(file, "\\\\  \n");
end
fclose(file);

% replace the format E of matlab with \cdot10^{ of latex using a unix
% command line -> DO NOT WORK IN WINDOWS
% if isunix
%     unix("sed -i 's/E/\\cdot10^{/g' tab_identified_param_percent.txt");
% end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% generateLaTeXparamTabPercent:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function []=generateLaTeXparamTabPercent2(robot,  benchmarkSettings, resultDataStruct, averageParamError, stdParamError)

N_param = robot.paramVectorSize;
N_methode = benchmarkSettings.nbAlg;

% Generation of the tabular containing the average identified parameters and there standard deviation
averageParameters = zeros(N_param, N_methode);
stdParameters = zeros(N_param, N_methode);

for i=1:N_methode
    if benchmarkSettings.identificationMethods.showResults(i)==1
        Betas = eval(sprintf('resultDataStruct{i}.results_%s.Betas',benchmarkSettings.identificationMethods.algName{i}));
        averageParameters(:,i) = reshape(mean(reshape(Betas(1:benchmarkSettings.numberOfInitialEstimates, 1:benchmarkSettings.numberOfExperimentsPerInitialPoint,:), benchmarkSettings.numberOfInitialEstimates*robot.paramVectorSize,robot.paramVectorSize)), robot.paramVectorSize, 1);
        stdParameters(:, i) = reshape(std(reshape(Betas(1:benchmarkSettings.numberOfInitialEstimates, 1:benchmarkSettings.numberOfExperimentsPerInitialPoint,:), benchmarkSettings.numberOfInitialEstimates*robot.paramVectorSize,robot.paramVectorSize)), robot.paramVectorSize,1); % maybe replace 0 by 1 to weight by N-1 instead of N iyswIm
    end
end


file = fopen('tab_identified_param_percent.txt', 'w');

for i = 1:N_param
    fprintf(file, "$\\beta_{%d}$=\\textbf{%1.4f}", i, benchmarkSettings.Beta_obj(i));
    indexMethods = find(benchmarkSettings.identificationMethods.showResults(:));
    
    %     [~, indexMin(i)] = min(abs(averageParamError(i,indexMethods)));
    
    [~,indexMin(i,:)]=sort(abs(averageParamError(i,indexMethods)));
    counter = 0;
    
    for j = indexMethods(1):indexMethods(end) % For all the possible methods
        if benchmarkSettings.identificationMethods.showResults(j)==1
            counter = counter +1;
            if counter == indexMin(i,1) % Best estimate
                fprintf(file, "&\\cellcolor{blue!25}\\textbf{%1.1f(%1.1f)}", averageParameters(i, j), stdParameters(i, j)/abs(averageParameters(i, j)));
                %               fprintf(file, " & \\color{blue}%1.2f \\color{black} (%1.1f\\%%)", averageParameters(i, j), sqrt(stdParameters(i, j))/abs(averageParameters(i, j)));
            elseif counter == indexMin(i,2) % Second best estimate
                fprintf(file, "&\\cellcolor{cyan!25}\\textbf{%1.1f(%1.1f)}", averageParameters(i, j), stdParameters(i, j)/abs(averageParameters(i, j)));
            elseif counter == indexMin(i,3) % Third best estimate
                fprintf(file, "&\\cellcolor{green!25}\\textbf{%1.1f(%1.1f)}", averageParameters(i, j), stdParameters(i, j)/abs(averageParameters(i, j)));
            elseif counter == indexMin(i,end) % Worst estimate
                fprintf(file, "&\\cellcolor{red!25}\\textbf{%1.1f(%1.0f)}", averageParameters(i, j), stdParameters(i, j)/abs(averageParameters(i, j)));
            elseif counter == indexMin(i,end-1) % Second worst estimate
                fprintf(file, "&\\cellcolor{orange!25}\\textbf{%1.1f(%1.0f)}", averageParameters(i, j), stdParameters(i, j)/abs(averageParameters(i, j)));
            elseif counter == indexMin(i,end-2) % Third worst estimate
                fprintf(file, "&\\cellcolor{yellow!25}\\textbf{%1.1f(%1.0f)}", averageParameters(i, j), stdParameters(i, j)/abs(averageParameters(i, j)));
            else
                fprintf(file, "&%1.1f(%1.1f)", averageParameters(i, j), stdParameters(i, j)/abs(averageParameters(i, j)));
                %               fprintf(file, " & %1.2f (%1.1f\\%%)", averageParameters(i, j), sqrt(stdParameters(i, j))/abs(averageParameters(i, j)));
            end
        end
    end
    fprintf(file, "\\\\  \n");
end
fclose(file);

% replace the format E of matlab with \cdot10^{ of latex using a unix
% command line -> DO NOT WORK IN WINDOWS
% if isunix
%     unix("sed -i 's/E/\\cdot10^{/g' tab_identified_param_percent.txt");
% end
end

%%%%%%%%%%%%%%%%%%%%%%
%% getCovarianceVideo:
%%%%%%%%%%%%%%%%%%%%%%

function []=getCovarianceVideo(Covariance_iteration, algName, iteration, writeVideo)

if nargin < 3
    writeVideo = false;
end

figure('Name','Covariance Matrix')
for i=1:size(Covariance_iteration,3)
    image(1000*Covariance_iteration(:,:,i,iteration));
    title(sprintf('Covariance Matrix %s, Iteration %d', algName, iteration-47));
    F(i) = getframe(gcf);
    drawnow
end

if writeVideo == true
    % create the video writer with 1 fps
    writerObj = VideoWriter(sprintf('covariance_it_%d.avi',iteration));
    writerObj.FrameRate = 10;
    % set the seconds per image
    % open the video writer
    open(writerObj);
    % write the frames to the video
    for i=1:length(F)
        % convert the image to a frame
        frame = F(i) ;
        writeVideo(writerObj, frame);
    end
    % close the writer object
    close(writerObj);
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% plotRecalculatedTorques:
%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [status] = plotRecalculatedTorques(robot, benchmarkSettings, Q, Qp, Qpp, Tau, t, averageParam, jointNb)

% Compute the observation matrix:
W = observationMatrix(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q, Qp, Qpp);

referenceTau = W*benchmarkSettings.Beta_obj;
h=figure('Name','Torques');
plot1 = plot(t, Tau(jointNb,:),'Color',[0.5 0.5 1],'LineWidth',0.5);
hold on
graph(2) = plot(t, referenceTau(jointNb:robot.nbDOF:end),'b', 'LineWidth',3.5);
index = find(benchmarkSettings.identificationMethods.showResults);
j=1;
type = {'--', '-'};
k = -1;
r=1;
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    hold on
    k = k+1;
    recomputedTau = W*averageParam(:,index(i));
    ax = gca;
    if k == 2
        r=r+1;
        k=0;
    end
    ax.ColorOrderIndex = r;
    graph(i+2) = plot(t, recomputedTau(jointNb:robot.nbDOF:end),type{j+1},'LineWidth',1);
    legendInfo{i+2}=sprintf('%s',benchmarkSettings.identificationMethods.algName{index(i)},'Interpreter', 'LaTeX');
    j= ~j;
    
end
hold off
plot1(1,1).Color(4)=0.25;
graph(1) = plot1;
legendInfo{1}='real';
legendInfo{2}='reference';
legend(graph,legendInfo{:})
xlabel('\textbf{Time [s]}','Interpreter', 'LaTeX');
ylabel('\textbf{Torque [N.m]}','Interpreter', 'LaTeX');
title(sprintf('\\textbf{Recomputed torque} \\boldmath{$%s$} \\textbf{joint %d}',robot.name, jointNb),'Interpreter', 'LaTeX')
grid on
grid minor

set(h,'Units','Inches');
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
print(h,sprintf('%s/%s/recomputedTorqueJoint%d',benchmarkSettings.outputPath ,robot.name, jointNb),'-dsvg','-r0')
savefig(h,sprintf('%s/%s/recomputedTorqueJoint%d.fig',benchmarkSettings.outputPath ,robot.name, jointNb));
status = true;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% plotRecalculatedTorquesFull:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [status] = plotRecalculatedTorquesFull(robot, benchmarkSettings, experimentDataStruct, averageParam, saveGraph)

options.filter = 'butterworth';
Tau_decim = [];
Qpp_decim = [];
Qp_decim = [];
Q_decim = [];

for expNb = 1:1%benchmarkSettings.numberOfExperimentsPerInitialPoint % Concatenating the experiment data
    [t_decim, Tau_decim_exp, Qpp_decim_exp, Qp_decim_exp, Q_decim_exp, ~, ~, ~, ~, ~, ~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb,[]);
    Tau_decim = [Tau_decim Tau_decim_exp];
    Qpp_decim = [Qpp_decim Qpp_decim_exp];
    Qp_decim = [Qp_decim Qp_decim_exp];
    Q_decim = [Q_decim Q_decim_exp];
end

% Compute the observation matrix:
W = observationMatrix(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q_decim, Qp_decim, Qpp_decim);
num = [   1,    0.0781,    0.2344;... % Crimson Red
     0,    0.9000,         0.1;... % Real Green
         0.1172,    0.5625,    1.0000;... % Deep Sky Blue
         1.0000,         0,    1.0000;... % Magenta
         0,    1.0000,    1.0000;... % Cyan
    0.8000,    0.2695,         0;... % Orange
         0,    1.0000,    0.25000;... % Light Green
         0,         0,    0.5000;... % Navy Blue
    0.5000,         0,    0.5000;... % Purple
    0.5391,    0.1680,    0.8828;... % BlueViolet
    0,         0,    1.0000;... % Blue
    0.5000,    0.5000,    0.5000];   % Gray

set(groot,'defaultAxesColorOrder',num)

figure('Name','Torques','Renderer', 'painters', 'Position', [10 10 1200 600]);
for jointNb = 1:robot.nbDOF
    referenceTau = W*benchmarkSettings.Beta_obj;
    subplot(ceil(robot.nbDOF/2),2,jointNb)
    graph(1) = plot(t_decim, Tau_decim(jointNb,:),'Color',[0,0,0.5],'LineWidth',1);
    %     hold on
    %     graph(2) = plot(referenceTau(jointNb:robot.nbDOF:end),'g', 'LineWidth',3.5);
    index = find(benchmarkSettings.identificationMethods.showResults);
    j=1;
    type = {'-', '-'};
    k = -1;
    r=1;
    NN = sum(benchmarkSettings.identificationMethods.showResults);
    for i=1:NN
        hold on
        k = k+1;
        recomputedTau = W*averageParam(:,index(i));
        ax = gca;
        if k == 1
            r=r+1;
            k=0;
        end
        nameAlg = getNameId(benchmarkSettings,index,i);
        graph(i+NN+1) = plot(t_decim, Tau_decim(jointNb,:).'-recomputedTau(jointNb:robot.nbDOF:end),'Color',[1/((i+1)^(i/2)),    1/((i+1)^(i/2)),    1/((i+1)^(i/2))],'LineWidth',1);
        legendInfo{i+NN+1}=sprintf('Error %s',nameAlg);
        ax.ColorOrderIndex = r;
        graph(i+1) = plot(t_decim, recomputedTau(jointNb:robot.nbDOF:end),type{j+1},'LineWidth',1);
        legendInfo{i+1}=sprintf('%s',nameAlg);
        
%         ax.ColorOrderIndex = r;
        
        j= ~j;
        
    end
    hold off
%         graph(1,1).Color(4)=1;
    %     graph(1) = plot1;
    legendInfo{1}='real';
    %     legendInfo{2}='reference';
    xlabel('\textbf{Time [s]}','Interpreter', 'LaTeX');
    ylabel(sprintf('\\textbf{Torque [N.m]}'),'Interpreter', 'LaTeX');
    title(sprintf('\\textbf{Recomputed torque} \\boldmath{$%s$} \\textbf{joint %d}',robot.name, jointNb),'Interpreter', 'LaTeX')
    grid on
    grid minor
end
legend(graph,legendInfo{:},'Interpreter', 'LaTeX','Orientation','horizontal')
status = true;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% plotRecalculatedClosedLoopDDM:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [status]=plotRecalculatedClosedLoopDDM(robot, benchmarkSettings, resultDataStruct, experimentDataStruct)

% This function plot the trajectory errors obtained with each identification method
t_control = linspace(benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples);  % Control epochs
augmentedInitialState = [experimentDataStruct.Qpp(:,1,1); experimentDataStruct.Qp(:,1,1); experimentDataStruct.Q(:,1,1)]; % X = [Qp;Q]:
augmentedDesiredState = benchmarkSettings.trajectoryData.getTrajectoryData(t_control, benchmarkSettings.interpolationAlgorithm); % augmentedState = [Qpp; Qp; Q];

t = linspace(benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbSamples); % Sampling times

for i=1:numel(benchmarkSettings.identificationMethods.showResults)
    if benchmarkSettings.identificationMethods.showResults(i)==1
        Betas = eval(sprintf('resultDataStruct{i}.results_%s.Betas',benchmarkSettings.identificationMethods.algName{i}));
        averageParameters(:,i) = reshape(mean(reshape(Betas, benchmarkSettings.numberOfInitialEstimates*robot.paramVectorSize,robot.paramVectorSize)), robot.paramVectorSize, 1);
        stdParameters(:, i) = reshape(std(reshape(Betas, benchmarkSettings.numberOfInitialEstimates*robot.paramVectorSize,robot.paramVectorSize)), robot.paramVectorSize,1); % maybe replace 0 by 1 to weight by N-1 instead of N iyswIm
    end
end
foundResults = find(benchmarkSettings.identificationMethods.showResults);

for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    % Simulate robot dynamic behavior with the estimated parameters
    [~, State_it(:,:,i), ~] = integrateClosedLoopDynamics_mex(augmentedDesiredState, averageParameters(:,foundResults(i)), robot.name, robot.numericalParameters.Geometry, ...
        robot.numericalParameters.Gravity, benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples, benchmarkSettings.nbSamples, robot.controlParameters.Kp, ...
        robot.controlParameters.Ki, robot.controlParameters.Kd, robot.controlParameters.Ktau, robot.controlParameters.antiWindup, robot.physicalConstraints.limQ_L, robot.physicalConstraints.limQ_U,...
        robot.physicalConstraints.limQp_L, robot.physicalConstraints.limQp_U, robot.physicalConstraints.limQpp_L, robot.physicalConstraints.limQpp_U, robot.physicalConstraints.limTau_L, ...
        robot.physicalConstraints.limTau_U, benchmarkSettings.integrationAlgorithm);
    for j=1:size(State_it,2)
        H = feval(sprintf('HT_dh%d_world_%s', robot.nbDOF,robot.name), State_it(robot.nbDOF+1:end,j,i), robot.numericalParameters.Geometry);
        X(:,j,i) = H(1:3,4);
    end
end


figure('Name','Parameter validation')
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    subplot(sum(benchmarkSettings.identificationMethods.showResults),1,i)
    plot((experimentDataStruct.Q(:,:,1) - State_it(robot.nbDOF+1:end,:,i))');
    benchmarkSettings.identificationMethods.algName{foundResults(i)};
    grid on
    grid minor
    legend
    title(sprintf('Joint errors due to parameter mismatch of %s', benchmarkSettings.identificationMethods.algName{foundResults(i)}));
end


figure('Name','Parameter validation 3D')
realParameters = plot3(experimentDataStruct.X(1,:,1), experimentDataStruct.X(2,:,1), experimentDataStruct.X(3,:,1), 'LineStyle','-', 'Linewidth',2,'HandleVisibility','off');
hold on
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    identifiedParameters(i) = plot3(X(1,:,i), X(2,:,i), X(3,:,i), 'LineStyle','--', 'Linewidth',2,'HandleVisibility','off');
    k = benchmarkSettings.identificationMethods.algName{foundResults(i)};
    legendInfo{i}=sprintf('%s trajectory',k);
    hold on
end
grid on
grid minor
hold off
title('Trajectory');
legend([realParameters identifiedParameters],{'Real trajectory' legendInfo{:}})

status = true;
end

%%%%%%%%%%%%%%%%%
%% errorFunction:
%%%%%%%%%%%%%%%%%

function [ Error ] = errorFunction(robot, benchmarkSettings, experimentDataStruct, Beta, index, expNb)
q = experimentDataStruct.Q(:,:,expNb)';

% Simulation of the robot using the computed parameters:
[~, State_it, ~] = integrateClosedLoopDynamics_mex(augmentedDesiredState, averageParameters(:,foundResults(i)), robot.name, robot.numericalParameters.Geometry, ...
    robot.numericalParameters.Gravity, benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples, benchmarkSettings.nbSamples, robot.controlParameters.Kp, ...
    robot.controlParameters.Ki, robot.controlParameters.Kd, robot.controlParameters.Ktau, robot.controlParameters.antiWindup, robot.physicalConstraints.limQ_L, robot.physicalConstraints.limQ_U,...
    robot.physicalConstraints.limQp_L, robot.physicalConstraints.limQp_U, robot.physicalConstraints.limQpp_L, robot.physicalConstraints.limQpp_U, robot.physicalConstraints.limTau_L, ...
    robot.physicalConstraints.limTau_U, benchmarkSettings.integrationAlgorithm);

size(q)
size(State_it(robot.nbDOF+1:end,:)')
Err=(q-State_it(robot.nbDOF+1:end,:)');
figure('Name',sprintf('%s',benchmarkSettings.identificationMethods.algName{index}));
subplot(2,1,1)
plot(State_it(robot.nbDOF+1:end,:)','--','LineWidth',2)
hold on
plot(q,'LineWidth',1)
legend('q1 simulated','q1 robot')
xlabel('Samples');
ylabel('Q');
title(sprintf('Trajectory %s',benchmarkSettings.identificationMethods.algName{index}))
grid on
grid minor
subplot(2,1,2)
plot(100*Err)
legend('100 x Error')
xlabel('Samples');
ylabel('Joint Error');
title(sprintf('Trajectory Error %s',benchmarkSettings.identificationMethods.algName{index}))
grid on
grid minor
Error = kstest(Err);
figure
hist(Err)
end

%%%%%%%%%%%%%%%
%% plotSummary:
%%%%%%%%%%%%%%%

function [status] = plotSummary(benchmarkSettings, averageTime, averageIter, averageError, stdError, methods)
% Summary
h = figure('Name','Summary','units','normalized','outerposition',[0 0 1 1]);
colormap parula
cmap = colormap;
subplot(4,1,1)
bm = bar(methods, averageError, 0.5, 'FaceColor', 'flat');
for k = 1:benchmarkSettings.nbAlg
    bm.CData(k,:) =  cmap(k,:);
end
text(1:length(averageError),averageError,num2str(averageError',3),'vert','bottom','horiz','center');
box off
% yticks(1:2:benchmarkSettings.nbAlg);
% ylim([0 benchmarkSettings.nbAlg]);
grid on
grid minor
title("Average error")

subplot(4,1,2);
bs = bar(methods, stdError, 0.5, 'FaceColor', 'flat');
for k = 1:benchmarkSettings.nbAlg
    bs.CData(k,:) =  cmap(k,:);
end
text(1:length(stdError),stdError,num2str(stdError','%.1e'),'vert','bottom','horiz','center');
box off
grid on
grid minor
% ylim([0 3.5])
title("Error Standard Deviation")

subplot(4,1,3)
bt = bar(methods, log10(averageTime+1), 0.5, 'FaceColor', 'flat');
for k = 1:benchmarkSettings.nbAlg
    bt.CData(k,:) =  cmap(k,:);
end
text(1:length(averageTime),log10(averageTime+1),num2str(averageTime','%.1us'),'vert','bottom','horiz','center');
box off
yticks(log10([1 2 10 60 600 3600]));
ylim([0 log10(20000)]);
grid on
grid minor
yticklabels({'0', '1s', '10s', '1min', '10min', '1h'});
title("Average Computation Time")

subplot(4,1,4)
it = bar(methods, averageIter, 0.5, 'FaceColor', 'flat');
for k = 1:benchmarkSettings.nbAlg
    it.CData(k,:) =  cmap(k,:);
end
text(1:length(averageIter),averageIter,num2str(averageIter','%.1u'),'vert','bottom','horiz','center');
box off
grid on
grid minor
title("Average Number of Model Recalculation to Convergence")

set(h,'Units','Inches');

pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
print(h,'Summary','-dpdf','-r0')
status = true;
end

%%%%%%%%%%%%%%%%%%%%%
%% plot3DMeanResults:
%%%%%%%%%%%%%%%%%%%%%

function [status] = plot3DMeanResults(robot, benchmarkSettings, averageParamError)

% Parameter error mean value
h=figure('Name','Parameter error mean value')
colormap jet
b = bar3(1:robot.paramVectorSize, averageParamError(:,find(benchmarkSettings.identificationMethods.showResults)));
%     for k = 1:numel(b)
%         cdata = get(b(k),'zdata');
%         cdata=repmat(max(abs(cdata),[],2),1,4);
%         set(b(k),'cdata',cdata,'facecolor','interp')
%     end
for k = 1:length(b)
    zdata = b(k).ZData;
    b(k).CData = zdata;
    b(k).FaceColor = 'interp';
end
%     daspect([3 20 0.0330]);
grid on
grid minor
title("Average Parameter error")
set(h,'Units','Inches');
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
print(h,'Mean results','-dpdf','-r0')
status = true;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% plotRawResultsParamwise:
%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [status] = plotRawResultsParamwise(robot, benchmarkSettings, fEP, fT)

colormap hsv
cmap = colormap;
h=figure('Name','Raw Results')
index = find(benchmarkSettings.identificationMethods.showResults);

for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    for j=1:robot.paramVectorSize
        graph(i) = plot3(fEP(:,j,index(i))',j*ones(benchmarkSettings.numberOfInitialEstimates*benchmarkSettings.numberOfExperimentsPerInitialPoint,1),log(fT(:,index(i))+1),'.','color',cmap(2*i,:));
        hold on
    end
    legendInfo{i}=sprintf('%s',benchmarkSettings.identificationMethods.algName{index(i)});
end
zlabel('\textbf{Computation Time [s]}','Interpreter', 'LaTeX');
xlabel('\textbf{Error}','Interpreter', 'LaTeX');
ylabel('\textbf{Parameter Index}','Interpreter', 'LaTeX');
grid on
grid minor
% ylim([0, 5])
hold off
title('Algotithm Performance');
legend(graph,legendInfo{:})
set(h,'Units','Inches');
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
print(h,'Raw_paramwise','-dpdf','-r0')
status = true;

end

%%%%%%%%%%%%%%%%%%
%% plotRawResults:
%%%%%%%%%%%%%%%%%%

function [status] = plotRawResults(benchmarkSettings, fEP, fT)


marker = {'o','+','*','.','x','s','d','^','v','>','<','p','h','o','+','*','.','x','s','d','^','v','>','<','p','h'};
h=figure('Name','Raw Results')
index = find(benchmarkSettings.identificationMethods.showResults);

for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    graph(i) = scatter(vecnorm(fEP(:,:,index(i))'),log2(fT(:,index(i))+1),40,marker{i});
    hold on
    legendInfo{i}=sprintf('%s',benchmarkSettings.identificationMethods.algName{index(i)});
end
ylabel('\textbf{Computation Time [s]}','Interpreter', 'LaTeX');
xlabel('\textbf{Error RMS}','Interpreter', 'LaTeX');
grid on
grid minor
ylim([0, 5])
hold off
title('Algotithm Performance');
legend(graph,legendInfo{:})
set(h,'Units','Inches');
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
print(h,'Raw_mean','-dpdf','-r0')
status = true;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% plotRawResultsHistogram:
%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [status] = plotRawResultsHistogram(benchmarkSettings, fEP, fT)

% Graph computation time vs error RMS
h=figure('Name','Raw Results')
index = find(benchmarkSettings.identificationMethods.showResults);

for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    graph(i) = histogram2(vecnorm(fEP(:,:,index(i))'),fT(:,index(i))');
    hold on
    legendInfo{i}=sprintf('%s',benchmarkSettings.identificationMethods.algName{index(i)});
end
ylabel('\textbf{Computation Time [s]}','Interpreter', 'LaTeX');
xlabel('\textbf{Error RMS}','Interpreter', 'LaTeX');
grid on
grid minor
ylim([0, 2])
hold off
title('Algotithm Performance');
legend(graph,legendInfo{:})
set(h,'Units','Inches');
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
print(h,'raw_hist','-dpdf','-r0')
status = true;

end

%%%%%%%%%%%%%%%%%%%%%%%%%
%% plotConvergenceStatus:
%%%%%%%%%%%%%%%%%%%%%%%%%

function [status] = plotConvergenceStatus(robot, benchmarkSettings, convergence, decimRate)

% Convergence Status
h=figure('Name','Convergence')
index = find(benchmarkSettings.identificationMethods.showResults);
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    subplot(ceil(sum(benchmarkSettings.identificationMethods.showResults)/2),2,i)
    imagesc(repmat(convergence(:,:,index(i)),1,1,3));
%     colorbar
    title(sprintf('\\textbf{Convergence} \\boldmath{$%s$}', benchmarkSettings.identificationMethods.algName{index(i)}), 'Interpreter', 'LaTeX')
    ylabel('Initial estimate', 'Interpreter', 'LaTeX')
    xlabel('Independant run', 'Interpreter', 'LaTeX')
end
set(h,'Units','Inches');
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
versionMatlab = ver('matlab');
if str2double(versionMatlab.Version)>=9.8
    exportgraphics(h,sprintf('%s/%s/decim%d/paramError/Conv-%s%s',benchmarkSettings.outputPath ,robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)},'.pdf'),'BackgroundColor','none');
else
    print(h,sprintf('%s/%s/decim%d/paramError/Conv-%s',benchmarkSettings.outputPath ,robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}), '-dpdf','-r0');
end

status = true;

end

%%%%%%%%%%%%%%%%%%%%%%
%% plotParamwiseHoriz:
%%%%%%%%%%%%%%%%%%%%%%

function [status] = plotParamwiseHoriz(robot, benchmarkSettings, averageParamError, stdParamError)

% Parameter error mean value and stdandard deviation
h=figure('Name','Parameter error mean value and stdandard deviation')
index = find(benchmarkSettings.identificationMethods.showResults);
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    subplot(sum(benchmarkSettings.identificationMethods.showResults),1,i)
    bar(1:robot.paramVectorSize, averageParamError(:,index(i)));
    hold on
    errorbar(averageParamError(:,index(i)), stdParamError(:,index(i)),'.','LineWidth',2)
    hold off
    grid on
    grid minor
    xlabel('\textbf{Index}','Interpreter', 'LaTeX');
    ylabel('\textbf{Error}','Interpreter', 'LaTeX');
    ylim([-0.26 0.26])
    %         text(1:length(averageParamError(:,index(i))),averageParamError(:,index(i)),num2str(averageParamError(:,index(i)),'%10.2e\n'),'Rotation',90);
    box off
    title(sprintf('\\textbf{Average Parameter Errors and Standard Deviations} \\boldmath{$%s$}', benchmarkSettings.identificationMethods.algName{index(i)}), 'Interpreter', 'LaTeX')
end
legend({'mean', 'std'},'Interpreter', 'LaTeX')
set(h,'Units','Inches');
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
print(h,'Hist_horiz','-dpdf','-r0')
status = true;

end

%%%%%%%%%%%%%%%%%%%%%
%% plotParamwiseVert:
%%%%%%%%%%%%%%%%%%%%%

function [status] = plotParamwiseVert(robot, benchmarkSettings, averageParamError, stdParamError)

% Parameter error mean value and stdandard deviation 2
h=figure('Name','Parameter error mean value and stdandard deviation 2')
index = find(benchmarkSettings.identificationMethods.showResults);
%     subplot(1,sum(benchmarkSettings.identificationMethods.showResults),1)
%         barh(1:robot.paramVectorSize, averageParamError(:,index(1)));
%         hold on
%         errorbar(averageParamError(:,index(1)),1:robot.paramVectorSize, stdParamError(:,index(1)),'horizontal','.','LineWidth',2)
%         hold off
%         grid on
%         grid minor
%         ylabel('\textbf{Index}','Interpreter', 'LaTeX');
%         xlabel('\textbf{Error}','Interpreter', 'LaTeX');
% %         xlim([-0.26 0.26])
% %         text(max(0,averageParamError(:,index(1))), 1:length(averageParamError(:,index(1))),strcat(strcat(num2str(averageParamError(:,index(1)),'%10.2e\n'),'±'),num2str(stdParamError(:,index(1)),'%10.2e\n')),'vert','bottom','horiz','left','Rotation',0);
%         box off
%         title(sprintf('\\boldmath{$%s$}', benchmarkSettings.identificationMethods.algName{index(1)}), 'Interpreter', 'LaTeX')
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    subplot(1,sum(benchmarkSettings.identificationMethods.showResults),i)
    barh(1:robot.paramVectorSize, averageParamError(:,index(i)));
    hold on
    errorbar(averageParamError(:,index(i)),1:robot.paramVectorSize, stdParamError(:,index(i)),'horizontal','r.','LineWidth',0.5)
    hold off
    grid on
    grid minor
    ylabel('\textbf{Index}','Interpreter', 'LaTeX');
    xlabel('\textbf{Parameter Error}','Interpreter', 'LaTeX');
    %     xlim([-1.5 1.6])
    %         text(max(0,averageParamError(:,index(i)))+0.025, 1:length(averageParamError(:,index(i))),strcat(strcat(num2str(averageParamError(:,index(i)),'%10.2e\n'),'±'),num2str(stdParamError(:,index(i)),'%10.2e\n')),'vert','bottom','horiz','left','Rotation',0);
    box off
    title(sprintf('\\boldmath{$%s$}', benchmarkSettings.identificationMethods.algName{index(i)}), 'Interpreter', 'LaTeX')
end
legend({'mean', 'std'},'Interpreter', 'LaTeX')
set(h,'Units','Inches');
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
print(h,'Hist_vert','-dpdf','-r0')
status = true;

end

%%%%%%%%%%%%%%%%%%%%%%%%
%% plotParamwiseVertNew:
%%%%%%%%%%%%%%%%%%%%%%%%

function [status] = plotParamwiseVertNew(robot, benchmarkSettings, averageParamError, stdParamError, decimRate)

% Parameter error mean value and stdandard deviation 2
index = find(benchmarkSettings.identificationMethods.showResults);
%     subplot(1,sum(benchmarkSettings.identificationMethods.showResults),1)
%         barh(1:robot.paramVectorSize, averageParamError(:,index(1)));
%         hold on
%         errorbar(averageParamError(:,index(1)),1:robot.paramVectorSize, stdParamError(:,index(1)),'horizontal','.','LineWidth',2)
%         hold off
%         grid on
%         grid minor
%         ylabel('\textbf{Index}','Interpreter', 'LaTeX');
%         xlabel('\textbf{Error}','Interpreter', 'LaTeX');
% %         xlim([-0.26 0.26])
% %         text(max(0,averageParamError(:,index(1))), 1:length(averageParamError(:,index(1))),strcat(strcat(num2str(averageParamError(:,index(1)),'%10.2e\n'),'±'),num2str(stdParamError(:,index(1)),'%10.2e\n')),'vert','bottom','horiz','left','Rotation',0);
%         box off
%         title(sprintf('\\boldmath{$%s$}', benchmarkSettings.identificationMethods.algName{index(1)}), 'Interpreter', 'LaTeX')
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    h=figure('Name','Parameter error mean value and stdandard deviation 2');
    barh(1:robot.paramVectorSize, averageParamError(:,index(i)), 'FaceColor', [0 .39 .74], 'EdgeColor', [0 .59 .94], 'FaceAlpha',0.75);
    hold on
    errorbar(averageParamError(:,index(i)),1:robot.paramVectorSize, stdParamError(:,index(i)),'horizontal','r.','LineWidth',0.5)
    hold off
    grid on
    grid minor
    ylabel('\textbf{Index}','Interpreter', 'LaTeX');
    xlabel('\textbf{Parameter Error}','Interpreter', 'LaTeX');
    xlim([-0.5 0.5])
    x=0.5;
            text(0.4*(-ones(1,robot.paramVectorSize)).^(0:robot.paramVectorSize-1), linspace(1-x,robot.paramVectorSize-x,robot.paramVectorSize),strcat('err(',strcat(num2str(linspace(1,robot.paramVectorSize,robot.paramVectorSize).','%d\n'),strcat(')=',strcat(strcat(num2str(averageParamError(:,index(i)),'%5.3f\n'),'±'),num2str(stdParamError(:,index(i)),'%5.3f\n'))))), 'Interpreter', 'LaTeX','vert','bottom','horiz','center','Rotation',0,'FontSize', 10);
    box off
%     title(sprintf('\\textbf{Parameter Identification} \\boldmath{$%s$}', benchmarkSettings.identificationMethods.algName{index(i)}), 'Interpreter', 'LaTeX')
    legend({'mean', 'std'},'Interpreter', 'LaTeX','Location','northoutside','Orientation','horizontal')
    set(h,'PaperPositionMode','Auto','Position', [0 0 960 540])
    h.Color='w';
    h.OuterPosition=h.InnerPosition;
    
    if ~exist(sprintf('%s/%s_%s/decim%d/paramError',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i))), 'dir')
        mkdir(sprintf('%s/%s_%s/decim%d/paramError',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i))))
    end

    print(h,sprintf('%s/%s_%s/decim%d/paramError/paramError-%s',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}),'-dsvg','-r0')
    savefig(h,sprintf('%s/%s_%s/decim%d/paramError/paramError-%s',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}));
    
    versionMatlab = ver('matlab');
    if str2double(versionMatlab.Version)>=9.8
        exportgraphics(h,sprintf('%s/%s_%s/decim%d/paramError/paramError-%s%s',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)},'.pdf'),'BackgroundColor','none');
    else
        print(h,sprintf('%s/%s_%s/decim%d/paramError/paramError-%s',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}), '-dpdf','-r0');
    end
end
status = true;

end

function [status] = plotParamwiseMatrix(robot, benchmarkSettings, averageParamError, stdParamError, decimRate)

% Parameter error mean value and stdandard deviation 2
index = find(benchmarkSettings.identificationMethods.showResults);

averageParamErrorMat = [];
stdParamErrorMat = [];
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    averageParamErrorMat = [averageParamErrorMat averageParamError(:,index(i))];
    stdParamErrorMat = [stdParamErrorMat stdParamError(:,index(i))];
    l2Error(i) = norm(averageParamError(:,index(i)));
    name{i} = getNameId(benchmarkSettings,index,i);
end

[~,I] = sort(l2Error,'descend');

for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    namesSorted{i} = name{I(i)};
end
averageParamErrorMatSorted = averageParamErrorMat(:,I);
stdParamErrorMatSorted = stdParamErrorMat(:,I);
[X,Y] = meshgrid(1:robot.paramVectorSize,1:sum(benchmarkSettings.identificationMethods.showResults));
ticks = linspace(1,2*sum(benchmarkSettings.identificationMethods.showResults), sum(benchmarkSettings.identificationMethods.showResults));
figure('Name','Parameter error mean value and stdandard deviation 6');
bar3(ticks,abs(averageParamErrorMatSorted)',0.8);
% colormap(jet);
hold on
q = quiver3(X,Y, abs(averageParamErrorMatSorted)', zeros(size(averageParamErrorMatSorted')), zeros(size(averageParamErrorMatSorted')), abs(averageParamErrorMatSorted)'+abs(stdParamErrorMatSorted)','linewidth',2, 'color', 'r');
q.ShowArrowHead = 'off';
grid on
grid minor
xticks(1:sum(benchmarkSettings.identificationMethods.showResults));
xticklabels(namesSorted);
title("Average Parameter error")
% set(b,'Units','Inches');
% pos = get(b,'Position');
% set(b,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
% print(b,'Mean results','-dpdf','-r0')





% Change the colormap to gray (so higher values are
                         %   black and lower values are white)

% textStrings = num2str(averageParamError(:), '%0.2f');       % Create strings from the matrix values
% textStrings = strtrim(cellstr(textStrings));  % Remove any space padding
% [x, y] = meshgrid(1:52);  % Create x and y coordinates for the strings
% hStrings = text(x(:), y(:), textStrings(:), ...  % Plot the strings
%                 'HorizontalAlignment', 'center');
% midValue = mean(get(gca, 'CLim'));  % Get the middle value of the color range
% textColors = repmat(averageParamError(:) > midValue, 1, 3);  % Choose white or black for the
                                               %   text color of the strings so
                                               %   they can be easily seen over
                                               %   the background color
% set(hStrings, {'Color'}, num2cell(textColors, 2));  % Change the text colors

% set(gca, 'XTick', 1:5, ...                             % Change the axes tick marks
%          'XTickLabel', {'A', 'B', 'C', 'D', 'E'}, ...  %   and tick labels
%          'YTick', 1:5, ...
%          'YTickLabel', {'A', 'B', 'C', 'D', 'E'}, ...
%          'TickLength', [0 0]);
% 
% for i=1:sum(benchmarkSettings.identificationMethods.showResults)
%     h=figure('Name','Parameter error mean value and stdandard deviation 2');
%     barh(1:robot.paramVectorSize, averageParamError(:,index(i)), 'FaceColor', [0 .39 .74], 'EdgeColor', [0 .59 .94], 'FaceAlpha',0.75);
%     hold on
%     errorbar(averageParamError(:,index(i)),1:robot.paramVectorSize, stdParamError(:,index(i)),'horizontal','r.','LineWidth',0.5)
%     hold off
%     grid on
%     grid minor
%     ylabel('\textbf{Index}','Interpreter', 'LaTeX');
%     xlabel('\textbf{Parameter Error}','Interpreter', 'LaTeX');
%     xlim([-0.5 0.5])
%     x=0.5;
%             text(0.4*(-ones(1,robot.paramVectorSize)).^(0:robot.paramVectorSize-1), linspace(1-x,robot.paramVectorSize-x,robot.paramVectorSize),strcat('err(',strcat(num2str(linspace(1,robot.paramVectorSize,robot.paramVectorSize).','%d\n'),strcat(')=',strcat(strcat(num2str(averageParamError(:,index(i)),'%5.3f\n'),'±'),num2str(stdParamError(:,index(i)),'%5.3f\n'))))), 'Interpreter', 'LaTeX','vert','bottom','horiz','center','Rotation',0,'FontSize', 10);
%     box off
% %     title(sprintf('\\textbf{Parameter Identification} \\boldmath{$%s$}', benchmarkSettings.identificationMethods.algName{index(i)}), 'Interpreter', 'LaTeX')
%     legend({'mean', 'std'},'Interpreter', 'LaTeX','Location','northoutside','Orientation','horizontal')
%     set(h,'PaperPositionMode','Auto','Position', [0 0 960 540])
%     h.Color='w';
%     h.OuterPosition=h.InnerPosition;
%     
%     if ~exist(sprintf('%s/%s_%s/decim%d/paramError',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i))), 'dir')
%         mkdir(sprintf('%s/%s_%s/decim%d/paramError',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i))))
%     end
% 
%     print(h,sprintf('%s/%s_%s/decim%d/paramError/paramError-%s',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}),'-dsvg','-r0')
%     savefig(h,sprintf('%s/%s_%s/decim%d/paramError/paramError-%s',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}));
%     
%     versionMatlab = ver('matlab');
%     if str2double(versionMatlab.Version)>=9.8
%         exportgraphics(h,sprintf('%s/%s_%s/decim%d/paramError/paramError-%s%s',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)},'.pdf'),'BackgroundColor','none');
%     else
%         print(h,sprintf('%s/%s_%s/decim%d/paramError/paramError-%s',benchmarkSettings.outputPath, robot.name, benchmarkSettings.noiseLevel, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}), '-dpdf','-r0');
%     end
% end
status = true;

end

function [status] = plotParamwiseSeparated(robot, benchmarkSettings, averageParamError, stdParamError)

%% Parameter error mean value and stdandard deviation 3

index = find(benchmarkSettings.identificationMethods.showResults);
subplot(1,sum(benchmarkSettings.identificationMethods.showResults),1)
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    h = figure('Name',sprintf('Parameter error mean value and stdandard deviation %s',benchmarkSettings.identificationMethods.algName{index(i)}),'units','normalized','outerposition',[0 0 1 1]);
    bar(1:robot.paramVectorSize, averageParamError(:,index(i)));
    hold on
    errorbar(averageParamError(:,index(i)), stdParamError(:,index(i)),'.','LineWidth',2)
    hold off
    grid on
    grid minor
    ylabel('\textbf{Index}','Interpreter', 'LaTeX');
    xlabel('\textbf{Error}','Interpreter', 'LaTeX');
    %         xlim([-0.1 0.26])
    %         text(max(0,averageParamError(:,index(i)))+0.025, 1:length(averageParamError(:,index(i))),strcat(strcat(num2str(averageParamError(:,index(i)),'%10.2e\n'),'±'),num2str(stdParamError(:,index(i)),'%10.2e\n')),'vert','bottom','horiz','left','Rotation',0);
    box off
    title(sprintf('\\boldmath{$%s$}', benchmarkSettings.identificationMethods.algName{index(i)}), 'Interpreter', 'LaTeX')
    legend({'mean', 'std'},'Interpreter', 'LaTeX')
    set(h,'Units','Inches');
    pos = get(h,'Position');
    set(h,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
    print(h,sprintf('%s',benchmarkSettings.identificationMethods.algName{index(i)}),'-dpdf','-r0')
end
status = true;

end

%%%%%%%%%%%%%%%%%%%%%%%
%% checkStatHypotheses:
%%%%%%%%%%%%%%%%%%%%%%%

function [status] = checkStatHypotheses(robot, benchmarkSettings, experimentDataStruct, decimRate, averageParam, stdParamError)

options.filter = 'no';
Tau_decim = [];
Qpp_decim = [];
Qp_decim = [];
Q_decim = [];

for expNb = 1:1 %benchmarkSettings.numberOfExperimentsPerInitialPoint % Concatenating the experiment data
    [~, ~, ~, ~, ~, ~, Tau_decim_exp, Qpp_decim_exp, Qp_decim_exp, Q_decim_exp, ~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb,[]);
    Tau_decim = [Tau_decim Tau_decim_exp];
    Qpp_decim = [Qpp_decim Qpp_decim_exp];
    Qp_decim = [Qp_decim Qp_decim_exp];
    Q_decim = [Q_decim Q_decim_exp];
end

% [~, ~, ~, ~, ~, ~, Tau_decim, Qpp_decim, Qp_decim, Q_decim, ~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, 1,[]);

index = find(benchmarkSettings.identificationMethods.showResults);
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    Y_tau = torqueVectorOrdered(Tau_decim);
    W = observationMatrixOrdered(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q_decim, Qp_decim, Qpp_decim);
    error_e = Y_tau - W*averageParam(:,index(i));
    [~,nbSamples]=size(Tau_decim);
    disp('Relative torque error:')
    
    % Autocorrelation plots:
    h0 = figure('Name','Residuals Autocorrelations');
    for jointNb = 1:robot.nbDOF
        %         error_e_joint = error_e((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        error_e_joint = filloutliers(error_e((jointNb-1)*nbSamples+1:jointNb*nbSamples),'next');
        subplot(ceil(robot.nbDOF/2),2,jointNb)
        [acf,lags,bounds,h] = autocorr(error_e_joint,'NumLags',20,'NumSTD',2);
        grid on
        grid minor
        title('')
        ylabel(sprintf('\\textbf{Link %d}',jointNb),'Interpreter', 'LaTeX');
        xlabel('\textbf{Lag}','Interpreter', 'LaTeX');
    end
    sgtitle(sprintf('\\textbf{Residuals Autocorrelations} \\boldmath{$%s$} \\boldmath{$%s$}', benchmarkSettings.identificationMethods.algName{index(i)}, robot.name),'Interpreter', 'LaTeX')
    
    % Kolmogorov-Smirnov test:
    for jointNb = 1:robot.nbDOF
        %         error_e_joint = error_e((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        error_e_joint = filloutliers(error_e((jointNb-1)*nbSamples+1:jointNb*nbSamples),'next');
        error_e_norm = error_e_joint/std(error_e_joint);
        if kstest(error_e_norm) == 1
            disp(sprintf('Kolmogorov-Smirnov goodness-of-fit hypothesis test %s joint %d: UNSUCCESSFUL',benchmarkSettings.identificationMethods.algName{index(i)}, jointNb))
        else
            disp(sprintf('Kolmogorov-Smirnov goodness-of-fit hypothesis test %s joint %d: SUCCESSFUL',benchmarkSettings.identificationMethods.algName{index(i)}, jointNb))
        end
    end
    
    % Saving figures:
%     set(h0,'Units','Inches');
%     pos = get(h0,'Position');
%     set(h0,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
%     print(h0,sprintf('%s/%s/decim%d/residualAutocorrelations/residualAutocorrelations-%s',robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}),'-dsvg','-r0')
%     savefig(h0,sprintf('%s/%s/decim%d/residualAutocorrelations/residualAutocorrelations-%s.fig',robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}));
%     
end

status = true;

end

%%%%%%%%%%%%%%%%%%%%%%
%% plotErrorHistogram:
%%%%%%%%%%%%%%%%%%%%%%

function [status] = plotErrorHistogram(robot, benchmarkSettings, experimentDataStruct, decimRate, averageParam, stdParamError)

options.filter = 'no';
Tau_decim = [];
Qpp_decim = [];
Qp_decim = [];
Q_decim = [];
t_decim = [];

for expNb = 1:benchmarkSettings.numberOfExperimentsPerInitialPoint % Concatenating the experiment data
    [~, ~, ~, ~, ~, t_decim_exp, Tau_decim_exp, Qpp_decim_exp, Qp_decim_exp, Q_decim_exp, ~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb,[]);
    if isempty(t_decim)
        dt = t_decim_exp(2)-2*t_decim_exp(1);
    else
        dt = t_decim(end)+t_decim_exp(2)-2*t_decim_exp(1);
    end
    t_decim = [t_decim t_decim_exp+dt];
    Tau_decim = [Tau_decim Tau_decim_exp];
    Qpp_decim = [Qpp_decim Qpp_decim_exp];
    Qp_decim = [Qp_decim Qp_decim_exp];
    Q_decim = [Q_decim Q_decim_exp];
end

index = find(benchmarkSettings.identificationMethods.showResults);
for i=1:sum(benchmarkSettings.identificationMethods.showResults)
    Y_tau = torqueVectorOrdered(Tau_decim);
    W = observationMatrixOrdered(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q_decim, Qp_decim, Qpp_decim);
    error_e = Y_tau - W*averageParam(:,index(i));
    [~,nbSamples]=size(Tau_decim);
    %     disp(' Relative error ')
    %     100*norm(error_e((jointNb-1)*nbSamples+1:jointNb*nbSamples))/norm(Y_tau((jointNb-1)*nbSamples+1:jointNb*nbSamples))
    %     fitdist(error_e/std(error_e),'Normal')
    
    
    h0 =  figure('Name',sprintf('Torque error signal %s', benchmarkSettings.identificationMethods.algName{index(i)}),'units','normalized');
    for jointNb = 1:robot.nbDOF
        error_e_joint = error_e((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        subplot(ceil(robot.nbDOF/2),2,jointNb)
        plot(t_decim, error_e_joint)
        %         plot(error_e_joint/std(error_e_joint))
        title(sprintf('\\textbf{Joint %d}', jointNb),'Interpreter', 'LaTeX');
        xlabel('\textbf{Time [s]}','Interpreter', 'LaTeX');
        ylabel('\textbf{Torque error [N.m]}','Interpreter', 'LaTeX');
        grid on
        grid minor
    end
    
    h1 =  figure('Name',sprintf('Torque error signal %s no outliers', benchmarkSettings.identificationMethods.algName{index(i)}),'units','normalized');
    for jointNb = 1:robot.nbDOF
        error_e_joint = filloutliers(error_e((jointNb-1)*nbSamples+1:jointNb*nbSamples),'next');
        subplot(ceil(robot.nbDOF/2),2,jointNb)
        plot(t_decim, error_e_joint)
        %         plot(error_e_joint/std(error_e_joint))
        title(sprintf('\\textbf{Joint %d}', jointNb),'Interpreter', 'LaTeX');
        xlabel('\textbf{Time [s]}','Interpreter', 'LaTeX');
        ylabel('\textbf{Torque error [N.m]}','Interpreter', 'LaTeX');
        grid on
        grid minor
    end
    
    h2 =  figure('Name',sprintf('Normalized histogram of %s error and estimated Gaussian', benchmarkSettings.identificationMethods.algName{index(i)}),'units','normalized');
    for jointNb = 1:robot.nbDOF
        error_e_joint = error_e((jointNb-1)*nbSamples+1:jointNb*nbSamples);
        subplot(ceil(robot.nbDOF/2),2,jointNb)
        hst = histfit(error_e_joint/std(error_e_joint));
        grid on
        grid minor
        title(sprintf('\\textbf{Joint %d}', jointNb),'Interpreter', 'LaTeX');
        xlabel('\textbf{Normalized torque}','Interpreter', 'LaTeX');
        xlim([-6,6]);
        ylabel('\textbf{Population}','Interpreter', 'LaTeX');
        hst(1).FaceColor = [.8 .8 1];
    end
    sgtitle(sprintf('\\textbf{Normalized histograms of} \\boldmath{$%s$} \\textbf{error and estimated Gaussians}', benchmarkSettings.identificationMethods.algName{index(i)}),'Interpreter', 'LaTeX')
    
%     h3 =  figure('Name',sprintf('Normalized histogram of %s error and estimated Gaussian no outliers', benchmarkSettings.identificationMethods.algName{index(i)}),'units','normalized');
%     for jointNb = 1:robot.nbDOF
%         error_e_joint = filloutliers(error_e((jointNb-1)*nbSamples+1:jointNb*nbSamples),'next');
%         size(error_e_joint)
%         subplot(ceil(robot.nbDOF/2),2,jointNb)
%         hst = histfit(error_e_joint/std(error_e_joint));
%         grid on
%         grid minor
%         title(sprintf('\\textbf{Joint %d}', jointNb),'Interpreter', 'LaTeX');
%         xlabel('\textbf{Normalized torque}','Interpreter', 'LaTeX');
%         xlim([-6,6]);
%         ylabel('\textbf{Population}','Interpreter', 'LaTeX');
%         hst(1).FaceColor = [.8 .8 1];
%     end
%     sgtitle(sprintf('\\textbf{Normalized histograms of} \\boldmath{$%s$} \\textbf{error and estimated Gaussians}', benchmarkSettings.identificationMethods.algName{index(i)}),'Interpreter', 'LaTeX')
    
    % Saving figures:
    set(h0,'Units','Inches');
    pos = get(h0,'Position');
    set(h0,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
    print(h0,sprintf('%s/%s/decim%d/errorHistogram/torqueError-%s',benchmarkSettings.outputPath ,robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}),'-dsvg','-r0')
    savefig(h0,sprintf('%s/%s/decim%d/errorHistogram/torqueError-%s.fig',benchmarkSettings.outputPath ,robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}));
    
    set(h1,'Units','Inches');
    pos = get(h1,'Position');
    set(h1,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
    print(h1,sprintf('%s/%s/decim%d/errorHistogram/torqueError-%sNoOutliers',benchmarkSettings.outputPath ,robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}),'-dsvg','-r0')
    savefig(h1,sprintf('%s/%s/decim%d/errorHistogram/torqueError-%sNoOutliers.fig',benchmarkSettings.outputPath ,robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}));
    
    set(h2,'Units','Inches');
    pos = get(h2,'Position');
    set(h2,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
    print(h2,sprintf('%s/%s/decim%d/errorHistogram/histogramTorqueError-%s',benchmarkSettings.outputPath ,robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}),'-dsvg','-r0')
    savefig(h2,sprintf('%s/%s/decim%d/errorHistogram/histogramTorqueError-%s.fig',benchmarkSettings.outputPath ,robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}));
    
%     set(h3,'Units','Inches');
%     pos = get(h3,'Position');
%     set(h3,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])
%     print(h3,sprintf('%s/%s/decim%d/errorHistogram/histogramTorqueError-%sNoOutliers',robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}),'-dsvg','-r0')
%     savefig(h3,sprintf('%s/%s/decim%d/errorHistogram/histogramTorqueError-%sNoOutliers.fig',robot.name, decimRate(index(i)), benchmarkSettings.identificationMethods.algName{index(i)}));
%     
end

status = true;
end


%%%%%%%%%%%%%%%%%%%%%%%
%% torqueVectorOrdered:
%%%%%%%%%%%%%%%%%%%%%%%

function [Y_tau] = torqueVectorOrdered(Tau)

% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
%
% Building the sampled torque vector.

[nbDOF,nbSamples]=size(Tau);
Y_tau = zeros(nbDOF*nbSamples, 1);

for i=1:nbDOF
    for j=1:nbSamples
        Y_tau((i-1)*nbSamples+j)=Tau(i,j);
    end
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% observationMatrixOrdered:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [W_ord] = observationMatrixOrdered(robotName, paramVectorSize, Geometry, Gravity, Q, Qp, Qpp)

% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
%
% Building of the observation matrix W for the current iteration of Beta

[nbDOF,nbSamples]=size(Q);
W = zeros(nbSamples*nbDOF, paramVectorSize);
W_ord = zeros(nbSamples*nbDOF, paramVectorSize);

for i=1:nbSamples
    % Compute the observation matrix:
    switch robotName % [toyRobot, SCARA, UR3, UR5, UR10, TX40, TX40_uncoupled, RX90, PUMA560, REEMC_right_arm, ICUB_right_arm, NAO_right_arm]
        case 'TX40'
            W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_TX40(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
        case 'TX40_uncoupled'
            W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_TX40_uncoupled(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
        case 'RV2SQ'
            W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_RV2SQ(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
        otherwise
            W(nbDOF*(i-1)+1:nbDOF*i,:) = feval(sprintf('Regressor_Y_%s',robotName),Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity); % Much slower when compiled !
    end
end

for i=1:nbDOF
    W_ord(nbSamples*(i-1)+1:nbSamples*i,:) = W(i:nbDOF:end,:);
end


end


function nameAlg = getNameId(benchmarkSettings,index,i)
    switch benchmarkSettings.identificationMethods.algName{index(i)}
        case 'OLS'
            nameAlg = 'IDIM-OLS';
        case 'OLS_f'
            nameAlg = 'IDIM-OLS filtered';
        case 'WLS'
            nameAlg = 'IDIM-WLS';
        case 'WLS_f'
            nameAlg = 'IDIM-WLS filtered';
        case 'TLS'
            nameAlg = 'IDIM-TLS';
        case 'TLS_f'
            nameAlg = 'IDIM-TLS filtered';
        case 'ML'
            nameAlg = 'ML';
        case 'ML_f'
            nameAlg = 'ML filtered';
        case 'IV'
            nameAlg = 'IDIM-IV';
        case 'DIDIM'
            nameAlg = 'DIDIM';
        case 'CLIE'
            nameAlg = 'CLIE';
        case 'CLOE'
            nameAlg = 'CLOE';
        case 'EKF'
            nameAlg = 'EKF';
        case 'SREKF'
            nameAlg = 'SREKF';
        case 'UKF'
            nameAlg = 'UKF';
        case 'SRUKF'
            nameAlg = 'SRUKF';
        case 'CDKF'
            nameAlg = 'CDKF';
        case 'SRCDKF'
            nameAlg = 'SRCDKF';
        case 'ANN'
            nameAlg = 'ANN';
        case 'ANN_f'
            nameAlg = 'ANN filtered';
        case 'HTRNN'
            nameAlg = 'HTRNN';
        case 'HTRNN_f'
            nameAlg = 'HTRNN filtered';
        case 'IRLS'
            nameAlg = 'IRLS';
        case 'IRLS_f'
            nameAlg = 'IRLS filtered';
        case 'PC_OLS_Euclidean'
            nameAlg = 'PC-OLS Euclidean';
        case 'PC_OLS_Euclidean_f'
            nameAlg = 'PC-OLS filtered Euclidean';
        case 'PC_OLS_Entropic'
            nameAlg = 'PC-OLS Entropic';
        case 'PC_OLS_Entropic_f'
            nameAlg = 'PC-OLS filtered Entropic';
        case 'PC_OLS_ConstPullback'
            nameAlg = 'PC-OLS ConstPullback';
        case 'PC_OLS_ConstPullback_f'
            nameAlg = 'PC-OLS filtered ConstPullback';
        case 'PC_IV'
            nameAlg = 'PC-IDIM-IV';
        case 'PC_DIDIM'
            nameAlg = 'PC-DIDIM';
        case 'PC_OLS'
            nameAlg = 'PC-IDIM-OLS';
        case 'PC_OLS_f'
            nameAlg = 'PC-IDIM-OLS filtered';
        case 'PC_WLS'
            nameAlg = 'PC-IDIM-WLS';
        case 'PC_WLS_f'
            nameAlg = 'PC-IDIM-WLS filtered';
        case 'PC_IRLS'
            nameAlg = 'PC-IDIM-IRLS';
        case 'PC_IRLS_f'
            nameAlg = 'PC-IDIM-IRLS filtered';
        otherwise
            benchmarkSettings.identificationMethods.algName{index(i)}
            error('Unknown algorithm');
    end
end