%=======================================================================
% Chapter 9, code 5: particle dynamics with fixed-step velocity Verlet.
% Based on the revised Fortran companion code.
% One particle moves around a fixed central mass at the origin.
% Acceleration: a = -mu*r/|r|^3, where mu = G*M.
%
% Default circular orbit: scaled units, mu = 1, radius = 1, speed = 1.
% The period is 2*pi. The same equations describe a satellite or planet
% when appropriate units, mu, and initial conditions are supplied.
%
% To use another position-dependent force:
%   1. Edit parameters and initial data below.
%   2. Edit acceleration; disable or update the analytic comparison.
% Standard velocity Verlet assumes acceleration depends on position,
% not velocity. Velocity-dependent drag requires another treatment.
% Initial r0 and v0 are column vectors; returned R and V have one time
% per row, with horizontal and vertical components in columns 1 and 2.
%=======================================================================
clear;

%-------------------- Parameters and initial data: edit here -------------
mu = 1;                % Gravitational parameter G*M
t0 = 0;
dt = 0.01;             % Fixed step size
steps = 6283;          % About ten circular-orbit periods
r0 = [1; 0];           % Initial x and y
v0 = [0; 1];           % Initial vx and vy
check_exact = true;    % Circular orbit starting on +x only
%------------------------------------------------------------------------
if mu <= 0 || dt <= 0 || steps < 1 || steps ~= floor(steps)
    error('Use positive mu and dt, and a positive integer number of steps.');
end
% Choose the number of fixed steps; the final time follows.
t_end = t0+steps*dt;
if check_exact
    if r0(1) <= 0 || r0(2) ~= 0 || v0(1) ~= 0
        error('Disable check_exact for different initial conditions.');
    end
    circular_speed = sqrt(mu/r0(1));
    if abs(abs(v0(2))-circular_speed) > 1.0e-12*circular_speed
        error('Disable check_exact for a noncircular orbit.');
    end
end
[t,R,V] = velocity_verlet(@acceleration,t0,r0,v0,dt,steps,mu);

% Print and save the initial point and every subsequent point.
filename = 'ch09_velocity_verlet_orbit.dat';
fid = fopen(filename,'w');
if fid == -1
    error('Cannot open data file: %s',filename);
end
fprintf('Fixed-step velocity Verlet: central gravitational field\n');
fprintf('Fixed step: %.4e\n',dt);
fprintf('Final time: %.6f\n',t_end);
fprintf('%10s %18s %18s %18s %18s\n','t','x','y','vx','vy');
fprintf(fid,'# t x y vx vy\n');
for i = 1:length(t)
    fprintf('%10.6f %18.10e %18.10e %18.10e %18.10e\n', ...
            t(i),R(i,1),R(i,2),V(i,1),V(i,2));
    fprintf(fid,'%.16e %.16e %.16e %.16e %.16e\n', ...
            t(i),R(i,1),R(i,2),V(i,1),V(i,2));
end
fclose(fid);
fprintf('Steps: %d\n',steps);
if check_exact
    [rexact,vexact] = exact_solution(t_end,t0,r0,v0);
    fprintf('%10s %18s %18s %12s\n','variable','numerical','exact','abs error');
    names = {'x','y'};
    for j = 1:2
        fprintf('%10s %18.10e %18.10e %12.4e\n', ...
                names{j},R(end,j),rexact(j),abs(R(end,j)-rexact(j)));
    end
    names = {'vx','vy'};
    for j = 1:2
        fprintf('%10s %18.10e %18.10e %12.4e\n', ...
                names{j},V(end,j),vexact(j),abs(V(end,j)-vexact(j)));
    end
end
fprintf('Data file: %s\n',filename);

% Orbit: equal axis scales preserve the shape of the trajectory.
figure;
if check_exact
    % One exact revolution is enough to show the circular reference orbit.
    omega = v0(2)/r0(1);
    t_plot = linspace(t0,t0+2*pi/abs(omega),1000)';
    R_plot = exact_solution(t_plot,t0,r0,v0);
    plot(R_plot(:,1),R_plot(:,2),'k--','LineWidth',1.2);
    hold on;
    plot(R(:,1),R(:,2),'k-');
    plot(0,0,'k+','MarkerSize',10,'LineWidth',1.2);
    hold off;
    legend('Exact circle','Velocity Verlet','Central mass','Location','best');
else
    plot(R(:,1),R(:,2),'k-');
    hold on;
    plot(0,0,'k+','MarkerSize',10,'LineWidth',1.2);
    hold off;
    legend('Velocity Verlet','Central mass','Location','best');
end
xlabel('x');
ylabel('y');
title(sprintf('Gravitational orbit, fixed step = %g',dt));
axis equal;
grid on;

% Position versus time also reveals phase differences over many periods.
figure;
if check_exact
    R_exact = exact_solution(t,t0,r0,v0);
end
labels = {'x(t)','y(t)'};
for j = 1:2
    subplot(2,1,j);
    if check_exact
        plot(t,R_exact(:,j),'k--','LineWidth',1.2);
        hold on;
        plot(t,R(:,j),'k-');
        hold off;
        if j == 1
            legend('Exact','Velocity Verlet','Location','best');
        end
    else
        plot(t,R(:,j),'k-');
    end
    ylabel(labels{j});
    grid on;
    if j == 1
        title('Orbital position versus time');
    else
        xlabel('t');
    end
end

%==================== Acceleration and exact solution ===================
function a = acceleration(r,mu)
% Supply another position-dependent acceleration here if needed.
distance = sqrt(r(1)^2+r(2)^2);
if distance == 0
    error('Gravity is undefined at the origin.');
end
a = zeros(2,1);
a(1) = -mu*r(1)/distance^3;
a(2) = -mu*r(2)/distance^3;
end

function [R,V] = exact_solution(t,t0,r0,v0)
% Circular orbit starting on +x, either clockwise or counterclockwise.
% This comparison is valid only for the initial conditions checked above.
radius = r0(1);
omega = v0(2)/radius;
angle = omega*(t(:)-t0);
R = [radius*cos(angle), radius*sin(angle)];
V = [-radius*omega*sin(angle), radius*omega*cos(angle)];
end

%==================== Numerical method ==================================
function [t,R,V] = velocity_verlet(f,t0,r0,v0,dt,steps,mu)
% FIXED-step velocity Verlet for two-dimensional motion.
% Update position, evaluate new acceleration, then update velocity.
% Both position components are updated before evaluating acceleration.
% Return the initial point and every subsequent point.
t = t0+(0:steps)'*dt;  % Indexed times avoid repeated-addition drift.
R = zeros(steps+1,2);
V = zeros(steps+1,2);
r = r0(:);
v = v0(:);
a = f(r,mu);
R(1,:) = r';
V(1,:) = v';
for i = 1:steps
    r = r+dt*v+0.5*dt^2*a;
    anew = f(r,mu);
    v = v+0.5*dt*(a+anew);
    a = anew;
    R(i+1,:) = r';
    V(i+1,:) = v';
end
end