%
%--------------------------------------------------------------------------
% function [offset,radius,residual,fail] = FT_fit_LG(data, p, l, pixel, lambda, radius, center_row, center_col, tolerance, verbose)
%
% A function for Matlab which fits a helical LG mode shape to a
% 2D data grid
%
% data:      2D data grid, with image of LG mode
% p:         radial mode index of LG mode
% l:         azimuthal mode index of LG mode
% pixel:     scale of on image pixel [m]
% lambda:    wavelength [m]
% radius:    estimated beam radius (given in camera pixels)  [px]
% center_row: estimates beam center in vertical direction [px]
% center_col: estimates beam center in horizontal direction [px]
% tolerance: target accuracy for fitting routine (e.g 0.01)
% verbose:   if verbose=1 this functions plots intermediate results
%
% offset:    vector of center point (x,y,phi) [m,m,rad]
% radius:    estimated beam radius (given in camera pixels)  [px]
% residual:  2D data grid with residual image
% fail:      equals 1 if fit is considered a fail, 0 for success 
%
% Andreas Freise  23.07.2009
%--------------------------------------------------------------------------
%

% Description: Fits a helical LG mode shape to a 2D data grid
% Keywords: fit, LG, mode, 2D

function [offset,radius,residual,fail] = FT_fit_LG(data, p, l, pixel, lambda, radius, center_row, center_col, tolerance, verbose)
  tic
    
  baseid='fit_LG';

  if (verbose>=1)
    disp(sprintf('Computing initial guess for LG %d %d fit',p,l));
  end
  
  [rows,cols]=size(data);
  x=(linspace(1,cols,cols))*pixel;
  y=(linspace(1,rows,rows))*pixel;
  
  wscale=1/100.0; % TODO replace magic scaling factor by something better
  offset_scale=1.0;
  
  if (verbose>=1)
    fit_display='iter';
  else
    fit_display='off';
  end
  
  % initialise fit   
  options=optimset('Display',fit_display, 'TolX', tolerance, 'TolFun',tolerance, 'MaxIter', 1000);
  paramsin=zeros(1,4);
  paramsin(1)=radius*pixel*wscale;
  paramsin(2)=center_col*pixel*offset_scale;
  paramsin(3)=center_row*pixel*offset_scale;
  paramsin(4)=1.0; % scaling factor;
  paramsout=fminsearch(@mytestf,paramsin,options,lambda,p,l,x,y,wscale,offset_scale,data);
  
  % rescale parameters back to proper scale
  paramsout(1)=1.0/wscale*paramsout(1);
  paramsout(2:3)=1.0/offset_scale*paramsout(2:3);
  paramsin(1)=1.0/wscale*paramsin(1);
  paramsin(2:3)=1.0/offset_scale*paramsin(2:3);
  
  gp=FT_init_gauss_param(lambda,1,'w0',paramsout(1),'z',0);
  [field1]=FT_LG_field(gp,p,l,x,y,[paramsout(2),paramsout(3),0]);
  field1=abs(field1).^2;
  field1=paramsout(4)*field1/max(max(field1));
  residual=(data-field1).^2;
  
  disp(sprintf('Fit results:'));
  disp(sprintf('  started with w0=%g, x=%g, y=%g, factor=%g',paramsin(1),paramsin(2),paramsin(3), ...
               paramsin(4)));
  disp(sprintf('  ended with w0=%g, x=%g, y=%g, factor=%g',paramsout(1),paramsout(2),paramsout(3), ...
               paramsin(4)));
  disp(sprintf('  max residual %g',   max(max(residual))));
  
  testfit(1)=max(abs(paramsout(1)/paramsin(1)),abs(paramsin(1)/paramsout(1)));
  testfit(2)=abs(paramsout(2)-paramsin(2))/paramsin(1);
  testfit(3)=abs(paramsout(3)-paramsin(3))/paramsin(1);
  %if (verbose==1)
  disp(sprintf('  difference between guessed and final [a.u.]: w0 = %g, x=%g, y=%g',testfit(1),testfit(2),testfit(3)));
  % end
  
  disp(sprintf('  maximum residual: %g\n',max(max(residual))));
  max_res=0.9;
  res=length(find(residual>max_res));
  disp(sprintf('  number of residual points >%g is %d out of %d\n',max_res,res,length(residual(:))));
  %  if (testfit(1)>1.8 || testfit(2)>0.3 || testfit(3)>0.3  || max(max(residual))>1.4)
  if (testfit(1)>1.8 || testfit(2)>0.3 || testfit(3)>0.3  || res/length(residual(:))>0.001)
    disp(sprintf('  *** Consider fit a FAILURE!! ***\n'));
    fail=1;
  else
    disp(sprintf('  *** Consider fit a SUCCESS!! ***\n'));
    fail=0;
  end
  
  radius=abs(paramsout(1));
  offset(1)=paramsout(2);
  offset(2)=paramsout(3);
  offset(3)=0;
  
  toc
  
function [diff] = mytestf(params,lambda,p,l,x,y,wscale,offset_scale,data)
  gp=FT_init_gauss_param(lambda,1,'w0',1.0/wscale*abs(params(1)),'z',0);
  [field1]=FT_LG_field(gp,p,l,x,y,1/offset_scale*[params(2),params(3),0]);
  field1=abs(field1).^2;
  field1=params(4)*field1/max(max(field1));
  diff1=(data-field1).^2;
  diff=sqrt(sum(sum(diff1)))/length(diff1(:));
  
