close all;
clear;

% Load and set up data
PSD_pwelch_setup_v2;

% Rename the variables for each axis
acc_x = brl_x;
acc_y = brr_y;
acc_z = brl_z;

% Define the lower and upper threshold values for each axis (absolute values)
lower_threshold_x = 1;
lower_threshold_y = 1;
lower_threshold_z = 0.1;

upper_threshold_x = 5;
upper_threshold_y = 5;
upper_threshold_z = 25;

% Find the indices of values outside both lower and upper thresholds for each axis
indices_below_lower_threshold_x = find(abs(acc_x) < lower_threshold_x);
indices_above_upper_threshold_x = find(abs(acc_x) > upper_threshold_x);

indices_below_lower_threshold_y = find(abs(acc_y) < lower_threshold_y);
indices_above_upper_threshold_y = find(abs(acc_y) > upper_threshold_y);

indices_below_lower_threshold_z = find(abs(acc_z) < lower_threshold_z);
indices_above_upper_threshold_z = find(abs(acc_z) > upper_threshold_z);

% Set those values to 0 for each axis
acc_x([indices_below_lower_threshold_x; indices_above_upper_threshold_x]) = 0;
acc_y([indices_below_lower_threshold_y; indices_above_upper_threshold_y]) = 0;
acc_z([indices_below_lower_threshold_z; indices_above_upper_threshold_z]) = 0;

% Find the indices of the areas to cut out for each axis
indices_to_remove_x = find(acc_x == 0);
indices_to_remove_y = find(acc_y == 0);
indices_to_remove_z = find(acc_z == 0);

% Remove the areas with 0 for each axis
acc_x_cut = acc_x;
acc_x_cut(indices_to_remove_x) = [];
acc_y_cut = acc_y;
acc_y_cut(indices_to_remove_y) = [];
acc_z_cut = acc_z;
acc_z_cut(indices_to_remove_z) = [];

% Create a folder to save the plots
folderName = 'filtered_cut_signal';
if ~exist(folderName, 'dir')
    mkdir(folderName);
end

% Calculate time vector based on sampling frequency
sampling_frequency = 500; % Replace with your actual sampling frequency
t = (0:length(brr_x)-1) / sampling_frequency;
%% Original signal
figure('Position', [100, 100, 1280, 720]);  % Set plot dimensions
subplot(3,1,1);
plot(t, brl_x, 'Color', [0.3, 0.3, 0.3]);
title('X-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
saveas(gcf, fullfile(folderName, 'original_x_axis.png'));

subplot(3,1,2);
plot(t, brr_y, 'Color', [0.3, 0.3, 0.3]);
title('Y-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
saveas(gcf, fullfile(folderName, 'original_y_axis.png'));

subplot(3,1,3);
plot(t, brl_z, 'Color', [0.3, 0.3, 0.3]);
title('Z-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
saveas(gcf, fullfile(folderName, 'original_z_axis.png'));

%% Single Plots for Z-Axis
figure('Position', [100, 100, 1280, 720]);  % Set plot dimensions
subplot(1,1,1);
plot(t, brl_z, 'Color', [0.3, 0.3, 0.3]);
title('Pump bracket L Z-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
saveas(gcf, fullfile(folderName, 'single_z_axis.png'));

%% Filtered 
figure('Position', [100, 100, 1280, 720]);  % Set plot dimensions
subplot(3,1,1);
plot(t, acc_x, 'Color', [0.3, 0.3, 0.3]);
title('Filtered X-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
ylim([min(acc_x) max(acc_x)]);
saveas(gcf, fullfile(folderName, 'filtered_x_axis.png'));

subplot(3,1,2);
plot(t, acc_y, 'Color', [0.3, 0.3, 0.3]);
title('Filtered Y-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
ylim([min(acc_y) max(acc_y)]);
saveas(gcf, fullfile(folderName, 'filtered_y_axis.png'));

subplot(3,1,3);
plot(t, acc_z, 'Color', [0.3, 0.3, 0.3]);
title('Filtered Z-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
ylim([min(acc_z) max(acc_z)]);
saveas(gcf, fullfile(folderName, 'filtered_z_axis.png'));

%% Single Plot for Filtered Z-Axis
figure('Position', [100, 100, 1280, 720]);  % Set plot dimensions
subplot(1,1,1);
plot(t, acc_z, 'Color', [0.3, 0.3, 0.3]);
title('Pump bracket L Filtered Z-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
ylim([min(acc_z) max(acc_z)]);
saveas(gcf, fullfile(folderName, 'single_filtered_z_axis.png'));
%% Filtered and cut out
figure('Position', [100, 100, 1280, 720]);  % Set plot dimensions
subplot(3,1,1);
plot((1:length(acc_x_cut)) / sampling_frequency, acc_x_cut, 'Color', [0.3, 0.3, 0.3]);
title('Filtered and Cut X-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
saveas(gcf, fullfile(folderName, 'filtered_cut_x_axis.png'));

subplot(3,1,2);
plot((1:length(acc_y_cut)) / sampling_frequency, acc_y_cut, 'Color', [0.3, 0.3, 0.3]);
title('Filtered and Cut Y-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
saveas(gcf, fullfile(folderName, 'filtered_cut_y_axis.png'));

subplot(3,1,3);
plot((1:length(acc_z_cut)) / sampling_frequency, acc_z_cut, 'Color', [0.3, 0.3, 0.3]);
title('Filtered and Cut Z-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
saveas(gcf, fullfile(folderName, 'filtered_cut_z_axis.png'));

%% Single Plot for Filtered and Cut Z-Axis
figure('Position', [100, 100, 1280, 720]);  % Set plot dimensions
subplot(1,1,1);
plot((1:length(acc_z_cut)) / sampling_frequency, acc_z_cut, 'Color', [0.3, 0.3, 0.3]);
title('Pump bracket L Filtered and Cut Z-Axis Acceleration');
xlabel('Time (s)');
ylabel('Acceleration (g)');
saveas(gcf, fullfile(folderName, 'single_filtered_cut_z_axis.png'));
%% RMS

% Compute RMS of signals
rms_brr = rms(brl_z);
rms_acc = rms(acc_z);
rms_acc_cut = rms(acc_x_cut);

% Print RMS values
fprintf('RMS of blr_x: %f\n', rms_brr);
fprintf('RMS of acc_x: %f\n', rms_acc);
fprintf('RMS of acc_x_cut: %f\n', rms_acc_cut);

% Display RMS values
figure;
bar([rms_brr, rms_acc, rms_acc_cut]);
xticklabels({'brr_x', 'acc_x', 'acc_x_cut'});
ylabel('RMS value');
title('RMS values of signals');

%% Find and print the maximum value of 't'
max_t = max((1:length(acc_z_cut)) / sampling_frequency);
fprintf('The maximum value of t is: %f\n', max_t);

%% hist fit

%% Use histfit on the variable brl_z
figure;
histfit(brl_z);

% Add labels and title
xlabel('Value');
ylabel('Frequency');
title('Histogram and Fitted Distribution of brl_z');

figure;
histfit(acc_z_cut);

% Add labels and title
xlabel('Value');
ylabel('Frequency');
title('Histogram and Fitted Distribution of brl_z');
