%% Light Calibration Script, run this before the processededgetime.m file
%% to get a valid light source calibration
%% The idea here is to triangulate on the light source using two similar
%% triangles that are perpendicular to the plane and having one edge on it.
tmp = 0;
if tmp == 1 
    imgin = imread('shadow1.jpg');
    imshow(imgin);
    ptip1 = ginput(1);
    plong1 = ginput(1);
    pbase1 = ginput(1);
    imgin = imread('shadow2.jpg');
    imshow(imgin);
    ptip2 = ginput(1);
    plong2 = ginput(1);
    pbase2 = ginput(1);
end
%% pbase and plong are on groundplane (z=0)
%% ptip is 186mm above (z+) pbase

%% represent board in camera space

Origin=[0,0,0]';
Xaxis=[100,0,0]';
Yaxis=[0,100,0]';

COrigin=Rc_1*Origin+Tc_1;
CXaxis=Rc_1*Xaxis+Tc_1;
CYaxis=Rc_1*Yaxis+Tc_1;
CXaxis-COrigin
CYaxis-COrigin
%%% sanity check, should return 2 and does
% rayplane([0,0,0],0.5*COrigin,CXaxis,CYaxis,COrigin)


rayplane([0,0,0],0.5*COrigin,CXaxis,CYaxis,COrigin)
%X-Yplane in camera space
CPlaneNormal = cross(CXaxis-COrigin,CYaxis-COrigin);
%normalize
CPlaneNormal = CPlaneNormal/sqrt(dot(CPlaneNormal,CPlaneNormal));

%find ray 3d point through image in camera space

%for base
xn = normalize(pbase1',fc,cc,kc,alpha_c);
xn = [xn(1) xn(2) 1]';
xn
t = rayplane([0,0,0],xn,CXaxis,CYaxis,COrigin);
CBase1 = xn*t;
%extrapolate Tip 186mm above ground plane (CPlaneNormal)
CTip1=CPlaneNormal*186+CBase;

%% Check to see if our predicted tip projected onto the image plane matches
%% where the user clicked
ptip1
KK*CTip1

xn = normalize(plong1',fc,cc,kc,alpha_c);
xn = [xn(1) xn(2) 1]';
xn
t = rayplane([0,0,0],xn,CXaxis,CYaxis,COrigin);
CLong1 = xn*t;

%% 2nd ray
%find ray 3d point through image in camera space

%for base
xn = normalize(pbase2',fc,cc,kc,alpha_c);
xn = [xn(1) xn(2) 1]';
xn
t = rayplane([0,0,0],xn,CXaxis,CYaxis,COrigin);
CBase2 = xn*t;
%extrapolate Tip 186mm above ground plane (CPlaneNormal)
CTip2=CPlaneNormal*186+CBase;

%% Check to see if our predicted tip projected onto the image plane matches
%% where the user clicked
ptip2
KK*CTip2

xn = normalize(plong2',fc,cc,kc,alpha_c);
xn = [xn(1) xn(2) 1]';
xn
t = rayplane([0,0,0],xn,CXaxis,CYaxis,COrigin);
CLong2 = xn*t;

%% Find Light source With (CTip2-CLong2) & (CTip1-CLong1) 

%% skew line intersection
%% from http://sun.uni-regensburg.de/idl-5.5/html/idl4/jhuapl.doc/vectors/v_skew.html
u1 = (CTip1-CLong1)/norm(CTip1-CLong1)
u2 = (CTip2-CLong2)/norm(CTip2-CLong2)
u3 = cross(u1,u2)/norm(cross(u1,u2));
E = dot((CLong1-CLong2),u3)*u3;
Cp = CLong2 + E;
Dp = CTip2 + E;
V = CLong1 - Cp;
V_perp = V - (dot(V,u2)*u2);
u4 = V_perp/norm(V_perp);
P = dot((CLong1-Cp),u4);
p = dot((CTip1-Cp),u4);
Ip = CLong1 + u1*(P/(P+p));
I = Ip - E/2;

LightSource = I;