%function x = marq(init)
% Marq method - an attempt to write a script that does it,
% instead of using MatLab's fsolve function. 

% OPTIONS:	 	init, 1 = initialize, 0 = don't
%					plot, 1 = plot results, 0 = don't
% CALLS: 		resid_marq.m, of course

global resid_plot_results	%temp
global lowest
global ss_count 
% Globals from INIT.M:
global n_abs n_pix abs_gas filenames dir
% Absorber cross section global variables:
global wv_o3 wv_no2 wv_oclo wv_o4 wv_h2o wv_bro wv_ring wv_no3
global sm_o3 sm_no2 sm_oclo sm_o4 sm_h2o sm_bro sm_ring sm_no3

op.init = 0;
op.plot = 1;
op.view_marq_matrices = 1;

clear diff		% I don't know why, but this must be done. Somewhere, somehow,
					% I've created a variable called diff in another program...

% Initialize - read in spectra, initial a,b parameters, and absorber
% gas cross sections. 
if op.init
   % Read in absolute reference spectrum:
	disp(strcat('Absolute reference spectrum:', std_name))
	% Initialize array:
	ref = zeros(1,n_pix);
	% Read in the absolute reference spectrum:
	ref = read_spectrum(strcat(dir.ip, std_name));
	% Verify that the correct number of points were read in:
	if length(ref) ~= n_pix
	   disp('Hey! something wrong with # of pixels in absolute ref spectrum')
	end

	% Read in daily reference spectrum:
	disp(strcat('Daily reference spectrum:', file_nms(lowest,1:12)))
	% Initialize array:
	sp = zeros(1,n_pix);
	% Read in the daily reference spectrum:
	sp = read_spectrum(strcat(dir.data, file_nms(lowest,1:12)));
	% Verify that the correct number of points were read in:
	if length(sp) ~= n_pix
	   disp('Hey! something wrong with # of pixels in daily ref spectrum')
   end
   
   % Load the absorber cross sections from their various .mat files:
   load(strcat(dir.cs_op, 'cs_o3.mat'));
	load(strcat(dir.cs_op, 'cs_no2.mat'));
	load(strcat(dir.cs_op, 'cs_oclo.mat'));
	load(strcat(dir.cs_op, 'cs_o4.mat'));
	load(strcat(dir.cs_op, 'cs_bro.mat'));
   load(strcat(dir.cs_op, 'cs_h2o.mat'));
   
end % if init

% Give index numbers of gases to include in the fit:
abs_fit = [1 2 4 5 6];

% Apply Marq method:
ss_UB = [1 2];			% upper bound on ss
ss_LB = [-1 0];		% lower bound on ss
% note, if we are using fsolve, upper and lower bounds are not used
% Create global counter to keep track of shift/stretch modifications:
ss_count = 0;
clear global ss_record_OLD
global ss_record_OLD
ss_record_OLD = ss_record;		% might be useful
clear global ss_record
global ss_record

resid_plot_results = 0;

ss(1) = 0.5;		% shift
ss(2) = 1.2; 		% stretch

% a,b,c parameters to start with:
cba_sp = [0 b_sp a_sp];			% a_sp, b_sp read in from A_B.REF
cba_ref = [0 b_ref a_ref];		% a_ref, b_ref read in from A_B.REF

% Calculate chi^2
x = resid_marq(ss, sp, ref, cba_sp, cba_ref, abs_fit);
res 		= x{1};
wv_spec 	= x{2};
spec 		= x{3};
cba 		= x{4};
ss 		= x{5};
w_cent 	= x{6};
  
chi_sq = sum(res.^2);		% assume std dev = 1, it will just cancel anyways
orig_chi_sq = chi_sq;

% It's easier to keep track of things if these variables are given
% more easily recognizable names:
sh = ss(1);		% shift
st = ss(2);		% stretch
b = cba(2);
a = cba(3);

a_orig = a;
b_orig = b;

chi_cut = 0.45;			%1.422;
lambda = 0.1;			% Marq parameter, a modest starting value
iter = 0;					% count the # of while loop iterations

% Some output to keep the user informed of what's going on:
disp(['Starting Marq method...'])
disp(['   chi cutoff value = ' num2str(chi_cut)])
disp(['Note that this fit is for the case c = 0'])
disp(['Initial shift and stretch:'])
disp(['   shift = ' num2str(ss(1)) ', stretch = ' num2str(ss(2))])
disp(['Original a,b parameters:'])
disp(['   a = ' num2str(a_orig) ', b = ' num2str(b_orig)])
disp(['Initial value of lambda = ' num2str(lambda)])

tic;

while chi_sq >= chi_cut
   
   % For now, only the case c = 0 is treated. 
   if cba(1) ~= 0
      disp('Error, c is not zero. Quitting MARQ.M.')
      return
   end
   
   % Calculate matrices used for the Marq method:
   
   % First, some analytical derivatives:
   z = sh + w_cent*(1 - st);
   db_dst = -b/st^2;
   db_dsh = 0;
   da_dst = b*(z/st + w_cent)/st;
   da_dsh = -b/st;
   
   pix = 1:1:length(wv_spec);
   dwv_dst = (pix - da_dst)/b - (pix - a)*db_dst/b^2;
   dwv_dsh = (pix - da_dsh)/b - (pix - a)*db_dsh/b^2;
   
   % Probably this is a bad way to approximate the next derivative, but it will
   % do for now...   
   dspec_dwv = diff(spec)./diff(wv_spec);
   % The length of this array needs to be increased by one so that the 
   % algebra below will be okay:
   dspec_dwv(length(dspec_dwv)+1) = dspec_dwv(length(dspec_dwv));
   
   % Calculate Marq matrices:
   beta(1) = -sum( res.*dspec_dwv.*dwv_dsh );
   beta(2) = -sum( res.*dspec_dwv.*dwv_dst );
   alpha(1,1) = (1 + lambda)*sum( dspec_dwv.^2.*dwv_dsh.^2 );
   alpha(1,2) = sum( dspec_dwv.^2.*dwv_dsh.*dwv_dst );
   alpha(2,1) = alpha(1,2);
   alpha(2,2) = (1 + lambda)*sum( dspec_dwv.^2.*dwv_dst.^2 );
   
   if op.view_marq_matrices
      disp(['beta:                      alpha:'])
      disp([num2str(beta(1)) '           ' num2str(alpha(1,1)) '  ' num2str(alpha(1,2))])
      disp([num2str(beta(2)) '           ' num2str(alpha(2,1)) '  ' num2str(alpha(2,2))])
   end
   % Solve the linear equations for dss, the change in shift/stretch
   % The matrix equation for the system is 
   %			alpha * dss = beta
   % where alpha is a 2x2 matrix, dss and beta are 2x1 vectors
   
   dss = alpha\beta';
   
   % Now use dss to perturb the system and find the new chi^2:
   
	x = resid_marq(ss + dss', sp, ref, cba_sp, cba_ref, abs_fit);
	res			= x{1};
	wv_spec		= x{2};
	spec			= x{3};
	cba_new		= x{4};
	%ss_new 		= x{5};
	w_cent_new 	= x{6};
   
   prev_chi_sq = chi_sq;
   chi_sq = sum( res.^2 );
   
   % Determine which way to change lambda:
   if chi_sq >= prev_chi_sq
      lambda = 10*lambda;
      chi_sq = prev_chi_sq;
   else
      lambda = lambda/10;
      ss = ss + dss';	% or, equivalently, ss = ss_new
      sh = ss(1);		% shift
		st = ss(2);		% stretch
		b = cba_new(2);
      a = cba_new(3);
      w_cent = w_cent_new;
   end
   
   % Increment loop counter and global var keeping track of the history of
   % shift/stretch values:
   iter = iter + 1;
   ss_record(iter,:) = ss;
   
   % Output some stuff so that the user knows what's happening:
   disp(['Iteration #' num2str(iter) ' of Marq method:'])
   disp(['   chi squared = ' num2str(chi_sq)])
   disp(['   previous chi squared = ' num2str(prev_chi_sq)])
   disp(['   shift = ' num2str(ss(1)) ', stretch = ' num2str(ss(2))])
   disp(['   dss = ' num2str(dss(1)) '    ' num2str(dss(2))])
   disp(['   new lambda = ' num2str(lambda)])
   
   % Exit loop if it's taking too long to find a solution:
   if iter > 12
      break
   end
   
end 	% while chi_sq > chi_cut

loop_time = toc;

% Output summary of infomation about the fit:
disp([' '])
disp(['SUMMARY OF FIT RESULTS:'])
disp(['   Total # of loop iterations = ' num2str(iter)])
disp(['   chi squared = ' num2str(chi_sq)])
disp(['   original chi squared = ' num2str(orig_chi_sq)])
disp(['   Final a,b parameters:'])
disp(['      a = ' num2str(a)])
disp(['      b = ' num2str(b)])
disp(['   Original a,b parameters:'])
disp(['      a = ' num2str(a_orig) ])
disp(['      b = ' num2str(b_orig) ])
disp(['   Final value of lambda = ' num2str(lambda)])
disp(['   Loop execution time = ' num2str(loop_time) ' seconds'])

% Plot results
if op.plot
   % get spectra from last fit:
   wv_spec	= x{2};
	spec		= x{3};
	% plot:
   figure(30)
   clf
   plot(wv_spec, spec, 'r')
   hold on
   plot(wv_spec, spec - res, 'g')
   title('Fit results from MARQ.M: data = red, fit = green')
end

