clc;
close all;
clear all;

% L1 - 14
% L2 - 15
% R9 - 22
% R10 - 23
% LHipAngleX - 470
% RHipAngleX - 486
% LKneeAngleX - 474
% RKneeAngleY - 490

fileName_a = '..\data\cw_1_chod_do_przodu_T01a.CSV';
fileName_v = '..\data\cw_1_chod_do_przodu_T01v.CSV';

% fileName_a = '..\data\cw_3_chod_szybki_do_przodu_T01a.CSV';
% fileName_v = '..\data\cw_3_chod_szybki_do_przodu_T01v.CSV';

% fileName_a = '..\data\cw_3_chod_szybki_do_przodu_T02a.CSV';
% fileName_v = '..\data\cw_3_chod_szybki_do_przodu_T02v.CSV';

importedData = importdata(fileName_a,  ',', 1);
data1 = importedData.data(:, 22)';
data2 = importedData.data(:, 23)';

importedData = importdata(fileName_v,  ',', 1);
dataPosition = importedData.data(:, 490)';

fs = 1000;
N = size(data1, 2);
time = 0:(1/fs):((N-1)/fs);

color1 = 'magenta';
color2 = 'r';

%% high pass filter
[B,A] = cheby1(5, 2, 6/fs*2, 'high');

[h,w] = freqz(B,A);
mag = abs(h);
phase = angle(h);

x = 0.5*(1:size(mag,1))/size(mag,1);

filteredData1 = filter(B,A, data1);
filteredData2 = filter(B,A, data2);
%% MAV
window = 10;

mavData1 = mav(data1, window);
mavFilteredData1 = mav(filteredData1, window);
mavFilteredData1_2 = mavFilteredData1.^2;

mavData2 = mav(data2, window);
mavFilteredData2 = mav(filteredData2, window);
mavFilteredData2_2 = mavFilteredData2.^2;

%% FFT
fftOryginal1 = abs(fft(data1));
fftOryginal2 = abs(fft(data2));
fftFiltered1 = abs(fft(filteredData1));
fftFiltered2 = abs(fft(filteredData2));

%% plot

% filter characteristic
hFilChar = figure;
subplot(2,1,1);
plot(x, mag);
title('magnitude characteristics');
XLabel('f/fs');
subplot(2,1,2);
plot(x, phase/pi*180);
title('phase characteristics');
XLabel('f/fs');

% FFT of signal 1
hSig1Char = figure;
subplot(2,1,1);
plot((0:(N/2-1))/N, fftOryginal1(1:N/2)/N*2, color1);
title('oryginal syginal [rectus femoris]');
XLabel('f/fs');
subplot(2,1,2);
plot((0:(N/2-1))/N, fftFiltered1(1:N/2)/N*2, color1);
title('high pass filter [rectus femoris]');
XLabel('f/fs');

% FFT of signal 2
hSig2Char = figure;
subplot(2,1,1);
plot((0:(N/2-1))/N, fftOryginal2(1:N/2)/N*2, color2);
title('oryginal syginal [rectus femoris]');
subplot(2,1,2);
plot((0:(N/2-1))/N, fftFiltered2(1:N/2)/N*2, color2);
title('high pass filter [rectus femoris]');

% MAV on signal 1
hSig1Mav = figure;
subplot(3,1,1);
plot(time, data1, color1);
title('oryginal data [rectus femoris]');
XLabel('time [s]');
subplot(3,1,2);
plot(time(1:10:end),mavFilteredData1, color1);
title('high pass and MAV [rectus femoris]');
XLabel('time [s]');
subplot(3,1,3);
plot(time(1:10:end),mavFilteredData1_2, color1);
title('signal energy [rectus femoris]');
XLabel('time [s]');

% MAV on signal 2
hSig2Mav = figure;
subplot(3,1,1);
plot(time, data2, color2);
title('oryginal data [biceps femoris]');
XLabel('time [s]');
subplot(3,1,2);
plot(time(1:10:end),mavFilteredData2, color2);
title('high pass and MAV [biceps femoris]');
XLabel('time [s]');
subplot(3,1,3);
plot(time(1:10:end),mavFilteredData2_2, color2);
title('signal energy [biceps femoris]');
XLabel('time [s]');

% data normaliation
dataPosition = dataPosition/90;
mavFilteredData1_2 = mavFilteredData1_2/max(mavFilteredData1_2);
mavFilteredData2_2 = mavFilteredData2_2/max(mavFilteredData2_2);

% plot summary
hSum = figure;
subplot(3,1,1);
plot(time(1:10:end), dataPosition, time(1:10:end), mavFilteredData1_2, color1);
title('summary [rectus femoris]');
legend('knee position', 'rectus femoris');
XLabel('time [s]');
subplot(3,1,2);
plot(time(1:10:end), dataPosition, time(1:10:end), mavFilteredData2_2, color2);
XLabel('time [s]');
title('summary [biceps femoris]');
legend('knee position', 'biceps femoris');
subplot(3,1,3);
plot(time(1:10:end), dataPosition, time(1:10:end), mavFilteredData1_2, color1, time(1:10:end), -mavFilteredData2_2, color2);
XLabel('time [s]');
title('summary');
legend('knee position', 'rectus femoris', 'biceps femoris');

%% figure saving
folderName = 'images';

if exist(folderName, 'dir') == 0
    mkdir(folderName);
end

delete('images\*');
saveas(hFilChar, 'images\fiterCharacteristic', 'png');
saveas(hSig1Char, 'images\signal1Characteritic', 'png');
saveas(hSig2Char, 'images\signal2Characteritic', 'png');
saveas(hSig1Mav, 'images\signal1Mav', 'png');
saveas(hSig2Mav, 'images\signal2Mav', 'png');
saveas(hSum, 'images\summary', 'png');