%We are using a threshold-based version for bpm estimation
%Only the z axis of the acc is used

%NOTE: depending on the measurement device we have a highly different sample rate. the smartwatches are not capable of providing a constant sample rate. the xsens on the other hand is able to do this.

%load file provided by the sensor readout app
measurements = dlmread('../measurements/recording_80bpm_4-4_173835787.txt', ';'); %400hz
%measurements = dlmread('../measurements/PR_recording_80bpm_4-4_177596720.txt', ';'); %400hz
%measurements = dlmread('../measurements/recording_48bpm_4-4_176527527.txt', ';'); %400hz *
%measurements = dlmread('../measurements/recording_48bpm_4-4_176606785.txt', ';'); %400hz
%measurements = dlmread('../measurements/recording_48bpm_4-4_176696356.txt', ';'); %400hz
%measurements = dlmread('../measurements/recording_48bpm_4-4_176820066.txt', ';'); %400hz *
%measurements = dlmread('../measurements/recording_48bpm_4-4_176931941.txt', ';'); %400hz
%measurements = dlmread('../measurements/recording_72bpm_4-4_176381633.txt', ';'); %400hz
%measurements = dlmread('../measurements/recording_72bpm_4-4_176453327.txt', ';'); %400hz
%measurements = dlmread('../measurements/recording_100bpm_4-4_176073767.txt', ';'); %400hz
%measurements = dlmread('../measurements/recording_100bpm_4-4_176165357.txt', ';'); %400hz
%measurements = dlmread('../measurements/recording_100bpm_4-4_176230146.txt', ';'); %400hz *
%measurements = dlmread('../measurements/recording_100bpm_4-4_176284687.txt', ';'); %400hz
%measurements = dlmread('../measurements/recording_100bpm_4-4_177368860.txt', ';'); %400hz (besonders)
%measurements = dlmread('../measurements/recording_120bpm_4-4_165987552.txt', ';'); %400hz defect
%measurements = dlmread('../measurements/recording_180bpm_4-4_177011641.txt', ';'); %400hz *
%measurements = dlmread('../measurements/recording_180bpm_4-4_177064915.txt', ';'); %400hz *
%measurements = dlmread('../measurements/recording_180bpm_4-4_177331664.txt', ';'); %400hz	defect

%save the bpm into its own little matrix
bpm_idx = (measurements(:,2)==99);
bpm = measurements(bpm_idx, :);

%draw the raw acc data
m_idx = [];
m_idx = (measurements(:,2)==2);
m = measurements(m_idx, :);

figure(1);
plot(m(:,1),m(:,3)) %x
legend("x", "location", "eastoutside");
hold ("on");
	plot([bpm(:,1), bpm(:,1)], [max(max(m(:,3))),min(min(m(:,3)))], ":", "color", [0.0, 0.0, 0.0]) %plot the bpm onto this data
hold ("off")

figure(2);
plot(m(:,1),m(:,4)) %y
legend("y", "location", "eastoutside");
hold ("on");
	plot([bpm(:,1), bpm(:,1)], [max(max(m(:,4))),min(min(m(:,4)))], ":", "color", [0.0, 0.0, 0.0]) %plot the bpm onto this data
hold ("off")

figure(3);
plot(m(:,1),m(:,5)) %z
legend("z", "location", "eastoutside");
hold ("on");
	plot([bpm(:,1), bpm(:,1)], [max(max(m(:,5))),min(min(m(:,5)))], ":", "color", [0.0, 0.0, 0.0]) %plot the bpm onto this data
hold ("off")

%save timestamps
timestamps = m(:,1);
data = m(:,3); %only z

window_size = 4096; %about 2 seconds using 2000hz and 10 sec for 400hz
overlap = 256; %0.64 seconds using 400hz
bpm_per_window_ms = [];
bpm_per_window = [];
for i = window_size+1:length(data)

	%wait until window is filled with new data
	if(mod(i,overlap) == 0)

		%measure periodicity of window and use axis with best periodicity
		[corr_x, lag_x] = xcov(m(i-window_size:i,3), (window_size/4), "coeff");
		[corr_y, lag_y] = xcov(m(i-window_size:i,4), (window_size/4), "coeff");
		[corr_z, lag_z] = xcov(m(i-window_size:i,5), (window_size/4), "coeff");

		corr_x(corr_x<0)=0;
		corr_y(corr_y<0)=0;
		corr_z(corr_z<0)=0;

		[peak_x, idx_x] = findpeaks(corr_x, "DoubleSided", "MinPeakHeight", 0.1,"MinPeakDistance", 50);
		[peak_y, idx_y] = findpeaks(corr_y, "DoubleSided", "MinPeakHeight", 0.1,"MinPeakDistance", 50);
		[peak_z, idx_z] = findpeaks(corr_z, "DoubleSided", "MinPeakHeight", 0.1,"MinPeakDistance", 50);

		mean_x = length(peak_x);
		mean_y = length(peak_y);
		mean_z = length(peak_z);

		%waitforbuttonpress();

		idx_x = sort(idx_x);
		idx_y = sort(idx_y);
		idx_z = sort(idx_z);

		Xwindow = m(i-window_size:i,3);
		Xwindow_mean_ts_diff = mean(diff(lag_x(idx_x) * 2.5));
		Xwindow_mean_bpm = (60000 / (Xwindow_mean_ts_diff));

		figure(11);
			plot(lag_x, corr_x, lag_x(idx_x), corr_x(idx_x), ".r") %z
		hold ("on")
			m_label_ms = strcat(" mean ms: ", num2str(Xwindow_mean_ts_diff));
			m_label_bpm = strcat(" mean bpm: ", num2str(Xwindow_mean_bpm));
			title(strcat(" ", m_label_ms, " ", m_label_bpm));
		hold ("off");

		Ywindow = m(i-window_size:i,4);
		Ywindow_mean_ts_diff = mean(diff(lag_y(idx_y) * 2.5));
		Ywindow_mean_bpm = (60000 / (Ywindow_mean_ts_diff));

		figure(12);
			plot(lag_y, corr_y, lag_y(idx_y), corr_y(idx_y), ".r") %z
		hold ("on")
			m_label_ms = strcat(" mean ms: ", num2str(Ywindow_mean_ts_diff));
			m_label_bpm = strcat(" mean bpm: ", num2str(Ywindow_mean_bpm));
			title(strcat(" ", m_label_ms, " ", m_label_bpm));
		hold ("off");

		Zwindow = m(i-window_size:i,5);
		Zwindow_mean_ts_diff = mean(diff(lag_z(idx_z)* 2.5));
		Zwindow_mean_bpm = (60000 / (Zwindow_mean_ts_diff));

		figure(13);
			plot(lag_z, corr_z, lag_z(idx_z), corr_z(idx_z), ".r") %z
		hold ("on")
			m_label_ms = strcat(" mean ms: ", num2str(Zwindow_mean_ts_diff));
			m_label_bpm = strcat(" mean bpm: ", num2str(Zwindow_mean_bpm));
			title(strcat(" ", m_label_ms, " ", m_label_bpm));
		hold ("off");


		%window = data(i-window_size:i,:);
		%window_timestamps = timestamps(i-window_size:i,:);	

		%choose axis with most points TODO: better method
		if(mean_x > mean_y && mean_x > mean_z)
			window_mean_ts_diff = Xwindow_mean_ts_diff;
			window_mean_bpm = Xwindow_mean_bpm;
		elseif(mean_y > mean_z)
			window_mean_ts_diff = Ywindow_mean_ts_diff;
			window_mean_bpm = Ywindow_mean_bpm;
		else
			window_mean_ts_diff = Zwindow_mean_ts_diff;
			window_mean_bpm = Zwindow_mean_bpm;
		endif

		bpm_per_window_ms = [bpm_per_window_ms, window_mean_ts_diff];
		bpm_per_window = [bpm_per_window, window_mean_bpm];
			
	endif
end

%remove the first 40% of the results, due to starting delays while recording.
number_to_remove = abs(0.4 * length(bpm_per_window_ms));
num_all = length(bpm_per_window_ms);
bpm_per_window_ms = bpm_per_window_ms(number_to_remove:num_all);
bpm_per_window = bpm_per_window(number_to_remove:num_all);

mean_final_ms = mean(bpm_per_window_ms)
std_final_ms = std(bpm_per_window_ms)

mean_final_bpm = mean(bpm_per_window)
std_final_bpm = std(bpm_per_window)