clear all
close all
clc

data = csvread('Results_PID_move22/recorded_cop.csv');
% data_2 = csvread('Results/recorded_cop_filtered.csv');
%data_3 = csvread('Results_PID/recorded_bos.csv');

time = data(:,1);
time = time/100000;
cop_x = data(:,2);
cop_y = data(:,3);
cop_z = data(:,4);
 desvio_copx=[5];
  desvio_copy=[5];
% cop_x_filtered = data_2(:,2);
% cop_y_filtered = data_2(:,3);
% cop_z_filtered = data_2(:,4);

% bos_points = data_3(end,2:end-2);
% 
% bos_x = zeros(1,numel(bos_points)/2);
% bos_y = zeros(1,numel(bos_points)/2);
% 
% for i=1:numel(bos_points)/2
%     bos_x(i) = bos_points((i*2)-1);
%     bos_y(i) = bos_points(i*2);
% end 

for i=2:size(cop_x)
    desvio_copx=[desvio_copx; cop_x(i-1)-cop_x(i)];
    desvio_copy=[desvio_copy; cop_y(i-1)-cop_y(i)];
end

% -----------------------------------------------------------------------


figure('position', [80 80 420 320], 'color', [1 1 1])

AxisFontName = 'Helvetica';
AxisFontSize = 9;

subplot(2,1,1)
hold on
plot(time, cop_x, 'r', 'LineWidth',0.5);
plot(time,desvio_copx, 'g', 'LineWidth',1);

set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{x} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
grid on
axis([0 200 -0.075 0.075])


% -----------------------------------------------------------------------

subplot(2,1,2)
hold on
plot(time, cop_y, 'b', 'LineWidth',0.5);
plot(time,desvio_copy, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{y} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
box on
axis([0 200 -0.075 0.075])


set(gcf, 'NextPlot', 'add');
axes;
h = title('CoP Components', 'Fontsize', AxisFontSize, 'FontName', AxisFontName);
set(gca, 'Visible', 'off');
set(h, 'Visible', 'on');

%%export_fig cop_x_y -pdf -transparent
%----------------------------------------------------------------------------------

figure('position', [80 80 420 320], 'color', [1 1 1])

AxisFontName = 'Helvetica';
AxisFontSize = 9;

subplot(2,1,1)
hold on
plot(time, cop_x, 'r', 'LineWidth',0.5);
plot(time,desvio_copx, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{x} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
grid on
axis([200 400 -0.075 0.075])


% -----------------------------------------------------------------------

subplot(2,1,2)
hold on
plot(time, cop_y, 'b', 'LineWidth',0.5);
plot(time,desvio_copy, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{y} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
box on
axis([200 400 -0.075 0.075])


set(gcf, 'NextPlot', 'add');
axes;
h = title('CoP Components', 'Fontsize', AxisFontSize, 'FontName', AxisFontName);
set(gca, 'Visible', 'off');
set(h, 'Visible', 'on');



%----------------------------------------------------------------------------------

figure('position', [80 80 420 320], 'color', [1 1 1])

AxisFontName = 'Helvetica';
AxisFontSize = 9;

subplot(2,1,1)
hold on
plot(time, cop_x, 'r', 'LineWidth',0.5);
plot(time,desvio_copx, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{x} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
grid on
axis([400 600 -0.075 0.075])


% -----------------------------------------------------------------------

subplot(2,1,2)
hold on
plot(time, cop_y, 'b', 'LineWidth',0.5);
plot(time,desvio_copy, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{y} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
box on
axis([400 600 -0.075 0.075])


set(gcf, 'NextPlot', 'add');
axes;
h = title('CoP Components', 'Fontsize', AxisFontSize, 'FontName', AxisFontName);
set(gca, 'Visible', 'off');
set(h, 'Visible', 'on');



%----------------------------------------------------------------------------------


figure('position', [80 80 420 320], 'color', [1 1 1])

AxisFontName = 'Helvetica';
AxisFontSize = 9;

subplot(2,1,1)
hold on
plot(time, cop_x, 'r', 'LineWidth',0.5);
plot(time,desvio_copx, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{x} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
grid on
axis([600 800 -0.075 0.075])


% -----------------------------------------------------------------------

subplot(2,1,2)
hold on
plot(time, cop_y, 'b', 'LineWidth',0.5);
plot(time,desvio_copy, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{y} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
box on
axis([600 800 -0.075 0.075])


set(gcf, 'NextPlot', 'add');
axes;
h = title('CoP Components', 'Fontsize', AxisFontSize, 'FontName', AxisFontName);
set(gca, 'Visible', 'off');
set(h, 'Visible', 'on');



%----------------------------------------------------------------------------------




figure('position', [80 80 420 320], 'color', [1 1 1])

AxisFontName = 'Helvetica';
AxisFontSize = 9;

subplot(2,1,1)
hold on
plot(time, cop_x, 'r', 'LineWidth',0.5);
plot(time,desvio_copx, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{x} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
grid on
axis([800 1000 -0.075 0.075])


% -----------------------------------------------------------------------

subplot(2,1,2)
hold on
plot(time, cop_y, 'b', 'LineWidth',0.5);
plot(time,desvio_copy, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{y} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
box on
axis([800 1000 -0.075 0.075])


set(gcf, 'NextPlot', 'add');
axes;
h = title('CoP Components', 'Fontsize', AxisFontSize, 'FontName', AxisFontName);
set(gca, 'Visible', 'off');
set(h, 'Visible', 'on');


%----------------------------------------------------------------------------------


figure('position', [80 80 420 320], 'color', [1 1 1])

AxisFontName = 'Helvetica';
AxisFontSize = 9;

subplot(2,1,1)
hold on
plot(time, cop_x, 'r', 'LineWidth',0.5);
plot(time,desvio_copx, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{x} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
grid on
axis([1000 1200 -0.075 0.075])


% -----------------------------------------------------------------------

subplot(2,1,2)
hold on
plot(time, cop_y, 'b', 'LineWidth',0.5);
plot(time,desvio_copy, 'g', 'LineWidth',1);
set(gca,  'YTick', -0.075:0.025:0.075, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('CoP_{y} [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
box on
axis([1200 1400 -0.075 0.075])


set(gcf, 'NextPlot', 'add');
axes;
h = title('CoP Components', 'Fontsize', AxisFontSize, 'FontName', AxisFontName);
set(gca, 'Visible', 'off');
set(h, 'Visible', 'on');