clear all
close all
clc

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

time = data(:,1);
time = time/100000;
cop_x = data(:,2);
cop_y = data(:,3);
cop_z = data(:,4);
 
% 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 

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

AxisFontName = 'Helvetica';
AxisFontSize = 9;

rectangle('Position',[-0.089,-0.0543,0.060,0.110], 'Curvature',0.1, 'LineWidth',1, 'LineStyle','-', 'FaceColor','w')   
hold on
rectangle('Position',[0.029,-0.0543,0.060,0.110], 'Curvature',0.1, 'LineWidth',1, 'LineStyle','-', 'FaceColor','w')   
hold on
plot(cop_x, cop_y, 'b', 'LineWidth',0.5);

radius = 0.005;
rectangle('Position',[0.037-radius,0.0447-radius,0.01,0.01], 'Curvature',[1,1], 'LineWidth',0.5, 'LineStyle','-', 'FaceColor',[0.5 0.5 0.5])   
hold on
rectangle('Position',[0.081-radius,0.0447-radius,0.01,0.01], 'Curvature',[1,1], 'LineWidth',0.5, 'LineStyle','-', 'FaceColor',[0.5 0.5 0.5])   
hold on
rectangle('Position',[0.081-radius,-0.0453-radius,0.01,0.01], 'Curvature',[1,1], 'LineWidth',0.5, 'LineStyle','-', 'FaceColor',[0.5 0.5 0.5])   
hold on
rectangle('Position',[0.037-radius,-0.0453-radius,0.01,0.01], 'Curvature',[1,1], 'LineWidth',0.5, 'LineStyle','-', 'FaceColor',[0.5 0.5 0.5])   
hold on

rectangle('Position',[-0.037-radius,0.0447-radius,0.01,0.01], 'Curvature',[1,1], 'LineWidth',0.5, 'LineStyle','-', 'FaceColor',[0.5 0.5 0.5])   
hold on
rectangle('Position',[-0.081-radius,0.0447-radius,0.01,0.01], 'Curvature',[1,1], 'LineWidth',0.5, 'LineStyle','-', 'FaceColor',[0.5 0.5 0.5])   
hold on
rectangle('Position',[-0.081-radius,-0.0453-radius,0.01,0.01], 'Curvature',[1,1], 'LineWidth',0.5, 'LineStyle','-', 'FaceColor',[0.5 0.5 0.5])   
hold on
rectangle('Position',[-0.037-radius,-0.0453-radius,0.01,0.01], 'Curvature',[1,1], 'LineWidth',0.5, 'LineStyle','-', 'FaceColor',[0.5 0.5 0.5])   
hold on

text(0.059, 0.0447, 'Front', 'horizontalAlignment', 'center', 'verticalAlignment', 'middle', 'FontName', AxisFontName, 'FontSize', 8);
hold on
text(-0.059, 0.0447, 'Front', 'horizontalAlignment', 'center', 'verticalAlignment', 'middle', 'FontName', AxisFontName, 'FontSize', 8);
hold on
text(0.059, -0.0453, 'Right', 'horizontalAlignment', 'center', 'verticalAlignment', 'middle', 'FontName', AxisFontName, 'FontSize', 8);
hold on
text(-0.059, -0.0453, 'Left', 'horizontalAlignment', 'center', 'verticalAlignment', 'middle', 'FontName', AxisFontName, 'FontSize', 8);


% for i=1:2:numel(bos_points)-1
%     plot(bos_points(i), bos_points(i+1), 'or', 'LineWidth',0.7)
%     hold on
% end
%     
% plot(bos_x, bos_y, '-r')

grid on
box on
axis([-0.1500 0.1500 -0.125 0.125]); 
% axis equal

AxisFontName = 'Helvetica';
AxisFontSize = 9;

set(gca, 'XTick', -0.15:0.05:0.15, 'YTick', -0.125:0.025:0.125, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

title('CoP Trajectory', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
xlabel('X [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('Y [m]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

export_fig cop_trajectory -pdf -transparent


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


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

AxisFontName = 'Helvetica';
AxisFontSize = 9;

subplot(2,1,1)
plot(time, cop_x, 'r', 'LineWidth',0.5);

set(gca, 'XTick', 0:5:60, '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 60 -0.075 0.075])

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

subplot(2,1,2)
plot(time, cop_y, 'b', 'LineWidth',0.5);

set(gca, 'XTick', 0:5:60, '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 60 -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
