%% Laser Scans
clear all; clc;

syms X Y Z x_i alpha phi h;

a=h/tan(alpha)
b=h/(tan(alpha)*tan(phi))
c=-h

Y=(X-x_i)*b/a
Z=Y*c/b+h

%

%%
syms x_p y_p z_p a b c r;
rand_point=[x_p,y_p,z_p];
normal=[a,b,c];

plane = (x_p-X)*a+(y_p-Y)*b+(z_p-Z)*c == 0;
plane
sphere= (X-x_p)^2+(Y-y_p)^2+(Z-z_p)^2 <= r^2;
sphere
% sss=solve([plane,sphere],X)
solx = solve([plane,sphere],X);

%

circ=(z_p - h + (tan(alpha)*(a*x_p + b*y_p - c*(h - z_p)))/(a + b/tan(phi) - c*tan(alpha)))^2 + (x_p - (a*x_p + b*y_p - c*(h - z_p))/(a + b/tan(phi) - c*tan(alpha)))^2 + (y_p - (a*x_p + b*y_p - c*(h - z_p))/(tan(phi)*(a + b/tan(phi) - c*tan(alpha))))^2;
circ=simplify(circ)

%
%

hh=0.4;
centro_rand=[5,10,0];
norm_rand=[1,1,1];
% mudar para 220 pontos para ter 0.5graus de amplitude
pp = linspace(35.0/180*pi, 145.0/180.0*pi, 4);
aa=linspace(1,4,1);
Sol_points=zeros(3,numel(pp),numel(aa));
for ind_a = 1:numel(aa)
    for ind_p = 1:numel(pp)
        alph=aa(ind_a);
        ph=pp(ind_p);
        sol_X = subs(solx, [phi,alpha,h,a,b,c,x_p,y_p,z_p]...
            ,[ph,alph,hh,norm_rand(1),norm_rand(2),norm_rand(3)...
            ,centro_rand(1),centro_rand(2),centro_rand(3)]);
        
        sol_Y = subs(Y,[X,phi],[sol_X,ph]);
        
        sol_Z = subs(Z,[h,X,alpha],[hh,sol_X,alph]);
        
        Sol_points(1:3,ind_p,ind_a)=[sol_X,sol_Y,sol_Z];
    end
end

Sol_points(:,:,1)