clc; clear; close all

file_k = 1;
tZoom = 20+[0 0.5];

%% Read file and assign variables

fileNames = dir('*vary1_28s_20130618*.mat');
for j = 1 : max(size(fileNames))
    TEMP_IDX(j) = fileNames(j).datenum;
end
[TEMP IDX] = sort(TEMP_IDX,'descend');

disp(fileNames(IDX(file_k)).name);
load(fileNames(IDX(file_k)).name);
t = t - 5.575;

t = downsample(t,1);
r = downsample(r,1)+0.1;
y = downsample(y,1)+0.1;
e = downsample(e,1);
u = downsample(u,1)+0.1;

e1 = -0.3 * ones(size(t));
e2 = 0.3 * ones(size(t));

samp_fac = 10;

figure('Name','TrackingData_AngleDomain_N10');
scaleData = [5 6]*1.125;
set(gcf, 'PaperPosition', [1 7 scaleData]);

subplot(4,2,1); plot(t,r,'-k','Linewidth',1); hold on; grid on;
ylabel('r (mm)'); ylim([-1 4]+0.1); xlim([0 0.5]);
% xlabel('t (s)');
subplot(4,2,2); plot(t,r,'-k','Linewidth',1); hold on; grid on;
ylabel('r (mm)'); ylim([-1 4]+0.1); xlim([max(t)-0.5 max(t)]);
% xlabel('t (s)');

subplot(4,2,[3 4]); plot(downsample(t,samp_fac),downsample(e2,samp_fac),'--m','Linewidth',0.75); hold on; grid on;
subplot(4,2,[3 4]); plot(downsample(t,samp_fac),downsample(e ,samp_fac),'-g','Linewidth',1); hold on; grid on;
subplot(4,2,[3 4]); plot(downsample(t,samp_fac),downsample(e1,samp_fac),'--m','Linewidth',0.75); hold on; grid on;
subplot(4,2,[3 4]); plot(downsample(t,samp_fac),downsample(e2,samp_fac),'--m','Linewidth',0.75); hold on; grid on;
% xlabel('t (s)');
ylabel('e (mm)'); ylim([-2.5 2.5]); xlim([0 max(t)]);
legend('+/- 0.3mm');


subplot(4,2,[5 6]); plot(t,r,'-k','Linewidth',1); hold on; grid on;
subplot(4,2,[5 6]); plot(t,y,'-r','Linewidth',1); hold on; grid on;
ylabel('r, y (mm)');ylim([-1 4]+0.1); xlim(tZoom); xlabel('t (s)');

subplot(4,2,[7 8]); plot(t,u,'-b','Linewidth',1); hold on; grid on;
ylabel('u (mm)'); ylim([-1 3.5]+0.1); xlim(tZoom); xlabel('t (s)');


IDX = find(u~=circshift(u,1));
subplot(4,2,1); plot(t(IDX),r(IDX),'*k','Linewidth',1); hold on; grid on;
subplot(4,2,2); plot(t(IDX),r(IDX),'*k','Linewidth',1); hold on; grid on;
subplot(4,2,[7 8]); plot(t(IDX),u(IDX),'*b','Linewidth',1); hold on; grid on;




%%

clc;

IDX1 = find(t > 0.05   & t < 5);
IDX2 = find(t > 5);

maxE_1 = max(abs(e(IDX1)));
maxE_2 = max(abs(e(IDX2)));

RMSE_1 = sqrt((e(IDX1)' * e(IDX1)) / length(IDX1));
RMSE_2 = sqrt((e(IDX2)' * e(IDX2)) / length(IDX1));

disp('--------------');
disp(maxE_1);
disp(RMSE_1);
disp('--------------');
disp(maxE_2);
disp(RMSE_2);


%%