global h_mixedmirrors_fig;
%
%Instituto Superior Técnico - Instituto de Sistemas e Robótica - 			             09/2001
%OMNIVIEWS PROJECT    -   SVAVISCA sensor									[ccgd,jasv@isr.ist.utl.pt]
%
%global h_mixedmirrors_fig;
%
% Update mirror button - This guide shows the fovea and retina mirror profiles
% which view the floor and vertical dimension respc. Additionally, analysis of the mirror
% profiles are given. The fovea mirror is mapped in the first part of the ccd and the
% retina mirror is mapped in the rest part of the ccd.
%  
% t [cm] - distance vector in the horizontal coordinate for the mirror
% Yo [cm]: minimum distance from the mirror to the camera, Yo=Fo = (t*f)/rho;
% log_polar = 1/0 : log polar / cartesian camera
% fovea = 1/0 : fovea / retina mirror
% f [cm]- lens focal length in cm (2.5 for log-polar; 1.25 for cartesian)
% a - gain of the mirror 
% b [cm] - offset  (set by minimum angle for the retina mirror)
% dmin_fovea - minimum distance in the floor covered by the minimum angle of the fovea
% dmax_fovea _ maximum distance in the floor covered by the maximun angle of the fovea
% ipheight [cm]- image plane height: distance between the floor and image plane



%criating handles to objects
EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText1');
Strf_convcamera = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText2');
Strf_logpcamera = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText3');
Str_dist = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText4');
Str_fimin_fovea = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText5');
Str_fimax_fovea = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText6');
Str_fimin_retina = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText7');
Str_fimax_retina = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText8');
Str_ipheight = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText9');
Str_radiusfovea = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText10');
Str_radiusmirror = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText11');
Str_convccd = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText12');
Str_logpccd = get(EditHandle, 'String');

EditHandle = findobj(h_mixedmirrors_fig, 'Tag', 'EditText13');
Str_outputfname = get(EditHandle, 'String');


%Conversion of string to double precision value
f_convcamera = str2double(Strf_convcamera);
f_logpcamera = str2double(Strf_logpcamera);
f_convcamera = f_convcamera/10;              % converts mm -> cm.
f_logpcamera = f_logpcamera/10;					% converts mm -> cm.
dist = str2double(Str_dist);
fimin_fovea = str2double(Str_fimin_fovea);
fimax_fovea = str2double(Str_fimax_fovea);
fimin_retina = str2double(Str_fimin_retina);
fimax_retina = str2double(Str_fimax_retina);
ipheight = str2double(Str_ipheight);
radiusfovea = str2double(Str_radiusfovea);
radiusmirror = str2double(Str_radiusmirror);
convccd = str2double(Str_convccd);
logpccd = str2double(Str_logpccd);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% dist = ipheight for fovea 						%
% dist = d for retina (radius of cylinder)   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

distances = [200];
dt = 0.1;  %cm

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%																											 %
% 									CARTESIAN IMAGE													 %
%																											 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

									%		      %
									%	FOVEA 	%
									%  ------	%

log_polar = 0;										%%conventional camera
fovea = 1;											%%foveal region
   
   %Calculus of the minimal distance betwen image plane and the first point in the mirror 
   %This relation is given by perspective projection Fo = (t*f)/rho, gives the first point to
   %ode45 function calculates de points of the mirror profile
	y0 = ((radiusmirror*f_convcamera)/convccd);
   
   %Fovea Cross Section - fovea is used to visualize the floor (birds eye view)
   tsf = 0:dt:radiusfovea;				%% step in the horizontal distance
   if length( tsf) >  1,  				%% fovea region exists

		%Dimension of the ccd that is covered by the fovea mirror, this relation is given
      %by perspective projection rho = t*f/Fo
      rhof_conv = (radiusfovea*f_convcamera)/y0;
   
      %Minimum and maximum distance in the horizintal plane that is covered by foveal mirror
		dmin_fovea = tan(fimin_fovea*pi/180)*(ipheight+y0);			
   	dmax_fovea = radiusfovea + tan(fimax_fovea*pi/180)*(ipheight+y0);

   	%Calculus of the gain "a" of the fovea mirror
		a_fovea = (dmax_fovea-dmin_fovea)/rhof_conv;
      
      %Calculus of the mirror points
      [Tf,Yf,dyf] = analysis(log_polar,fovea,tsf,y0,f_convcamera,dmin_fovea,...
         					   	a_fovea,0,ipheight, distances,'Up_right');
	
	   height_fovea = Yf(end) - Yf(1); 	%height of the foveal mirror

      
   else  % foveal region empty
      
      Tf = [];
      Yf = [];
      dyf = [];
      rhof_conv = 0;
      height_fovea = 0;
   end;


									%						%
									%		RETINA		%
									%		--------		%
                           
   fovea = 0;										%%retinal region

	%Retina Cross Section
   tsr = radiusfovea:dt:radiusmirror;
   if length( tsr ) >  1,  			% retinal region exists   
      
      %calculus of the first point in the retina mirror 
      y0_retina = y0 + height_fovea; 	
      
		%Dimension of the ccd that is covered by the retina mirror
      rhor_conv = convccd - rhof_conv;
      
      %Minimum and maximun height in the vertical plane that is covered by retinal region
		hmin_conv = -(dist-radiusfovea) /tan(fimin_retina*pi/180) + y0;
		hmax_conv = -(dist-radiusmirror)/tan(fimax_retina*pi/180) + y0;

		%Mirror gain, "a" and intercept "b" 
		a_retina = (hmax_conv-hmin_conv)/rhor_conv; 	      %%mirror gain, "a" 
      b_retina =  hmin_conv - a_retina * rhof_conv;      %%and intercept "b"
      
      %Calculus of the mirror points
      [Tr,Yr,dyr] = analysis(log_polar,fovea,tsr,y0_retina,f_convcamera,0,...
      								a_retina,b_retina,dist, distances,'Up_right');
                           
      height_retina = Yr(end) - Yr(1); %retina height
      
   else  % retina region left empty
      
      Tr = [];
      Yr = [];
      dyr = [];
   	height_retina = 0;
      
   end;      
   
   height_mirrorconv = height_fovea + height_retina;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%																											 %
% 											LOG-POLAR IMAGE											 %
%																											 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

								%											   %
								%	FOVEA + RETINA - maps the floor	%
								%  -------------------------------	%

log_polar = 1;										%%log polar camera
fovea = 1;											%%foveal region

      %									   %
		%	FOVEA -  maps the floor		%
      %  -------------------------  %                    


	%constants values
	rhof = 0.027273;   %cm - innermost radiusmirror for fovea projection
	k = 1.02337;       % growth rate of the pixel size

   %Calculus of the minimal distance betwen image plane and the first point in the mirror 
   %This relation is given by perspective projection Fo = (t*f)/rho, gives the first point to
   %ode45 function calculates de points of the mirror profile
   y0 = ((radiusmirror*f_logpcamera)/logpccd);		
   
   %Fovea Cross Section (constant resolution)
   %Fovea is used to visualize the floor (birds eye view)

   to=rhof*y0/f_logpcamera;			%%radius fovea mirror
   tsf = dt:dt:radiusfovea;			%% step in the horizontal distance
   
   if length(tsf) >  1,  				%% foveal region exists

      %Radial length of the ccd covered by the foveal region is equal rhof
		tsf = dt:dt:to;					%%step in the foveal mirror (constant resolution)
         
      %Minimum and maximum distance in the horizintal plane that is covered by fovea mirror
      dmin_fovea = tan(fimin_fovea*pi/180)*(ipheight+y0);		   	
   	dmax_fovea = radiusfovea + tan(fimax_fovea*pi/180)*(ipheight+y0);

   	%Calculus of the gain "a" of the fovea mirror
      a_fovea = (dmax_fovea-dmin_fovea)/rhof;
      
      
      log_polar = 0;			%% PS:because the innermost radius rhof is linear (d_floor=a.rho+b)!
            
      %Calculus of the mirror points
		[Tf_floor,Yf_floor,dy_floor] = analysis(log_polar,fovea,tsf,y0,f_logpcamera,dmin_fovea,...
      											a_fovea,0,ipheight, distances,'Down_right');
                                    
      
      height_fovea = Yf_floor(end) - Yf_floor(1); 	%height of the fovea mirror
      y0_retina 	 = Yf_floor(end);						%distance "y0" for retina
      
      
      %									   %
		%	RETINA -  maps the floor	%
      %  -------------------------  %         
      
      log_polar = 1;								%%%PS:goes to logarithm constraint!!!!
      
      %Retina Cross Section  - used to visualize the floor (birds eye view)

      tsr = to:dt:radiusfovea;
      
		%Dimension of the ccd that is covered by the retina mirror, this relation is given
      %by perspective projection rho = t*f/Fo; where t is given by the radius of the fovea mirror
	   rhof_log = (radiusfovea*f_logpcamera)/(y0 + height_fovea);

   	%Calculus of the gain "a" of the mirror
      a_fovealog = (dmax_fovea-dmin_fovea)/((log(rhof_log)-log(rhof))/log(k));
      
      %Calculus of the mirror points
      [Tf_log,Yf_log,dyf_log] = analysis(log_polar,fovea,tsr,y0_retina,f_logpcamera,dmin_fovea,...
      											a_fovealog,0,ipheight, distances,'Down_right');
                                    
      height_retina = Yf_log(end) - y0;	  	%height of the BEV (fovea+retina) mirror
		y0_retina 	 = Yf_log(end);				%distance "Fo" for retina - vertical resolution mirror
      
		%update of rhof_log since height_fovea is now known
      rhof_log  = (f_logpcamera*tsr(end))./(y0+height_fovea-f_logpcamera);
      tf = tsr(end);
      
   else   % no foveal region
      
      Tf_floor = [];
      Yf_floor = [];
      dy_floor = [];
      Tf_log = [];
      Yf_log = [];
      dyf_log = [];
	   rhof_log = rhof;
      height_fovea = 0;
      y0_retina = y0;
      tf = to;

   end;   
   
   								%														%
									%	RETINA  - maps the vertical dimension	%
									%  -------------------------------------	%
				
fovea = 0;										%%retinal region

   %Mirror Cross Section
   tsr = tf:dt:radiusmirror;
   
   if length( tsr) >  1,  					% retinal region exists
      
   	%Minimum and maximun height in the vertical dimension
   	hmin_log = -(dist-radiusfovea )/tan(fimin_retina*pi/180) + y0_retina;
      hmax_log = -(dist-radiusmirror)/tan(fimax_retina*pi/180) + y0_retina;

		%Mirror gain, "a" and intercept "b" 
      a_retina = (hmax_log - hmin_log)/((log(logpccd)-log(rhof_log))/log(k));	%%mirror gain, "a"
      b_retina = hmin_log - a_retina * ((log(rhof_log)-log(rhof))/log(k) );	%%and intercept "b"
      
      %Calculus of the mirror points
      [Tr_log,Yr_log,dyr_log] = analysis(log_polar,fovea,tsr,y0_retina,f_logpcamera, 0, ...
   											   a_retina,b_retina,dist, distances,'Down_right');
                                    
		height_retina = Yr_log(end) - Yr_log(1);	%%%mirror height

   else % no retinal region
      
      Tr_log = [];
      Yr_log = [];
      dyr_log = [];
	 	height_retina = 0;
       
   end;
    
   height_mirror = height_fovea + height_retina;
   
   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%																											 %
% 									Plot Mirror Profiles												 %
%																											 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

h_figure_upleft = findobj(h_mixedmirrors_fig, 'Tag', 'Up_left');
axes(h_figure_upleft);
cla;
plot(Tf,Yf);
grid on;
hold on;
plot(Tr,Yr); 
hold off;
set(h_figure_upleft, 'Tag', 'Up_left');

%plot
h_figure_downleft = findobj(h_mixedmirrors_fig, 'Tag', 'Down_left');
axes(h_figure_downleft);
cla;
plot(Tf_floor,Yf_floor);
hold on;
plot(Tf_log,Yf_log);
grid on;
hold on;
plot(Tr_log,Yr_log);
hold off;
set(h_figure_downleft, 'Tag', 'Down_left');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%																											 %
% 										Save to a file													 %
%																											 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~isempty(Str_outputfname),
   %
   if (length(Yr) > 0 )
      lastFValue_lin= Yr(end);
   else
      lastFValue_lin= Yf(end); % when retina does not exist
   end;

	if (length( Yr_log) > 0)
      lastFValue_log= Yr_log(end);
   else
      lastFValue_log= Yf_log(end);
   end;
   %
   save_scalars([Str_outputfname '_params.m'], ...
      				f_convcamera, f_logpcamera, ...
      				dist, fimin_fovea, fimax_fovea, ...
      				fimin_retina, fimax_retina, ...
      				ipheight, radiusfovea, radiusmirror,...
      				convccd, logpccd, ...
                  lastFValue_lin, lastFValue_log );
               
   save_mirror_pts([Str_outputfname '_lin.txt'], [Tf  Yf;  Tr  Yr]);
   save_mirror_pts([Str_outputfname '_log.txt'], [Tf_floor  Yf_floor;  Tf_log  Yf_log  ; Tr_log  Yr_log]);
   %
   
   TT = [Tf_floor; Tf_log; Tr_log];
   YY = [Yf_floor; Yf_log; Yr_log];
   dY = [dy_floor; dyf_log; dyr_log];

   save [Str_outputfname '_data.mat'] TT YY dY;
end