%Laser Test
function laser_test
%focal_length = norm([740.22264   704.79837 ]);
focal_length = norm([320.22264   284.79837 ]);

principal = [319.50000   239.5000 ];
laser1 = [ 2.650245 	 -1.081858 	 67.464725 ]; % these are pixels
laser2 = [ 2.768218 	 0.689920 	 20.177195 ];
laser3 = [ 2.782151 	 -0.623514 	 46.423471 ];
laser4 = [ 1.405204 	 -3.200764 	 126.271832 ];
laser5 = [ 2.527773 	 -1.024197 	 47.930343 ];
laser6 = [ 3.160233 	 -0.070482 	 26.013074 ];
laser7 = [ 3.054851 	 0.705897 	 15.700427 ];
laser8 = [ 2.990557 	 0.620719 	 15.468127 ];
laser9 = [ 2.901431 	 0.539708 	 15.320106 ];
laser10= [ 3.186011 	 1.184264 	 14.084884 ];
% 
% real1 = [0.0547   -0.0640    0.9587]; % these are real coordinates
% real2 = [0.0181    0.0275    4.5537];
% real3 = [0.0510   -0.0165    1.8862];
% real4 = [0.0460   -0.0269    1.4206];
% real5 = [0.0606   -0.0308    0.8821];

mm_per_pixel1 = 0.0865; % mm/pixel
mm_per_pixel2 = 0.0298;
mm_per_pixel3 =  0.0709;
mm_per_pixel4 = 0.1845;
mm_per_pixel5 = 0.0736;
mm_per_pixel6 = 0.0388; % mm/pixel
mm_per_pixel7 = 0.0249;
mm_per_pixel8 =  0.0242;
mm_per_pixel9 = 0.0249;
mm_per_pixel10 = 0.0225;

real1 = laser1*mm_per_pixel1; %% mm distance from laser spot on object to image plane
real2 = laser2*mm_per_pixel2;
real3 = laser3*mm_per_pixel3;
real4 = laser4*mm_per_pixel4;
real5 = laser5*mm_per_pixel5;
real6 = laser6*mm_per_pixel6;
real7 = laser7*mm_per_pixel7;
real8 = laser8*mm_per_pixel8;
real9 = laser9*mm_per_pixel9;
real10 = laser10*mm_per_pixel10;

real_dist_per_box = 1.91; % millimeters

image = imread('laser_dist4.bmp');
figure
imshow(image)
hold on

plot(principal(1),principal(2),'b*')
hold on
[px1, py1] = ginput(1)
plot(px1,py1,'r*');


% [x1, y1] = ginput(1);
% plot(x1,y1,'r*')



% 
% real_bar = (real1+real2+real3+real4+real5)/5;
% r1 = real1 - real_bar;
% r2 = real2 - real_bar;
% r3 = real3 - real_bar;
% r4 = real4 - real_bar;
% r5 = real5 - real_bar;
% covariance = ((r1'*r1)+(r2'*r2)+(r3'*r3)+(r4'*r4)+(r5'*r5))/5;
% [U,S,V] = svd(covariance);
% largest_evector = V(:,1);
% 
% 
% 
 
% focal_length_in_millimeters = 15;
% 
% laser_spot1 = [2.5517   -1.254   focal_length_in_millimeters]; % distance from laser spot on image to camera center in mm
% laser_spot2 = [3.0545    0.7599   focal_length_in_millimeters];
% laser_spot3 = [3.2260   -0.4609   focal_length_in_millimeters];
% laser_spot4 = [1.1992   -2.8598   focal_length_in_millimeters];
% laser_spot5 = [2.8336   -1.0672   focal_length_in_millimeters];
% laser_spot6 = [3.2786   -0.0194   focal_length_in_millimeters];
% laser_spot7 = [3.5482    0.6847   focal_length_in_millimeters];
% laser_spot8 = [3.4001    0.6897   focal_length_in_millimeters];
% laser_spot9 = [3.4237    0.7345   focal_length_in_millimeters];
% laser_spot10 = [3.8137    1.1137   focal_length_in_millimeters];
laser_spot1 = [2.5517   -1.254   focal_length*mm_per_pixel1]; % distance from laser spot on image to camera center in mm
laser_spot2 = [3.0545    0.7599   focal_length*mm_per_pixel2];
laser_spot3 = [3.2260   -0.4609   focal_length*mm_per_pixel3];
laser_spot4 = [1.1992   -2.8598   focal_length*mm_per_pixel4];
laser_spot5 = [2.8336   -1.0672   focal_length*mm_per_pixel5];
laser_spot6 = [3.2786   -0.0194   focal_length*mm_per_pixel6];
laser_spot7 = [3.5482    0.6847   focal_length*mm_per_pixel7];
laser_spot8 = [3.4001    0.6897   focal_length*mm_per_pixel8];
laser_spot9 = [3.4237    0.7345   focal_length*mm_per_pixel9];
laser_spot10 = [3.8137    1.1137   focal_length*mm_per_pixel10];


% 
figure
hold on

laser_spot = -1*[(px1-principal(1))*mm_per_pixel10 (py1-principal(2))*mm_per_pixel10 (focal_length)*mm_per_pixel10]
plot3(laser_spot(1),laser_spot(2),laser_spot(3),'gv');
hold on




% 
% %t=-10:0.1:10; %laser beam
% %plot3(real_bar(1)+t*largest_evector(1),real_bar(2)+t*largest_evector(2),real_bar(3)+t*largest_evector(3),'r')
% hold on
 tt=-8:0.1:1; %ray coming into camera
 plot3(laser_spot(1).*tt,laser_spot(2).*tt,laser_spot(3).*tt,'b')
light_ray_points(1,:) = laser_spot(1).*tt;
light_ray_points(2,:) = laser_spot(2).*tt;
light_ray_points(3,:) = laser_spot(3).*tt;
 
% ttt=-2:0.01:2; %axes
% plot3(ttt,0,0,'.r')
% hold on
% plot3(0,ttt,0,'.g')
% hold on
% plot3(0,0,ttt*50,'.b')


hold on
plot3(0,0,0,'rv')
axis([-100 100 -100 100 -100 100])
grid on
scatter3(real1(1)+laser_spot1(1),real1(2)+laser_spot1(2),real1(3)+laser_spot1(3)) % distance from camera to laser spot on object
scatter3(real2(1)+laser_spot2(1),real2(2)+laser_spot2(2),real2(3)+laser_spot2(3))
scatter3(real3(1)+laser_spot3(1),real3(2)+laser_spot3(2),real3(3)+laser_spot3(3))
scatter3(real4(1)+laser_spot4(1),real4(2)+laser_spot4(2),real4(3)+laser_spot4(3))
scatter3(real5(1)+laser_spot5(1),real5(2)+laser_spot5(2),real5(3)+laser_spot5(3))
scatter3(real6(1)+laser_spot6(1),real6(2)+laser_spot6(2),real6(3)+laser_spot6(3))
scatter3(real7(1)+laser_spot7(1),real7(2)+laser_spot7(2),real7(3)+laser_spot7(3))
scatter3(real8(1)+laser_spot8(1),real8(2)+laser_spot8(2),real8(3)+laser_spot8(3))
scatter3(real9(1)+laser_spot9(1),real9(2)+laser_spot9(2),real9(3)+laser_spot9(3))
scatter3(real10(1)+laser_spot10(1),real10(2)+laser_spot10(2),real10(3)+laser_spot10(3))
% 
% scatter3(real1(1),real1(2),real1(3))
% scatter3(real2(1),real2(2),real2(3))
% scatter3(real3(1),real3(2),real3(3))
%% scatter3(real4(1),real4(2),real4(3))
% scatter3(real5(1),real5(2),real5(3))
% scatter3(real6(1),real6(2),real6(3))
% scatter3(real7(1),real7(2),real7(3))
% scatter3(real8(1),real8(2),real8(3))
% scatter3(real9(1),real9(2),real9(3))
% scatter3(real10(1),real10(2),real10(3))

real_points = [real1+laser_spot1;real2+laser_spot2;real3+laser_spot3;real4+laser_spot4;real5+laser_spot5;real6+laser_spot6;real7+laser_spot7;real8+laser_spot8;real9+laser_spot9;real10+laser_spot10];
laser_line_points = plot_best_fit_line(real_points);
[light_point, laser_point] = find_closest_points(light_ray_points, laser_line_points);
hold on
plot3(light_point(1),light_point(2),light_point(3),'*r')
hold on
plot3(laser_point(1),laser_point(2),laser_point(3),'*r')
hold on

laser_spot
light_point
sprintf('dist from camera to laser spot on object is %.3f mm', norm(light_point-laser_spot'))

%scatter3(laser_spot6(1),laser_spot6(2),laser_spot6(3),'m*')

% [x2, y2] = ginput(1);
% plot(x2,y2,'r*')
% 
% p1 = [x1 y1]; %pixels
% p2 = [x2 y2]; %pixels
%  
% pixel_dist = norm(p1-p2); %pixels
% millimeter_to_pixel_ratio = real_dist_per_box / pixel_dist  % mm/pixel
% dist_to_laser_spot = (norm(laser1)+focal_length)*millimeter_to_pixel_ratio %% mm
end

function [light_point, laser_point] = find_closest_points(light_points,laser_points)
min_dist = Inf;
light_point =[0 0 0]';
laser_point=[0 0 0]';
    for i=1:size(light_points,2)
        light_p = light_points(:,i);
        for j=1:size(laser_points,2)
            laser_p = laser_points(:,j);
            dist = norm(light_p-laser_p);
            if dist < min_dist
                min_dist = dist;
                light_point = light_p;
                laser_point = laser_p;
            end
        end
    end
end

function [laser_line_points] = plot_best_fit_line(X)
% calculate centroid
  x0 = mean(X)';

% form matrix A of translated points
  A = [(X(:, 1) - x0(1)) (X(:, 2) - x0(2)) (X(:, 3) - x0(3))];

% calculate the SVD of A
  [U, S, V] = svd(A, 0);

% find the largest singular value in S and extract from V the
% corresponding right singular vector
  [s, i] = max(diag(S));
  a = V(:, i);
%   for t=1:100,
%     P(t,:) = x0 + a.*t;
%   end
  hold on
  t = -100:1:100;
   plot3(x0(1)+a(1).*t,x0(2)+a(2).*t, x0(3)+a(3).*t,'r');
   hold on
   
   laser_line_points(1,:) = x0(1)+a(1).*t;
   laser_line_points(2,:) = x0(2)+a(2).*t;
   laser_line_points(3,:) = x0(3)+a(3).*t;
   
end

