%
%--------------------------------------------------------------------------
% function [offset,radius,gc,residual,fail] = FT_fit_GaussBeam_intensity(data, gc, pixel, lambda, radius, center_row, center_col, tolerance, verbose)
%
% A function for Matlab which fits a Gaussian mode shape to a
% 2D data grid
%
% data:      2D data grid, with image of LG mode
% gc:        structure with Gaussian beam coefficients, see FT_init_gauss_coefficients
% l:         azimuthal mode index of LG mode
% pixel:     scale of on image pixel [m]
% 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:    beam radius [m]
% residual:  2D data grid with residual image
% fail:      equals 1 if fit is considered a fail, 0 for success 
%
% Andreas Freise  07.03.2010
%--------------------------------------------------------------------------
%

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

function [radius,pedestal, offset, gc, model,fail] = FT_fit_GaussBeam_intensity(data, gc, pixel, lambda, radius, pedestal, offset, ...
                                                    tolerance, verbose)
  tic
    
  baseid='fit_GaussBeam_intensity';

  %{
  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;
    
  if (verbose>=1)
    fit_display='iter';
    figure
  else
    fit_display='off';
  end
  
  nc=gc.number_of_coeffs;
  % initialise fit   
  options=optimset('Display',fit_display, 'TolX', tolerance, 'TolFun',tolerance, 'MaxIter', 1000);
  paramsin=zeros(1,5+nc);
  paramsin(1)=radius;
  paramsin(2)=offset(1);
  paramsin(3)=offset(2);
  paramsin(4)=offset(3);
  paramsin(5)=pedestal;
  for i=1:nc
    paramsin(5+i)=gc.value(i); % scaling factor;
  end
  
  paramsout=fminsearch(@mytestf,paramsin,options,lambda,gc,x,y,data,verbose);
  
  % rescale parameters back to proper scale
  
  gp=FT_init_gauss_param(lambda,1,'w0',abs(paramsout(1)),'z',0);
  radius=abs(paramsout(1));
  offset(1)=paramsout(2);
  offset(2)=paramsout(3);
  offset(3)=paramsout(4);
  pedestal=paramsout(5);
  for i=1:gc.number_of_coeffs
    gc.value(i)=paramsout(5+i); % scaling factor;
    %fprintf('i= %d, value = %g\n',i,gc.value(i));
  end
  [field1]=FT_mode_coefficients_to_field(gc, gp, x, y, offset,0);
  model=abs(field1).^2;
  %field1=paramsout(5)*field1/max(max(field1));
  residual=(data-model).^2;
  
  disp(sprintf('Fit results:'));
  disp(sprintf('  started with w0=%g, x=%g, y=%g, phi=%g, factor=%g',paramsin(1),paramsin(2),paramsin(3),paramsin(4), ...
               paramsin(5)));
  disp(sprintf('  ended with w0=%g, x=%g, y=%g, phi=%g, factor=%g',paramsout(1),paramsout(2),paramsout(3),paramsout(4), ...
               paramsin(5)));
  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
    fail=0;
  end
  
  toc
  
function [diff] = mytestf(params,lambda,gc,x,y,data,verbose)
  gp=FT_init_gauss_param(lambda,1,'w0',abs(params(1)),'z',0);
  offset(1)=params(2);
  offset(2)=params(3);
  offset(3)=params(4);
  pedestal=params(5);
  for i=1:gc.number_of_coeffs
    gc.value(i)=params(5+i); % scaling factor;
    %fprintf('i= %d, value = %g\n',i,gc.value(i));
  end
  [field1]=FT_mode_coefficients_to_field(gc, gp, x, y, offset,0);
  intensity=pedestal+abs(field1).^2;
  if (verbose)
  clf;
  hold on;
  imagesc(intensity);
  contour(data,'w');
  hold off;
  drawnow;
  end
  %field1=params(5)*field1/max(max(field1));
  diff1=(data-intensity).^2;
  diff=sqrt(sum(sum(diff1)))/length(diff1(:));
  
