%% project for Guanqun Bao

clc;
close all;
clear all;
step=3;


C=load('li.txt');
si_x=C(1:step:end);
si_y=C(2:step:end);
si_z=C(3:step:end);
h3=plot3(si_x,si_y,si_z,'k.');
hold on


p1=[si_x' si_y' si_z'];
[t1]=MyCrustOpen(p1);
temp=rgb('darkgreen');
surf3=trisurf(t1,p1(:,1),p1(:,2),p1(:,3),'facecolor','white','edgecolor',[temp]); %plot della superficie
alpha(surf3,0.05);

R=load('skeleton_large_intestine.txt');
plot3(R(:,1),R(:,2),R(:,3),'r*');
hold on;

% Pp=[80.61 -48.81 -580.8];
% Pc=[80.61 -46.81 -580.8];

Pp=[51.6100   20.8100 -645.8000];
Pc=[51.6100   20.8100 -645.8000];

Data_x=zeros(1000,1);
Data_y=zeros(1000,1);
Data_z=zeros(1000,1);

iteration=0;
flag=1;

for i=1:30

Pn_1=[Pc(1) Pc(2)+1 Pc(3)];
Pn_2=[Pc(1) Pc(2)-1 Pc(3)];
Pn_3=[Pc(1)+1 Pc(2) Pc(3)];
Pn_4=[Pc(1)-1 Pc(2) Pc(3)];
Pn_5=[Pc(1) Pc(2) Pc(3)+1];
Pn_6=[Pc(1) Pc(2) Pc(3)-1];
Pn_7=[Pc(1)+1 Pc(2)+1 Pc(3)+1];
Pn_8=[Pc(1)-1 Pc(2)+1 Pc(3)+1];
Pn_9=[Pc(1)-1 Pc(2)-1 Pc(3)+1];
Pn_10=[Pc(1)+1 Pc(2)-1 Pc(3)+1];
Pn_11=[Pc(1)+1 Pc(2)+1 Pc(3)-1];
Pn_12=[Pc(1)-1 Pc(2)+1 Pc(3)-1];
Pn_13=[Pc(1)+1 Pc(2)-1 Pc(3)-1];
Pn_14=[Pc(1)-1 Pc(2)-1 Pc(3)-1];
%legend(surf3,'small intestine');

% Set up the starting point
% Pc means current point
% Pn means the neighborhood of current point Pc

D_min=100;
D1=zeros(1926,1);
D2=zeros(1926,1);
D3=zeros(1926,1);
D4=zeros(1926,1);
D5=zeros(1926,1);
D6=zeros(1926,1);
D7=zeros(1926,1);
D8=zeros(1926,1);
D9=zeros(1926,1);
D10=zeros(1926,1);
D11=zeros(1926,1);
D12=zeros(1926,1);
D13=zeros(1926,1);
D14=zeros(1926,1);

D_b=zeros(10,1);

for m=1:1926
    D1(m)=E_dis(Pn_1(1),Pn_1(2),Pn_1(3),si_x(m),si_y(m),si_z(m));
    D2(m)=E_dis(Pn_2(1),Pn_2(2),Pn_2(3),si_x(m),si_y(m),si_z(m));
    D3(m)=E_dis(Pn_3(1),Pn_3(2),Pn_3(3),si_x(m),si_y(m),si_z(m));
    D4(m)=E_dis(Pn_4(1),Pn_4(2),Pn_4(3),si_x(m),si_y(m),si_z(m));
    D5(m)=E_dis(Pn_5(1),Pn_5(2),Pn_5(3),si_x(m),si_y(m),si_z(m));
    D6(m)=E_dis(Pn_6(1),Pn_6(2),Pn_6(3),si_x(m),si_y(m),si_z(m));
    D7(m)=E_dis(Pn_7(1),Pn_7(2),Pn_7(3),si_x(m),si_y(m),si_z(m));
    D8(m)=E_dis(Pn_8(1),Pn_8(2),Pn_8(3),si_x(m),si_y(m),si_z(m));
    D9(m)=E_dis(Pn_9(1),Pn_9(2),Pn_9(3),si_x(m),si_y(m),si_z(m));
    D10(m)=E_dis(Pn_10(1),Pn_10(2),Pn_10(3),si_x(m),si_y(m),si_z(m));
    D11(m)=E_dis(Pn_11(1),Pn_11(2),Pn_11(3),si_x(m),si_y(m),si_z(m));
    D12(m)=E_dis(Pn_12(1),Pn_12(2),Pn_12(3),si_x(m),si_y(m),si_z(m));  
    D13(m)=E_dis(Pn_13(1),Pn_13(2),Pn_13(3),si_x(m),si_y(m),si_z(m));
    D14(m)=E_dis(Pn_14(1),Pn_14(2),Pn_14(3),si_x(m),si_y(m),si_z(m)); 
end

D_max(1)=E_dis(Pn_1(1),Pn_1(2),Pn_1(3),Pp(1),Pp(2),Pp(3));
D_max(2)=E_dis(Pn_2(1),Pn_2(2),Pn_2(3),Pp(1),Pp(2),Pp(3));
D_max(3)=E_dis(Pn_3(1),Pn_3(2),Pn_3(3),Pp(1),Pp(2),Pp(3));
D_max(4)=E_dis(Pn_4(1),Pn_4(2),Pn_4(3),Pp(1),Pp(2),Pp(3));
D_max(5)=E_dis(Pn_5(1),Pn_5(2),Pn_5(3),Pp(1),Pp(2),Pp(3));
D_max(6)=E_dis(Pn_6(1),Pn_6(2),Pn_6(3),Pp(1),Pp(2),Pp(3));
D_max(7)=E_dis(Pn_7(1),Pn_7(2),Pn_7(3),Pp(1),Pp(2),Pp(3));
D_max(8)=E_dis(Pn_8(1),Pn_8(2),Pn_8(3),Pp(1),Pp(2),Pp(3));
D_max(9)=E_dis(Pn_9(1),Pn_9(2),Pn_9(3),Pp(1),Pp(2),Pp(3));
D_max(10)=E_dis(Pn_10(1),Pn_10(2),Pn_10(3),Pp(1),Pp(2),Pp(3));
D_max(11)=E_dis(Pn_11(1),Pn_11(2),Pn_11(3),Pp(1),Pp(2),Pp(3));
D_max(12)=E_dis(Pn_12(1),Pn_12(2),Pn_12(3),Pp(1),Pp(2),Pp(3));
D_max(13)=E_dis(Pn_13(1),Pn_13(2),Pn_13(3),Pp(1),Pp(2),Pp(3));
D_max(14)=E_dis(Pn_14(1),Pn_14(2),Pn_14(3),Pp(1),Pp(2),Pp(3));


D_min(1)=min(D1);   
D_min(2)=min(D2);  
D_min(3)=min(D3);  
D_min(4)=min(D4); 
D_min(5)=min(D5); 
D_min(6)=min(D6);
D_min(7)=min(D7);   
D_min(8)=min(D8);  
D_min(9)=min(D9);  
D_min(10)=min(D10); 
D_min(11)=min(D11); 
D_min(12)=min(D12);
D_min(13)=min(D13);   
D_min(14)=min(D14);  

if(i>10)
    [r,c,v]=find(D_min<7);
    D_min(c)=0;
end



if (max(D_min)==0)
    plot3(Pc(1),Pc(2),Pc(3),'blue o');
    disp('no where to go!')
    break;
end

D_all=D_max*2+D_min;
[D_ele,Index] = max(D_all); 

switch Index
  case 1 
      Pp=Pc;
      Pc=Pn_1;
  case 2
      Pp=Pc;
      Pc=Pn_2;
  case 3 
      Pp=Pc;
      Pc=Pn_3;
  case 4
      Pp=Pc;
      Pc=Pn_4;
  case 5 
      Pp=Pc;
      Pc=Pn_5;
  case 6
      Pp=Pc;
      Pc=Pn_6;
  case 7 
      Pp=Pc;
      Pc=Pn_7;
  case 8
      Pp=Pc;
      Pc=Pn_8;
  case 9 
      Pp=Pc;
      Pc=Pn_9;
  case 10
      Pp=Pc;
      Pc=Pn_10;
  case 11 
      Pp=Pc;
      Pc=Pn_11;
  case 12
      Pp=Pc;
      Pc=Pn_12;
  case 13 
      Pp=Pc;
      Pc=Pn_13;
  case 14
      Pp=Pc;
      Pc=Pn_14;
  otherwise
    dsip('wrong');
end


if (i>10)
    for n=i-10:-1:1
        if (E_dis(Pc(1),Pc(2),Pc(3),Data_x(n),Data_y(n),Data_z(n))<4)
            flag=0;
            break;    
        end
    end
end

if (flag==0)
    disp('warning! form loops!')
    break;
end

Data_x(i)=Pc(1);
Data_y(i)=Pc(2);
Data_z(i)=Pc(3);

plot3(Pc(1),Pc(2),Pc(3),'yellow*');
    
iteration=iteration+1
end

% plot3(Data_x(:),Data_y(:),Data_z(:),'yellow*');
%plot3(Pc(1),Pc(2),Pc(3),'blue o')