function TestP1

clc

% Example 1 (Blackboard)
disp('Example 1:')
disp(' ')
thetaE = (76 + 29/30 + 5.23/3600)*pi/180;
phiE   = (42 + 26/30 + 40.4/3600)*pi/180;
M = [45000000;0;0];

fprintf('Martian: x = %10.2e  y = %10.2e  z = %10.2e\n',M)
fprintf('Target :     phiE = %11.8f       thetaE  = %11.8f \n',phiE,thetaE)
[phi_Star,theta_Star,EXIT,OUT] = LaserPointerCV(phiE,thetaE,M);
fprintf('CVL:     phi_Star = %11.8f    theta_Star = %11.8f ',phi_Star,theta_Star)
fprintf(' Flag = %1d  F-Evals = %3d \n',EXIT,OUT.funcCount)
[phi_Star,theta_Star,EXIT,OUT] = LaserPointer(phiE,thetaE,M);
fprintf('You:     phi_Star = %11.8f    theta_Star = %11.8f ',phi_Star,theta_Star)
fprintf(' Flag = %1d  F-Evals = %3d \n',EXIT,OUT.funcCount)


% Example 2 (New Zealand Blackboard)
disp(' ')
disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ')
disp('Example 2:')
disp(' ')
thetaE = (76 + 29/30 + 5.23/3600)*pi/180 + pi;
phiE   = -(42 + 26/30 + 40.4/3600)*pi/180  ;
M = [45000000;0;0];

fprintf('Martian: x = %10.2e  y = %10.2e  z = %10.2e\n',M)
fprintf('Target :     phiE = %11.8f       thetaE  = %11.8f \n',phiE,thetaE)
[phi_Star,theta_Star,EXIT,OUT] = LaserPointerCV(phiE,thetaE,M);
fprintf('CVL:     phi_Star = %11.8f    theta_Star = %11.8f ',phi_Star,theta_Star)
fprintf(' Flag = %1d  F-Evals = %3d \n',EXIT,OUT.funcCount)
[phi_Star,theta_Star,EXIT,OUT] = LaserPointer(phiE,thetaE,M);
fprintf('You:     phi_Star = %11.8f    theta_Star = %11.8f ',phi_Star,theta_Star)
fprintf(' Flag = %1d  F-Evals = %3d \n',EXIT,OUT.funcCount)


% Example 3 (Polar)
disp(' ')
disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ')
disp('Example 3:')
disp(' ')
thetaE =  1.0;
phiE   =  .95*pi/2 ;
M = [45000000;0;0];

fprintf('Martian: x = %10.2e  y = %10.2e  z = %10.2e\n',M)
fprintf('Target :     phiE = %11.8f       thetaE  = %11.8f \n',phiE,thetaE)
[phi_Star,theta_Star,EXIT,OUT] = LaserPointerCV(phiE,thetaE,M);
fprintf('CVL:     phi_Star = %11.8f    theta_Star = %11.8f ',phi_Star,theta_Star)
fprintf(' Flag = %1d  F-Evals = %3d \n',EXIT,OUT.funcCount)
[phi_Star,theta_Star,EXIT,OUT] = LaserPointer(phiE,thetaE,M);
fprintf('You:     phi_Star = %11.8f    theta_Star = %11.8f ',phi_Star,theta_Star)
fprintf(' Flag = %1d  F-Evals = %3d \n',EXIT,OUT.funcCount)

% Example 4 (Blackboard/ Close Martian)
disp(' ')
disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ')
disp('Example 4:')
disp(' ')
thetaE = (76 + 29/30 + 5.23/3600)*pi/180;
phiE   = (42 + 26/30 + 40.4/3600)*pi/180;
M = [10000;20000;3000];

fprintf('Martian: x = %10.2e  y = %10.2e  z = %10.2e\n',M)
fprintf('Target :     phiE = %11.8f       thetaE  = %11.8f \n',phiE,thetaE)
[phi_Star,theta_Star,EXIT,OUT] = LaserPointerCV(phiE,thetaE,M);
fprintf('CVL:     phi_Star = %11.8f    theta_Star = %11.8f ',phi_Star,theta_Star)
fprintf(' Flag = %1d  F-Evals = %3d \n',EXIT,OUT.funcCount)
[phi_Star,theta_Star,EXIT,OUT] = LaserPointer(phiE,thetaE,M);
fprintf('You:     phi_Star = %11.8f    theta_Star = %11.8f ',phi_Star,theta_Star)
fprintf(' Flag = %1d  F-Evals = %3d \n',EXIT,OUT.funcCount)

disp(' ')
disp(' ')
disp('Using Fsolve (5pts):')
disp(' ')
disp('The function F and starting values(5pts):')
disp(' ')
disp('Results (5pts):')


%%%%%%
% Sample solution...

function [phi_Star,theta_Star,EXITFLAG,OUTPUT] = LaserPointerCV(phi,theta,M)
% Tolerances...
del_x = 0.0000001;
del_y = 0.0000001;
% radii..
r1 = 6378.206; r2 = 6356.583; rAve = (r1+r2)/2;
% Initial guess is the Martian's target...
xInitial = [1;phi;theta];
u = [ rAve*cos(theta)*cos(phi);...
      rAve*sin(theta)*cos(phi);...
      rAve*sin(phi)];
% Use fsolve...
options = optimset('Display','off','Tolx',del_x,'TolFun',del_y);
[x_Star,F_Star,EXITFLAG,OUTPUT] = fsolve(@(x) F(x,u,M,r1,r2), xInitial,options);
% Return the oblate longitude and latitude...
phi_Star   = x_Star(2);
theta_Star = x_Star(3);



  function y = F(x,u,M,r1,r2)
% Martian at location M
% r1 = equatorial radius
% r2 = polar radius
% x = [ t-value for ray ; longitude ; latitude]
% y = difference between the ray location specified by t and the
%     oblate point specified by longitude and latitude
t = x(1); 
phi_star = x(2); 
theta_star = x(3);
% Where are we on the ray?
Ray = M + t*(u-M);
% Where are we on the surface?
SurfacePoint = [r1*cos(theta_star)*cos(phi_star);...
                r1*sin(theta_star)*cos(phi_star);...
                r2*sin(phi_star)];
% Return the difference...         
y = Ray-SurfacePoint;
            
           




 