%
%--------------------------------------------------------------------------
% function[map_out,x_tilt,y_tilt,offset]=FT_remove_piston_from_mirror_map(map,w,display)
%
% A function for Matlab which tries to fit a tilt in both the x and y
% directions to a mirror map, see e.g. FT_read_virgo_map.
%
% It uses 'fminsearch' from matlab and returns the best angles for the
% tilts in the x direction and the y direction, as well as a new map 
% structure in which the tilt has been removed from the map.
%
% map   :   the surface map (variable converted to 'map' structure
% 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
% 
% x_tilt:   angle of fitted and removed tilt along the x direction [rad]
% y_tilt:   angle of fitted and removed tilt along the y direction [rad]
% offset:     value of fitted and removed offset [nm]
% map_out:  surface map with tilt removed
%
% Part of the Simtools package, http://www.gwoptics.org/simtools
% Charlotte Bond, Andreas Freise  05.09.2008
%--------------------------------------------------------------------------
%

% Description: fits a tilted plane to a mirror map and removes it
% Keywords: mirror, map, tilt, fit, remove

function [map_out,x_tilt,y_tilt,offset]=FT_remove_piston_from_mirror_map(map,w, display)

  baseid='remove_tilt_from_mirror_map';

  if(display<0 || display>3 || round(display)~=display)
  msg='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

  % offsets and tilts should be small so initial settings at 0
  x_tilt_in=0;
  y_tilt_in=0;
  offset_in=0;
  
  params=[x_tilt_in,y_tilt_in,offset_in];
  
  % set options for fminsearch ('help optimset' gives the list of options)
  options=optimset('Display',disp_str, 'TolX', 1e-04, 'TolFun',1e-05, 'MaxIter', 1000);
  
  % create link to test function below
  f=@testfunc;
  
  % run the fitting algorithm
  params=fminsearch(f,params,options,map,w);
  
  x_tilt=1e-9*params(1);
  y_tilt=1e-9*params(2);
  offset=params(3);
  
  % Create tilted surface
  [Rsq,Z]=FT_create_sphere_for_map(map,0,map.x0,map.y0,offset,x_tilt, y_tilt);
  
  % remove tilt from output map
  map_out=map;
  [idx]=map_out.notnan;
  map_out.data(idx)=map_out.data(idx)-Z(idx);
   
function[y]=testfunc(params,map,w)
  
  x_tilt=1e-9*params(1);
  y_tilt=1e-9*params(2);
	zoffset=params(3);
    
	% Create tilted surface
  [Rsq,Z]=FT_create_sphere_for_map(map,0,map.x0,map.y0,zoffset,x_tilt, y_tilt);
      
  [idx]=map.notnan;
  npoints=length(idx);
  if (w==0)
    % Compute normalised difference^2 between titlted surface 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
