clc;
clearvars;
close all;

% Load Data
ina229evm_data = readtable('../INA229EVM LOGS/Test3_1000RPM_EC60PressureTestMotor_INA229EVM.csv');
motor_pilot_data = readtable('../MotorPilotLogs/500RPM_EC60PressureTestMotorPilotLog_7_29_2025.csv');

%% Define Logging Rates
motor_pilot_sample_freq = 16000;             % Hz
ina229evm_sample_time = 16e-3;            % seconds

% Extract Columns
i_q_meas   = motor_pilot_data.I_Q_MEAS(:);
v_bus_meas = ina229evm_data.EVM1VBUSResults_V_(:);
i_bus_meas = ina229evm_data.EVM1CURRENTResults_A_(:);

% Index
motor_pilot_idx = (0:height(motor_pilot_data)-1);
ina229evm_idx   = (0:height(ina229evm_data)-1);

%
%% Plot Raw Bus Current


figure;
plot(ina229evm_idx, i_bus_meas);
xlabel('Sample Index');
ylabel('Current (A)');
title('Raw Shunt Current Readings');

%
%% Clip Initial Garbage Samples


ina229evm_garbage_len = 927405;
i_bus_meas_clean = i_bus_meas(ina229evm_garbage_len:end);
clean_idx = ina229evm_idx(ina229evm_garbage_len:end);

figure;
plot(clean_idx, i_bus_meas_clean);
xlabel('Sample Index');
ylabel('Current (A)');
title('Clipped Shunt Current Readings');

%
%% Gaussian Smoothing


gaussian_window_size = round(0.5 / ina229evm_sample_time);  % Convert seconds to samples
smoothed_current = smoothdata(i_bus_meas_clean, 'gaussian', gaussian_window_size);

figure;
plot(clean_idx, smoothed_current);
xlabel('Sample Index');
ylabel('Current (A)');
title('Gaussian Smoothed Shunt Current');

%
%% Plot Smoothed Data in Time Domain


time_vector = clean_idx * ina229evm_sample_time;

figure;
plot(time_vector, smoothed_current);
xlabel('Time (s)');
ylabel('Current (A)');
title('Gaussian Smoothed Shunt Current (Time Domain)');

%
%% Create Timetable


time = ina229evm_sample_time * (0:length(i_bus_meas_clean)-1);
time_table = timetable(seconds(time(:)), i_bus_meas_clean, ...
    'VariableNames', {'Shunt_Current_A'});
signalAnalyzer(time_table);

%
%% Apply low pass filter to data

% low-pass filter object
Hd = CurrentShuntLPFilter();

% Apply the filter using zero-phase filtering
filtered_current = filtfilt(Hd.Numerator, 1, i_bus_meas_clean);

% Create time vector for plotting
Fs = 1 / ina229evm_sample_time;

% Plot the filtered result
figure;
plot(time, filtered_current);
xlabel('Time (s)');
ylabel('Filtered Current (A)');
title('Filtered Shunt Current Using Low-Pass Filter');
grid on;

figure;
plot((0:length(i_bus_meas_clean)-1), filtered_current);
xlabel('Sample idx');
ylabel('Filtered Current (A)');
title('Filtered Shunt Current Using Custom Low-Pass Filter');
grid on;

%
%% Take Avrage and STD over a range

pressure_start = 64712;
pressure_end = 70496;
rangeStats(filtered_current, pressure_start, pressure_end);

%

 function [avg, std_dev] = rangeStats(data, idx_start, idx_end)

    sub_data = data(idx_start:idx_end);

    avg = mean(sub_data);
    std_dev = std(sub_data);

    fprintf('Data from index %d to %d:\n', idx_start, idx_end);

    fprintf('\nStatistics:\n');
    fprintf('Mean           = \n%.6f\n', avg);
    fprintf('Standard Dev   = \n%.6f\n', std_dev);
end

