%------------------------------------------------------------------------
% NAME:     qpcls
%
%           x
%
% FORMAT:   [X,G,y_corrected,y_fitted] = qpcls( Q, y )
%             or
%           [Dy,Kx] = qpcls( Q ) 
%
% OUT:      x
% IN:       -
% OPTIONAL: -
% TEMPLATE: -
%------------------------------------------------------------------------

% HISTORY: 2001.01.04  Started by by Patrick Eriksson.


function [X,G,y_corrected,y_fitted,Xiter] = qpcls( Q, y )


if ~qpcls_methods(Q.RETRIEVAL_METHOD)
  error( sprintf('You have selected an unknown retrieval method (%s)', ...
                                                         Q.RETRIEVAL_METHOD));
end


out(1,1);


%### Determine calculation options ############################################
%
%= Set 0 as first guess for the options
%
nonlin     = 0;
return_Dy  = 0;  
do_post    = 0;
recalc_abs = 0;
logspecies = 0;
do_xiter   = 0;
%
if (nargin<2) | isempty(y)
  %
  return_Dy  = 1;  
  %
  out(2,'ONLY CALCULATING Dy');
  %
else
  %
  nonlin     = Q.CLS_NONLIN_ON;
  %
  if nargout > 1
    do_post = 1;
  end
  %
  if nonlin 
    %
    if Q.SPECIES_POSITIVE_ON
      logspecies = 1;
    end
    %
    if Q.TEMPERATURE_DO == 3
      recalc_abs = 1;
    end
  end
  %
  out(1,'RETRIEVAL BY CONSTRAINED LEAST SQUARES');
end
%
if nargout > 4
  do_xiter = 1;
end



%### Some printing ############################################################
out(2,0);
if nonlin
  if Q.CLS_GA_START_VALUE == 0
    out(2,sprintf('Method         : %s, Gauss-Newton', ... 
                                                  upper(Q.RETRIEVAL_METHOD) ));
  else
    out(2,sprintf('Method         : %s, Marquardt-Levenberg', ...
                                                   upper(Q.RETRIEVAL_METHOD)));
  end
  out(2,sprintf('Start gamma    : %.1f', Q.CLS_GA_START_VALUE ));
  out(2,sprintf('Factor OK      : %.1f', Q.CLS_GA_FAC_WHEN_OK ));
  out(2,sprintf('Factor not OK  : %.1f', Q.CLS_GA_FAC_WHEN_NOT_OK ));
  out(2,sprintf('Lower treshold : %.2f', Q.CLS_GA_LOWER_TRESHOLD ));
  out(2,sprintf('Stop value     : %.3f', Q.CLS_STOP ));
  out(2,sprintf('Max iterations : %.0f', Q.CLS_MAX_ITER ));
  %
  if recalc_abs
    out(2,'   Absorption will be recalculated for each iteration.');
  else
    out(2,'   Absorption will be scaled by species amounts.');
  end
else
  out(2,sprintf('Method : Linear %s', upper(Q.RETRIEVAL_METHOD) ));
end
%
if ~return_Dy
  if do_post
    out(2,'   Spectrum, cost etc. will be calculated after retrieval.');
  else
    out(2,'   No post-retrieval calculations will be performed.');
  end
end



%### Load H matrices ##########################################################
%
load( [Q.OUT,'.h'], '-mat' );
load( [Q.OUT,'.hd'], '-mat' );
%
if ~return_Dy & size(H,1)>1 & length(y) ~= size(H,1)
  error('Wrong length of input measurement vector.');
end


%### Create temporary dir #####################################################
%
tmpdir    = temporary_directory( Q.TMP_AREA );


%### Run ARTS for first iteration #############################################
%
do_nonlin = nonlin | do_post;
[ fy, Kx, kx_names, kx_index, kx_aux, cfile, basename ] = ...
                        qp_iter1( Q, H, Hd, tmpdir, 3, do_nonlin, recalc_abs );
%
if ~return_Dy & length(y) ~= length(fy)
  error('Wrong length of input measurement vector.');
end
%
%= Some sizes
xa     = kx_aux(:,2);
nx     = length( xa );



%### Load the covariance matrices #############################################
%
load( [Q.OUT,'.dxinv'], '-mat' );
load( [Q.OUT,'.seinv'], '-mat' );
%
if strcmp( lower( Q.RETRIEVAL_METHOD ), 'oem' )
  load( [Q.OUT,'.sxinv'], '-mat' );
  RM = Sxinv;
  clear Sxinv
end
%
if size(Kx,2) ~= size(RM,1)
  error(['Sizes of Kx and regularization matrix (Sx) do not match', ...
					      ' (recalculation needed?).'] );
end 



%### Return here if only Dy shall be calculated ###############################
%
if  return_Dy
  %
  KtSeinv = Kx'*Seinv;
  X       = inv( RM + KtSeinv*Kx ) * KtSeinv;
  G       = Kx; 
  %
  out(1,-1);
  if out(1)
    fprintf('\n');
  end
  return   %>>>>> Exit >>>>>
end



%### Special stuff: non-linear ################################################
if nonlin

  gamma  = Q.CLS_GA_START_VALUE;

  %=== Build cfile for calculation of Kx
  template = which( 'cls_iter2_kx.tmplt' );
  %
  QE            = [];
  QE.RECALC_ABS = recalc_abs;
  %
  qtool( Q, tmpdir, template, QE );
  cfile_kx  = [ cfile, '-FOR_KX' ];
  copyfile( cfile, cfile_kx );

  %=== Index for species profiles
  %
  ispecies = [];
  for i = 1 : size( kx_names, 1 )
    group = qp_kinfo( kx_names{i} );
    if strcmp( group, 'Species' )
      ispecies = [ ispecies; (kx_index(i,1):kx_index(i,2))' ];
    end
  end
  %

  %=== Take log of positive constraint for species
  %
  if logspecies
    xa(ispecies) = log( xa(ispecies) );
  end


%### Linear ###################################################################
else
  %
  gamma = 0;
  %
end



%### Special stuff: non-linear or post calculations ###########################
if nonlin | do_post

  %=== Build cfile for calculation of FY 
  template = which( 'cls_iter2_y.tmplt' );
  %
  QE            = [];
  QE.RECALC_ABS = recalc_abs;
  %
  qtool( Q, tmpdir, template, QE );
  cfile_y  = [ cfile, '-FOR_Y' ];
  copyfile( cfile, cfile_y );

  %=== Start cost
  cost_x = 0;
  a      = y - fy;
  cost_y = a' * Seinv * a;
  cost   = cost_x + cost_y;

  %=== Printing
  out(2,0);
  out(1,sprintf(' Iter              Total    Profile    Spectrum   Conver-'));
  out(1,sprintf('  nr    Gamma      cost      cost       cost       gence '));
  out(1,0);
  out(1,sprintf('  %2.0f      -      %.2e     0.00     %.2e       -',...
                                                         0, cost_y, cost_y ));

  %=== Load needed variables
  %
  %= Absorption
  if ~recalc_abs
    Abs  = read_artsvar( basename, 'abs');
    Absp = read_artsvar( basename, 'abs_per_tg');
  else
    Abs  = [];
    Absp = [];
  end
  %
  %= Zenith angles
  if Q.POINTING_DO == 3
    za_pencil = read_artsvar( basename, 'za_pencil');
  else
    za_pencil = [];
  end
  %
  %= Temparature profile
%  if Q.TEMPERATURE_DO == 3
%    t_abs = read_artsvar( basename, 't_abs');
%  else
%    t_abs = [];
%  end
end



%### Read ARTS variables always needed ########################################
%
p_abs = read_artsvar( basename, 'p_abs');
vmrs  = read_artsvar( basename, 'vmrs');




%### Loop until convergence ###################################################
%
%      ||          ||          ||          ||          ||          ||       
%      ||          ||          ||          ||          ||          ||       
%      ||          ||          ||          ||          ||          ||       
%      ||          ||          ||          ||          ||          ||       
%    \ || /      \ || /      \ || /      \ || /      \ || /      \ || /
%     \||/        \||/        \||/        \||/        \||/        \||/
%      \/          \/          \/          \/          \/          \/
%
%
iteration   = 0;
if nonlin
  max_iter  = Q.CLS_MAX_ITER;
else
  max_iter  = 1;
end
%
x           = xa;
y_corrected = y;
%
if do_xiter
  Xiter     = [];
end
%
G           = [];
G.converged = 0;
G.failure   = 'max iterations reached';    % default assumption
%
while ~G.converged & ( iteration < max_iter )

  iteration = iteration + 1;

  %=== Calculate new KX if iteration > 1
  %
  % Is this correct?:
  % The Kx to apply for the the log-retrieval of species is Kx*x (see Eq. 45
  % of Paper A in my thesis), but this is achieved automatically as Kx is
  % calculated in fractions of x (and not xa).
  %
  if iteration > 1
    %
    copyfile( cfile_kx, cfile );
    qp_arts( Q, cfile ); 
    %
    Kx = qp_Kx( Q, basename, H, Hd, fy );
    %
    if ~logspecies
      Kx(:,ispecies) = Kx(:,ispecies) ./ (ones(size(Kx,1),1)*x(ispecies)');
    end
    %
  end


  %=== Make inversion
  %
  % For CLS-Gauss-Newton, the error covariance matrix (Sd) is calculated 
  % as a step in the solution to have the error estimation ready.
  % For CLS-ML, Gaussian elimination is used as this is faster (the last 
  % iteration is always CLS-GN.
  %
  KtSeinv = Kx'*Seinv;
  %
  ok_iter = 0;
  %
  while ~ok_iter

    if ~nonlin
      %= If linear, set ok flag for iteration to true
      ok_iter     = 1;
    end

    %= A vector needed in different places
    if iteration == 1
      wdy = KtSeinv * ( y_corrected - fy );
    else
      wdy = KtSeinv * ( y_corrected - fy ) - RM * ( x - xa );
    end

    %= New x
    if gamma == 0
      Sd = inv( RM + KtSeinv*Kx ); 
      x_new = x + Sd * wdy;
    else
      dx  = (RM + KtSeinv*Kx + gamma*Dxinv) \ wdy;
      x_new = x + dx;
    end

    if nonlin | do_post

      %= Map X onto absorption etc. yc is correction term for the spectrum.
      yc = qp_x2arts( Q, x_new, basename, Kx, kx_names, kx_index, kx_aux, ...
                   recalc_abs, logspecies, p_abs, vmrs, Abs, Absp, za_pencil );
      %
      y_corrected = y - yc;
  
      %= New fy
      copyfile( cfile_y, cfile );
      qp_arts( Q, cfile ); 
      fy_new = read_artsvar( basename, 'y' );
      fy_new = H*fy_new;

      %= New cost
      a        = x_new - xa;
      cost_x   = a' * RM * a;
      a        = y_corrected - fy_new;
      cost_y   = a' * Seinv * a;
      cost_new = cost_x + cost_y;
    end

    if nonlin

      %= Is OK?
      if cost_new < cost
        ok_iter = 1;
      else
        out(1,sprintf('FAILED %.1e   %.2e', gamma, cost_new ));
       % Modifications!!! 
       if gamma > Q.CLS_GA_UPPER_LIMIT
          % -- to stop it, discuss what to do !!
          ok_iter = 1;     
          G.failure = 'runaway gamma';
        elseif gamma > 0
          gamma = gamma * Q.CLS_GA_FAC_WHEN_NOT_OK;
        else
          gamma = Q.CLS_GA_LOWER_TRESHOLD * Q.CLS_GA_FAC_WHEN_NOT_OK;
        end
      end
    end
  end % while


  if nonlin | do_post  % Note that gamma=0 when do_post=1

    %=== If gamma=0, check convergence
    if gamma==0 
      %
      conv = (( x_new - x )'*wdy) / nx;
      %
      out(1,sprintf('  %2.0f   %.1e   %.2e   %.2e   %.2e   %.2e',...
			   iteration, gamma, cost_new, cost_x, cost_y, conv ));
      %
      if nonlin & ( conv < Q.CLS_STOP )
        G.converged = 1;
        G.failure   = [];
      end 
      
    %=== Or, update gamma
    else
      out(1,sprintf('  %2.0f   %.1e   %.2e   %.2e   %.2e       -',...
				 iteration, gamma, cost_new, cost_x, cost_y ));
      %
      gamma = gamma / Q.CLS_GA_FAC_WHEN_OK;
      %
      if gamma < Q.CLS_GA_LOWER_TRESHOLD
	gamma = 0;
      end
    end

    fy   = fy_new;
    cost = cost_new;

  end

  x = x_new;
  
  if do_xiter
    Xiter = [Xiter, x];
  end

end

%##############################################################################
%### End of retrieval loop
%##############################################################################



%=== If log species, convert back
%
if logspecies
  x(ispecies) = exp( x(ispecies) );
end



%=== Fill X structure
%
X = qp_x2X( x, basename, kx_names, kx_index, kx_aux, p_abs, vmrs );



%=== Other return varaibles
%
if do_post
  %
  y_fitted = fy;
  %
  G.COST   = cost;
  G.COST_X = cost_x;
  G.COST_Y = cost_y;
end



%=== Delete the temporary directory
delete_tmp_dir( Q.TMP_AREA, tmpdir );



out(1,-1);
if out(1)
  fprintf('\n');
end