%%%%% smallfit_noMPI_MSS.m %%%%%%%%%%%%%%%%%%%%%%

% check that all information is provided for vector data
N_sources = size(options.filename_vector, 1);
if N_sources ~= length(params.dt_Euler);     error('Wrong number of elements in params.dt_Euler'); end
if N_sources ~= length(params.Euler_weight); error('Wrong number of elements in params.Euler_weights'); end
if N_sources ~= size(params.Euler_start, 1); error('Wrong number of elements in params.Euler_start'); end

hold off
clf

disp(' ');
disp(['**********************   ' model.name '   ***********************   '  datestr(now) ]);


% Some initial setup
rad = pi/180;
t_cpu = cputime;
clock_start_time=clock();
step = 1;
t_start = model.knots(1);
t_end = model.knots(end);
filename_update=options.filename_update;

% get code (this program and driver program)
st = dbstack;
source = textread(st(1).file, '%s', 'delimiter', '\n', 'whitespace', ''); % Matlab 7
source_driver = textread(st(2).file, '%s', 'delimiter', '\n', 'whitespace', '');
source_driver = source_driver(1:st(2).line); % only driver program up to the calling line

if exist('options', 'var') && isfield(options, 'update') && options.update == 1
    
    options_old = options;  % Retain parameters from current driver in case needed, before loading update
    params_old = params;
    model_old = model;
    options.fields = fieldnames(options);
    
    names_mod = fieldnames(model);
    names_opt = fieldnames(options);
    names_par = fieldnames(params);
    
    load(options.filename_update)  % Load previous run
    options.update=1;  % Need to reset incase previous model had options.update=0
    
    options.dir=options_old.dir;  % Reload directories and named needed from current driver
    options.dir_save=options_old.dir_save;
    
    if exist('model_update','var') == 1
        for k = 1:length(names_mod)   % reset update options
            if isfield(model_update, names_mod{k})
                model = setfield(model, names_mod{k}, getfield(model_update, names_mod{k}));
            end
        end
    end
    
    if exist('options_update','var') == 1
        for k = 1:length(names_opt)   % reset update options
            if isfield(options_update, names_opt{k})
                options = setfield(options, names_opt{k}, getfield(options_update, names_opt{k}));
            end
        end
    end
    
    if exist('params_update','var') == 1
        for k = 1:length(names_par)   % reset update params
            if isfield(params_update, names_par{k})
                params = setfield(params, names_par{k}, getfield(params_update, names_par{k}));
            end
        end
    end
    
else
    %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% read data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    
    % Load Dst
    [t_Dst, Dst_all, Dst_e_all, Dst_i_all]  = textread(options.filename_Dst, '%f %f %f %f %*s', 'commentstyle', 'shell');
    dDst_all = diff(Dst_all);
    dDst_all = [dDst_all(1); dDst_all];
    t_Dst = [t_Dst; 1e99];     dDst_all = [dDst_all; 0]; % adds zero values at end
    Dst_all = [Dst_all; 0]; Dst_e_all = [Dst_e_all; 0]; Dst_i_all = [Dst_i_all; 0];
    
    % Load Kp
    [t_kp, kp_all]  = textread(options.filename_Kp, '%f %d %*d  %*s', 'commentstyle', 'shell', 'headerlines', 4);
    
    %     % Load E_m (merging electric field)
    %     load(options.filename_Em);
    %     t_E_all = tEm;
    %     E_all_new = Em;
    %
    %     index = isnan(E_all_new);
    %     t_E_all(index) = []; E_all_new(index) = [];
    %     b = [ones(12,1)/12]; % filter: 1 hour history (if 5 min sampling)
    %     E_all_filtered = filter(b, 1, E_all_new);
    
    
    % Load scalar data
    data_scalar.t = []; data_scalar.r = []; data_scalar.theta = []; data_scalar.phi = []; data_scalar.F = []; data_scalar.source = [];
    for i_source = 1:size(options.filename_scalar, 1)
        if exist(strtrim(options.filename_scalar(i_source,:)), 'file') > 0
            tmp = load(strtrim(options.filename_scalar(i_source,:)));
            index = abs(interp1(t_Dst, dDst_all, tmp(:,1), 'linear')) > params.dDst_max ...
                | tmp(:,1) < t_start | tmp(:,1) > t_end;
            tmp(index,:) = [];
            data_scalar.t      = [data_scalar.t;      tmp(1:options.N_Data_Step:end,1)];
            data_scalar.r      = [data_scalar.r;      tmp(1:options.N_Data_Step:end,2)/params.a];
            data_scalar.theta  = [data_scalar.theta;  tmp(1:options.N_Data_Step:end,3)*rad];
            data_scalar.phi    = [data_scalar.phi;    tmp(1:options.N_Data_Step:end,4)*rad];
            data_scalar.F      = [data_scalar.F;      tmp(1:options.N_Data_Step:end,5)];
            data_scalar.source = [data_scalar.source; i_source*ones(size(tmp(1:options.N_Data_Step:end,5),1), 1)];
        end
    end
    
    % remove NaN's
    index = isnan(data_scalar.r);
    data_scalar = remove_datastructure_entries(data_scalar, index);
    
    % determine QD latitude
    data_scalar.QD_lat = qdipole(data_scalar.t, data_scalar.r, data_scalar.theta, data_scalar.phi, options.filename_apexsh);
    
    % delete non-polar data that do not fit Kp criteria
    kp = interp1(t_kp, kp_all, data_scalar.t, 'nearest');
    index = find((abs(data_scalar.QD_lat) < params.lat_gm_max) & (kp >= params.kp_max));
    data_scalar = remove_datastructure_entries(data_scalar, index);
    
    % Load vector data
    data_vector.t = []; data_vector.r = []; data_vector.theta = []; data_vector.phi = [];
    data_vector.B = [];
    data_vector.q_j2000 = []; data_vector.q_ecef = []; data_vector.source = []; data_vector.SIM_flag = [];
    for i_source = 1:size(options.filename_vector, 1)
        if exist(strtrim(options.filename_vector(i_source,:)), 'file') > 0
            
            tmp = load(strtrim(options.filename_vector(i_source,:)));
            
            index = abs(interp1(t_Dst, dDst_all, tmp(:,1), 'linear')) > params.dDst_max ...
                | tmp(:,1) < t_start | tmp(:,1) > t_end;
            tmp(index,:) = [];
            
            data_vector.t        = [data_vector.t; tmp(1:options.N_Data_Step:end,1)];
            data_vector.r        = [data_vector.r; tmp(1:options.N_Data_Step:end,2)/params.a];
            data_vector.theta    = [data_vector.theta; tmp(1:options.N_Data_Step:end,3)*rad];
            data_vector.phi      = [data_vector.phi; tmp(1:options.N_Data_Step:end,4)*rad];
            data_vector.B        = [data_vector.B; tmp(1:options.N_Data_Step:end,5:7)];
            data_vector.q_j2000  = [data_vector.q_j2000; tmp(1:options.N_Data_Step:end,8:11)];
            data_vector.q_ecef   = [data_vector.q_ecef; tmp(1:options.N_Data_Step:end,12:15)];
            data_vector.source   = [data_vector.source; i_source*ones(size(tmp(1:options.N_Data_Step:end,5),1), 1)];
%             if i_source == 1
%                 data_vector.SIM_flag = [data_vector.SIM_flag; 1000*ones(size(tmp(1:options.N_Data_Step:end,5),1), 1)];
%                 % pre-rotation of Oersted data
%                 data_vector.B = [data_vector.B(:,2) data_vector.B(:,3) data_vector.B(:,1)];
%             elseif i_source == 2
%                 data_vector.SIM_flag = [data_vector.SIM_flag; tmp(1:options.N_Data_Step:end,16)];
%             else % Swarm
                data_vector.SIM_flag = [data_vector.SIM_flag; 1100*ones(size(tmp(1:options.N_Data_Step:end,5),1), 1)];
%             end
        end
    end
    
    clear tmp
    
    % remove non valid positions
    index = isnan(data_vector.r);
    data_vector = remove_datastructure_entries(data_vector, index);
    
    % determine QD latitude
    data_vector.QD_lat = qdipole(data_vector.t, data_vector.r, data_vector.theta, data_vector.phi, options.filename_apexsh);
    
    % remove non-polar data that do not fit Kp criteria
    kp = interp1(t_kp, kp_all, data_vector.t, 'nearest'); % Kp of present time instant
    index = find((abs(data_vector.QD_lat) < params.lat_gm_max) & (kp >= params.kp_max));
    data_vector = remove_datastructure_entries(data_vector, index);
    
    %     % remove polar scalar data that do not fit E_m criteria
    %     Em = interp1(t_E_all, E_all_filtered, data_scalar.t, 'nearest');
    %     index = find((abs(data_scalar.QD_lat) > params.lat_gm_max) & (Em >= params.Em_max));
    %     data_scalar.t(index) = []; data_scalar.r(index) = []; data_scalar.theta(index) = []; data_scalar.phi(index) = [];
    %     data_scalar.F(index) = []; data_scalar.source(index) = []; data_scalar.QD_lat(index) = [];
        
%     % extract only vector data that have both star cameras operating
%     if exist('options', 'var') && isfield(options, 'both_stcam_CH') && options.both_stcam_CH == 1
%         index = find(data_vector.source == 2 & data_vector.SIM_flag == 1100);
%         disp([num2str(length(index)) ' vector triplets with data from both star cameras selected'])
%         data_vector.t = data_vector.t(index); data_vector.r = data_vector.r(index); data_vector.theta = data_vector.theta(index); data_vector.phi = data_vector.phi(index);
%         data_vector.B = data_vector.B(index, :); data_vector.QD_lat = data_vector.QD_lat(index);
%         data_vector.q_j2000 = data_vector.q_j2000(index,:); data_vector.q_ecef = data_vector.q_ecef(index,:);
%         data_vector.source = data_vector.source(index); data_vector.SIM_flag = data_vector.SIM_flag(index);
%     end
    
    % sort scalar data according to time
    [data_scalar.t,index] = sort(data_scalar.t);
    data_scalar.r = data_scalar.r(index); data_scalar.theta = data_scalar.theta(index); data_scalar.phi = data_scalar.phi(index);
    data_scalar.F = data_scalar.F(index); data_scalar.source = data_scalar.source(index); data_scalar.QD_lat = data_scalar.QD_lat(index);
    
    % sort vector data according to time
    [data_vector.t,index] = sort(data_vector.t);
    data_vector.r = data_vector.r(index); data_vector.theta = data_vector.theta(index); data_vector.phi = data_vector.phi(index);
    data_vector.B = data_vector.B(index, :);
    data_vector.q_j2000 = data_vector.q_j2000(index,:); data_vector.q_ecef = data_vector.q_ecef(index,:);
    data_vector.source = data_vector.source(index); data_vector.SIM_flag = data_vector.SIM_flag(index);
    data_vector.QD_lat = data_vector.QD_lat(index);
    
    % define sequence of breaks in time for q10 determination
    if params.delta_t_q10 > 1e5 % use constant value, not time dependent
        model.t_break_q10 = [-1e4; 1e4];
    else
        model.t_break_q10 = [floor(min([data_scalar.t; data_vector.t])/params.delta_t_q10)*params.delta_t_q10:params.delta_t_q10:max([data_scalar.t; data_vector.t]+params.delta_t_q10)]';
    end
    
    % define sequence of breaks in time for q11 etc. determination
    if params.delta_t_qs11 > 1e5 % use constant value, not time dependent
        model.t_break_qs11 = [-1e4; 1e4];
    else
        model.t_break_qs11 = [floor(min([data_scalar.t; data_vector.t])/params.delta_t_qs11)*params.delta_t_qs11:params.delta_t_qs11:max([data_scalar.t; data_vector.t]+params.delta_t_qs11)]';
    end
    
    % remove data and bins that do not contain enough data
    bin = 1;
    while bin < length(model.t_break_q10)
        t_tmp = [data_scalar.t; data_vector.t];
        index = model.t_break_q10(bin) < t_tmp & t_tmp < model.t_break_q10(bin+1);
        N_bin = length(find(index > 0));
        if N_bin < options.N_min
            % delete scalar data of that bin
            index = model.t_break_q10(bin) < data_scalar.t & data_scalar.t < model.t_break_q10(bin+1);
            data_scalar = remove_datastructure_entries(data_scalar, index);
            % delete vector data of that bin
            index = model.t_break_q10(bin) < data_vector.t & data_vector.t < model.t_break_q10(bin+1);
            data_vector = remove_datastructure_entries(data_vector, index);
            
            model.t_break_q10(bin+1) = [];
        else
            bin = bin + 1;
        end
    end
    
    % remove data and bins that do not contain enough data
    bin = 1;
    while bin < length(model.t_break_qs11)
        t_tmp = [data_scalar.t; data_vector.t];
        index = model.t_break_qs11(bin) < t_tmp & t_tmp < model.t_break_qs11(bin+1);
        N_bin = length(find(index > 0));
        if N_bin < options.N_min
            % delete scalar data of that bin
            index = model.t_break_qs11(bin) < data_scalar.t & data_scalar.t < model.t_break_qs11(bin+1);
            data_scalar = remove_datastructure_entries(data_scalar, index);
            % delete vector data of that bin
            index = model.t_break_qs11(bin) < data_vector.t & data_vector.t < model.t_break_qs11(bin+1);
            data_vector = remove_datastructure_entries(data_vector, index);
            % extend the bin by removing its right side
            model.t_break_qs11(bin+1) = [];
        else
            bin = bin + 1;
        end
    end
    
    N_t_segments_q10 = length(model.t_break_q10)-1;
    disp(['Length (in days) of time segments for estimation of q_1^0: ',num2str(params.delta_t_q10)])
    disp(['Number of time-segments for estimation of q_1^0: ',int2str(N_t_segments_q10)])
    N_t_segments_qs11 = length(model.t_break_qs11)-1;
    disp(['Length (in days) of time segments for estimation of q_1^1: ',num2str(params.delta_t_qs11)])
    disp(['Number of time-segments for estimation of q_1^1: ',int2str(N_t_segments_qs11)])
    
    % Dst for scalar data
    if model.maxdeg_ext > 0
        data_scalar.Dst = interp1(t_Dst, Dst_all, data_scalar.t, 'linear');
        if any(isnan(data_scalar.Dst)); warning('data_scalar.Dst contains NaN'); end
        data_scalar.Est = interp1(t_Dst, Dst_e_all, data_scalar.t, 'linear');
        data_scalar.Ist = interp1(t_Dst, Dst_i_all, data_scalar.t, 'linear');
        index = isnan(data_scalar.Dst);
        data_scalar = remove_datastructure_entries(data_scalar, index);
    end
    
    % bins for Euler angle determination
    for i_source = 1:length(params.dt_Euler)
        if params.dt_Euler(i_source) > 0
            if params.dt_Euler(i_source) > 1e5 % use constant value, not time dependent
                model.t_break_Euler{i_source} = [-1e4; 1e4];
            else
                t_tmp = data_vector.t(data_vector.source == i_source);
                model.t_break_Euler{i_source} = [floor(min(t_tmp)/params.dt_Euler(i_source))*params.dt_Euler(i_source):params.dt_Euler(i_source):max(t_tmp)]';
                model.t_break_Euler{i_source} = [model.t_break_Euler{i_source}; model.t_break_Euler{i_source}(end)+params.dt_Euler(i_source)];
            end
            
            if i_source == 1
                % add additional Euler angle break at 24. Jan 2000 for Oersted
                if min(t_tmp) < 24; model.t_break_Euler{1} = sort([model.t_break_Euler{1}(:); 24]); end; % include options.a break on Jan 22, 2000
                if isempty(data_vector.source == 1)
                    model.t_break_Euler{1} = 0;
                end
            end
            
            % remove data and bins that do not contain enough data
            bin = 1;
            while bin < length(model.t_break_Euler{i_source})
                t_tmp = data_vector.t(data_vector.source == i_source);
                index = model.t_break_Euler{i_source}(bin) < t_tmp & t_tmp < model.t_break_Euler{i_source}(bin+1);
                
                if length(find(index > 0)) < 10 % extend the bin by removing its right side
                    model.t_break_Euler{i_source}(bin+1) = [];
                else
                    bin = bin + 1;
                end
            end
            N_Euler(i_source) = length(model.t_break_Euler{i_source})-1;
            disp(['Length (in days) of time segments for estimation of Euler angles for sat ' int2str(i_source) ': ' num2str(params.dt_Euler(i_source))])
            disp(['Number of time-segments for estimation of Euler angles for sat ' int2str(i_source) ': ' int2str(N_Euler(i_source))])
        else
        end
    end
    
    if model.maxdeg_ext > 0 % Dst for vector data
        data_vector.Dst = interp1(t_Dst, Dst_all, data_vector.t, 'linear');
        if any(isnan(data_vector.Dst)); warning('data_vector.Dst contains NaN'); end
        index = isnan(data_vector.Dst);
        data_vector = remove_datastructure_entries(data_vector, index);
        data_vector.Est = interp1(t_Dst, Dst_e_all, data_vector.t, 'linear');
        data_vector.Ist = interp1(t_Dst, Dst_i_all, data_vector.t, 'linear');
    end
    
    % read observatory data
    if exist(options.filename_obs_data, 'file') > 0
        load(options.filename_obs_data);
        data_obs.t = t_obs_all;
        data_obs.theta = theta_obs_all;
        data_obs.phi = phi_obs_all;
        data_obs.names = obs_all;
        data_obs.dB = dB_obs_all;
        data_obs.C_1_11 = C_1_11;
        data_obs.C_1_12 = C_1_12;
        data_obs.C_1_13 = C_1_13;
        data_obs.C_1_23 = C_1_23;
        data_obs.C_1_22 = C_1_22;
        data_obs.C_1_33 = C_1_33;
        clear t_obs_all theta_obs_all phi_obs_all obs_all dB_obs_all w_obs
        clear C_1_11  C_1_12 C_1_13 C_1_22 C_1_23 C_1_33
        
        index = find(data_obs.t < model.knots(1) | isnan(data_obs.dB(:,1)) ...
            | isnan(data_obs.dB(:,2)) | isnan(data_obs.dB(:,3)));
        data_obs.t(index) = []; data_obs.theta(index) = []; data_obs.phi(index) = [];
        data_obs.names(index,:) = []; data_obs.dB(index,:) = [];
        data_obs.C_1_11(index) = []; data_obs.C_1_12(index) = []; data_obs.C_1_13(index) = [];
        data_obs.C_1_22(index) = []; data_obs.C_1_23(index) = []; data_obs.C_1_33(index) = [];
    else
        data_obs.t = []; data_obs.theta = []; data_obs.phi = [];
        data_obs.names = []; data_obs.dB = [];
        data_obs.C_1_11 = []; data_obs.C_1_12 = []; data_obs.C_1_13 = [];
        data_obs.C_1_22 = []; data_obs.C_1_23 = []; data_obs.C_1_33 = [];
        data_obs.w = [];
        data_obs.dB_synth = [];
    end
    
    % initialize scalar data weights and residuals
    N_data_F   = length(data_scalar.t);
    data_scalar.w = zeros(N_data_F, 1);
    data_scalar.delta_F = zeros(N_data_F, 1);
    
    N_data_vec = length(data_vector.t);
    data_vector.B_synth = zeros(N_data_vec, 3);
    data_vector.dB_rtp = zeros(N_data_vec, 3);
    data_vector.dB_B23 = zeros(N_data_vec, 3);
    data_vector.w_B23 = zeros(N_data_vec, 3);
    
end %%%%%%%%%%%%%%% read data %%%%%%%%%%%%%%%%%%%%%%

% calculate transformation matrices for vector data
rot_j2000 = qu2arot(data_vector.q_j2000);
rot_ecef  = qu2arot(data_vector.q_ecef);
rot_local = ecef_use(data_vector.theta/rad, data_vector.phi/rad);
data_vector.R_rtp     = mat_mul_mat(rot_local, mat_mul_mat(rot_ecef, rot_j2000));
data_vector.n       = data_vector.R_rtp(:,1:3,3);

% % bore-sight direction if only one (1st or 2nd) CHAMP ASC
ASC_1_q = [0.9024604684  0.0139371969  0.0064337971  0.4304990868];
ASC_2_q = [-0.0069842388 -0.9025747103  0.4304634224  0.0033696754];
Rot_1 = squeeze(qu2arot(ASC_1_q));
Rot_2 = squeeze(qu2arot(ASC_2_q));
R_tmp = mat_mul_mat(data_vector.R_rtp, squeeze(qu2arot(ASC_1_q)), 1, -1);
index = find(data_vector.source == 2 & data_vector.SIM_flag == 100);
data_vector.n(index, :)       = R_tmp(index,1:3,3);
R_tmp = mat_mul_mat(data_vector.R_rtp, squeeze(qu2arot(ASC_2_q)), 1, -1);
index = find(data_vector.source == 2 & data_vector.SIM_flag == 1000);
data_vector.n(index, :)       = R_tmp(index,1:3,3);

N_data_F   = length(data_scalar.r); disp(['Scalar data points: ',       int2str(N_data_F)])
N_data_vec = length(data_vector.r); disp(['Vector data point triples: ',int2str(N_data_vec)])
N_data_obs = length(data_obs.t);

data_vector.F = sqrt(data_vector.B(:,1).^2 + data_vector.B(:,2).^2 + data_vector.B(:,3).^2);
data_vector.psi = params.psi*ones(size(data_vector.t));
data_vector.chi = params.chi*ones(size(data_vector.t));

N_data = N_data_F + 3*N_data_vec;

%%%%%%%%%%%%%%%%%%%% model setup %%%%%%%%%%%%%%%%%%%%%%%%%

if  exist('options', 'var') && isfield(options, 'update') && options.update == 1
    disp(['   Update of model', filename_update])
    N_para = size(iC_prior, 1); % number of model parameters
    N_model.knots = length(model.knots) - model.sp_order; % number of options.knots
    N_coeff_MF = model.maxdeg_MF*(model.maxdeg_MF+2); % max. number of MF coefficients
    N_coeff_SHA_spline = model.maxdeg_spline*(model.maxdeg_spline+2); % number of SHA coefficient per knot
    
    N_coeff_spline = N_coeff_SHA_spline*N_model.knots;
    
    N_t_segments_q10 = length(model.t_break_q10)-1;
    N_t_segments_qs11 = length(model.t_break_qs11)-1;
    
    for i_source = 1:length(params.dt_Euler)
        N_Euler(i_source) = length(model.t_break_Euler{i_source})-1;
    end
    
    N_coeff = N_para - 3*sum(N_Euler);
    
    N_coeff_ext    = 8+2+3 -1 + N_t_segments_q10;
    N_coeff_ext    = N_coeff_ext-2   + 2*N_t_segments_qs11;    
    
else
    %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% prepare starting models and weights %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%
    %
    N_coeff_MF = model.maxdeg_MF*(model.maxdeg_MF+2);
    N_coeff_SHA_spline = model.maxdeg_spline*(model.maxdeg_spline+2);
    
    % read start model
    [m_start, model.epoch_start, model.maxdeg_MF_start, model.maxdeg_spline_start, r_ref, m_start_ext] = read_model(options.filename_start_model);
    
    if r_ref ~= params.a; warning('Initial model: r_ref <> params.a'); end
    
    % Load starting MF, taking care of change in degree
    if model.maxdeg_MF_start > model.maxdeg_MF
        m_start_MF = m_start(1:model.maxdeg_MF*(model.maxdeg_MF+2));
    else
        m_start_MF = [m_start(1:model.maxdeg_MF_start*(model.maxdeg_MF_start+2)); zeros(model.maxdeg_MF*(model.maxdeg_MF+2)-model.maxdeg_MF_start*(model.maxdeg_MF_start+2),1)];
    end
    % Load starting SV, taking care of change in degree
    if model.maxdeg_spline_start > model.maxdeg_spline
        m_start_SV = m_start(model.maxdeg_MF_start*(model.maxdeg_MF_start+2)+1:model.maxdeg_MF_start*(model.maxdeg_MF_start+2)+model.maxdeg_spline*(model.maxdeg_spline+2));
    else
        m_start_SV = [m_start(model.maxdeg_MF_start*(model.maxdeg_MF_start+2)+1:model.maxdeg_MF_start*(model.maxdeg_MF_start+2)+model.maxdeg_spline_start*(model.maxdeg_spline_start+2)); zeros(model.maxdeg_spline*(model.maxdeg_spline+2)-model.maxdeg_spline_start*(model.maxdeg_spline_start+2),1)];
    end
    
    %%% Update starting MF using SV linear extrapolation to model.epoch
    if model.epoch ~= model.epoch_start
        warning('Initial model: model.epoch <> model.epoch_start. MF updated according to spline SV');
        m_start_MF(1:model.maxdeg_spline*(model.maxdeg_spline+2)) = m_start_MF(1:model.maxdeg_spline*(model.maxdeg_spline+2)) + m_start_SV(1:model.maxdeg_spline*(model.maxdeg_spline+2))*(model.epoch-model.epoch_start);
    end
    
    if model.maxdeg_spline > 0
        disp(['    spline representation: N_max = ',int2str(model.maxdeg_spline)])
    else
        disp('    no secular variation')
    end
    
    if model.maxdeg_ext > 0
        disp(['    external contribution: N_max = ',int2str(model.maxdeg_ext)])
        N_coeff_ext_1 = 8+2+3;   % Warning- this is hardwired?
        N_coeff_ext = N_coeff_ext_1-1 + N_t_segments_q10; % add parameters for q_1^0 t_break
        N_coeff_ext = N_coeff_ext-2   + 2*N_t_segments_qs11; % add parameters for q11 + s11
    else
        N_coeff_ext = 0;
        disp('    no external contribution')
    end
    
    m_start_ext = zeros(N_coeff_ext, 1);  %%% This forces ext starting model
    
    %%% Now build starting model
    
    if model.sp_order == 5
        t_tmp = [model.knots(4):30:model.knots(end-3)]';
    else
        t_tmp = [model.knots(1):30:model.knots(end)]';
    end
    
    coll = spcol(model.knots, model.sp_order, t_tmp, 'sp', 'no');
    
    N_model.knots = size(coll, 2); % number of options.knots
    N_coeff_SHA_spline = model.maxdeg_spline*(model.maxdeg_spline+2); % number of SHA coefficient per knot
    N_coeff_spline = N_coeff_SHA_spline*N_model.knots;
    
    m_start_spline = [];
    for k = 1:N_coeff_SHA_spline
        tmp = m_start_MF(k)+m_start_SV(k)*t_tmp/365.25; % linear model field prediction for all times
        m_start_spline = [m_start_spline; coll\tmp];  % projects this using coll into spline knots
    end
    
    % add static coefficients above spline truncation level
    m_i = [m_start_spline; m_start_MF(N_coeff_SHA_spline+1:N_coeff_MF)];
    
    % add external coefficients
    m_i = [m_i; m_start_ext];
    
    m_weights = zeros(size(m_i));
    N_coeff = length(m_i);
    
    % add starting (= a-priori) values and weights for Euler angles
    for i_angle = 1:3
        for i_source = 1:length(params.dt_Euler)
            m_i = [m_i; ...
                params.Euler_start(i_source, i_angle)*ones(N_Euler(i_source), 1)];
            %%%%%%%%% NIO %%%%%%% only constrain alpha Euler angle
            %             if i_angle == 1;
            m_weights = [m_weights; ...
                params.Euler_weight(i_source)*ones(N_Euler(i_source), 1)];
            %             else
            %             m_weights = [m_weights; ...
            %                 0*params.Euler_weight(i_source)*ones(N_Euler(i_source), 1)];
            %             end
        end
    end
    
    N_para = length(m_weights); % number of model parameters
    disp(['Number of parameters: N_para = ',int2str(N_para)])
    iC_prior = spdiags(m_weights, 0, N_para, N_para);
    m_prior = m_i; % a-priori model equal to start model
end

T_ll = [spalloc(N_para, N_para, 0)];

% regularisation of crustal field

n_min = 85; n_max = 95;

T_cr = [];
for n = 1:model.maxdeg_MF
    factor_t = options.damping_function(n);
    if n < n_min; factor_t = 0; end
    T_cr = [T_cr; repmat(factor_t, 2*n+1, 1)];
end
if model.maxdeg_MF > n_min        % taper high-degrees to zero
    
    N = [];
    for n = 1:model.maxdeg_MF
        N = [N; repmat(n, 2*n+1, 1)];
    end
    k_low = n_min*(n_min+2)+1;
    k_up = n_max*(n_max+2);
    taper = zeros(model.maxdeg_MF*(model.maxdeg_MF+2),1);
    taper(k_low:k_up) = 1-.5*(1+cos(2*pi/(N(k_up)-N(k_low)+(n_max-n_min))*(N(k_low:k_up)-N(k_low))));
    taper(k_up+1:end) = 1;
    T_cr = T_cr.*taper;
end

T_cr = [zeros(N_coeff_spline, 1); T_cr(N_coeff_SHA_spline+1:end); zeros(N_coeff_ext+3*sum(N_Euler), 1)];
T_cr = spdiags(T_cr, 0, length(m_i), length(m_i));

if model.sp_order > 3 
    % constrain: second or third time derivative of splines coefficients
    dt = 1;   
    
    coll_t = spcol(model.knots, model.sp_order, [model.knots(4):dt:model.knots(end-3)]', 'sp');
        
    coll_2 = diff(coll_t, 2)/(dt/365.25)^2;
    if options.spline_constrain_derivative == 2
        T_time = coll_2'*coll_2*(dt/365.25);
    elseif options.spline_constrain_derivative == 3
        coll_3 = diff(coll_t, 3)/(dt/365.25)^3;
        T_time = coll_3'*coll_3*(dt/365.25);
    else
        warning('options.spline_constrain_derivative <> 2 or 3')
    end
    
    
    % T_1 is damping matrix for second time derivative at first spline knot
    % T_3 is damping matrix for second time derivative at last spline knot
    T_ll =   [];
    T_1 = []; T_2 = []; T_3 = []; k = 0;
    T_tmp_1 = coll_2(1,:)'*coll_2(1,:)*(dt/365.25);
    T_tmp_2 = coll_2'*coll_2*(dt/365.25);
    T_tmp_3 = coll_2(end,:)'*coll_2(end,:)*(dt/365.25);
    L = size(coll_t, 2); % number of options.knots
    K = N_coeff_SHA_spline;
    for n = 1:model.maxdeg_spline
        factor_t = options.damping_function(n)/((model.knots(end-model.sp_order+1)-model.knots(model.sp_order))/365.25)*(params.a/params.r_c)^(2*n+4);
        for m = 0:n
            k = k + 1;
            if n == 1 & m == 0 && params.lambda_t_g10 > 0
                factor_tmp = params.lambda_t_g10/params.lambda_t_ll;
                T_ll = [T_ll; [spalloc(L, L*(k-1), 0) factor_tmp*factor_t*T_time spalloc(L, L*(K-k), 0)]];
                T_1 = [T_1; [spalloc(L, L*(k-1), 0) factor_tmp*factor_t*T_tmp_1 spalloc(L, L*(K-k), 0)]];
                T_2 = [T_2; [spalloc(L, L*(k-1), 0) factor_tmp*factor_t*T_tmp_2 spalloc(L, L*(K-k), 0)]];
                T_3 = [T_3; [spalloc(L, L*(k-1), 0) factor_tmp*factor_t*T_tmp_3 spalloc(L, L*(K-k), 0)]];
            else
                T_ll = [T_ll; [spalloc(L, L*(k-1), 0) factor_t*T_time spalloc(L, L*(K-k), 0)]];
                T_1 = [T_1; [spalloc(L, L*(k-1), 0) factor_t*T_tmp_1 spalloc(L, L*(K-k), 0)]];
                T_2 = [T_2; [spalloc(L, L*(k-1), 0) factor_t*T_tmp_2 spalloc(L, L*(K-k), 0)]];
                T_3 = [T_3; [spalloc(L, L*(k-1), 0) factor_t*T_tmp_3 spalloc(L, L*(K-k), 0)]];
            end
            if m > 0
                k = k + 1;
                T_ll = [T_ll; [spalloc(L, L*(k-1), 0) factor_t*T_time spalloc(L, L*(K-k), 0)]];
                T_1 = [T_1; [spalloc(L, L*(k-1), 0) factor_t*T_tmp_1 spalloc(L, L*(K-k), 0)]];
                T_2 = [T_2; [spalloc(L, L*(k-1), 0) factor_t*T_tmp_2 spalloc(L, L*(K-k), 0)]];
                T_3 = [T_3; [spalloc(L, L*(k-1), 0) factor_t*T_tmp_3 spalloc(L, L*(K-k), 0)]];
            end
        end
    end
    
    % construction of damping matrix
    K = N_coeff_SHA_spline;
    
    T_ll = [[T_ll spalloc(K*L, N_para-N_coeff_spline, 0)]; ...
        spalloc(N_para-N_coeff_spline, K*L+N_para-N_coeff_spline, 0)];
    T_1 = [[T_1 spalloc(K*L, N_para-N_coeff_spline, 0)]; ...
        spalloc(N_para-N_coeff_spline, K*L+N_para-N_coeff_spline, 0)];
    T_2 = [[T_2 spalloc(K*L, N_para-N_coeff_spline, 0)]; ...
        spalloc(N_para-N_coeff_spline, K*L+N_para-N_coeff_spline, 0)];
    T_3 = [[T_3 spalloc(K*L, N_para-N_coeff_spline, 0)]; ...
        spalloc(N_para-N_coeff_spline, K*L+N_para-N_coeff_spline, 0)];
end

% start with A matrix based on degree n=10 field model : Needed for initial transformation to R. Holme's basis
if exist('options', 'var') && isfield(options, 'update') && options.update == 1
else
    data_vector.B_MF = synth_values(data_vector.r*params.a, data_vector.theta/rad, data_vector.phi/rad, m_start_MF(1:120));
    data_vector.e_B = [data_vector.B_MF(:,1)./data_vector.F, data_vector.B_MF(:,2)./data_vector.F, data_vector.B_MF(:,1)./data_vector.F];
    data_vector.e_2 = [data_vector.n(:,2).*data_vector.e_B(:,3)-data_vector.n(:,3).*data_vector.e_B(:,2), data_vector.n(:,3).*data_vector.e_B(:,1)-data_vector.n(:,1).*data_vector.e_B(:,3), data_vector.n(:,1).*data_vector.e_B(:,2)-data_vector.n(:,2).*data_vector.e_B(:,1)];
    e_tmp = sqrt(data_vector.e_2(:,1).^2 + data_vector.e_2(:,2).^2 + data_vector.e_2(:,3).^2); % normalization of data_vector.e_2
    data_vector.e_2 = [data_vector.e_2(:,1)./e_tmp data_vector.e_2(:,2)./e_tmp data_vector.e_2(:,3)./e_tmp]; % normalization of data_vector.e_2
    data_vector.e_3 = [data_vector.e_B(:,2).*data_vector.e_2(:,3)-data_vector.e_B(:,3).*data_vector.e_2(:,2), data_vector.e_B(:,3).*data_vector.e_2(:,1)-data_vector.e_B(:,1).*data_vector.e_2(:,3), data_vector.e_B(:,1).*data_vector.e_2(:,2)-data_vector.e_B(:,2).*data_vector.e_2(:,1)];
end
A =  [[spdiags(data_vector.e_B(:,1), 0, N_data_vec, N_data_vec) spdiags(data_vector.e_B(:,2), 0, ...
    N_data_vec, N_data_vec) spdiags(data_vector.e_B(:,3), 0, N_data_vec, N_data_vec)];
    [spdiags(data_vector.e_2(:,1), 0, N_data_vec, N_data_vec) spdiags(data_vector.e_2(:,2), 0, ...
    N_data_vec, N_data_vec) spdiags(data_vector.e_2(:,3), 0, N_data_vec, N_data_vec)];
    [spdiags(data_vector.e_3(:,1), 0, N_data_vec, N_data_vec) spdiags(data_vector.e_3(:,2), 0, ...
    N_data_vec, N_data_vec) spdiags(data_vector.e_3(:,3), 0, N_data_vec, N_data_vec)]];

if exist('options', 'var') && isfield(options, 'update') && options.update == 1
    model.header = [model.header, ' update of ', filename_update, ', params.lambda_t_ll=', num2str(params.lambda_t_ll, '%2.3e')];
end
disp(model.header)

%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% do the iteration %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%

disp('start iterations')
for iter = 1:options.Max_iter
    
    if iter == 1
        fac = 1e3;
    else
        fac = 1/params.w_outlier;
    end   
    
    s_2 = 0;
    
    % Process scalar data   
    disp('evaluate scalar')
    
    filename_log = 'master.log';
    
    % no-MPI:
    index_current = [1:length(data_scalar.t)];
    index_all = [];    
    
    [GtG, Gty, w, delta_d_F] = do_slave_scalar(data_scalar.t(index_current), data_scalar.r(index_current), data_scalar.theta(index_current), data_scalar.phi(index_current), data_scalar.Est(index_current), data_scalar.Ist(index_current), data_scalar.F(index_current), options, model, params, m_i, N_coeff, N_Euler, N_para);
    data_scalar.w(index_current) = w;
    data_scalar.delta_F(index_current) = delta_d_F;
    index_all = [index_all; index_current];
    
    disp([int2str(length(find(data_scalar.w(index_all) < 1/fac))) ' Scalar outliers (> ' int2str(fac) ' sigma) removed'])
    
       
    %%%%%%%%% Process vector data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
    disp('evaluate vector')
    
    % no-MPI
    index_current = [1:length(data_vector.t)];
    index_all = [];
    
    index_all = [index_all; index_current];
    [GtG_tmp, Gty_tmp, w_B23, dB, dB_B23, B_synth] = do_slave_vector(data_vector.t(index_current), data_vector.r(index_current), data_vector.theta(index_current), data_vector.phi(index_current), data_vector.Est(index_current), data_vector.Ist(index_current), options, model, params, m_i, N_coeff, data_vector.source(index_current), data_vector.R_rtp(index_current,:,:), data_vector.B(index_current, :), data_vector.e_B(index_current,:), data_vector.e_2(index_current,:), data_vector.e_3(index_current,:), data_vector.n(index_current, :), data_vector.chi(index_current), data_vector.psi(index_current), A, N_para, index_current, N_data_vec);
    
    GtG = GtG + GtG_tmp;
    Gty = Gty + Gty_tmp;
    data_vector.w_B23(index_current,:) = w_B23;
    data_vector.dB_rtp(index_current, :) = dB;
    data_vector.dB_B23(index_current,:) = dB_B23;
    data_vector.B_synth(index_current,:) = B_synth;
    clear GtG_tmp Gty_tmp w_B23 dB dB_B23 B_synth
           
    delta_d_vec = reshape(data_vector.dB_rtp, [], 1);
    disp([int2str([length(find(data_vector.w_B23(index_all, 1) < 1/fac)) length(find(data_vector.w_B23(index_all, 2) < 1/fac)) length(find(data_vector.w_B23(index_all, 3) < 1/fac))]) ' Vector outliers (> ' int2str(fac) ' sigma) removed'])
    
    % update matrix A which transforms from (r, theta, phi) coo system to
    % B23 = (B, nxB, nx(nxB)) system
    data_vector.e_B(index_all,:) = [data_vector.B_synth(index_all, 1)./data_vector.F(index_all), data_vector.B_synth(index_all, 2)./data_vector.F(index_all), data_vector.B_synth(index_all, 3)./data_vector.F(index_all)];
    data_vector.e_2(index_all,:) = [data_vector.n(index_all, 2).*data_vector.e_B(index_all,3)-data_vector.n(index_all, 3).*data_vector.e_B(index_all,2), data_vector.n(index_all, 3).*data_vector.e_B(index_all,1)-data_vector.n(index_all, 1).*data_vector.e_B(index_all,3), data_vector.n(index_all, 1).*data_vector.e_B(index_all,2)-data_vector.n(index_all, 2).*data_vector.e_B(index_all,1)];
    e_tmp = sqrt(data_vector.e_2(index_all,1).^2 + data_vector.e_2(index_all,2).^2 + data_vector.e_2(index_all,3).^2); % normalization of data_vector.e_2
    data_vector.e_2(index_all,:) = [data_vector.e_2(index_all,1)./e_tmp data_vector.e_2(index_all,2)./e_tmp data_vector.e_2(index_all,3)./e_tmp]; % normalization of data_vector.e_2
    data_vector.e_3(index_all,:) = [data_vector.e_B(index_all,2).*data_vector.e_2(index_all,3)-data_vector.e_B(index_all,3).*data_vector.e_2(index_all,2), data_vector.e_B(index_all,3).*data_vector.e_2(index_all,1)-data_vector.e_B(index_all,1).*data_vector.e_2(index_all,3), data_vector.e_B(index_all,1).*data_vector.e_2(index_all,2)-data_vector.e_B(index_all,2).*data_vector.e_2(index_all,1)];
    A =  [[spdiags(data_vector.e_B(:,1), 0, N_data_vec, N_data_vec) spdiags(data_vector.e_B(:,2), 0, ...
        N_data_vec, N_data_vec) spdiags(data_vector.e_B(:,3), 0, N_data_vec, N_data_vec)];
        [spdiags(data_vector.e_2(:,1), 0, N_data_vec, N_data_vec) spdiags(data_vector.e_2(:,2), 0, ...
        N_data_vec, N_data_vec) spdiags(data_vector.e_2(:,3), 0, N_data_vec, N_data_vec)];
        [spdiags(data_vector.e_3(:,1), 0, N_data_vec, N_data_vec) spdiags(data_vector.e_3(:,2), 0, ...
        N_data_vec, N_data_vec) spdiags(data_vector.e_3(:,3), 0, N_data_vec, N_data_vec)]];
    
    % weights in (r, theta, phi) frame
    data_vector.B_rtp = data_vector.dB_rtp + data_vector.B_synth;
    
    cos_kappa_2 = ((data_vector.B_rtp(:,1).*data_vector.n(:,1) + data_vector.B_rtp(:,2).*data_vector.n(:,2) + data_vector.B_rtp(:,3).*data_vector.n(:,3))./data_vector.F(:)).^2;
    sin_kappa_2 = 1 - cos_kappa_2;
    
    C_tilde_B = params.sigma_csc_2*ones(length(data_vector.t), 1);
    C_tilde_2 = params.sigma_csc_2 + data_vector.F(:).^2.*(data_vector.psi(:).^2.*cos_kappa_2 + data_vector.chi(:).^2.*sin_kappa_2);
    C_tilde_3 = params.sigma_csc_2 + data_vector.psi(:).^2.*data_vector.F(:).^2;
    
    var_rtp=A*[C_tilde_B; C_tilde_2; C_tilde_3];
    
    var_rtp = reshape(var_rtp, [], 3);
    
    data_vector.w_rtp(:,1) = min(1.5./abs(1./sqrt(var_rtp(:,1)).*data_vector.dB_rtp(:,1)),1);
    data_vector.w_rtp(:,2) = min(1.5./abs(1./sqrt(var_rtp(:,2)).*data_vector.dB_rtp(:,2)),1);
    data_vector.w_rtp(:,3) = min(1.5./abs(1./sqrt(var_rtp(:,3)).*data_vector.dB_rtp(:,3)),1);
    
    
    %%%%%%%% process observatory data %%%%%%%%%
    if exist(options.filename_obs_data, 'file') > 0
        disp('process observatory data')
        N_coeff_SHA_spline = model.maxdeg_spline*(model.maxdeg_spline+2);
        K = model.maxdeg_spline*(model.maxdeg_spline+2); % number of SHA coefficients per knot
        [A_r_i, A_theta_i, A_phi_i] = design_SHA(1, data_obs.theta, data_obs.phi, model.maxdeg_spline, 'int');
        
        % Calculation of the B-spline collocation matrix coll
        % since spcol has to be called with non-decreasing (i.e., ordered) time, we
        % sort data_scalar.t, call spcol, and re-arrange its output coll to obtain coll
        % corresponding to the original time series
        [t_sort, i_sort] = sort(data_obs.t);
        [tmp, i_sort_1] = sort(i_sort);
        
        %%%%%% analytical first derivative
        
        coll = zeros(length(t_sort), length(model.knots));
        if model.sp_order == 5
            sp = spap2(model.knots, model.sp_order, [model.knots(4):model.knots(end-3)]', ...
                [model.knots(4):model.knots(end-3)]');
        else
            sp = spap2(model.knots, model.sp_order, [model.knots(1):model.knots(end)]', ...
                [model.knots(1):model.knots(end)]');
        end
        for i = 1:length(sp.knots)
            sp.coefs = zeros(size(model.knots)); sp.coefs(i) = 1;
            coll(:,i) = fnval(t_sort, fnder(fn2fm(sp, 'pp'), 1));
        end
        coll = coll(i_sort_1, 1:length(model.knots)-model.sp_order)*365.25; % re-arrange into initial order of data_scalar.t
        
        L = size(coll, 2);              % number of options.knots
        N_coeff_spline = K*L;
        
        index_sort = []; for l=1:K; index_sort = [index_sort; [l:K:N_coeff_spline]']; end;
        % the following lines contain an accelerated version of
        % G = []; for l=1:L; G = [G spdiags(repmat(coll(index, l)', l, K)', 0, N_data_obs, N_data_obs)*A_r_i(index,:)]; end
        A_r = zeros(N_data_obs, N_coeff_spline); A_theta = zeros(N_data_obs, N_coeff_spline); A_phi = zeros(N_data_obs, N_coeff_spline);
        for l=1:L; tmp = spdiags(repmat(coll(:, l)', l, K)', 0, N_data_obs, N_data_obs)*A_r_i(:, 1:K);     A_r(:, (l-1)*K+1:l*K) = tmp;     end
        for l=1:L; tmp = spdiags(repmat(coll(:, l)', l, K)', 0, N_data_obs, N_data_obs)*A_theta_i(:, 1:K); A_theta(:, (l-1)*K+1:l*K) = tmp; end
        for l=1:L; tmp = spdiags(repmat(coll(:, l)', l, K)', 0, N_data_obs, N_data_obs)*A_phi_i(:, 1:K);   A_phi(:, (l-1)*K+1:l*K) = tmp;   end
        A_r = A_r(:, index_sort); A_theta = A_theta(:, index_sort); A_phi = A_phi(:, index_sort);
        
        data_obs.dB_synth(:,1)  = A_r     * m_i(1:N_coeff_spline);
        data_obs.dB_synth(:,2)  = A_theta * m_i(1:N_coeff_spline);
        data_obs.dB_synth(:,3)  = A_phi   * m_i(1:N_coeff_spline);
        
        delta_d_obs = data_obs.dB - data_obs.dB_synth;
        C_tilde_obs = [1./data_obs.C_1_11 1./data_obs.C_1_22 1./data_obs.C_1_33];
        data_obs.w = min(1.5./abs(delta_d_obs./sqrt(C_tilde_obs)),1); % Huber weights
        
        % inverse of data covariance matrix
        C_y_1 = params.w_obs_0*[[spdiags(data_obs.w(:,1).*data_obs.C_1_11, 0, N_data_obs, N_data_obs) ...
            spdiags(sqrt(data_obs.w(:,1).*data_obs.w(:,2)).*data_obs.C_1_12, 0, N_data_obs, N_data_obs) ...
            spdiags(sqrt(data_obs.w(:,1).*data_obs.w(:,3)).*data_obs.C_1_13, 0, N_data_obs, N_data_obs)]; ...
            [spdiags(sqrt(data_obs.w(:,1).*data_obs.w(:,2)).*data_obs.C_1_12, 0, N_data_obs, N_data_obs) ...
            spdiags(data_obs.w(:,2).*data_obs.C_1_22, 0, N_data_obs, N_data_obs) ...
            spdiags(sqrt(data_obs.w(:,2).*data_obs.w(:,3)).*data_obs.C_1_23, 0, N_data_obs, N_data_obs)]; ...
            [spdiags(sqrt(data_obs.w(:,1).*data_obs.w(:,3)).*data_obs.C_1_13, 0, N_data_obs, N_data_obs) ...
            spdiags(sqrt(data_obs.w(:,2).*data_obs.w(:,3)).*data_obs.C_1_23, 0, N_data_obs, N_data_obs) ...
            spdiags(data_obs.w(:,3).*data_obs.C_1_33, 0, N_data_obs, N_data_obs)]];
        
        % add to GtG and Gty (only the part describing the spline-part of the model)
        G = [A_r; A_theta; A_phi];
        GtG(1:N_coeff_spline, 1:N_coeff_spline) = GtG(1:N_coeff_spline, 1:N_coeff_spline) + G'*C_y_1*G;
        Gty(1:N_coeff_spline, 1) = Gty(1:N_coeff_spline, 1) + G'*C_y_1*delta_d_obs(:);
    end % process observatory data
    
    fprintf(2,'Iteration: %3i,    rms (nT)   F:%6.2f,   vec:%6.2f,   all:%6.2f\n', ...
        iter, rms_std(data_scalar.delta_F, data_scalar.w), rms_std(reshape(data_vector.dB_B23, [], 1), reshape(data_vector.w_B23, [], 1)), rms_std([data_scalar.delta_F; reshape(data_vector.dB_B23, [], 1)], [data_scalar.w; reshape(data_vector.w_B23, [], 1)]))
    
    %%%%%%% Now the essential solve %%%%%%%%%%%%
    if model.sp_order <= 3   % Model of crustal field only
        delta_m = (GtG + iC_prior + params.lambda_cr*T_cr)\(Gty + iC_prior*(m_prior-m_i)- params.lambda_cr*T_cr*m_i);      
    else
        if params.lambda_t_1 == 0 && params.lambda_t_2 == 0    % Without end constraints
            delta_m = (GtG + iC_prior + params.lambda_t_ll*T_ll)\(Gty + iC_prior*(m_prior-m_i)- params.lambda_t_ll*T_ll*m_i);
        else
            delta_m = (GtG + iC_prior + params.lambda_t_ll*T_ll  + params.lambda_t_1*T_1 + params.lambda_t_2*T_3)\(Gty + iC_prior*(m_prior-m_i) -params.lambda_t_ll*T_ll*m_i- params.lambda_t_1*T_1*m_i - params.lambda_t_2*T_3*m_i);
        end
    end
    
    m_i = m_i + delta_m;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    [delta_m_max, i] = max(delta_m);
    disp(['||delta_m||, max(delta_m), max(delta_m) = ', num2str(norm(delta_m)), ', ', ...
        num2str(delta_m_max), ', ', num2str(delta_m_max/m_i(i))])
    disp(['params.lambda_t_ll: ' num2str(params.lambda_t_ll, '%2.3e')])
    disp(['params.lambda_t_1: ' num2str(params.lambda_t_1, '%2.3e')])
    disp(['params.lambda_t_2: ' num2str(params.lambda_t_2, '%2.3e')])
    disp(['Integral over dddBot^2: ' num2str(m_i'*T_ll*m_i, '%2.3e')])
    if exist('T_2', 'var') == 2; disp(['Integral over  ddBot^2: ' ...
            num2str(m_i'*T_2*m_i, '%2.3e')]); end
    if exist('T_1', 'var') == 2; disp(['ddBot^2 at model start: ' ...
            num2str(m_i'*T_1*m_i, '%2.3e')]); end
    if exist('T_3', 'var') == 2; disp(['ddBot^2 at model end:   ' ...
            num2str(m_i'*T_3*m_i, '%2.3e')]); end
    
    if any(isnan(m_i)); error('model vector contains NaN'); end
    
    if iter == options.Max_iter
        fid_out = 2; % print table of statistics on stdout
    else
        fid_statistics = fopen(['statistics.out_', model.name], 'a');
        fid_out = fid_statistics;  % print table of statistics to file 'statistics.out'
    end
    coefs = reshape(m_i(1:N_coeff_spline), N_model.knots, N_coeff_SHA_spline);
    pp = spmak(model.knots', coefs');
    g = fnval((model.epoch-2000)*365.25, pp); % static coeff at options.epoch
    dg = fnval((model.epoch-2000)*365.25, fnder(pp))*365.25; % linear SV at options.epoch
    ddg = fnval((model.epoch-2000)*365.25, fnder(pp,2))*365.25^2; % quadratic SV at options.epoch
    dddg = fnval((model.epoch-2000)*365.25, fnder(pp,3))*365.25^3; % cubic SV at options.epoch
    g = [g; m_i(N_coeff_spline+1: N_coeff_spline+(N_coeff_MF-N_coeff_SHA_spline))]; % add static coefficients
    dg = [dg; m_i(N_coeff_spline+(N_coeff_MF-N_coeff_SHA_spline)+1:N_coeff_spline+(N_coeff_MF-N_coeff_SHA_spline)+(N_coeff_SHA_spline-N_coeff_SHA_spline))]; % add linear SV coefficients
    
    save_model_3([g; dg; ddg; dddg], model.maxdeg_MF, model.maxdeg_spline, model.maxdeg_spline, model.maxdeg_spline, 0, model.epoch, [model.name,'.cof'], model.header);   % Saves model only to current dir
    
    N_knots = length(model.knots) - model.sp_order; % number of knots
    N_coeff_SHA_spline = model.maxdeg_spline*(model.maxdeg_spline+2); % number of SHA coefficient per knot
    N_coeff_spline = N_coeff_SHA_spline*N_knots;
    N_coeff_MF = model.maxdeg_MF*(model.maxdeg_MF+2);
    
    coefs = reshape(m_i(1:N_coeff_spline), N_knots, N_coeff_SHA_spline);
    model.pp = fn2fm(spmak(model.knots', coefs'), 'pp');
    
    model.g = fnval((model.epoch-2000)*365.25, model.pp); % static coeff at epoch
    model.g = [model.g; m_i(N_coeff_spline+1: N_coeff_spline+(N_coeff_MF-N_coeff_SHA_spline))]; % add static coefficients
    model.g(1:model.pp.dim) = 0; % set the coefficients of the spline representation to zero
    
    % external coefficients
    N_t_segments_1 = length(model.t_break_q10)-1;
    N_t_segments_2 = length(model.t_break_qs11)-1;
    N_coeff_ext = 8+2+3 -1 + N_t_segments_1; % add parameters for q_1^0 t_break
    N_coeff_ext = N_coeff_ext-2   + 2*N_t_segments_2; % add parameters for q11 + s11
    
    m_ext = m_i(N_coeff_spline+(N_coeff_MF-N_coeff_SHA_spline)+1:N_coeff_spline+(N_coeff_MF-N_coeff_SHA_spline)+N_coeff_ext);
    model.m_q10 = m_ext(1:N_t_segments_1);
    m_q11 = m_ext(N_t_segments_1+1:N_t_segments_1+N_t_segments_2);
    m_s11 = m_ext(N_t_segments_1+N_t_segments_2+1:N_t_segments_1+2*N_t_segments_2);
    m_N2 = m_ext(N_t_segments_1+2*N_t_segments_2+1:N_t_segments_1+2*N_t_segments_2+5);
    model.m_gsm = m_ext(N_t_segments_1+2*N_t_segments_2+5+1:N_t_segments_1+2*N_t_segments_2+5+2);
    model.m_Dst = m_ext(N_t_segments_1+2*N_t_segments_2+5+2+1:N_t_segments_1+2*N_t_segments_2+5+2+3);
    
    model.t_q10 = (model.t_break_q10(2:end)+model.t_break_q10(1:end-1))/2;
    model.t_qs11 = (model.t_break_qs11(2:end)+model.t_break_qs11(1:end-1))/2;
    model.m_qs11 = [m_q11 m_s11];
    
    model.m_sm = [mean(model.m_q10) mean(m_q11) mean(m_s11) m_N2']';
    
    N_Euler_cumsum = [0 cumsum(N_Euler)];
    
    for i_source = 1:length(params.dt_Euler)
        model.alpha{i_source} = m_i(N_coeff +                         N_Euler_cumsum(i_source)+1:N_coeff +                         N_Euler_cumsum(i_source)+N_Euler(i_source));
        model.beta{i_source}  = m_i(N_coeff +   N_Euler_cumsum(end) + N_Euler_cumsum(i_source)+1:N_coeff +   N_Euler_cumsum(end) + N_Euler_cumsum(i_source)+N_Euler(i_source));
        model.gamma{i_source} = m_i(N_coeff + 2*N_Euler_cumsum(end) + N_Euler_cumsum(i_source)+1:N_coeff + 2*N_Euler_cumsum(end) + N_Euler_cumsum(i_source)+N_Euler(i_source));
    end  
    
    model_header=model.header;
    save([options.dir, sprintf('iteration_%1.1d.mat', iter)], ...
        'data_vector',  ...
        'data_scalar', ...
        'data_obs',  ...
        's_2', 'iC_prior', 'source', 'source_driver', 'options', 'params', 'model', ...
        'model_header', 'm_i', 'm_prior');
    save([options.dir_save, sprintf('/model_iteration_%1.1d.mat', iter)], 'm_i', ...
        'model_header', 's_2', ...
        'options');
        
    F_csc_synth = sqrt(data_vector.B_synth(:,1).^2 + data_vector.B_synth(:,2).^2 + data_vector.B_synth(:,3).^2);
    delta_F_csc = data_vector.F - F_csc_synth;
    
    index = find(abs(data_scalar.QD_lat) > params.lat_gm_max_statistics); % find polar scalar data
    
    fprintf(fid_out, '**************** %s, %s ******************\n',model.name, datestr(now));
    fprintf(fid_out, 'Number F at polar latitudes: =  %6d\n',length(index));
    
    index_pol = find(abs(data_scalar.QD_lat) > params.lat_gm_max_statistics);
    
    fprintf(fid_out,'F (polar) [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length(index_pol), mean_rms(data_scalar.delta_F(index_pol)), mean_rms(data_scalar.delta_F(index_pol), data_scalar.w(index_pol)), sum(data_scalar.w(index_pol))/length(index_pol));
    fprintf(fid_out,'F+B_B     [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', N_data_F+N_data_vec, mean_rms([data_scalar.delta_F; data_vector.dB_B23(:,1)]), mean_rms([data_scalar.delta_F; data_vector.dB_B23(:,1)], [data_scalar.w; data_vector.w_B23(:,1)]), sum([data_scalar.w; data_vector.w_B23(:,1)])/(N_data_F+N_data_vec));
    index = find(abs(data_scalar.QD_lat) < params.lat_gm_max_statistics); % find mid/low-latitude scalar data
    fprintf(fid_out,'F+B_B low lat :    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length(index)+N_data_vec, mean_rms([data_scalar.delta_F(index); data_vector.dB_B23(:,1)]), mean_rms([data_scalar.delta_F(index); data_vector.dB_B23(:,1)], [data_scalar.w(index); data_vector.w_B23(:,1)]), sum([data_scalar.w(index); data_vector.w_B23(:,1)])/(length(index)+N_data_vec));
    fprintf(fid_out,'B_B       [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', N_data_vec, mean_rms(data_vector.dB_B23(:,1)), mean_rms(data_vector.dB_B23(:,1), data_vector.w_B23(:,1)), sum(data_vector.w_B23(:,1))/N_data_vec);
    fprintf(fid_out,'B_nxB     [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', N_data_vec, mean_rms(data_vector.dB_B23(:,2)), mean_rms(data_vector.dB_B23(:,2), data_vector.w_B23(:,2)), sum(data_vector.w_B23(:,2))/N_data_vec);
    fprintf(fid_out,'B_Bx(nxB) [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', N_data_vec, mean_rms(data_vector.dB_B23(:,3)), mean_rms(data_vector.dB_B23(:,3), data_vector.w_B23(:,3)), sum(data_vector.w_B23(:,3))/N_data_vec);
    fprintf(fid_out,'B_r       [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', N_data_vec, mean_rms(data_vector.dB_rtp(:,1)), mean_rms(data_vector.dB_rtp(:,1), data_vector.w_rtp(:,1)), sum(data_vector.w_rtp(:,1))/N_data_vec);
    fprintf(fid_out,'B_theta   [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', N_data_vec, mean_rms(data_vector.dB_rtp(:,2)), mean_rms(data_vector.dB_rtp(:,2), data_vector.w_rtp(:,2)), sum(data_vector.w_rtp(:,2))/N_data_vec);
    fprintf(fid_out,'B_phi     [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', N_data_vec, mean_rms(data_vector.dB_rtp(:,3)), mean_rms(data_vector.dB_rtp(:,3), data_vector.w_rtp(:,3)), sum(data_vector.w_rtp(:,3))/N_data_vec);
    
    if exist(options.filename_obs_data, 'file') > 0
        w_r_obs     = data_obs.w(:,1);
        w_theta_obs = data_obs.w(:,2);
        w_phi_obs   = data_obs.w(:,3);
        fprintf(fid_out,'dBdt_r_obs     [nT]:     %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', mean_rms((data_obs.dB(:,1)-data_obs.dB_synth(:,1))), mean_rms((data_obs.dB(:,1)-data_obs.dB_synth(:,1)), w_r_obs), sum(w_r_obs)/N_data_obs);
        fprintf(fid_out,'dBdt_theta_obs [nT]:     %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', mean_rms((data_obs.dB(:,2)-data_obs.dB_synth(:,2))), mean_rms((data_obs.dB(:,2)-data_obs.dB_synth(:,2)), w_theta_obs), sum(w_theta_obs)/N_data_obs);
        fprintf(fid_out,'dBdt_phi_obs   [nT]:     %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', mean_rms((data_obs.dB(:,3)-data_obs.dB_synth(:,3))), mean_rms((data_obs.dB(:,3)-data_obs.dB_synth(:,3)), w_phi_obs), sum(w_phi_obs)/N_data_obs);
    end
    
    for i_source = 1:max([data_scalar.source; data_vector.source])
        index_F   = find(data_scalar.source == i_source);
        index_vec = find(data_vector.source == i_source);
        if ~isempty(index_F)
            fprintf(fid_out, 'Data set %2d\n', i_source);
            index_pol = find(abs(data_scalar.QD_lat) > params.lat_gm_max_statistics & data_scalar.source == i_source); % find polar scalar data
            disp(['Number F at polar latitudes: = ',int2str(length(index_pol))])
            index_low = find(abs(data_scalar.QD_lat) < params.lat_gm_max_statistics & data_scalar.source == i_source); % find mid/low-latitude scalar data
            fprintf(fid_out,'F (polar) [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length(index_pol), mean_rms(data_scalar.delta_F(index_pol)), mean_rms(data_scalar.delta_F(index_pol), data_scalar.w(index_pol)), sum(data_scalar.w(index_pol))/length(index_pol));
            fprintf(fid_out,'F+B_B     [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length([data_scalar.delta_F(index_F); data_vector.dB_B23(index_vec, 1)]), mean_rms([data_scalar.delta_F(index_F); data_vector.dB_B23(index_vec, 1)]), mean_rms([data_scalar.delta_F(index_F); data_vector.dB_B23(index_vec, 1)], [data_scalar.w(index_F); data_vector.w_B23(index_vec, 1)]), sum([data_scalar.w(index_F); data_vector.w_B23(index_vec, 1)])/(length(index_F)+length(index_vec)));
            fprintf(fid_out,'F+B_B low lat :    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length([data_scalar.delta_F(index_low); data_vector.dB_B23(index_vec, 1)]),mean_rms([data_scalar.delta_F(index_low); data_vector.dB_B23(index_vec, 1)]), mean_rms([data_scalar.delta_F(index_low); data_vector.dB_B23(index_vec, 1)], [data_scalar.w(index_low); data_vector.w_B23(index_vec, 1)]), sum([data_scalar.w(index_low); data_vector.w_B23(index_vec, 1)])/(length(index_F)+length(index_vec)));
        end
        if ~isempty(index_vec)
            fprintf(fid_out,'B_B       [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length(data_vector.dB_B23(index_vec, 1)), mean_rms(data_vector.dB_B23(index_vec, 1)), mean_rms(data_vector.dB_B23(index_vec,1), data_vector.w_B23(index_vec, 1)), sum(data_vector.w_B23(index_vec, 1))/length(index_vec));
            fprintf(fid_out,'B_nxB     [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length(index_vec), mean_rms(data_vector.dB_B23(index_vec,2)), mean_rms(data_vector.dB_B23(index_vec, 2), data_vector.w_B23(index_vec, 2)), sum(data_vector.w_B23(index_vec, 2))/length(index_vec));
            fprintf(fid_out,'B_Bx(nxB) [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length(index_vec), mean_rms(data_vector.dB_B23(index_vec,3)), mean_rms(data_vector.dB_B23(index_vec, 3), data_vector.w_B23(index_vec, 3)), sum(data_vector.w_B23(index_vec, 3))/length(index_vec));
            fprintf(fid_out,'B_r       [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length(index_vec), mean_rms(data_vector.dB_rtp(index_vec,1)), mean_rms(data_vector.dB_rtp(index_vec, 1), data_vector.w_rtp(index_vec, 1)), sum(data_vector.w_rtp(index_vec, 1))/length(index_vec));
            fprintf(fid_out,'B_theta   [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length(index_vec), mean_rms(data_vector.dB_rtp(index_vec,2)), mean_rms(data_vector.dB_rtp(index_vec, 2), data_vector.w_rtp(index_vec, 2)), sum(data_vector.w_rtp(index_vec, 2))/length(index_vec));
            fprintf(fid_out,'B_phi     [nT]:    %7d    %6.2f    %6.2f   %6.2f    %6.2f    %6.2f\n', length(index_vec), mean_rms(data_vector.dB_rtp(index_vec,3)), mean_rms(data_vector.dB_rtp(index_vec, 3), data_vector.w_rtp(index_vec, 3)), sum(data_vector.w_rtp(index_vec, 3))/length(index_vec));
            fprintf(fid_out, '\n');
        end
    end
    
    if exist('fid_statistics', 'var') == 2
        if fid_out == fid_statistics; fclose(fid_out); end
    end
    
    fprintf(2,'%8.1f sec CPU time\n', cputime-t_cpu)
    elapsed_time = etime(clock(), clock_start_time);
    fprintf(2,'%8.1f sec clock time\n', elapsed_time)
end % iteration

coefs = reshape(m_i(1:N_coeff_spline), N_model.knots, N_coeff_SHA_spline);
pp = spmak(model.knots', coefs');
g = fnval((model.epoch-2000)*365.25, pp); % static coeff at model.epoch
dg = fnval((model.epoch-2000)*365.25, fnder(pp))*365.25; % linear SV at model.epoch
ddg = fnval((model.epoch-2000)*365.25, fnder(pp,2))*365.25^2; % quadratic SV at model.epoch
dddg = fnval((model.epoch-2000)*365.25, fnder(pp,3))*365.25^3; % cubic SV at model.epoch
g = [g; m_i(N_coeff_spline+1: N_coeff_spline+(N_coeff_MF-N_coeff_SHA_spline))]; % add static coefficients
dg = [dg; m_i(N_coeff_spline+(N_coeff_MF-N_coeff_SHA_spline)+1:N_coeff_spline+(N_coeff_MF-N_coeff_SHA_spline)+(N_coeff_SHA_spline-N_coeff_SHA_spline))];

save_model_3([g; dg; ddg; dddg], model.maxdeg_MF, model.maxdeg_spline, model.maxdeg_spline,   model.maxdeg_spline, 0, model.epoch, [model.name,'.cof'], model.header);  % Save .cof file

save([options.dir_save, model.name, '.mat'], 'data_vector',   'data_scalar', 'data_obs',  's_2', 'iC_prior', 'source', 'source_driver', 'options', 'model', 'params',  'model_header', 'm_i', 'm_prior');    % Save .mat version of run

save([[options.dir_save,'model_'], model.name, '.mat'], 'm_i', ...
    'model_header', 's_2', 'options', 'model', 'params');  % save model

% save all model parameters, core field as spline tructure
filename_out_model = ['model_' model.name];

save(filename_out_model, 'options', 'model', 'params')

