clear all
close all
clf
global ms mp ma G nAsteroids
ms = 1e7;
mp = 1e4;
ma = 1;
G = 1;
nAsteroids = 19;


%Setting initial conditions
xs = 0;
xp = 20;
rs = [xs, 0];
rp = [xp, 0];
rcm = cm_location(rs, ms, rp, mp);
rs = rs - rcm;
rp = rp - rcm;
r = [rs', rp'];
m = [ms, mp];
rcm = cm_locationMatrix(r, m);%setting center of mass at origin of coord system
R = norm(rs - rp); %radius of planetary orbit around sun
Rnew = norm(rp);

raMatrix = zeros(2, nAsteroids);
angaMatrix = zeros(1, nAsteroids);
orbitMatrix = zeros(1, nAsteroids);%(1/0)keeps track of asteroids still in system
%random asteroids
% for i = 1:nAsteroids
%     curang = rand()*2*pi;%random angle in radians
%     angaMatrix(i) = curang;
%     %angaMatrix(i) = 45;%rand()*360;
%     if i < 2/5*nAsteroids
%         curradius = Rnew; %50% of asteroids on same orbit as planet
%     else
%         curradius = rand()*Rnew + 0.25*Rnew;
%     end
%     raMatrix(:, i) = [curradius*cos(curang); curradius*sin(curang)]; %setting initial asteroid positions
%     orbitMatrix(i) = 1; %initially all asteroids are in system
% end


T = sqrt((4*pi^2*Rnew^3)/(G*(ms+mp)));%kepler's third law for orbital period of planet
dt = T/(10^3);%1000 time steps per orbit
n = 50000;%round(T*dt/4);%number of time steps taken in loop
%working out planet's angular velocity omegap
%equating centrifugal force and gravitational force,
%G*mp*ms/R^2 = (uv^2)/R = u*(omegap^2)*R where u = mp*ms/(mp + ms)
% ==> sqrt(G(mp+ms)/R^3) = omegap
mu = (ms*mp)/(mp+ms);
omegap = sqrt((G*mp*ms)/(mu*Rnew^3));

%Searching for L1/L2
radratio = 0.9;
dr = 0.005;
for i = 1:nAsteroids
    curang = atan(rp(2)/rp(1));
    angaMatrix(i) = curang;
    cura = newAsteroidL1(radratio + (i-1)*dr, rp, omegap)
    raMatrix(:, i) = [cura(1, 1), cura(2, 1)];%setting initial asteroid positions
    orbitMatrix(i) = 1; %initially all asteroids are in system
end

magvp = omegap*norm(rp);%angular velocity times the distance from cm
magvaMatrix = zeros(1, nAsteroids);
for i = 1:nAsteroids
    curRA = raMatrix(:, i);
    magvaMatrix(i) = omegap*norm(curRA);
end
%magva = omegap*norm(ra);%angular velocity times the distance from cm
magvs = omegap*norm(rs);%angular velocity times the distance from cm

vp = [magvp*cosd(90), magvp*sind(90)];%hard coding vp perpendicular to rp
vaMatrix = zeros(2, nAsteroids);
for i = 1:nAsteroids
    curang = angaMatrix(i);
    curmag = magvaMatrix(i);
    vx = curmag*(-sin(curang));
    vy = curmag*(cos(curang));
    vaMatrix(:, i) = [vx; vy];
end
%va = [magva*cosd(135); magva*sind(135)];%hard coding va perpendicular to ra
vs = [magvs*cosd(270), magvs*sind(270)];%hard coding vs perpendicular to rs

%constructing the Y vector
Y = zeros(8+nAsteroids, 1);
Y(1, 1) = rs(1);
Y(2, 1) = rs(2);
Y(3, 1) = vs(1);
Y(4, 1) = vs(2);
Y(5, 1) = rp(1);
Y(6, 1) = rp(2);
Y(7, 1) = vp(1);
Y(8, 1) = vp(2);
for m = 1:nAsteroids
     curx = 5+4*m;
     cury = 6+4*m;
     curvx = 7+4*m;
     curvy = 8+4*m;
     Y(curx, 1) = raMatrix(1, m);
     Y(cury, 1) = raMatrix(2, m);
     Y(curvx, 1) = vaMatrix(1, m);
     Y(curvy, 1) = vaMatrix(2, m);
end

% for graphing purposes, these track x and y positions of sun, planet and
% asteroid
colors = zeros(1, 3*nAsteroids); %colors for plotting asteroid paths
for m = 1:nAsteroids
    curcol = [rand(), rand(), rand()];
    if mean(curcol) < 0.4 %recalculate for higher intensity color if necessary
        m = m - 1;
    else
        colors(1, (3*m)-2:3*m) =  curcol;
    end
end
pxs = zeros(1, n);
pys = zeros(1, n);
pxp = zeros(1, n);
pyp = zeros(1, n);
pxaMatrix = zeros(nAsteroids, n);
pyaMatrix = zeros(nAsteroids, n);
%step through time (Runge-Kutta)
pxs10 = zeros(1, 100);
pys10 = pxs10;
pxp10 = pxs10;
pyp10 = pxs10;
pxa10Matrix = zeros(nAsteroids, 100);
pya10Matrix = pxa10Matrix;
figure
plots = zeros(1, nAsteroids);%list of plots that can be deleted as necessary
hold on
for i = 1:n
    
%     if i >= 100 %& mod(i, 100) == 0
%         %clf
%         axis equal;
%         plot(pxs10, pys10);
%         plot(pxp10, pyp10);
%         for k = 1:nAsteroids
%             curx = 5+4*k;
%             cury = 6+4*k;
%             if sqrt(Y(curx)^2+Y(cury)^2) <= Rnew*2
%                 plot(pxa10Matrix(k, :), pya10Matrix(k, :));
%             end
%         end
%         drawnow;
%         pause(0.1)
%     end
    pxs(i) = Y(1);
    pys(i) = Y(2);
    pxp(i) = Y(5);
    pyp(i) = Y(6);
    cur10ind = mod(i, 1000) + 1;
    pxs10(cur10ind) = Y(1);
    pys10(cur10ind) = Y(2);
    pxp10(cur10ind) = Y(5);
    pyp10(cur10ind) = Y(6);
    
%     for j = 1:nAsteroids %filling asteroid position matrices
%         curx = 5+4*j;
%         cury = 6+4*j;
%         if sqrt(Y(curx)^2 + Y(cury)^2) > Rnew*1.5
%             orbitMatrix(j) = 0;
%         end
%         pxa10Matrix(j, cur10ind) = Y(curx);
%         pya10Matrix(j, cur10ind) = Y(cury);
%     end
%     if i >= 10 %& mod(i, 100) == 0
%         %clf
%         axis equal;
%         plot(pxs, pys);
%         plot(pxp, pyp);
%         for k = 1:nAsteroids
%             curx = 5+4*k;
%             cury = 6+4*k;
%             if orbitMatrix(k) == 1
%                 %pxaMatrix(k, i);
%                 %pyaMatrix(k, i);
%                 %plot(pxaMatrix(k, i), pyaMatrix(k, i), 'b');
%                 
%             end
%         end
%         drawnow;
%         %pause(0.001)
%     end
    
    for j = 1:nAsteroids %filling asteroid position matrices
        curx = 5+4*j;
        cury = 6+4*j;
        if sqrt(Y(curx)^2 + Y(cury)^2) > Rnew*2
            orbitMatrix(j) = 0;
        end
        pxaMatrix(j, i) = Y(curx);
        pyaMatrix(j, i) = Y(cury);
        %if mod(i, 10) == 0 & orbitMatrix(j) == 1
            %axis([-16, 23, -13, 17]);
            %axis equal
            %plots(j) = plot(pxaMatrix(j, 1:i), pyaMatrix(j, 1:i), 'Color', [colors(3*j), colors(3*j-1), colors(3*j-2)]);
            %plot(pxs, pys)
            %plot(pxp, pyp)
            %drawnow
        %end
    end
    F1 = kepler_ode_manyBodies(i*dt, Y);
    F2 = kepler_ode_manyBodies(i*dt + dt/2, Y+ dt*F1/2);
    F3 = kepler_ode_manyBodies(i*dt + dt/2, Y + dt*F2/2);
    F4 = kepler_ode_manyBodies(i*dt + dt, Y + dt*F3);
    Y = Y + dt*(F1+2*F2+2*F3+F4)/6;
end

%Plotting
plotlen = 100;
plotfreq = 10;
for i = 1:n
    %plotmod = mod(i, plotlen);
    if i>plotlen & mod(i, plotfreq) == 0
        sunplot = plot(pxs(1, i-plotlen: i), pys(1, i-plotlen: i), 'y');
        planetplot = plot(pxp(1, i-plotlen:i), pyp(1, i-plotlen:i), 'w');
        scatter(rcm(1), rcm(2), 'y', 'filled')
        for j = 1:nAsteroids
            axis(1.5*[-25.4687   25.3489  -20.0407   20.0396]);%determined empirically for planet orbit
            plots(j) = plot(pxaMatrix(j, i - plotlen: i), pyaMatrix(j, i-plotlen:i),'Color', [colors(3*j), colors(3*j-1), colors(3*j-2)]);
            drawnow
        end
        fig = gcf;
        fig.InvertHardcopy = 'off';
        saveas(gcf, ['L1_', num2str(i/(plotfreq), '%03d'), '.png']);
        for k = 1:nAsteroids
            curplot = plots(k);
            delete(curplot);
        end
        delete(sunplot);
        delete(planetplot);
    end
end
