%% * Interceptor 3DOF* 
%% Executive
clear all; clc;
t0 = 0; % Initial time. Target launch time.
tCommit = XX.; % Commit time (sec)
sigEl = 0; % Missile elevation angle standard deviation
maxTime = 150; % estimated maximum engagement time in sec
dt = 0.01; % integration interval (sec)
maxNum = maxTime/dt; % maximum number of steps in integration loop
simTime(1) = t0;

%% Target initial conditions
Tp0 = [75000; 0; 0 ];
Tspeed0 = 1.e-06; 
QE = 63; LOF = -180; % Quadrant elevation and line-of-fire in degrees. LOF measured from x-axis.
Tv0 = Tspeed0*[cosd(QE)*cosd(LOF); cosd(QE)*sind(LOF); sind(QE)];%initial velocity vector in ENU frame
targetState = [Tp0; Tv0]; % initial target state vector (position & velocity) 6 states
Target_speed(1,:) = norm(targetState(4:6));
TxDot = T3dxdt(t0,targetState);
Toutput_vector(1,:)= [t0,targetState',TxDot(4:6)'];

%% Missile initial conditions
Mp0 = [0; 0; 0]; % Missile initial position in ENU frame (m)
Mspeed0 = 1.e-06;% Missile initial speed in ENU frame (m/sec); must be non-zero for unit vector calculation.
Maz = XX; Mel = XX; % Missile launch azimuth and elevation (deg).
Mv0 = Mspeed0*[cosd(Mel)*cosd(Maz); cosd(Mel)*sind(Maz); sind(Mel)];%initial velocity vector in ENU frame
missileState = [Mp0; Mv0]; % Missile state vector in ENU frame (m. and m/sec.)
Moutput_vector(1,:)= [t0,missileState', 0, 0, 0];
missileSpeed(1,:) = [t0,norm(missileState(4:6))];
Mmach(1) = 0.;

%% Trajectory Computation Loops
Tt = t0;
for ind = 2:maxNum
    % Get Target State Vector 
    [Ttime,Tx]=ode45(@T3dxdt,[Tt Tt+dt],targetState);% x and time are values of xVector at t+dt
    Tt = Ttime(end); % last element in the "time" vector
    targetState = Tx(end,:); % last row in the "x" array. state at time(end).
    TxDot = T3dxdt(Tt,targetState'); % find velocity and acceleration using dxdt; target xdot
    Toutput_vector(ind, :)= [Tt,targetState,TxDot(4:6)'];% xDot(4:6) is acceleration
    Target_speed(ind,:) = norm(targetState(4:6));
    Trange = norm(targetState(1:3));
    Mt = Tt-tCommit-dt; % Missile time. Clock starts at missile launch (tCommit).
    Mmach(ind) = Mmach(1);
    missileSpeed(ind,:) = missileSpeed(1,:);
    Moutput_vector(ind, :)= Moutput_vector(1, :);
    simTime(ind) = Tt; %Simulation time.

    % Get Missile State Vector
    if Mt >= 0
%     [Mtime,Mx]=ode45(@(t,x)M3dxdt(t,x,targetState),[Mt Mt+dt], missileState);% x and time are values of xVector at t+dt
    [Mtime,Mx]=ode45(@(Mt,x)M3dxdt(Mt,x,targetState),[Mt Mt+dt], missileState);% x and time are values of xVector at t+dt
    Mt = Mtime(end); % last element in the "time" vector
    missileState = Mx(end,:); % last row in the "x" array. state at time(end).
    MxDot = M3dxdt(Mt,missileState',targetState); % find velocity and acceleration using dxdt; missile xdot
    [acousticSpeed,rho] = getRho(missileState);
    Mspeed = norm(missileState(4:6));
    Mmach(ind) = Mspeed/acousticSpeed;
    missileSpeed(ind,:) = [Tt,norm(missileState(4:6))];
    missileAccel(ind,:) = [Mt,norm(MxDot(4:6))];
    Moutput_vector(ind, :)= [Mt,missileState,MxDot(4:6)'];% xDot(4:6) is acceleration

    % Compute Relative Data
    relOutput(ind,:) = [Tt,targetState(1:3),missileState(1:3)];
    Toutput(ind,:) = [Tt,targetState];
    relState = targetState-missileState; % Relative state vector (target-missile).
    relRange(ind) = norm(targetState(1:3)-missileState(1:3));
    vClosing(ind) = dot(relState(4:6)',relState(1:3)/norm(relState(1:3))); % Closing speed
        if relRange(ind)<10
        dt = 0.0001;
        end
   % disp(relRange(ind));
    if targetState(3)<=0 || vClosing(ind)>0
        break
    end
    end
end
missDist = getMD(vClosing(1:ind),simTime(1:ind),relRange(1:ind));
disp(missDist);
%% Plot and write results
figure(6)
clf
axis equal;
grid on;
hold on;
title('Target and Missile Trajectories')
plot(Toutput_vector(:,2),Toutput_vector(:,4),'r','linewidth',2);
plot(Moutput_vector(:,2),Moutput_vector(:,4),'b','linewidth',2);
xlabel('Downrange (m)')
ylabel('Altitude (m)')
hold on;
grid on;
figure(2)
%axis equal;
grid on;
hold on;
xlim([0  90000]);
ylim([-5000 5000]);
zlim([0 12000]);
plot3(Toutput_vector(:,2),Toutput_vector(:,3),Toutput_vector(:,4),'r','linewidth',2);
plot3(Moutput_vector(:,2),Moutput_vector(:,3),Moutput_vector(:,4),'b','linewidth',2);
hold on;
grid on;
figure(3)
clf
grid on;
plot(missileAccel(:,1),missileAccel(:,2),'k','linewidth',2);
title('Missile Acceleration Magnitude')
xlabel('Time Since Missile Launch (sec)')
ylabel('Missile Acceleration (m/sec^2)')
grid on;
figure(4)
clf
grid on;
plot(missileSpeed(:,1),missileSpeed(:,2),'k','linewidth',2);
title('Missile Speed versus Time')
xlabel('Time Since Target Launch (sec)')
ylabel('Missile Speed (m/sec)')
grid on;
figure(5)
clf
grid on;
plot(simTime(:),vClosing(:),'k','linewidth',2);
title('Closing Speed versus Time')
xlabel('Time Since Target Launch (sec)')
ylabel('Closing Speed (m/sec)')
grid on;
figure(1)
clf
grid on;
plot(simTime(:),Target_speed(:),'k','linewidth',2);
title('Target Speed versus Time')
xlabel('Time (sec)')
ylabel('Target Speed (m/sec)')
grid on;
figure(7)
clf
grid on;
plot(simTime(:),Mmach(:),'k','linewidth',2);
title('Missile Mach Number versus Time')
xlabel('Time (sec)')
ylabel('Missile Mach Number')
grid on;
writematrix(Toutput_vector,'Ttrajectory.csv');
writematrix(Moutput_vector,'Mtrajectory.csv');