%% rms_exter_pos.m
clear; close all
foldername = '/Volumes/Persistence Lab/Fault Prognostication/data_logs/Integration-test-2017-03-07';

f_num = 4;  % set to 1 for first file, 2 for second file, etc...

%% import data
type = 'external';
fnames = getTestbedDatafiles(foldername, type);
ex_fname = fnames{f_num};
filename = [foldername filesep ex_fname]; % last file on list
ex_data = readinTestbedData(filename);

pos = ex_data.position.val;

% create equally-spaced time grid
t0 = ex_data.position.time_lv(1); 
t1 = ex_data.position.time_lv(end);
t = linspace(t0, t1, length(pos)); 


type = 'intrinsic';
fnames = getTestbedDatafiles(foldername, type);
in_fname = fnames{f_num};
filename = [foldername filesep in_fname]; % last file on list
in_data = readinTestbedData(filename);

s = in_data.state.val;
ts = in_data.state.time_lv; 


us = unique(s);

s_ = zeros(size(s));  % READY = 0
s_(strcmp(s,'MOVE')) = 1;  % MOVE = 1

% put data labels on external sensor time-grid
si = interp1(ts, s_, t, 'nearest-neighbour');
si(isnan(si)) = 0;

figure;
plot(t - t0, pos); hold on; 
plot(t(si==1) - t0, pos(si==1))
ylabel('mm')
xlabel('time elapsed (sec)')
legend('READY', 'MOVE')
title(ex_fname)

% x_fig([foldername '/plots/' ex_fname(1:(end-4)) '.png'])
%% compute RMS of transient ('MOVE')

% index first transient segment in dataset
id1 = find(si,1,'first');
id1 = id1:(id1 + find(~si(id1:end), 1, 'first'));

tm = t(id1);
m = pos(id1); xm = tm - tm(1);

b_m = polyfit(xm, m, 1);
m1 = polyval(b_m, xm);

RMSE_move = std(m - m1);  % Root Mean Squared Error

% visualize
figure;
plot(xm, m); hold on;
plot(xm, m1);
ylabel('mm')
xlabel('time elapsed (sec)')
legend('signal', 'fit')
title({ex_fname ,['Transient RMSE = ' num2str(RMSE_move) 'mm']})

% x_fig([foldername '/plots/' ex_fname(1:(end-4)) 'Transient RMSE.png'])
%% compute RMS of stationary ('READY')

id0 = 1:find(diff(si==0),1,'first')+1;  % index first stationary segment
% id0 = find(si,1,'last'):length(si);  % index last stationary segment

tr = t(id0);
r = pos(id0); xr = tr - tr(1);
b_r = polyfit(xr, r, 1);
r1 = polyval(b_r, xr);

RMSE_rest = std(r - r1);  % Root Mean Squared Error

% visualize
figure;
plot(xr, r); hold on;
plot(xr, r1);
ylabel('mm')
xlabel('time elapsed (sec)')
legend('signal', 'fit')
title({ex_fname ,['Stationary RMSE = ' num2str(RMSE_rest) 'mm']})

% x_fig([foldername '/plots/' ex_fname(1:(end-4)) 'Stationary RMSE.png'])