%% Convert format

clear all;clc

main_folder = '/home/jorge/Escritorio/kitti/Person/';
trial_folder = fullfile(main_folder,'2011_09_28_drive_0183');
pose_text_folder = fullfile(trial_folder,'pose_text/data');

convertPoseData(pose_text_folder,fullfile(pose_text_folder,'results.txt'));


break
%% Plot trial

addpath matpcl

clear all;clc

main_folder = '/home/jorge/Escritorio/kitti/Person/';
trial_folder = fullfile(main_folder,'2011_09_28_drive_0171');
pose_text_folder = fullfile(trial_folder,'pose_text/data');
pcd_filtered_folder = fullfile(trial_folder,'pcd_filtered/data');
    
files = dir(fullfile(pose_text_folder,'*.txt'));

figure(1)
set(gcf,'position',[100 200 800 800])
clf

rot = rotation_angle_axis(-pi/2,[0 0 1])*rotation_angle_axis(-pi/2,[1 0 0]);

map=colormap('jet');
map=map(1:floor(64/14):64,:);

for i=8:length(files)
    
    
    clf

%     pcd_file_name = files(i).name;
%     pcd_file_name = [pcd_file_name(1:end-3) 'pcd'];
%     pcd_filtered_path = fullfile(pcd_filtered_folder,pcd_file_name);
%     
%     %Load filtered pcd path
%     pcd = loadpcd(pcd_filtered_path);
% 
%     x=pcd(:,:,1);
%     y=pcd(:,:,2);
%     z=pcd(:,:,3);
%     r=pcd(:,:,4);
%     g=pcd(:,:,5);
%     b=pcd(:,:,6);
%     
%     colors=[r(:)';g(:)';b(:)'];
%     P=[x(:)';y(:)';z(:)'];
%     
%     invalidInf = isinf(P(1,:));
%     invalidNaN = isnan(P(1,:));
%     
%     invalid = logical(invalidInf + invalidNaN);
%     
%     P(:,invalid)=[];
%     colors(:,invalid)=[];
%     
%     Pr=rot*P;
%     
%     scatter3(Pr(1,:),Pr(2,:),Pr(3,:),12*ones(1,size(Pr,2)),colors','filled')
%     hold on
    
    
    file_name = fullfile(pose_text_folder,files(i).name);
    
    try
    pose = dlmread(file_name)';
    catch
        break
    end
    
    pose = rot*pose;
    
    %Extract individual elements
    head = pose(:,1);
    neck_base = pose(:,2);
    left_shoulder = pose(:,3);
    right_shoulder = pose(:,4);
    center_torso = pose(:,5);
    left_torso = pose(:,6);
    right_torso = pose(:,7);
    waist = pose(:,8);
    left_hip = pose(:,9);
    right_hip = pose(:,10);
    left_knee = pose(:,11);
    right_knee = pose(:,12);
    left_foot = pose(:,13);
    right_foot = pose(:,14);
    
    %Draw lines
    plot3([head(1) neck_base(1)],[head(2) neck_base(2)],[head(3) neck_base(3)])
    
    hold on
    
    plot3([left_shoulder(1) right_shoulder(1)],[left_shoulder(2) right_shoulder(2)],[left_shoulder(3) right_shoulder(3)])
    
    plot3([neck_base(1) center_torso(1)],[neck_base(2) center_torso(2)],[neck_base(3) center_torso(3)])
    
    plot3([center_torso(1) waist(1)],[center_torso(2) waist(2)],[center_torso(3) waist(3)])
    
    plot3([left_torso(1) right_torso(1)],[left_torso(2) right_torso(2)],[left_torso(3) right_torso(3)])
    
    plot3([left_shoulder(1) left_torso(1)],[left_shoulder(2) left_torso(2)],[left_shoulder(3) left_torso(3)])
    plot3([right_shoulder(1) right_torso(1)],[right_shoulder(2) right_torso(2)],[right_shoulder(3) right_torso(3)])
    
    plot3([left_torso(1) left_hip(1)],[left_torso(2) left_hip(2)],[left_torso(3) left_hip(3)])
    plot3([right_torso(1) right_hip(1)],[right_torso(2) right_hip(2)],[right_torso(3) right_hip(3)])
    
    plot3([left_hip(1) right_hip(1)],[left_hip(2) right_hip(2)],[left_hip(3) right_hip(3)])
    
    plot3([left_hip(1) left_knee(1)],[left_hip(2) left_knee(2)],[left_hip(3) left_knee(3)])
    plot3([right_hip(1) right_knee(1)],[right_hip(2) right_knee(2)],[right_hip(3) right_knee(3)])
    
    plot3([left_knee(1) left_foot(1)],[left_knee(2) left_foot(2)],[left_knee(3) left_foot(3)])
    plot3([right_knee(1) right_foot(1)],[right_knee(2) right_foot(2)],[right_knee(3) right_foot(3)])
    
    %
    
    xp = [left_shoulder(1) right_shoulder(1) right_torso(1) left_torso(1)];
    yp = [left_shoulder(2) right_shoulder(2) right_torso(2) left_torso(2)];
    zp = [left_shoulder(3) right_shoulder(3) right_torso(3) left_torso(3)];
    
    color = map(4,:);
    fill3(xp,yp,zp,color)
    
    xp = [left_torso(1) right_torso(1) right_hip(1) left_hip(1)];
    yp = [left_torso(2) right_torso(2) right_hip(2) left_hip(2)];
    zp = [left_torso(3) right_torso(3) right_hip(3) left_hip(3)];
    
    color = map(5,:);
    fill3(xp,yp,zp,color)
    
    %Draw points
    color = map(1,:);
    plot3(head(1),head(2),head(3),'o','markersize',18,'color',color,'markerfacecolor',color);
    
    
    color = map(2,:);
    plot3(neck_base(1),neck_base(2),neck_base(3),'o','markersize',6,'color',color,'markerfacecolor',color);
    
    color = map(3,:);
    plot3(left_shoulder(1),left_shoulder(2),left_shoulder(3),'o','markersize',10,'color',color,'markerfacecolor',color);
    
    color = map(8,:);
    plot3(right_shoulder(1),right_shoulder(2),right_shoulder(3),'o','markersize',10,'color',color,'markerfacecolor',color);
    
    color = map(4,:);
    plot3(left_torso(1),left_torso(2),left_torso(3),'o','markersize',6,'color',color,'markerfacecolor',color);
    
    color = map(9,:);
    plot3(right_torso(1),right_torso(2),right_torso(3),'o','markersize',6,'color',color,'markerfacecolor',color);
    
    color = map(13,:);
    plot3(center_torso(1),center_torso(2),center_torso(3),'o','markersize',6,'color',color,'markerfacecolor',color);
    
    color = map(14,:);
    plot3(waist(1),waist(2),waist(3),'o','markersize',6,'color',color,'markerfacecolor',color);
    
    color = map(5,:);
    plot3(left_hip(1),left_hip(2),left_hip(3),'o','markersize',10,'color',color,'markerfacecolor',color);
    
    color = map(10,:);
    plot3(right_hip(1),right_hip(2),right_hip(3),'o','markersize',10,'color',color,'markerfacecolor',color);
    
    color = map(6,:);
    plot3(left_knee(1),left_knee(2),left_knee(3),'o','markersize',10,'color',color,'markerfacecolor',color);
    
    color = map(11,:);
    plot3(right_knee(1),right_knee(2),right_knee(3),'o','markersize',10,'color',color,'markerfacecolor',color);
    
    color = map(7,:);
    plot3(left_foot(1),left_foot(2),left_foot(3),'o','markersize',10,'color',color,'markerfacecolor',color);
    
    color = map(12,:);
    plot3(right_foot(1),right_foot(2),right_foot(3),'o','markersize',10,'color',color,'markerfacecolor',color);
    
    
 
    xlabel('x')
    ylabel('y')
    zlabel('z')
    
    axis equal
    
    pause(0.5)
end

%% Iso curves

clear all;clc
figure(1)
set(gcf,'position',[100 200 800 800])
clf

l1=[0 0 0]';
l2=[0.5 0.5 0]';

max_c = 1;
res=50;
x=linspace(0 ,max_c ,res);
y=linspace(-max_c ,max_c ,res);
z=linspace(-max_c ,max_c ,res);

[X Y Z]=meshgrid(x,y,z);

% d=sqrt(X.*X + Y.*Y + Z.*Z);
p=[X(:) Y(:) Z(:)];
d=zeros(size(X));
for i=1:size(p,1)
    d(i)=distancePointToLineSegment(p(i,:)',l1,l2);
end


k=6;
delta=0.2;

f=(k/delta).*(power(d./delta,k-1)).*exp(-power(d./delta,k));
%f=d;

N=10;
fmax = 0.5*max(max(max(f)));
fmin = 0.0001;
map=colormap('hot');


dn=linspace(fmin,fmax,N);
for i=1:N
    color_i = max(((64-1)/(fmax-fmin))*dn(i),1);
    color_i = min(color_i,64);
    color = map(round(color_i),:);

    p=patch(isosurface(x,y,z,f,dn(i)));
    isonormals(d, p)                              %# compute and set normals
    set(p, 'FaceColor',color, 'EdgeColor','none','FaceAlpha',0.5)   %# set surface props
    daspect([1 1 1])                              %# axes aspect ratio    
end

view(3), axis vis3d tight, box on, grid on    %# set axes props

camproj perspective                           %# use perspective projection
% camlight, lighting phong           %# enable light, set transparency
% , alpha(1-dn)



%% Compress space in one allong vector

clear all;clc

figure(1)
set(gcf,'position',[100 200 800 800])
clf

v1 = [1 1 1];
v1 = v1/norm(v1);


v2 = [1 0 0];
if v1(3)==0
    v2 = [0 0 1];
else
    v2(3) = (-v1(1)*v2(1) - v2(1)*v2(2))/v1(3);
end

v2=v2/norm(v2);

v3=cross(v2,v1);

plot3([0 v1(1)],[0 v1(2)],[0 v1(3)],'r')
hold on
grid on
axis([-1 1 -1 1 -1 1])

plot3([0 v2(1)],[0 v2(2)],[0 v2(3)],'g')
plot3([0 v3(1)],[0 v3(2)],[0 v3(3)],'b')

xlabel('x')
ylabel('y')
zlabel('z')

N=2000;
ps = randn(N,3)*0.3;

dmax = 0.8;
map=colormap('jet');
for i=1:N
    p=ps(i,:);
    d=sqrt(p(1)*p(1) + p(2)*p(2) + p(3)*p(3));
    
    %Calculate distance allong v1, v2, v3
    
    dv1 = dot(p,v1)*5;
    dv2 = dot(p,v2);
    dv3 = dot(p,v3);
    
    d=sqrt(dv1*dv1+dv2*dv2+dv3*dv3);
    
    
    color_i = ((64-1)/dmax)*d+1;
    color_i = min(color_i,64);
    color = map(round(color_i),:);
    
    plot3(p(1),p(2),p(3),'o','color',color,'markerfacecolor',color,'markersize',5);
    
end

%% Complex score function

clf
% d=linspace(0,0.5,1000);
d = linspace(-2*pi,2*pi,1000);

k=2;
y=0.05;

f1=min(1./d,50);
f2=min(1./d,25);
f3=raylpdf(d,0.3);
f4=(k/y).*(power(d./y,k-1)).*exp(-power(d./y,k));

k=10;
y=0.35;
f4_1 = (k/y).*(power(d./y,k-1)).*exp(-power(d./y,k));
f4_1=f4_1*0.2;

% f4= f4+ f4_1;

period=0.4;
f5=min((1./d).*cos(d*(2*pi/period)),50);
f5(d>(3/4)*period)=0;
f6= atan(d);

% plot(d,f1)
hold on
% plot(d,f2,'r')
% plot(d,f3,'k')
% plot(d,f4,'k')
% plot(d,f5,'r')
plot(d,f6,'r')

figure(1)
axis([0 0.5 0 1])
plot([0 0.5],[0 0.5],'r')
% p=ginput();

plot(p(:,1),p(:,2))

%% 2d ellipse fit

clear all;close all;clc

addpath(genpath('fitellipse/'))

ellipsedemo



%%

p=[2 -0. 3.2]';
a=1;
b=0.5;
c=[-0.5 1 0.5]';
u=[0.866025 0 -0.5]';
v=[-0.482963 -0.258819 -0.836516]';

[min_dist, f_min] = distanceEllipsePoints(p,a,b,c,u,v)

%% Ellipse distances

N=3000;
psamples = (rand(N,3)-0.5)*2;


figure(1)
set(gcf,'position',[100 200 800 800])
clf

a=0.8;
b=0.3;
c=[0 0 0]';
u=[1 0 0]';
v=[0 1 0]';

theta = 0;
phi = 0;

rotSec = rotation_angle_axis(theta,[0 0 1]);
rotMain= rotation_angle_axis(phi,[1 0 0]);

u=rotSec*rotMain*u;
v=rotSec*rotMain*v;

h=ellipse(a,b,-rot,c(1),c(2));


hold on

[min_dist, f_min] = distanceEllipsePoints(psamples,a,b,c,u,v);

dmax=0.8;

map=colormap('jet');
for i=1:N
    d=min_dist(i);
    
    color_i = ((64-1)/dmax)*d+1;
    color_i = min(color_i,64);
    color = map(round(color_i),:);
    
    plot3(psamples(i,1),psamples(i,2),psamples(i,3),'o','color',color,'markerfacecolor',color,'markersize',5);
    
end

%% Colomap tests
clear all;close all;clc

map = colormap('autum');

% mr=map(1:5:end,:);
mr=map;

for i=1:size(mr,1)
    fprintf('%.4f, %.4f, %.4f,\n',mr(i,1),mr(i,2),mr(i,3));
    
end


%%


clear all;close all;clc

l1=[0 0 0]';
l2=[1 0 0]';

N=2000;
ps=randn(N,3)*0.5;

figure(1)
set(gcf,'position',[100 200 800 800])
clf

plot3([l1(1) l2(1)],[l1(2) l2(2)],[l1(3) l2(3)],'r');
hold on
grid on
axis([-2 2 -2 2 -2 2])

dmax=2;

map=colormap('jet');
for i=1:N
    d=distancePointToLineSegment(ps(i,:)',l1,l2);
    
    color_i = ((64-1)/dmax)*d+1;
    color_i = min(color_i,64);
    color = map(round(color_i),:);
    
    plot3(ps(i,1),ps(i,2),ps(i,3),'o','color',color,'markerfacecolor',color,'markersize',5);
    
end

%%
clear all;clc

figure(1)

max_theta=pi/6;
theta = linspace(-max_theta,max_theta,200);


R=1;
a=1;
b=1;
x0=sqrt((R*R)/a);

tv = theta*x0/(max_theta);
theta=tv;
%phi=(cos(theta*2)+5)*0.1;


phi = sqrt((R*R-a*theta.*theta)/b);

% plot(x,y)
plot(theta,phi)


%%
figure(2)
set(gcf,'position',[100 200 800 800])
clf

sp = [0 0 0]';
ep = [0 0 0.8]';

mdir = [sqrt(2) sqrt(2) 0];

vec_1 = ep-sp;
r=norm(vec_1);

vec_1=vec_1/r;

vec_2 = [1 0 0];
if vec_1(3)==0
    vec_2 = [0 0 1];
else
    vec_2(3) = (-vec_1(1)*vec_2(1) - vec_2(1)*vec_2(2))/vec_1(3);
end

vec_2=vec_2/norm(vec_2);

mdir_p = cross(mdir,vec_1);


plot3([sp(1) mdir(1)],[sp(2) mdir(2)],[sp(3) mdir(3)],'k')
hold on
grid on
axis([-1 1 -1 1 -1 1])

plot3([sp(1) ep(1)],[sp(2) ep(2)],[sp(3) ep(3)],'r');
plot3([sp(1) mdir_p(1)],[sp(2) mdir_p(2)],[sp(3) mdir_p(3)],'g')

neg_angle = deg2rad(30);
pos_angle = deg2rad(50);
neg_deviation = deg2rad(10);

for i=1:100
    max_theta=deg2rad(50);
    theta = ((rand(1)-0.5)*2)*max_theta;
    
    R=deg2rad(10);
    a=1;
    b=1;
    
    x0 =sqrt((R*R)/a);
    theta_i = theta*x0/max_theta;

    pos_rot = sqrt((R*R-a*theta_i.*theta_i)/b);
    
    
    f0 = (pi/2)/max_theta;
    scale = 0.5;
    
    min_limit = (cos(theta*f0)-1)*neg_angle;
    max_limit = (cos(theta*f0)-1)*neg_angle + pos_angle;
    
    randfactor = rand(1);
    
    phi= randfactor*min_limit+(1-randfactor)*max_limit;

    rotMain = rotation_angle_axis(theta,mdir_p);
    rotSec = rotation_angle_axis(phi,mdir);
    rotThird = rotation_angle_axis(neg_deviation,mdir);

    vec_r = rotThird*rotSec*rotMain*vec_1;

    pf=sp+vec_r*r;
    
    plot3([sp(1) pf(1)],[sp(2) pf(2)],[sp(3) pf(3)])
    

end

%% Non simetric shape

figure(2)
set(gcf,'position',[100 200 800 800])
clf

sp = [0 0 0]';
ep = [0 0 0.8]';

mdir = [sqrt(2) sqrt(2) 0];

vec_1 = ep-sp;
r=norm(vec_1);

vec_1=vec_1/r;

vec_2 = [1 0 0];
if vec_1(3)==0
    vec_2 = [0 0 1];
else
    vec_2(3) = (-vec_1(1)*vec_2(1) - vec_2(1)*vec_2(2))/vec_1(3);
end

vec_2=vec_2/norm(vec_2);

mdir_p = cross(mdir,vec_1);


plot3([sp(1) mdir(1)],[sp(2) mdir(2)],[sp(3) mdir(3)],'k')
hold on
grid on
axis([-1 1 -1 1 -1 1])

plot3([sp(1) ep(1)],[sp(2) ep(2)],[sp(3) ep(3)],'r');
plot3([sp(1) mdir_p(1)],[sp(2) mdir_p(2)],[sp(3) mdir_p(3)],'g')

for i=1:360
    max_theta=deg2rad(50);
%     theta = ((rand(1)-0.5)*2)*max_theta;
    theta = ((rand(1)-0.5)*2)*max_theta;
    
    R=deg2rad(30);
    a=1;
    b=1;
    
    x0 =sqrt((R*R)/a);
    theta_i = theta*x0/max_theta;

    phi = sqrt((R*R-a*theta_i.*theta_i)/b);
    phi = ((rand(1)-0.5)*2)*phi;
    
    rotMain = rotation_angle_axis(theta,mdir_p);
    rotSec = rotation_angle_axis(phi,mdir);
    rot2Main = rotation_angle_axis(-pi/6,mdir_p);

    vec_r = rot2Main*rotSec*rotMain*vec_1;

    pf=sp+vec_r*r;
    
    plot3([sp(1) pf(1)],[sp(2) pf(2)],[sp(3) pf(3)])
   
end

%% plot tear drop shape

figure(1)
set(gcf,'position',[100 200 800 800])
clf

t=linspace(0,2*pi);

m=1;

x=cos(t);
y=sin(t).*power(sin(0.5*t),m);

plot(x,y)


%% Hip rotation estimation


figure(1)
set(gcf,'position',[100 200 800 800])
clf

pwaist = [0 0 0];
p_angle = 0;
length=1.0;
max_theta=deg2rad(90);

p1=[0 -length/2 0];
p2=[0 length/2 0];

po=[p1; p2]';

plot3(pwaist(1),pwaist(2),pwaist(3),'ko','markerfacecolor',[0 0 0],'markersize',5)
hold on

% plot3(ps(1,:),ps(2,:),ps(3,:),'ro','markerfacecolor',[1 0 0],'markersize',5)

for N=1:20
    theta = p_angle + ((rand(1)-0.5)*2)*max_theta;
    rotMain = rotation_angle_axis(theta,[0 0 1]);

    ps=rotMain*po;
    
    plot3([ps(1,1) ps(1,2)],[ps(2,1) ps(2,2)],[ps(3,1) ps(3,2)],'r','markerfacecolor',[1 0 0],'markersize',5)
    
end



% plot3([pwaist(1) p_direction(1)],[pwaist(2) p_direction(2)],[pwaist(3) p_direction(3)],'k')

hold on
grid on
axis([-1 1 -1 1 -1 1])