addpath('cal_ref_vel')
mu1 = 0.66;
mu2 = 0.83;
ref_vel_vec = [];

% mu_data = csvread("mu.csv");
% time_mu = mu_data(:,1);
% val_mu = mu_data(:,2);
% vel_ref = [];
% for i = 1:length(val_mu)
%     velocity = -2*pi*disp/(2*pi*val_mu(i));
%     vel_ref(end+1)=velocity;
% end

%% Vel data
experimental_data = importdata("vel_z.tab");
data = experimental_data.data;
time_vel = data(:,1);
vel_new = data(:,4)/1000;
vel_old = data(:,2)/1000;
for i = 1:length(vel_new)
    [ref_v,val_mu] = reference_vel(mu1,mu2,time_vel(i));
    ref_vel_vec(end+1) = ref_v;
end


%% Ang_z data
experimental_data = importdata("ang_vel_z.tab");
data = experimental_data.data;
time_ang_z = data(:,1);
vel_ang_z_old = data(:,2)/1000;
vel_ang_z_new = data(:,4)/1000;

%% Ang_x data
experimental_data = importdata("ang_vel_x.tab");
data = experimental_data.data;
time_ang_x = data(:,1);
vel_ang_x_old = data(:,2)/1000;
vel_ang_x_new = data(:,4)/1000;

figure(1)

% len_v = length(y);
hold on
% plot(time,-vel)

plot(time_vel,-vel_old,'LineWidth',1.5,'Color',[0.5 0.5 0.5])
plot(time_vel,-vel_new,'Color',[0 0 0])
plot(time_vel,-ref_vel_vec')
hold off
xlabel('Time (sec)')
ylabel('Body Speed (m/s)')
xlim([0,30])


%% Vel Dev
dev_vel_new_vec = [];
dev_vel_old_vec = [];

start_index = find(time_vel==10);
end_index = find(time_vel==20);

length_v = end_index - start_index;

for i=start_index:end_index
    dev_vel_new_vec(end+1) = (vel_new(i)-ref_vel_vec(i))^2;
    dev_vel_old_vec(end+1) = (vel_old(i)-ref_vel_vec(i))^2;
end

dev_vel_new = sqrt(sum(dev_vel_new_vec)/(length_v-1))
dev_vel_old = sqrt(sum(dev_vel_old_vec)/(length_v-1))

%% angx Dev
dev_angx_new_vec = [];
dev_angx_old_vec = [];

start_index = find(time_ang_x==10);
end_index = find(time_ang_x==20);

length_angx = end_index - start_index;

for i=start_index:end_index
    dev_angx_new_vec(end+1) = (vel_ang_x_new(i)-0)^2;
    dev_angx_old_vec(end+1) = (vel_ang_x_old(i)-0)^2;
end

dev_angx_new = sqrt(sum(dev_angx_new_vec)/(length_angx-1))
dev_angx_old = sqrt(sum(dev_angx_old_vec)/(length_angx-1))

%% angz Dev
dev_angz_new_vec = [];
dev_angz_old_vec = [];

start_index = find(time_ang_z==10);
end_index = find(time_ang_z==20);

length_angz = end_index - start_index;

for i=start_index:end_index
    dev_angz_new_vec(end+1) = (vel_ang_z_new(i)-0)^2;
    dev_angz_old_vec(end+1) = (vel_ang_z_old(i)-0)^2;
end

dev_angz_new = sqrt(sum(dev_angz_new_vec)/(length_angz-1))
dev_angz_old = sqrt(sum(dev_angz_old_vec)/(length_angz-1))


