clear all
rad = pi/180;
a = 6371.2;

% CCDB values of Euler angles
q_A = [-0.013237940900440 0.618884832920591 0.022645867500753 0.785043619926120]; % Swarm Alpha
q_B = [0.011655028800383 0.619619452020370 -0.014490071300476 0.784682058425797]; % Swarm Bravo
q_C = [-0.001679525100088 0.621376719032451 0.003690655100193 0.783501455840917]; % Swarm Charlie


% read 4-9 May 2021 Swarm Alpha
year = 2021; month = 5;
t = []; r = []; theta = []; phi = []; B_NEC = []; F = []; B_VFM = []; q_NEC_CRF = [];
Step = 30; % downsample data to 30 sec
for day = 4:9
    d_B = dir(['SW_OPER_MAGA_LR_1B_' num2str(year, '%04d') num2str(month, '%02d') num2str(day, '%02d') '*.cdf']);
    filename_in = [d_B(1).name];
    data = cdfread(filename_in, 'CombineRecords', 1, 'variable', {'Timestamp', 'Latitude', 'Longitude', ...
        'Radius', 'B_NEC', 'F', 'B_VFM', 'q_NEC_CRF'});
    t = [t; todatenum(data{1}(1:Step:end)) - 730486];
    r = [r; double(data{4}(1:Step:end))/1e3]; % radius in km
    theta = [theta; 90-double(data{2}(1:Step:end))];
    phi = [phi; double(data{3}(1:Step:end))];
    B_NEC = [B_NEC; double(data{5}(1:Step:end,:))];
    F = [F; double(data{6}(1:Step:end,:))];
    B_VFM = [B_VFM; double(data{7}(1:Step:end,:))];
    q_NEC_CRF = [q_NEC_CRF; double(data{8}(1:Step:end,:))];
end

%% check that CCDB Euler angles (quaternions q_A) are the ones used in L1B processing
% rotation from VFM -> CRF -> NEC frame using quaternion multiplication
B_NEC_2 = qvxform(qmult(q_A, q_NEC_CRF), B_VFM);

plot(theta, B_NEC - B_NEC_2, '.')


% alternative; rotation by means of multiple matrix multiplications
R_VFM = squeeze(qu2arot(q_A)); % rotation matrix from VFM to CRF

% rotate B from VFM to CRF
B_CRF = zeros(size(B_VFM));
for k = 1:3
    for l = 1:3
        B_CRF(:,k) = B_CRF(:,k) + R_VFM(l,k)*B_VFM(:,l);
    end
end


R_1 = qu2arot(q_NEC_CRF); % rotation matrix from CRF to NEC
% rotate B from CRF to NEC frame
B_NEC_1 = zeros(size(B_VFM));
for k = 1:3
    for l = 1:3
        B_NEC_1(:,k) = B_NEC_1(:,k) + R_1(:,l,k).*B_CRF(:,l);
    end
end

for k = 1:3
    subplot(3,1,k)
    plot(theta, B_NEC(:,k)-B_NEC_1(:,k), '.', ...
        theta, B_NEC(:,k)-B_NEC_2(:,k), 'o', ...
        theta, B_NEC_1(:,k)-B_NEC_2(:,k), '.')
    xlabel '\theta'
    ylabel '[nT]'
    if k == 1
        ylabel('\DeltaB_{North}')
    elseif k == 2
        ylabel('\DeltaB_{East}')
    elseif k == 3
        ylabel('\DeltaB_{Center}')
    end
    grid on
end % k

%% calculate CHAOS model values

% CHAOS model
load ('\nio\m\mmod\CHAOS-7.8.mat')

% read CHAOS MMA model (from http://www.spacecenter.dk/files/magnetic-models/RC/MMA/ )
filename_MMA = ['SW_OPER_MMA_CHAOS__20210101T000000_20220126T233000_0708.cdf'];
s_MMA = read_MMA(filename_MMA);
s_MMA.theta_NGP = 9.6883;
s_MMA.phi_NGP = 287.3748;


% CHAOS model field
B_mod_int = synth_values(r, theta, phi, pp, t); % core field
N = 60; B_mod_int = B_mod_int + synth_values(r, theta, phi, g(1:N*(N+2))); % add crustal field up to N = 60;
[B_magn_1, B_magn_2] = synth_values_MMA(r, theta, phi, t, s_MMA); % magnetospheric + induced field
B_mod = B_mod_int + B_magn_1 + B_magn_2; % complete model field

B_mod_NEC = [-B_mod(:,2) B_mod(:,3) -B_mod(:,1)]; % from r-theta-phi frame to NEC

% plot difference between observed vector components and CHAOS model values
for k = 1:3
    subplot(3,1,k)
    plot(theta, B_NEC(:,k)-B_mod_NEC(:,k), '.')
    xlabel 'co-latitude \theta [deg]'
    ylabel '[nT]'
    if k == 1
        ylabel('\DeltaB_{North}')
    elseif k == 2
        ylabel('\DeltaB_{East}')
    elseif k == 3
        ylabel('\DeltaB_{Center}')
    end
    grid on
end % k

%% do alignment: estimation of Euler angles

% rotate B_mod_NEC to B_mod_CRF
B_mod_CRF = qvxform(qconj(q_NEC_CRF), B_mod_NEC);

% only non-polar data
theta_d = gg2gm(theta, phi);
idx = find(abs(90-theta_d) < 60);

Max_iter = 25;
step = 1;

% E1 = SpinCalc('QtoEA321', qconj(q_A), eps, 1); % starting values (gamma, beta, alpha)

alpha = 11.8; beta = 283.8074; gamma = 347.4419; % starting values
% alpha = 0; beta = 270; gamma = 360; % convergence even with these starting values
m_i = [alpha*rad; beta*rad; gamma*rad];
N_data = 3*length(idx);
for iter = 1:Max_iter
    [R_VFM, R_a, R_b, R_c] = euler_123_SCARF(m_i(1), m_i(2), m_i(3)); % rotation matrix VFM -> CRF

    B_mod_VFM = mat_mul_mat_3(shiftdim(R_VFM', -1), B_mod_CRF(idx,:));
    dB_dalpha = mat_mul_mat_3(shiftdim(R_a', -1), B_mod_CRF(idx,:));
    dB_dbeta  = mat_mul_mat_3(shiftdim(R_b', -1), B_mod_CRF(idx,:));
    dB_dgamma = mat_mul_mat_3(shiftdim(R_c', -1), B_mod_CRF(idx,:));

    delta_d_vec = [B_mod_VFM(:,1) - B_VFM(idx,1); ...
        B_mod_VFM(:,2) - B_VFM(idx,2); ...
        B_mod_VFM(:,3) - B_VFM(idx,3)];
    G = [dB_dalpha; dB_dbeta; dB_dgamma];
    if iter > 1
        rms = sqrt(((w'.*delta_d_vec') * (w.*delta_d_vec)) / sum(w));
        w = min(1.5./abs(delta_d_vec./rms),1);  % robust weights
    else
        w = ones(size(delta_d_vec));
    end
    iC_y = spdiags(w, 0, N_data, N_data);
    wG = iC_y*G;
    delta_m = (G'*wG)\(wG'*delta_d_vec);
    m_i = m_i + step*delta_m;
    fprintf(2,'Iteration: %3i, %6.2f %6.2f %6.2f rms (nT):%10.4f\n', iter, mod(m_i/rad, 360), rms_std(delta_d_vec, w))
end
fprintf(2,'%6.2f %6.2f %6.2f %8.4f %8.4f %8.4f %6.2f\n', alpha, beta, gamma, mod(m_i/rad, 360), rms_std(delta_d_vec, w));

q_A_estimated = dcm2q(R_VFM); % convert Euler angle matrix to quaternions

