mu_data = csvread("mu.csv");
time_mu = mu_data(:,1);
val_mu = mu_data(:,2);
vel_ref = [];


% pos = [0];
disp = 0.04;

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:3743,1);
vel_new = data(1:3743,4)/1000;
vel_old = data(1:3743,2)/1000;

%% Pos data
experimental_data = importdata("pos_z.tab");
data = experimental_data.data;
time_pos = data(1:3737,1);
pos_new = data(1:3737,4)/1000;
pos_old = data(1:3737,2)/1000;

bias_vec = ones([length(pos_new),1]);
bias_vec = pos_new(1)*bias_vec;

pos_new = pos_new - bias_vec;
pos_old = pos_old - bias_vec;

pos_ref = num_integration(vel_ref,time_mu);

%% 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_mu,-vel_ref')
hold off
xlabel('Time (sec)')
ylabel('Body Speed (m/s)')
xlim([0,30])
% legend('Vanilla','Ours','Theoretical')
fig = gcf;
print(fig,'vel.svg','-dsvg')
fontsize(15,"points")
figure(2)
% len_v = length(y);
hold on
% plot(time,-vel)
plot(time_pos,-pos_old,'Color',[0 0 0])
plot(time_pos,-pos_new,'LineWidth',1.5,'Color',[0.5 0.5 0.5])
plot(time_mu,-pos_ref)
xlim([0,30])
xlabel('Time (sec)')
ylabel('Body Displacement (m)')
hold off

% legend('Ours','Vanilla','Theoretical')
fig = gcf;
print(fig,'pos.svg','-dsvg')
fontsize(15,"points")

figure(3)
% len_v = length(y);
hold on
plot(time_ang_z,vel_ang_z_old,'LineWidth',1.5,'Color',[0.5 0.5 0.5])
plot(time_ang_z,vel_ang_z_new,'Color',[0 0 0])
hold off
xlim([0,30])
ylim([-0.1,0.1])
xlabel('Time (sec)')
ylabel('Angular Velocity (deg/sec)')
% legend('Vanilla','Ours')
fig = gcf;
print(fig,'ang_z.svg','-dsvg')
fontsize(15,"points")


figure(4)
% len_v = length(y);
hold on
% plot(time,-vel)
plot(time_ang_x,vel_ang_x_old,'LineWidth',1.5,'Color',[0.5 0.5 0.5])
plot(time_ang_x,vel_ang_x_new,'Color',[0 0 0])
xlim([0,30])
ylim([-0.1,0.1])
xlabel('Time (sec)')
ylabel('Angular Velocity (deg/sec)')
hold off
fig = gcf;
print(fig,'ang_x.svg','-dsvg')
fontsize(15,"points")
legend('Vanilla','Ours')



