% autor: Matouš Přikryl
% thesis: HIL testing of BMS
% code purpose: Automatic assessment of the HIL test (general - can be used
% for any test)

%% Clear workspace
clear all;
close all;
clc;

%% choose assessment settings
test = 3;               % Choose test (1-3)
mode = 1;               % 1 - normal, 2 - debug
faultErrorRemoval = 0;  % 0 - no error removal, 1 - short error removal

%% err removal tresholds
BattErrorTreshold = 0.5;
LoadErrorTreshold = 3.6;
SDCErrorTreshold = 0.1;
AirErrorTreshold = 0.1;

%% load files
TestString = string(test);

% load data from HIL
switch test
    case 1
        TestNumber = string(29);
    case 2
        TestNumber = string(31);
    case 3
        TestNumber = string(37);
    otherwise
        fprintf('zadáno špatné číslo testu - opakuj znovu se správným číslem\n');
        return;
end

NameFile = 'data_hil_2_0' + TestNumber;
NameFile_mat = append(NameFile, '.mat');

output_HIL = load(NameFile_mat);
output_HIL = output_HIL.(NameFile);

% load data from model
NameFileModel = 'HIL_model_out_' + TestString + '.mat';
output_model = load(NameFileModel);


%% Prepare data for use
Time = output_HIL.X.Data;

BattV = output_HIL.Y(1).Data;
LoadV = output_HIL.Y(2).Data;
NEG_CTRL = output_HIL.Y(3).Data;
POS_CTRL = output_HIL.Y(4).Data;
PRE_CTRL = output_HIL.Y(5).Data;
SDC_final = output_HIL.Y(6).Data;
ErrCode = output_HIL.Y(7).Data;
BMS_Status = output_HIL.Y(8).Data;
current = output_HIL.Y(9).Data;
TsStatus = output_HIL.Y(11).Data;


%% Check SUT's behaviour and save to vector
% prepare ok/fail vectors
AirsOk = zeros(1, length(Time));
LoadVOk = zeros(1, length(Time));
BattVOk = zeros(1, length(Time));
SDCFinalOk = zeros(1, length(Time));

% general rules check
for i = 1:length(Time)
    % AirsOk
    switch BMS_Status(i)
        case 0
            if NEG_CTRL(i) || POS_CTRL(i) || PRE_CTRL(i)
                AirsOk(i) = 0;
            else
                AirsOk(i) = 1;
            end
        case 1
            AirsOk(i) = 1;
        case 2
            if ~NEG_CTRL(i) || ~POS_CTRL(i) || PRE_CTRL(i)
                AirsOk(i) = 0;
            else
                AirsOk(i) = 1;
            end
        case 3
            if NEG_CTRL(i) || POS_CTRL(i) || PRE_CTRL(i)
                AirsOk(i) = 0;
            else
                AirsOk(i) = 1;
            end
        case 4
            if ~NEG_CTRL(i) || ~POS_CTRL(i) || PRE_CTRL(i)
                AirsOk(i) = 0;
            else
                AirsOk(i) = 1;
            end
    end

    % napětí meziobvodu měniče
    switch BMS_Status(i)
        case 0
            if LoadV >= 60
                LoadVOk(i) = 0;
            else
                LoadVOk(i) = 1;
            end
        case 1
            LoadVOk(i) = 1;
        case 2
            if (LoadV(i) <= 408) || (LoadV(i) >= 591.6)
                LoadVOk(i) = 0;
            else
                LoadVOk(i) = 1;
            end
        case 3
            if LoadV(i) >= 60
                LoadVOk(i) = 0;
            else
                LoadVOk(i) = 1;
            end
        case 4
            if (LoadV(i) <= 408) || (LoadV(i) >= 591.6)
                LoadVOk(i) = 0;
            else
                LoadVOk(i) = 1;
            end
    end

    % napětí baterie
    switch BMS_Status(i)
        case 0
            BattVOk(i) = 1;
        case 1
            if (BattV(i) <= 408) || (BattV(i) >= 591.6)
                BattVOk(i) = 0;
            else
                BattVOk(i) = 1;
            end
        case 2
            if (BattV(i) <= 408) || (BattV(i) >= 591.6)
                BattVOk(i) = 0;
            else
                BattVOk(i) = 1;
            end
        case 3
            BattVOk(i) = 1;
        case 4
            if (BattV(i) <= 408) || (BattV(i) >= 591.6)
                BattVOk(i) = 0;
            else
                BattVOk(i) = 1;
            end
    end

    % SDC final (1 = OK)
    switch BMS_Status(i)
        case 0
            SDCFinalOk(i) = 1;
        case 1
            SDCFinalOk(i) = 1;
        case 2
            if SDC_final(i) >= 20
                SDCFinalOk(i) = 1;
            else
                SDCFinalOk(i) = 0;
            end
        case 3
            SDCFinalOk(i) = 1;
        case 4
            if SDC_final(i) >= 20
                SDCFinalOk(i) = 1;
            else
                SDCFinalOk(i) = 0;
            end
    end
end

% find errors
AirErrorStart = zeros(1, 10);
AirErrorEnd = zeros(1, 10);
LoadErrorStart = zeros(1, 10);
LoadErrorEnd = zeros(1, 10);
BattErrorStart = zeros(1, 10);
BattErrorEnd = zeros(1, 10);
SDCErrorStart = zeros(1, 10);
SDCErrorEnd = zeros(1, 10);

air_count = 1;
air_count_2 = 1;
load_count = 1;
load_count_2 = 1;
batt_count = 1;
batt_count_2 = 1;
SDC_count = 1;
SDC_count_2 = 1;

for i = 1:(length(Time)-1)
    % airs
    if AirsOk(i) > AirsOk(i+1)
        AirErrorStart(air_count) = Time(i);
        air_count = air_count + 1;
    elseif AirsOk(i) < AirsOk(i+1)
        AirErrorEnd(air_count_2) = Time(i);
        air_count_2 = air_count_2 + 1;
    end

    % LoadV
    if LoadVOk(i) > LoadVOk(i+1)
        LoadErrorStart(load_count) = Time(i);
        load_count = load_count + 1;
    elseif LoadVOk(i) < LoadVOk(i+1)
        LoadErrorEnd(load_count_2) = Time(i);
        load_count_2 = load_count_2 + 1;
    end

    % BattV
    if BattVOk(i) > BattVOk(i+1)
        BattErrorStart(batt_count) = Time(i);
        batt_count = batt_count + 1;
    elseif BattVOk(i) < BattVOk(i+1)
        BattErrorEnd(batt_count_2) = Time(i);
        batt_count_2 = batt_count_2 + 1;
    end

    % LoadV
    if SDCFinalOk(i) > SDCFinalOk(i+1)
        SDCErrorStart(SDC_count) = Time(i);
        SDC_count = SDC_count + 1;
    elseif SDCFinalOk(i) < SDCFinalOk(i+1)
        SDCErrorEnd(SDC_count_2) = Time(i);
        SDC_count_2 = SDC_count_2 + 1;
    end
end

AirErrorStart = AirErrorStart(AirErrorStart~=0);
AirErrorEnd = AirErrorEnd(AirErrorEnd~=0);
LoadErrorStart = LoadErrorStart(LoadErrorStart~=0);
LoadErrorEnd = LoadErrorEnd(LoadErrorEnd~=0);
BattErrorStart = BattErrorStart(BattErrorStart~=0);
BattErrorEnd = BattErrorEnd(BattErrorEnd~=0);
SDCErrorStart = SDCErrorStart(SDCErrorStart~=0);
SDCErrorEnd = SDCErrorEnd(SDCErrorEnd~=0);


%% plot data
% BattV
figure('Renderer', 'painters', 'Position', [100 100 770 370]);
plot(output_HIL.X.Data, BattV, 'LineWidth', 1.5)
grid on
grid minor
xlabel('time [s]')
ylabel('Battery voltage')
xlim([0 Time(end)])
ylim([0 600])
hold on;
xregion(Time(1), Time(end), 1, 600);
for i = 1:length(BattErrorStart)
    if ((BattErrorEnd(i) - BattErrorStart(i)) < BattErrorTreshold) && faultErrorRemoval
        break;
    end
    xregion(BattErrorStart(i), BattErrorEnd(i), 2, 600);
    fprintf('Chyba nalezena (BattV), čas: %4.2f sec, trvani erroru: %4.2f sec\n', BattErrorStart(i), (BattErrorEnd(i) - BattErrorStart(i)))
end
hold off;

% LoadV
figure('Renderer', 'painters', 'Position', [100 100 770 370]);
plot(output_HIL.X.Data, LoadV, 'LineWidth', 1.5)
grid on
grid minor
xlabel('time [s]')
ylabel('Load voltage')
xlim([0 Time(end)])
ylim([0 600])
hold on;
xregion(Time(1), Time(end), 1, 600);
for i = 1:length(LoadErrorStart)
    if ((LoadErrorEnd(i) - LoadErrorStart(i)) < LoadErrorTreshold) && faultErrorRemoval
        break;
    end
    xregion(LoadErrorStart(i), LoadErrorEnd(i), 2, 600);
    fprintf('Chyba nalezena (LoadV), čas: %4.2f sec, trvani erroru: %4.2f sec\n', LoadErrorStart(i), (LoadErrorEnd(i) - LoadErrorStart(i)))
end
hold off;

% SDC final
figure('Renderer', 'painters', 'Position', [100 100 770 370]);
plot(output_HIL.X.Data, SDC_final, 'LineWidth', 1.5)
grid on
grid minor
xlabel('time [s]')
ylabel('SDC final')
xlim([0 Time(end)])
ylim([0 25])
hold on;
xregion(Time(1), Time(end), 1, 25);
for i = 1:length(SDCErrorStart)
    if ((SDCErrorEnd(i) - SDCErrorStart(i)) < SDCErrorTreshold) && faultErrorRemoval
        break;
    end
    xregion(SDCErrorStart(i), SDCErrorEnd(i), 2, 25);
    fprintf('Chyba nalezena (SDC final), čas: %4.2f sec, trvani erroru: %4.2f sec\n', SDCErrorStart(i), (SDCErrorEnd(i) - SDCErrorStart(i)))
end
hold off;

% AIRs plot
figure('Renderer', 'painters', 'Position', [100 100 770 370]);
legend();
grid on
grid minor
xlabel('time [s]')
ylabel('PRE, POS, NEG CTRL')
xlim([0 Time(end)])
hold on;
plot(output_HIL.X.Data, NEG_CTRL, 'DisplayName', 'NEG CTRL', 'LineWidth', 1.5)
plot(output_HIL.X.Data, POS_CTRL, 'DisplayName', 'POS CTRL', 'LineWidth', 1.5)
plot(output_HIL.X.Data, PRE_CTRL, 'DisplayName', 'PRE CTRL', 'LineWidth', 1.5)
xregion(Time(1), Time(end), 1, 1);
for i = 1:length(AirErrorStart)
    if ((AirErrorEnd(i) - AirErrorStart(i)) < AirErrorTreshold) && faultErrorRemoval
        break;
    end
    xregion(AirErrorStart(i), AirErrorEnd(i), 2, 1);
    fprintf('Chyba nalezena (AIRs), čas: %4.2f sec, trvani erroru: %4.2f sec\n', AirErrorStart(i), (AirErrorEnd(i) - AirErrorStart(i)))
end
hold off;

% BMS status
figure('Renderer', 'painters', 'Position', [100 100 770 370]);
plot(output_HIL.X.Data, BMS_Status, 'LineWidth', 1.5)
grid on
grid minor
xlabel('time [s]')
ylabel('status')

%% debug
if mode == 2
    figure()
    plot(output_HIL.X.Data, ErrCode)
    grid on
    grid minor
    xlabel('time [s]')
    ylabel('error code')
end