clf;
clear;
% Time Controls
max_t = 1000;
numAsteroids = 800; % Number of ASTEROIDS
numPlanets = 5; % Number of LARGE PLANETS (Sun does not count)
black_hole = 1; % 1 for black hole simulation, 0 for solar system simulation

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DO NOT EDIT BELOW THIS LINE PROBABLY %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% If numPlanets > 0 for black_hole, they are just considered part of the
% planet's "n"
n = 1 + numPlanets + numAsteroids;

if ~black_hole
    % Constants
    G = 1.0;
    
    % Simulation Parameters
    positions = zeros(n,3);
    velocities = zeros(n,3);
    masses = 1.0*ones(n,1);
    
    % Display Parameters
    boundarySize = 100000.0;

    % THE SUN %
    positions(1,:) = [0 0 0];
    masses(1) = 1e+7;

    % THE LARGE PLANETS %
    positions(2:numPlanets+1,1) = linspace(40000,0.9*boundarySize,numPlanets);
    masses(2:numPlanets+1) = linspace(masses(1)*1e-4,masses(1)*1e-3,numPlanets);
    for i = 2:numPlanets+1
        velocities(i,:) = sqrt(masses(1)).*[0 1+1e-6*(rand()-0.5) 1e-6*(rand()-0.5)];
    end

    % THE ASTEROIDS %
    for i = numPlanets+2:n
            planet_index = floor(rand()*numPlanets)+2;
            planet_mass = masses(planet_index);
            planet_pos = positions(planet_index);
            asteroid_base_radius = planet_mass;
            if asteroid_base_radius > 5000
                asteroid_base_radius = 5000;
            end

            [x,y,z] = sph2cart((2*pi)*rand(),(2*pi)*rand(),asteroid_base_radius*0.1*rand()+asteroid_base_radius);

            positions(i,:) = [x+planet_pos,y,z];
            velocities(i,:) = [0 sqrt(masses(1)) 0];
            velocities(i,3) = 1-2*floor(2*rand())*sqrt(planet_mass)*sind(45)+((sqrt(planet_mass)/10)*(rand()-0.5));
            velocities(i,1) = 1-2*floor(2*rand())*sqrt(planet_mass)*sind(45)+((sqrt(planet_mass)/10)*(rand()-0.5));
    end
    space3D = plot3(positions(:,1),positions(:,2),positions(:,3),'k.');
    colormap([1.0 0.8 0.0; 1.0 1.0 0.0; 1 0.5 0; 0 1 1]);
else
    % Constants
    G = 0.001;
    
    % Simulation Parameters
    positions = zeros(n,3);
    velocities = zeros(n,3);
    masses = 0.001*ones(n,1);

    % Display Parameters
    boundarySize = 200.0;
    
    % THE BLACK HOLE %
    positions(1,:) = [0 0 0];
    masses(1) = 100000;

    % THE "PLANET" %
    for i = 2:n
            [x,y,z] = sph2cart((2*pi)*rand(),(2*pi)*rand(),10*rand());
            positions(i,:) = [x-50,y,z];
    end

    space3D = plot3(positions(:,1),positions(:,2),positions(:,3),'.');
    axis equal;
    axis(boundarySize.*[-1 1 -1 1 -1 1]);
end

% Black Background
whitebg(gcf,'black');
set(gcf,'InvertHardcopy','off');
if ~black_hole
    set(space3D,'Color',[1.0 1.0 1.0]);
else
    set(space3D,'Color',[1.0 1.0 0.0]);
end

% Fix Axis Stuff
set(gca,'XTick',[]);
set(gca,'YTick',[]);
set(gca,'ZTick',[]);
xlabel('x');
ylabel('y');
zlabel('z');

% No Distortion
axis equal;
axis vis3d;
axis(boundarySize.*[-1 1 -1 1 -1 1]);

if ~black_hole
    hold on
    k = 5;
    n_s = 2^k-1;
    c = (hadamard(2^k)+1)/2-1;
    [s_x,s_y,s_z] = sphere(n_s);
    spacePlots = zeros(1,1+numPlanets);
    for s_i = 1:1+numPlanets
        s_m = (25*log(masses(s_i)*3)^2);
        spacePlots(s_i) = surf(s_x.*s_m+positions(s_i,1),s_y.*s_m+positions(s_i,2),s_z.*s_m+positions(s_i,3),c+2*s_i);
        set(spacePlots(s_i),'LineStyle','none');
    end
    hold off
    view([0 90]);
else
    view([45 45]);
end

pause(1.0); % To show initial state

for t = 0:max_t
    for i = 1:n
        R = bsxfun(@minus,positions([1:i-1 i+1:end],:),positions(i,:));
        A = bsxfun(@rdivide,R,sum(R.^2,2));
        A = bsxfun(@times,A,G*masses([1:i-1 i+1:end]));
        A = sum(A,1);
        velocities(i,:) = velocities(i,:) + A;
    end
    positions = positions + velocities;
    
    plot_positions = bsxfun(@minus,positions,positions(1,:));
    set(space3D,'XData',plot_positions(:,1));
    set(space3D,'YData',plot_positions(:,2));
    set(space3D,'ZData',plot_positions(:,3));
    drawnow;
    
    if ~black_hole
        for s_i = 1:numPlanets+1
            s_m = (25*log(masses(s_i)*3)^2);
            set(spacePlots(s_i),'XData',s_x.*s_m+plot_positions(s_i,1));
            set(spacePlots(s_i),'YData',s_y.*s_m+plot_positions(s_i,2));
            set(spacePlots(s_i),'ZData',s_z.*s_m+plot_positions(s_i,3));
        end
    end
end