% -------------------------------------------------------------------------
% [ULTRACONTROL]
% Compute the deviation from a Halo orbit due to SRP using DST semi-analytically
%
% input file: input_DST.mat, input_sail.mat
% output file: output_ULTRACONTROL.mat
%
% -------------------------------------------------------------------------
% [1] 01-Sep-2019   Masaki NAKAMIYA
% -------------------------------------------------------------------------

function main_ULTRACONTROL
clc;clear;close all;
%% --- set the DST info --- %%
load('input_DST.mat', 'Eval', 'Evec_1', 'Evec_2', 'Evec_3', 'Evec_4', 'Evec_5', 'Evec_6', 't_halo');

N_point = numel(t_halo);    % number of segment
Poincare_exp1 = log(Eval(1,1))/t_halo(end);

%% --- set the solar sail info --- %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           tool for 'input_sail.mat'                                    %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% beta = 1/10000000; % sail lightness number
% phi0 = [45,-45];    % sail angle in-plane [deg]
% ChngSegIN = 0;      % segment to change phi
% gamma0 = [-45,-45]; % sail angle out-of-plane [deg]
% ChngSegOUT = 12;    % segment to change gamma
% phi = repelem(phi0,[ChngSegIN N_point-ChngSegIN]);
% gamma = repelem(gamma0,[ChngSegOUT N_point-ChngSegOUT]);
% 
% save('input_sail.mat', 'beta', 'phi', 'gamma');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
load('input_sail.mat', 'beta', 'phi', 'gamma');

SRP0 = (.99^2)*beta;
ang_sail(1,:) = pi/180*phi;
ang_sail(2,:) = pi/180*gamma;

%% --- SRP effect & P^{-1} for each segment --- %%
for k = 1:N_point
    %%% Set the sail direction %%%
    n_sail(k,1:3)=[cos(ang_sail(1,k))*cos(ang_sail(2,k)) sin(ang_sail(1,k))*cos(ang_sail(2,k)) sin(ang_sail(2,k))];
    cosA(k) = dot(-[1 0 0]', n_sail(k,1:3)');
    
    a_SRP(k,4)=cosA(k)^2*n_sail(k,1)*SRP0;
    a_SRP(k,5)=cosA(k)^2*n_sail(k,2)*SRP0; 
    a_SRP(k,6)=cosA(k)^2*n_sail(k,3)*SRP0;
    %%% Set for the DST coefficient %%
    % a_SRP(:,4)=SRP0; a_SRP(:,5)=0; a_SRP(:,6)=0;	%%% for C^vx
    % a_SRP(:,4)=0; a_SRP(:,5)=SRP0; a_SRP(:,6)=0;  %%% for C^vy
    % a_SRP(:,4)=0; a_SRP(:,5)=0; a_SRP(:,6)=SRP0;  %%% for C^vz

    %%% compute P^{-1} %%%
    inv_P(k,:,:) = inv([Evec_1(k,:)' Evec_2(k,:)' Evec_3(k,:)' Evec_4(k,:)' Evec_5(k,:)' Evec_6(k,:)' ]);
end

%% --- Curve fitting for the coefficients of P^{-1} --- %%
f4 = fit(t_halo(1:N_point), real(inv_P(:,1,4)),'fourier4');
f5 = fit(t_halo(1:N_point), real(inv_P(:,1,5)),'fourier4');
f6 = fit(t_halo(1:N_point), real(inv_P(:,1,6)),'fourier4');

%% --- Compute the Integral deviation --- %%
syms ttt; 
dX_integral = vpaintegral(exp(-Poincare_exp1*(ttt-t_halo(N_point))) * (...
             ( a_SRP(1,4)* (f4.a0 + f4.a1*cos(ttt*f4.w) + f4.b1*sin(ttt*f4.w) + f4.a2*cos(2*ttt*f4.w) + f4.b2*sin(2*ttt*f4.w) + f4.a3*cos(3*ttt*f4.w) + f4.b3*sin(3*ttt*f4.w) + f4.a4*cos(4*ttt*f4.w) + f4.b4*sin(4*ttt*f4.w) ) ...
             + a_SRP(1,5)* (f5.a0 + f5.a1*cos(ttt*f5.w) + f5.b1*sin(ttt*f5.w) + f5.a2*cos(2*ttt*f5.w) + f5.b2*sin(2*ttt*f5.w) + f5.a3*cos(3*ttt*f5.w) + f5.b3*sin(3*ttt*f5.w) + f5.a4*cos(4*ttt*f5.w) + f5.b4*sin(4*ttt*f5.w) ) ...
             + a_SRP(1,6)* (f6.a0 + f6.a1*cos(ttt*f6.w) + f6.b1*sin(ttt*f6.w) + f6.a2*cos(2*ttt*f6.w) + f6.b2*sin(2*ttt*f6.w) + f6.a3*cos(3*ttt*f6.w) + f6.b3*sin(3*ttt*f6.w) + f6.a4*cos(4*ttt*f6.w) + f6.b4*sin(4*ttt*f6.w) ) ...
             )*1 ...
             ) , [0,t_halo(N_point)]);
fprintf('Integral Deviation = %e \n', dX_integral);

%% --- plot --- %%
%{
    figure(11); hold on; grid on; xlim([0 3.1]);
    plot(t_halo(1:N_point), inv_P(:,1,4),'.r','MarkerSize',10);
    
    fs1 = fit(t_halo(1:N_point), inv_P(:,1,4),'fourier1');
    plot(fs1,'--g');
    fs2 = fit(t_halo(1:N_point), inv_P(:,1,4),'fourier2');
    plot(fs2,'-.m');
    fs3 = fit(t_halo(1:N_point), inv_P(:,1,4),'fourier3');
    plot(fs3,'c');
    fs4 = fit(t_halo(1:N_point), inv_P(:,1,4),'fourier4');
    plot(fs4,'k');
    
    xlabel('time [non-dim]','Interpreter','latex'); 
    ylabel(['Coefficient C$^{\bar{1} x}_{(t_k)}$'],'Interpreter','latex','fontname','Times New Roman');
    legend({'Numerical','FS1','FS2','FS3','FS4'},'Location','northeast');
%}
figure(12); hold on; grid on;
plot(t_halo(1:N_point), exp(-Poincare_exp1*(t_halo(1:N_point)-t_halo(N_point))) .* ( ...
    ( a_SRP(1:N_point,4).*( f4.a0 + f4.a1*cos(t_halo(1:N_point)*f4.w) + f4.b1*sin(t_halo(1:N_point)*f4.w) + f4.a2*cos(2*t_halo(1:N_point)*f4.w) + f4.b2*sin(2*t_halo(1:N_point)*f4.w) + f4.a3*cos(3*t_halo(1:N_point)*f4.w) + f4.b3*sin(3*t_halo(1:N_point)*f4.w) + f4.a4*cos(4*t_halo(1:N_point)*f4.w) + f4.b4*sin(4*t_halo(1:N_point)*f4.w)  )...
    + a_SRP(1:N_point,5).*( f5.a0 + f5.a1*cos(t_halo(1:N_point)*f5.w) + f5.b1*sin(t_halo(1:N_point)*f5.w) + f5.a2*cos(2*t_halo(1:N_point)*f5.w) + f5.b2*sin(2*t_halo(1:N_point)*f5.w) + f5.a3*cos(3*t_halo(1:N_point)*f5.w) + f5.b3*sin(3*t_halo(1:N_point)*f5.w) + f5.a4*cos(4*t_halo(1:N_point)*f5.w) + f5.b4*sin(4*t_halo(1:N_point)*f5.w)  )...
    + a_SRP(1:N_point,6).*( f6.a0 + f6.a1*cos(t_halo(1:N_point)*f6.w) + f6.b1*sin(t_halo(1:N_point)*f6.w) + f6.a2*cos(2*t_halo(1:N_point)*f6.w) + f6.b2*sin(2*t_halo(1:N_point)*f6.w) + f6.a3*cos(3*t_halo(1:N_point)*f6.w) + f6.b3*sin(3*t_halo(1:N_point)*f6.w) + f6.a4*cos(4*t_halo(1:N_point)*f6.w) + f6.b4*sin(4*t_halo(1:N_point)*f6.w)  )...
    )...
    ) ,'r');
xlabel('time [non-dim]'); ylabel('Combined deviation [non-dim]');
%%% Set for the DST coefficient %%%
% xlabel('time [non-dim]'); ylabel('DST coefficient [non-dim]');
% legend('$$C ^ {\dot x} $$', 'interpreter', 'latex','Location','northeast'); 

%% --- save the data --- %%
save('output_ULTRACONTROL.mat', 'beta', 'phi', 'gamma', 'dX_integral');

end