%
%--------------------------------------------------------------------------
% function [radius]=FT_guess_size_LG(data_in, pixel, lambda, p,l, center_row, center_col, mode, verbose)
%
% **Warining** This function has only been written for and tested with
% special cases and probably won't work for you unless you know what
% you are doing!
%
% A function for Matlab that tries to guess the size of an LG mode shape
% in a given 2D array
% 
% data_in:   2D data grid of an LG image
% p:         radial mode index of LG mode
% l:         azimuthal mode index of LG mode
% pixel:     scale of on image pixel [m]
% center_row: row index of mode center 
% center_col: col index of mode center
% mode: switch, 0 means helical LG mode, 1 means sinusoidal LG mode (LG_cos)
% verbose: if verbose=1 the function plots and prints additional infor for debugging
%
% radius: beam size (w) [...]
%
% Andreas Freise  12.08.2009
%--------------------------------------------------------------------------
%

% Description: Tries to guess the size of an LG mode shape in a given 2D array
% Keywords: guess, size, LG, mode, 2D

function [radius, center_row, center_col]=FT_guess_size_LG(data_in, pixel, lambda, p,l, center_row, center_col, mode,  verbose)

  w_guess=55;
  [rows,cols]=size(data_in);
  
  crow=round(center_row);
  ccol=round(center_col);
  
  crow_org=crow;
  ccol_org=ccol;
  
  x=(linspace(1,cols,cols))*pixel;
  y=(linspace(1,rows,rows))*pixel;

  w0=w_guess*pixel;
  
  LGoffset=zeros(1,3);
  LGoffset(1)=center_col*pixel;
  LGoffset(2)=center_row*pixel;
  gp=FT_init_gauss_param(lambda,1,'w0',w0,'z',0);
  if (mode==0)
    [field]=FT_LG_field(gp,p,l,x,pixel*0.5*(1+rows), LGoffset);
  else
    [field]=FT_LG_cos_field(gp,p,l,x,pixel*0.5*(1+rows), LGoffset);
  end
  
  field=abs(field).^2;
  field=FT_normalise_data(field,0,1);
  
  roff=-20;
  reference=-1;
  for i=-20:20
    testline=sum(data_in(crow+i,:));
    if (testline>reference)
      reference=testline;
      roff=i;
    end
  end
  
  %center_row=center_row+roff;
  %crow=round(crow+roff);
  data_lin=data_in(crow,:);
  
  if (verbose>1)
    figure(220)
    plot(x,data_lin,x,field);
    legend('data','testfunction');
    title('horizontal cross section');
    text(max(x)*0.5,max(data_lin)*-0.1,datestr(now));
  end
  
  line_threshold=0.6;
  
  
  d1=data_lin(1:1:ccol);
  d2=data_lin(ccol:cols);   
  f1=field(1:ccol);
  f2=field(ccol:cols);
  
  if (l==0)
    % estimating the width of the peak for data and fit
    drange=length(d1)-min(find(d1>line_threshold*max(d1)))+max(find(d2>line_threshold*max(d2)));
    frange=length(f1)-min(find(f1>line_threshold))+max(find(f2>line_threshold));
  else
    drange=length(d1)-max(find(d1>line_threshold*max(d1)))+min(find(d2>line_threshold*max(d2)));
    frange=length(f1)-max(find(f1>line_threshold))+min(find(f2>line_threshold));
  end
  
  fcenter=round((length(d1)-max(find(d1>line_threshold*max(d1)))-min(find(d2>line_threshold*max(d2))))/2.0);
  
  % adjusting guess
  if ((length(drange)>0) && (length(frange)>0))
    w_guess=w_guess*drange/frange;
    % adjust center
    ccol=ccol-fcenter;
    center_col=center_col-fcenter;
  else
    disp(sprintf(' *** Second width adjustment failed!! ***'));
  end
  
  if (verbose>=3)
    figure(221)
    clf;
    hold on;
    surf(data_in,'EdgeColor','None');
    view(2)
    p1=plot(ccol_org,crow_org,'rx','Linewidth',4,'Erasemode','none');
    p2=plot(ccol,crow,'gx','Linewidth',4,'Erasemode','none');
    legend([p1,p2],'old center','new center');
    axis square;
    axis tight;
    hold off;
    text(cols*0.5,rows*-0.1,datestr(now));
  end
  
  radius=w_guess;
  

    w0=w_guess*pixel;
    LGoffset(1)=center_col*pixel;
    LGoffset(2)=center_row*pixel;
    gp=FT_init_gauss_param(lambda,1,'w0',w0,'z',0);
    if (mode==0)
      [field]=FT_LG_field(gp,p,l,x,y,LGoffset);
    else
      [field]=FT_LG_cos_field(gp,p,l,x,y,LGoffset);
    end
    
    field=abs(field).^2;
    field=FT_normalise_data(field,0,1);
    
      if (verbose>=3)
    figure(222)
    clf;
    %subplot(1,2,1)
    hold on;
    surf(data_in, 'Edgecolor','none');
    view(0,-90);
    contour(field,4,'Edgecolor','w','LineStyle','-.','Linewidth',3);
    %plot(center_guess(1),center_guess(2),'rx','Linewidth',4,'Erasemode','none');
    axis square;
    axis tight;
    hold off;
    title('initial guess')
    text(cols*0.5,rows*1.1,datestr(now));
    
  end
  
  
  if (verbose>1)
    [field]=FT_LG_cos_field(gp,p,l,x,pixel*0.5*(1+rows), LGoffset);
     field=abs(field).^2;
    field=FT_normalise_data(field,0,1);
    figure(228)
    plot(x,data_lin,x,field);
    legend('data','testfunction');
    title('horizontal cross section');
    text(max(x)*0.5,max(data_lin)*-0.1,datestr(now));
  end
  
    
  end