clear all; close all;


    % ===================
    %     Initialize
    % ===================

    % Set configuration parameters
    nPart = 50;         % Number of particles
    density = 0.85;     % Density of particles
    mass = 1;           % Particles' mass
    nDim = 3;           % The dimensionality of the system (3D in our case)

    % Set simulation parameters
    dt = 1e-4;          % Integration time
    dt2 = dt*dt;        % Integration time, squared
    Temp = 2.0;         % Simulation temperature
    nu = 0.1;           % Thermostat parameter - frequency of collisions with the heat bath
    velSTD = sqrt(Temp/mass); % Thermostat parameter - standard deviation of the velocity

    nSteps = 5000;      % Total simulation time (in integration steps)
    sampleFreq = 10;    % Sampling frequency to draw histogram
    sampleCounter = 0;  % Sampling counter
    printFreq = 100;    % Printing frequency

    % Set initial configuration
    [coords, L] = initCubicGrid(nPart,density);

    % Set initial velocities with random numbers
    vels = rand(nDim,nPart)-0.5;

    % Set initial momentum to zero
    totV = sum(vels,2)/nPart; % Center of mass velocity
    vels = vels - repmat(totV,[1,nPart]); % Fix the zero initial momentum

    % Set initial kinetic energy to nDim*KbT/2
    TempOld = sum(dot(vels,vels))/(nDim*nPart); % kinetic temperature
    velScale = sqrt(Temp/TempOld);    % Velocity scaling factor
    vels = vels*velScale;

    % Calculate initial forces
    forces = LJ_Force(coords,L);

    % Start up an empty histogram to follow the velocity of the particles
    h.count = 0;
    h.range = [-10 10];
    h.increment = 0.1;
    h.outFreq = 1000; % how frequently the data is saved
    h.saveFileName = 'LJvel_hist.dat';


    % ===================
    % Molecular Dynamics
    % ===================

    time = 0; % Following simulation time

    for step = 1:nSteps

        % === First integration step ===

        % Update positions - All coordinates are updated at once
        coords = coords + dt*vels + 0.5*dt2*forces;

        % Apply periodic boundary conditions
        coords = mod(coords,L);

        % Update velocities - All velocities are updated at once
        vels = vels + 0.5*dt*forces;

        % Calculate new forces
        forces = LJ_Force(coords,L);

        % === Second integration step ===

        % Update velocities - All velocities are updated at once
        vels = vels + 0.5*dt*forces;

        % Implement the Andersen thermostat
        randCollision = rand(1,nPart);
        partCollision = (randCollision<nu*dt);
        tempVar = 0 + velSTD*randn(nDim,nPart);
        vels(:,partCollision) = tempVar(:,partCollision);

        % === Move time forward ===
        time = time + dt;

        % === Sample ===
        if mod(step,sampleFreq) == 0
            sampleCounter = sampleCounter + 1;

            for part=1:nPart
                % Sample the velocity of the particles (only x component)
                h = histogram(h,vels(1,part));
            end

        end

        if mod(step,printFreq) == 0
            display(step); % Print the step
        end

    end


    % ===================
    % Simulation results
    % ===================

    % Plot the resulting velocity histogram
    h.histo = h.histo/(h.count*h.increment); % Normalize
    bar(h.values,h.histo);