% This is a sample code for the CS3414 project I step 3.

%   Students should fill in the launch function to generate the two angles.
%   Here we require that the missile should be launched at t_limit = 0.5
%   second. In the radar system, only information before t_limit is given.
%   Your code should give angles for launching the missile.
%
%   For more detailed description, please refer to the project assignment
%   document.

function [flag, angle1, angle2, t_interception] = project4_sample(plotflag)

g = [0; -9.8; 0]; %The gravity is -9.8 in the y direction
t_launch = 0.6; %the time time when your missile fires

radarpoints = radar();
v0_missile = 4000;
a0_missile = 2000;

tic;
%for i = 1:100
    [successflag, angle1, angle2] = launchmissile(v0_missile, a0_missile, radarpoints);
%end
toc;

if successflag == 0
    fprintf('Control system decided not to launch the missile since it cannot intercept!\n');
else

    t = t_launch; % Initiate the launch time
    w = [cos(angle1)*cos(angle2) ; sin(angle1); cos(angle1)*sin(angle2)]; %set the launch angle
    v0 = v0_missile*w; % set the initial speed of the missile
    a0 = a0_missile*w; % set the acceleration of the missile

    x_missile = [0; 0; 0]; % set initial location of missile

    x = plane_position(t_launch); % set attacking missile's location when your missile starts

    h = 1e-5; % set time increasement. This value may causes error in the judgement. So we set it to be small.
    if (plotflag == 1)
      figure;
      axis([0 10000 0 10000 -1000 1000]);
      plotcount = 1;
      plotdata = zeros(4, 100);
      plotdata(1:3, 1) = x;
      plotdata(4:6, 1) = x_missile;

      plot3(plotdata(1, plotcount), plotdata(2, plotcount), plotdata(3, plotcount), 'r');
      hold on
      plot3(plotdata(4, plotcount), plotdata(5, plotcount), plotdata(6, plotcount), 'b');
      M(1) = getframe;
    end

    while x(1) > 0 & x(2) > 0 & (norm(x-x_missile) > 1 )
        t = t + h;
        launchtime = t - t_launch;
        x_missile = v0 *launchtime + 0.5*launchtime^2*(a0 + g); % missile's location
        x =  plane_position(t);  % attacking missile's location

        if (plotflag ==1) && (t > plotcount*0.1)
            plotcount = plotcount + 1;
            plotdata(1:3, plotcount) = x;
            plotdata(4:6, plotcount) = x_missile;
            plot3(plotdata(1, 1:plotcount), plotdata(2, 1:plotcount), plotdata(3, 1:plotcount), 'r-');
            hold on;
            plot3(plotdata(4, 1:plotcount), plotdata(5, 1:plotcount), plotdata(6, 1:plotcount), 'b-');
            hold on;
            plot3(plotdata(1, plotcount), plotdata(2, plotcount), plotdata(3, plotcount), 'r*');
            hold on;
            plot3(plotdata(4, plotcount), plotdata(5, plotcount), plotdata(6, plotcount), 'b*');
            M(plotcount) = getframe;
        end
    end
    if (plotflag == 1)
        plotcount = plotcount + 1;
            plotdata(1:3, plotcount) = x;
            plotdata(4:6, plotcount) = x_missile;
            plot3(plotdata(1, 1:plotcount), plotdata(2, 1:plotcount), plotdata(3, 1:plotcount), 'r-');
            hold on;
            plot3(plotdata(4, 1:plotcount), plotdata(5, 1:plotcount), plotdata(6, 1:plotcount), 'b-');
            hold on;
            plot3(plotdata(1, plotcount), plotdata(2, plotcount), plotdata(3, plotcount), 'r*');
            hold on;
            plot3(plotdata(4, plotcount), plotdata(5, plotcount), plotdata(6, plotcount), 'b*');
            M(plotcount) = getframe;
    end
    if x(1) > 0 & x(2) > 0
        flag = 1;
        t_interception = t;
        fprintf('Intercepted! at time %f\n', t);
    else
        flag = 0;
        t_interception = NaN;
        fprintf('Missile was launched but did not intercept. \n');
    end

end
end

function x = plane_position(t)

c = [10000 -500 -100 -20 -10 -1; 10000 50 -10 2 -2 -1; 500 10 -1 0 0 0];
x = c(:, 6);
for i = 5:-1:1
  x = x*t + c(:, i);
end

end

function [x] = radar()

t = 0:0.1:0.5;
x = zeros(6, 3);

for i = 1:6
  x(i, :) = plane_position(t(i));
end

end


function [location velocity] = missile_estimation(t, radarpoints)

t_array = 0:0.1:0.5;
t_array = t_array';
x = radarpoints;

for i=2:1:6
    for j=6:-1:i
        x(j,:) = (x(j,:)-x(j-1,:))./(t_array(i+j-1)-t_array(i));
    end
end

location = x(6,:);
velocity = x(6,:);
for k=5:-1:1
    location(1,:) = x(k,:) + location(1,:).*(t-t_array(k));
    velocity(1,:) = velocity(1,:).*(t-t_array(k-1))+location(1,:);
end
end

function [x] = partialPivot(A, b)
b=b.*-1;
n=3;
for k = 1:n-1
    [col_max index] = max(abs(A(k:n,k)));
    index = index + k-1;
    if index ~= k
        tempA = A(k,k:n);
        A(k,k:n) = A(index,k:n);
        A(index,k:n) = tempA;
        tempb = b(k);
        b(k) = b(index);
        b(index) = tempb;
    end
    A(k+1:n,k) = A(k+1:n,k)/A(k,k);
    for i = k+1:n
        A(i,k+1:n) = A(i,k+1:n) - A(i,k)*A(k,k+1:n);
    end
    b(k+1:n) = b(k+1:n) - A(k+1:n,k)*b(k);
end

x = zeros(n,1);
x(n) = b(n)/A(n,n);
for i = n-1:-1:1
    x(i)=(b(i)-A(i,i+1:n)*x(i+1:n))/A(i,i);
end

end

function [successflag, angle1, angle2] = launchmissile(v0_missile, a0_missile, radarpoints)

successflag = 1;

g = [0; -9.8; 0];
w = pi/4;
v = 0;
t_launch = 0.6;


angle1 = 0.95085;
angle2 = 0.07355;

end