clear; clc;

load('trajectories/multiposition_kinematic.mat'); % load trajData struct
addpath('multi_position\'); addpath('multi_position\syed_2007\');

warning('off');

n = 200; % runs of the simulation
g = 9.81;
k_calib = 0.3; % weighting factor for the CE metric
maxIter = 400;

data_rohac = zeros(n, 8); data_syed = data_rohac; data_frosio = data_rohac; data_sipos = data_rohac; data_merayo = data_rohac; data_mvee = data_rohac;
MSEs_rohac = zeros(n, 9); MSEs_syed = MSEs_rohac; MSEs_frosio = MSEs_rohac; MSEs_sipos = MSEs_rohac; MSEs_merayo = MSEs_rohac; MSEs_mvee = MSEs_rohac;
crlbs = zeros(9, n);

biases = zeros(n, 3, 4);
scalefactors = zeros(n, 3, 4);

% Set accelerometer model properties (LSM6DS3 in this case):
accelParams.fsRange = 2*g; accelParams.res = 5.9875e-04; accelParams.typBiasX = (40/1000 * 9.81) / 3; accelParams.typBiasY = (40/1000 * 9.81) / 3;
accelParams.typBiasZ = (40/1000 * 9.81) / 3; accelParams.typSFcalibErrPerc = 3/3; accelParams.typNonOrthogAngleDeg = 1/3; accelParams.typMisalignAngleDeg = 1/3;
accelParams.noiseDens = 0.003; accelParams.rdnWalk = 2.2e-05; accelParams.biasInstab = 4.0e-04;

% accelParams.fsRange = 2*g; accelParams.res = 5.9875e-04; accelParams.typBiasX = 0/1000 * 9.81; accelParams.typBiasY = 0/1000 * 9.81;
% accelParams.typBiasZ = 0/1000 * 9.81; accelParams.typSFcalibErrPerc = 0; accelParams.typNonOrthogAngleDeg = 0; accelParams.typMisalignAngleDeg = 0;
% accelParams.noiseDens = 0.003; accelParams.rdnWalk = 2.2e-05;
% accelParams.biasInstab = 4.0e-04; % Uncomment for perfect accelerometer case

FTs = 1; % factor for probing sensitivity to initialisation conditions (affects optimisation methods only)

for i_FT = 1:length(FTs)

    FT = FTs(i_FT); % get the initialisation parameter scaling factor

    for i = 1:n

        % fsRange in m/s2
        % res in (m/s2)/LSB
        % typBias in m/s2
        % typSFcalibErrPerc % of FS
        % typNonOrthogAngleDeg in degrees
        % typMisalignAngleDeg in degrees
        % noiseDens in (m/s/s)/sqrt(Hz)
        % rdnWalk in (m/s2)*(√Hz)
        % biasInstab in in m/s2

        [orient, acc, angvel] = kintrajvarsFromkintrajData(trajData);

        %% IMU model:
        MPU_model = imuSensor('SampleRate', trajData.fs, 'Temperature', 20, 'IMUType', 'accel-gyro', 'ReferenceFrame', 'NED');
        % Create random accelerometer model based on typical values:
        [MPU_model.Accelerometer, scaleFactors, nonorthogAngles, misalignAngles] = genAccel(accelParams, 'normal');

        trueParams.Bias = MPU_model.Accelerometer.ConstantBias;
        trueParams.AxesMisalignment = MPU_model.Accelerometer.AxesMisalignment ./ 100;
        trueParams.ScaleFactors = scaleFactors; trueParams.NonorthogAngles = nonorthogAngles; trueParams.MisalignAngles = misalignAngles; % in rad

        %% Run IMU model
        an_nb = acc; % acceleration of body frame rel. to nav. frame expressed in nav. frame.
        gn_nb = angvel; % ang. velocity of body frame rel. to nav. frame expressed in nav. frame.
        qb_n = quaternion(compact(orient)); % orientation of body frame rel. to nav. frame
        [accel_MPU, ~] = MPU_model(an_nb, gn_nb, qb_n);

        trueAccels = computeTrueAccels(trajData, qb_n); crlbs(:, i) = computeCRLB_MPM(accelParams.noiseDens * sqrt(trajData.fs / 2), trueParams, trueAccels); % CRLB computation

        % Generate initial model parameters
        initCalibParams = genRdnAccelParams(accelParams.fsRange, accelParams.typBiasX * 3, accelParams.typBiasY * 3, accelParams.typBiasZ * 3, accelParams.typSFcalibErrPerc * 3, ...
            accelParams.typNonOrthogAngleDeg * 3, accelParams.typMisalignAngleDeg * 3, 'uniform');

        % initCalibParams = genRdnAccelParams(accelParams.fsRange, 50/1000 * 9.81 * FT, 50/1000 * 9.81 * FT, 80/1000 * 9.81 * FT, 3, ...
        %     1 * FT, 1 * FT, "uniform");

        % Rohac et al. - Multiposition method using fminunc (Quasi-Newton algorithm).
        estParamsRohac = rohac_2014(trajData, accel_MPU, initCalibParams, maxIter);
        [MASE_rohac, MABE_rohac, CE_rohac, MSSE_rohac, MSBE_rohac] = computeCalibError(estParamsRohac, trueParams, k_calib);
        [MASD_rohac, MABD_rohac, MACD_rohac] = computeMACD(initCalibParams, trueParams);
        data_rohac(i,:) = [MASD_rohac MABD_rohac MACD_rohac MASE_rohac MABE_rohac CE_rohac MSSE_rohac MSBE_rohac];
        MSEs_rohac(i,:) = [(estParamsRohac.ScaleFactors - scaleFactors).^2 (estParamsRohac.NonorthogAngles - nonorthogAngles).^2 (estParamsRohac.Bias - trueParams.Bias).^2];
        
        % Syed et al. - Multiposition method using weighted least-squares adjustment:
        estParamsSyed = syed_2007(trajData, accel_MPU, initCalibParams, maxIter);
        [MASE_syed, MABE_syed, CE_syed, MSSE_syed, MSBE_syed] = computeCalibError(estParamsSyed, trueParams, k_calib);
        [MASD_syed, MABD_syed, MACD_syed] = computeMACD(initCalibParams, trueParams);
        data_syed(i,:) = [MASD_syed MABD_syed MACD_syed MASE_syed MABE_syed CE_syed MSSE_syed MSBE_syed];
        MSEs_syed(i,:) = [(estParamsSyed.ScaleFactors - scaleFactors).^2 (estParamsSyed.NonorthogAngles - nonorthogAngles).^2 (estParamsSyed.Bias - trueParams.Bias).^2];
        
        % Frosio et al. - Multiposition method using Gauss-Newton method:
        estParamsFrosio = frosio_2009(trajData, accel_MPU, initCalibParams, maxIter);
        [MASE_frosio, MABE_frosio, CE_frosio, MSSE_frosio, MSBE_frosio] = computeCalibError(estParamsFrosio, trueParams, k_calib);
        [MASD_frosio, MABD_frosio, MACD_frosio] = computeMACD(initCalibParams, trueParams);
        data_frosio(i,:) = [MASD_frosio MABD_frosio MACD_frosio MASE_frosio MABE_frosio CE_frosio MSSE_frosio MSBE_frosio];
        MSEs_frosio(i,:) = [(estParamsFrosio.ScaleFactors - scaleFactors).^2 (estParamsFrosio.NonorthogAngles - nonorthogAngles).^2 (estParamsFrosio.Bias - trueParams.Bias).^2];
        
        % Sipos et al. - Multiposition method using lsqnonlin and LM algorithm.
        estParamsSipos = sipos_2012(trajData, accel_MPU, initCalibParams, maxIter);
        [MASE_sipos, MABE_sipos, CE_sipos, MSSE_sipos, MSBE_sipos] = computeCalibError(estParamsSipos, trueParams, k_calib);
        [MASD_sipos, MABD_sipos, MACD_sipos] = computeMACD(initCalibParams, trueParams);
        data_sipos(i,:) = [MASD_sipos MABD_sipos MACD_sipos MASE_sipos MABE_sipos CE_sipos MSSE_sipos MSBE_sipos];
        MSEs_sipos(i,:) = [(estParamsSipos.ScaleFactors - scaleFactors).^2 (estParamsSipos.NonorthogAngles - nonorthogAngles).^2 (estParamsSipos.Bias - trueParams.Bias).^2];

        % Merayo's method:
        estParamsMerayo = merayo(trajData, accel_MPU);
        [MASE_merayo, MABE_merayo, CE_merayo, MSSE_merayo, MSBE_merayo] = computeCalibError(estParamsMerayo, trueParams, k_calib);
        [MASD_merayo, MABD_merayo, MACD_merayo] = computeMACD(initCalibParams, trueParams);
        data_merayo(i,:) = [MASD_merayo MABD_merayo MACD_merayo MASE_merayo MABE_merayo CE_merayo MSSE_merayo MSBE_merayo];
        MSEs_merayo(i,:) = [(estParamsMerayo.ScaleFactors - scaleFactors).^2 (estParamsMerayo.NonorthogAngles - nonorthogAngles).^2 (estParamsMerayo.Bias - trueParams.Bias).^2];
        
        % MVEE:
        estParamsMVEE = MVEE(trajData, accel_MPU);
        [MASE_MVEE, MABE_MVEE, CE_MVEE, MSSE_MVEE, MSBE_MVEE] = computeCalibError(estParamsMVEE, trueParams, k_calib);
        [MASD_MVEE, MABD_MVEE, MACD_MVEE] = computeMACD(initCalibParams, trueParams);
        data_mvee(i,:) = [MASD_MVEE MABD_MVEE MACD_MVEE MASE_MVEE MABE_MVEE CE_MVEE MSSE_MVEE MSBE_MVEE];
        MSEs_mvee(i,:) = [(estParamsMVEE.ScaleFactors - scaleFactors).^2 (estParamsMVEE.NonorthogAngles - nonorthogAngles).^2 (estParamsMVEE.Bias - trueParams.Bias).^2];

        % disp(estParamsRohac.AxesMisalignment); disp(' '); disp(estParamsRohac.Bias); disp('-------------------------------');
        % disp(estParamsSyed.AxesMisalignment); disp(' '); disp(estParamsSyed.Bias); disp('-------------------------------');
        % disp(estParamsFrosio.AxesMisalignment); disp(' '); disp(estParamsFrosio.Bias); disp('-----------------------------------');
        % disp(estParamsSipos.AxesMisalignment); disp(' '); disp(estParamsSipos.Bias); disp('-----------------------------------');
        % disp(estParamsMerayo.AxesMisalignment); disp(' '); disp(estParamsMerayo.Bias); disp('-----------------------------------');
        % disp(estParamsMVEE.AxesMisalignment); disp(' '); disp(estParamsMVEE.Bias); disp('-----------------------------------');
        % disp(trueParams.AxesMisalignment); disp(' '); disp(trueParams.Bias); disp('---------------------------------');

        % nonorthogData(i,:) = [mean(abs(estParamsRohac.NonorthogAngles - nonorthogAngles)) mean(abs(estParamsSyed.NonorthogAngles - nonorthogAngles)) mean(abs(estParamsFrosio.NonorthogAngles - nonorthogAngles))...
        %     mean(abs(estParamsSipos.NonorthogAngles - nonorthogAngles)) mean(abs(estParamsMerayo.NonorthogAngles - nonorthogAngles)) mean(abs(estParamsMVEE.NonorthogAngles - nonorthogAngles))];
        % misalignData(i,:) = mean(abs(estParamsMVEE.MisalignAngles - misalignAngles));

        % disp([estParamsRohac.Bias estParamsRohac.ScaleFactors estParamsRohac.NonorthogAngles]);

        % Biases and SFs for the optimisation-based methods (for sensitvity
        % to initialisation conditions study):
        % biases(i, :, 1) = estParamsRohac.Bias; scalefactors(i, :, 1) = estParamsRohac.ScaleFactors;
        % biases(i, :, 2) = estParamsSyed.Bias; scalefactors(i, :, 2) = estParamsSyed.ScaleFactors;
        % biases(i, :, 3) = estParamsFrosio.Bias; scalefactors(i, :, 3) = estParamsFrosio.ScaleFactors;
        % biases(i, :, 4) = estParamsSipos.Bias; scalefactors(i, :, 4) = estParamsSipos.ScaleFactors;

        disp(i);

    end

    sigma_bar_biases = squeeze(mean(std(biases(:, 1:3, :)), [1 2])); sigma_bar_scalefactors = squeeze(mean(std(scalefactors), [1 2]));
    disp([FT mean([sigma_bar_biases'; sigma_bar_scalefactors'], 1)]);

end

%% Results:

% Mean CE and 1 sigma:
disp([mean(data_rohac(:,6)) std(data_rohac(:,6))]);
disp([mean(data_syed(:,6)) std(data_syed(:,6))]);
disp([mean(data_frosio(:,6)) std(data_frosio(:,6))]);
disp([mean(data_sipos(:,6)) std(data_sipos(:,6))]);
disp([mean(data_merayo(:,6)) std(data_merayo(:,6))]);
disp([mean(data_mvee(:,6)) std(data_mvee(:,6))]);

crlb = mean(crlbs, 'all'); % CRLBs

% MSEs
disp(mean(MSEs_rohac, 'all')); disp(mean(MSEs_syed, 'all')); disp(mean(MSEs_frosio, 'all')); disp(mean(MSEs_sipos, 'all')); disp(mean(MSEs_merayo, 'all')); disp(mean(MSEs_mvee, 'all'));

% MAE angular errors:
MAE_nonorthog_Rohac = rad2deg(mean(nonorthogData(:,1))); MAE_nonorthog_Syed = rad2deg(mean(nonorthogData(:,2))); MAE_nonorthog_Frosio = rad2deg(mean(nonorthogData(:,3)));
MAE_nonorthog_Sipos = rad2deg(mean(nonorthogData(:,4))); MAE_nonorthog_Merayo = rad2deg(mean(nonorthogData(:,5))); MAE_nonorthog_MVEE = rad2deg(mean(nonorthogData(:,6)));
MAE_misalign_MVEE = rad2deg(mean(misalignData));