clear all
close all
clc

data = csvread('Results/recorded_force_stab.csv');
data_2 = csvread('Results/recorded_force_instab.csv');
data_3 = csvread('Results/recorded_force.csv');
data_4 = csvread('Results/recorded_force_filtered.csv');

time = data_3(:,1);
time = time/100000;
feedback_force_stability_x = data(:,2);
feedback_force_stability_y = data(:,3);
feedback_force_stability_z = data(:,4);

feedback_force_instability_x = data_2(:,2);
feedback_force_instability_y = data_2(:,3);
feedback_force_instability_z = data_2(:,4);

feedback_force_x = data_3(:,2);
feedback_force_y = data_3(:,3);
feedback_force_z = data_3(:,4);

feedback_force_x_filtered = data_4(:,2);
feedback_force_y_filtered = data_4(:,3);
feedback_force_z_filtered = data_4(:,4);


% beta = 0.4;
% feedback_force_x_filtered = zeros(size(data_3,1), 1);
% feedback_force_y_filtered = zeros(size(data_3,1), 1);
% feedback_force_z_filtered = zeros(size(data_3,1), 1);
% 
% for i=1:size(data_3,1)
%     
%     if i==1
%         feedback_force_x_filtered(i) = (1-beta)*feedback_force_x(i) + beta*feedback_force_x(i);
%         feedback_force_y_filtered(i) = (1-beta)*feedback_force_y(i) + beta*feedback_force_y(i);
%         feedback_force_z_filtered(i) = 0;
%     else
%         feedback_force_x_filtered(i) = (1-beta)*feedback_force_x_filtered(i-1) + beta*feedback_force_x(i);
%         feedback_force_y_filtered(i) = (1-beta)*feedback_force_y_filtered(i-1) + beta*feedback_force_y(i);
%         feedback_force_z_filtered(i) = 0;
%     end
% end

figure('position', [80 80 420 320], 'color', [1 1 1])
% plot(time, feedback_force_stability_x, 'g', 'LineWidth',1.5);
hold on
% plot(time, feedback_force_stability_y, 'g', 'LineWidth',1.5);
hold on
% plot(time, feedback_force_instability_x, 'r', 'LineWidth',1.5);
hold on
% plot(time, feedback_force_instability_y, 'r', 'LineWidth',1.5);
hold on
plot(time, feedback_force_x, 'color', [1 0.5 0], 'LineWidth',0.5);
hold on
plot(time, feedback_force_y, 'color', [0.3 0.6 1], 'LineWidth',0.5);
hold on
plot(time, feedback_force_x_filtered, 'r', 'LineWidth',0.5);
hold on
plot(time, feedback_force_y_filtered, 'b', 'LineWidth',0.5);

grid on
box on
axis ([0 20 -5 5])

legend('Force X', 'Force Y', 'Force X (filtered)', 'Force Y (filtered)')

AxisFontName = 'Helvetica';
AxisFontSize = 9;

set(gca, 'XTick', 0:2:20, 'YTick', -5:1:5, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

title('Force Rendering', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('Force [N]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

export_fig force -pdf -transparent