clc;
close all;
clear all;

             
fig_export = figure;

fontName = 'Arial';
fontSize = 8;
fontWeight = 'normal';

thickLine = 2;
thinLine = 1;

fP = [];
dP = [];

idx = 1;
fP(idx).figIdx = 1;
fP(idx).dataIdx = [1 2 5];
fP(idx).plotRange = [0 1.05 0 1.06];
fP(idx).plotXTick = [0 0.53 1];
fP(idx).plotXTickLabel = {};
fP(idx).plotYTick = [0 1];
fP(idx).plotYTickLabel = {};
fP(idx).fontName = fontName;
fP(idx).fontSize = fontSize;
fP(idx).fontWeight = fontWeight;
fP(idx).xLabel = {'X (m)'};%{'Time (s)'};
fP(idx).yLabel = {'Y (m)'};
fP(idx).title  = {'Center Path'};
fP(idx).Units = 'centimeters';
fP(idx).PaperPosition = [0.25 1 4.5 4.5]; 
fP(idx).Position = fP(idx).PaperPosition./12;
fP(idx).AxisEqual = 0;
fP(idx).AxisSquare = 1;
fP(idx).flag_formatted = 0;


idx = 2;
fP(idx).figIdx = 1;
fP(idx).dataIdx = [3 4 6];
fP(idx).plotRange = [0 pi/2 0 pi/2];
fP(idx).plotXTick = [0 pi/4 pi/2];
fP(idx).plotXTickLabel = {'$0$','$\pi/4$','$\pi/2$'};
fP(idx).plotYTick = [0 1 1.3];
fP(idx).plotYTickLabel = {};
fP(idx).fontName = fontName;
fP(idx).fontSize = fontSize;
fP(idx).fontWeight = fontWeight;
fP(idx).xLabel = {'Tilt Angle (rad)'};
fP(idx).yLabel = {'Distance (m)'};
fP(idx).title  = {'Radius of Curvature'};
fP(idx).Units = 'centimeters';
fP(idx).PaperPosition = [4.75 1 4.5 4.5]; 
fP(idx).Position = fP(idx).PaperPosition./12;
fP(idx).AxisEqual = 0;
fP(idx).AxisSquare = 1;
fP(idx).flag_formatted = 0;
        

load('sim_rotateDisk_c1.mat');
path = calcPathProperties(sim.t, sim.x(:,7:9), sim.x(:,1:3),0);
vel = sim.x(:,1:3);
velM = sum(vel.^2,2).^0.5;
idx0 = min(find(velM > 0.001));
idx1 = max(find(velM > 0.001));

th = [pi:-0.01:pi/2];
timecirc = [min(sim.t):(max(sim.t)-min(sim.t))/(length(th)-1):max(sim.t)];
xcirc = cos(th)'+1;
ycirc = sin(th)';
xcircdt = dspline(timecirc,xcirc,timecirc);
ycircdt = dspline(timecirc,ycirc,timecirc);

circPath = calcPathProperties(timecirc,[xcirc zeros(size(xcirc)) ycirc],...
                              [xcircdt zeros(size(xcirc)) ycircdt],0);        

idx = 1;
    dP(idx).line = {'-'};
    dP(idx).lineWidth = [thinLine];
    dP(idx).lineColor = [0 0 0];
    dP(idx).xScaling = [1];
    dP(idx).yScaling = [1];
    dP(idx).xdata = xcirc;
    dP(idx).ydata = ycirc;
    dP(idx).special = [];

idx = 2;
    dP(idx).line = {'-'};
    dP(idx).lineWidth = [thinLine];
    dP(idx).lineColor = [0.5 0.5 0.5];
    dP(idx).xScaling = [1];
    dP(idx).yScaling = [1];
    dP(idx).xdata = sim.r(:,1);
    dP(idx).ydata = sim.r(:,3);
    dP(idx).special = [];

idx = 3;
    dP(idx).line = {'-'};
    dP(idx).lineWidth = [thinLine];
    dP(idx).lineColor = [0 0 0];
    dP(idx).xScaling = [1];
    dP(idx).yScaling = [1];
    dP(idx).xdata = pi-th;
    dP(idx).ydata = 1./circPath.k;
    dP(idx).special = [];   
    
idx = 4;
    dP(idx).line = {'-'};
    dP(idx).lineWidth = [thinLine];
    dP(idx).lineColor = [0.5 0.5 0.5];
    dP(idx).xScaling = [1];
    dP(idx).yScaling = [1];
    dP(idx).xdata = sim.x(idx0:idx1,11);
    dP(idx).ydata = 1./path.k(idx0:idx1);
    dP(idx).special = [];    


load('sim_rotateDisk_c10.mat');
path = calcPathProperties(sim.t, sim.x(:,7:9), sim.x(:,1:3),0);
vel = sim.x(:,1:3)-sim.copV;
velM = sum(vel.^2,2).^0.5;
idx0 = min(find(velM > 0.001));
idx1 = max(find(velM > 0.001));
       

idx = 5;
    dP(idx).line = {'-'};
    dP(idx).lineWidth = [thinLine];
    dP(idx).lineColor = [0.25 0.25 0.25];
    dP(idx).xScaling = [1];
    dP(idx).yScaling = [1];
    dP(idx).xdata = sim.r(:,1);
    dP(idx).ydata = sim.r(:,3);
    dP(idx).special = [];
    
idx = 6;
    dP(idx).line = {'-'};
    dP(idx).lineWidth = [thinLine];
    dP(idx).lineColor = [0.25 0.25 0.25];
    dP(idx).xScaling = [1];
    dP(idx).yScaling = [1];
    dP(idx).xdata = sim.x(idx0:idx1,11);
    dP(idx).ydata = 1./path.k(idx0:idx1);
    dP(idx).special = [];        
    
r = sum((sim.x(:,7:9)-sim.cop).^2,2).^0.5;    

idx = 7;
    dP(idx).line = {'--'};
    dP(idx).lineWidth = [thinLine];
    dP(idx).lineColor = [0.25 0.25 0.25];
    dP(idx).xScaling = [1];
    dP(idx).yScaling = [1];
    dP(idx).xdata = sim.x(idx0:idx1,11);
    dP(idx).ydata = r(idx0:idx1);
    dP(idx).special = [];

idx = 8;
rT = 1-exp(-10*sin(pi-th'));

    dP(idx).line = {'-.'};
    dP(idx).lineWidth = [thinLine];
    dP(idx).lineColor = [0.25 0.25 0.25];
    dP(idx).xScaling = [1];
    dP(idx).yScaling = [1];
    dP(idx).xdata = pi-th;
    dP(idx).ydata = rT;
    dP(idx).special = [];
    
%     
% 
% load('sim_rotateDisk_c100.mat');
% path = calcPathProperties(sim.t, sim.x(:,7:9), sim.x(:,1:3),0);
% vel = sim.x(:,1:3);
% velM = sum(vel.^2,2).^0.5;
% idx0 = min(find(velM > 0.001));
% idx1 = max(find(velM > 0.001));
%        
% 
% idx = 7;
%     dP(idx).line = {'-'};
%     dP(idx).lineWidth = [thinLine];
%     dP(idx).lineColor = [0 0 0];
%     dP(idx).xScaling = [1];
%     dP(idx).yScaling = [1];
%     dP(idx).xdata = sim.r(:,1);
%     dP(idx).ydata = sim.r(:,3);
%     dP(idx).special = [];
%     
% idx = 8;
%     dP(idx).line = {'-'};
%     dP(idx).lineWidth = [thinLine];
%     dP(idx).lineColor = [0 0 0];
%     dP(idx).xScaling = [1];
%     dP(idx).yScaling = [1];
%     dP(idx).xdata = sim.x(idx0:idx1,11);
%     dP(idx).ydata = 1./path.k(idx0:idx1);
%     dP(idx).special = [];   
    
for i=1:1:length(fP)    
    for j=1:1:length(fP(i).dataIdx)    
        idx = fP(i).dataIdx(j);    
        plotData(fig_export, dP(idx).xdata,dP(idx).ydata,dP(idx),fP(i));    
    end
end

for i=1:1:length(fP)
   formatFigure(fig_export,fP(i)); 
end

figure(fig_export);

set(gcf,'PaperUnits','centimeters');
dims = get(gcf,'PaperPosition');

width = 12;
sf = width/dims(3);
set(gcf,'PaperPosition', dims.*sf);

saveas(fig_export,'figures/fig_contact_kinematics.pdf');
    
%     subplot('Position',[0.5 0.1 0.4 0.4]);
%         plot(sim.x(idx0:idx1,11), 1./k(idx0:idx1),'Color',[0 0 0],'LineWidth',1);
%         hold on;
%         plot([0,pi/2]',[1,1]','--','Color',[0 0 0],'LineWidth',1);
%         axis([0 pi/2 0 1.5]);
%         set(gca,'XTick',[0,pi/4,pi/2],'YTick',[0,1,1.5],'FontName','Times');
%         
%         xticloc = get(gca,'xtick');
%         ay = axis;
%         set(gca,'xticklabel',[]);
%     
%         
%         xLblNice = {'$0$','$\pi/4$','$\pi/2$'};
%         for i=1:1:length(xticloc)
%            text(xticloc(i),ay(1)-0.1, xLblNice{i},...
%                 'HorizontalAlignment','Center','Interpreter','latex'); 
%         end
%         
%         
%         axis square
%         xlabel('Tilt Angle (rad)','Interpreter','latex');
%         ylabel('Distance (m)','Interpreter','latex');
%         title('Radius of Curvature','Interpreter','latex');
        
        
        
        