%
%--------------------------------------------------------------------------
% function [map_out,Rc_out, delta_x0, delta_y0] = 
%         FT_remove_curvature_from_mirror_map(map_in,Rc_in,w, display)
%
% A function for Matlab which tries to fit a sphere to the mirror surface 
% map see, e.g. FT_read_virgo_map.
% It uses 'fminsearch' from matlab and returns the best radius of curvature
% as well as a new map structure in which the curvature has been removed from
% the map.
%
% The fit also tries to determine the center of the sphere independently,
% however it is recommenrded to use FT_recenter_mirror_map(map) before
% calling this function.
%
% map_in:   the surface map (variable converted to 'map' structure by
%           FT_read_virgo_map)
% Rc_in:    initial guess for radius of curvature [m]
% w:        beam radius on mirror [m] (for weighting function). Use w=0 to
%           switch off weighting
% display:  integer, setting display mode of the fitting routine
%           0: off
%           1: iter
%           2: notify
%           3: final
% 
%
% Rc_out:   radius of curvature of fitted and removed sphere [m]
% delta_x0, delta_y0: difference between old map center and centre of [data points]
%                     curvature of fitted sphere
% map_out:  the surface map with the curvature removed
%
% Part of the Simtools package, http://www.gwoptics.org/simtools
% Charlotte Bond, Andreas Freise  04.09.2008
%--------------------------------------------------------------------------
%

% Description: fits a curvature to a mirror map and removes it
% Keywords: mirror, map, curvature, fit, remove

function [map_out,Rc_out,delta_x0,delta_y0]=FT_remove_curvature_from_mirror_map(map,Rc_in,w,display)

  baseid='remove_curvature_from_mirror_map';

  if(display<0 || display>3 || round(display)~=display)
  result='Invalid value: dsiplay must be 0,1,2 or 3';
  msgid=[baseid,':checkarguments']
    error(msgid,result);
  end
  
  switch(display)
   case 1
    disp_str='iter';
   case 1
    disp_str='notify';
   case 1
    disp_str='final';
   otherwise
    disp_str='off';
  end

  zoffset=0;
  %params=[Rc_in, zoffset, map.x0, map.y0];
  params=[Rc_in, zoffset];


  % set options for fminsearch ('help optimset' gives the list of options)
  options=optimset('Display',disp_str, 'TolX', 1e-03, 'TolFun',1e-05, 'MaxIter', 1000);
  
  % create link to test function below
  f=@testfunc;
  
  % run the fitting algorithm
  params=fminsearch(f,params,options,map,w);
  
  % assign new radius of curvature
  Rc_out=params(1);
  zoffset=params(2);
  
  if (length(params)>2)
    x0=params(3);
    y0=params(4);
  else
    % alternatively we don't fit x0, y0
    x0=map.x0;
    y0=map.y0;
  end
  
  
  % make sphere
  [Rsq,Z]=FT_create_sphere_for_map(map,Rc_out,x0,y0,zoffset,0, 0);
  
  % remove new curvature from old map
  map_out=map;
  [idx]=map_out.notnan; 
  map_out.data(idx)=map_out.data(idx)-Z(idx);
  delta_x0=map.x0-x0;
  delta_y0=map.y0-y0;
  map_out.x0=x0;
  map_out.y0=y0;
  
  
% test function for fminsearch
function [y]=testfunc(params,map,w) 
  
  Rc=params(1);
  zoffset=params(2);
  
  if (length(params)>2)
    x0=params(3);
    y0=params(4);
  else
    % alternatively we don't fit x0, y0
    x0=map.x0;
    y0=map.y0;
  end
  
  % make sphere
  [Rsq,Z]=FT_create_sphere_for_map(map,Rc,x0,y0,zoffset,0, 0);
  
  [idx]=map.notnan;
  npoints=length(idx);
  
  if (w==0)
    % compute normalised difference^2 between sphere and map
    y=sqrt(sum(sum((map.data(idx)-Z(idx)).^2)))/npoints;
  else
    % make Gaussian weight function
    weight=2/pi./w.^2.*exp(-2* Rsq(idx)./ w.^2);
    y=sqrt(sum(sum(weight.*(map.data(idx)-Z(idx)).^2)))/npoints;
  end
  