%% Energy Module data procesing
%
%% Load in the data

clc, close, clearvars;
data_30 = readtable("ProfileEnergy - 30m profile.csv");
data_100 = readtable("ProfileEnergy - 100m profile.csv");
data_350 = readtable("ProfileEnergy - 350m profile.csv");

%
%% Consts

BusVotage = 28.8;
BatteryCapacityJoules = 10948608;
PercentBateryForOther = 0.1;
ACBatteryCapacityJoules = BatteryCapacityJoules * (1 - PercentBateryForOther);

%
%% Caculate num profiles possable for 30

energy_30_sum = sum(data_30.Joules);
energy_30_elmo = sum(data_30.ElmoJoules);

energy_100_sum = sum(data_100.Joules);
energy_100_elmo = sum(data_100.ElmoJoules);

energy_350_sum = sum(data_350.Joules);
energy_350_elmo = sum(data_350.ElmoJoules);

depths = [30, 100, 350];
Count = [profilesInt(ACBatteryCapacityJoules, energy_30_sum), profilesInt(ACBatteryCapacityJoules, energy_100_sum), profilesInt(ACBatteryCapacityJoules, energy_350_sum)];
ElmoCount = [profilesInt(ACBatteryCapacityJoules, energy_30_elmo), profilesInt(ACBatteryCapacityJoules, energy_100_elmo), profilesInt(ACBatteryCapacityJoules, energy_350_elmo)];

figure;
plot(depths, Count);
hold on;
plot(depths, ElmoCount);

fig = figure('Color', 'w');


plot(depths, Count, 'LineWidth', 1, 'DisplayName', 'EVLSERVO1 at 1000 RPM', ...
    'Marker', 'x', 'MarkerSize', 4);
hold on;

plot(depths, ElmoCount, '--', 'LineWidth', 1, 'DisplayName', 'Elmo at 1000 RPM', ...
    'Marker', 'o', 'MarkerSize', 3);

grid on;

legend('Location', 'best', 'FontSize', 14);

xlabel('Profile Depth (m)', 'FontSize', 14, 'FontWeight', 'bold');
ylabel('Profile Count', 'FontSize', 14, 'FontWeight', 'bold');
title('Profile Improvments at Depths', 'FontSize', 16, 'FontWeight', 'bold');

set(gca, 'FontSize', 12, 'LineWidth', 1);
hold off;

%
%% Profile increases

increase = Count ./ ElmoCount;

%
%% Looking at pwr vs time


time_steps = zeros(1, 2 * 10);
elmo_pwr_steps = zeros(1, 2 * 10);
pwr_steps = zeros(1, 2 * 10);

segments = struct('start_time', {}, 'end_time', {}, 'elmo_pwr', {});

time_min = 0;

for i = 1:10
    start_time = time_min;
    end_time = time_min + data_100.Time_sec_(i);

    time_steps(2*i - 1) = start_time;
    time_steps(2*i) = end_time;

    elmo_pwr_value = data_100.ElmoCurrent(i) * BusVotage;
    manual_pwr_value = data_100.Current_byEye__A_(i) * BusVotage;

    elmo_pwr_steps(2*i - 1) = elmo_pwr_value;
    elmo_pwr_steps(2*i) = elmo_pwr_value;

    pwr_steps(2*i - 1) = manual_pwr_value;
    pwr_steps(2*i) = manual_pwr_value;

    segments(end+1).start_time = start_time;
    segments(end).end_time = end_time;
    segments(end).elmo_pwr = elmo_pwr_value;

    time_min = end_time;
end

figure;
plot(time_steps, elmo_pwr_steps, 'r-', 'DisplayName', 'Elmo');
hold on;
plot(time_steps, pwr_steps, 'b', 'DisplayName', 'EVALSERVO1','LineWidth', 1);
xlabel('Time (s)', 'FontSize', 16, 'FontWeight', 'bold');
ylabel('Power (W)', 'FontSize', 16, 'FontWeight', 'bold');
title('Power vs Time in a 100m Profile', 'FontSize', 16);
grid on;

yl = ylim;

legend_handles = [];
legend_labels = {};

added_sleep = false;
added_pwrdn = false;
added_run = false;

for i = 1:length(segments)
    st = segments(i).start_time;
    et = segments(i).end_time;
    pwr = segments(i).elmo_pwr;

    if pwr == 0
        color = [0.8 0.7 1];
        if ~added_pwrdn
            h = patch(NaN, NaN, color, 'FaceAlpha', 0.3, 'EdgeColor', 'none', 'DisplayName', 'Power Down');
            legend_handles(end+1) = h;
            legend_labels{end+1} = 'Power Down';
            added_pwrdn = true;
        end
    elseif pwr > 0 && pwr < 10
        color = [0.7 1 0.7];
        if ~added_sleep
            h = patch(NaN, NaN, color, 'FaceAlpha', 0.3, 'EdgeColor', 'none', 'DisplayName', 'Sleep');
            legend_handles(end+1) = h;
            legend_labels{end+1} = 'Sleep';
            added_sleep = true;
        end
    else
        color = [1, 0.85, 0.6];
        if ~added_run
            h = patch(NaN, NaN, color, 'FaceAlpha', 0.3, 'EdgeColor', 'none', 'DisplayName', 'Run');
            legend_handles(end+1) = h;
            legend_labels{end+1} = 'Run';
            added_run = true;
        end
    end

    patch([st et et st], [yl(1) yl(1) yl(2) yl(2)], ...
          color, 'FaceAlpha', 0.3, 'EdgeColor', 'none');
end

legend_handles(end+1) = plot(NaN, NaN, 'r-', 'DisplayName', 'Elmo');
legend_handles(end+1) = plot(NaN, NaN, 'b-', 'DisplayName', 'EVALSERVO1');
legend_labels{end+1} = 'Elmo';
legend_labels{end+1} = 'EVALSERVO1';

legend(legend_handles, legend_labels, 'FontSize', 14);

%
%% Profile Possable Solver

function n = profilesInt(batteryCap, energyPureProfile)
n = batteryCap/energyPureProfile;
end