#include <iostream>
#include <cmath>
#include <vector>

using namespace std;

#include "const.h"
#include "1sample.h"

// Setup the earth orbit (1 au, 1 year period) for the state which we start with
void initialize(state &start) {
    start.t = 0;
    start.m = mearth / m_unit;
    start.x = vec(au / l_unit, 0, 0);
    start.v = vec(0, 2.0 * M_PI * au / (sec_per_year * v_unit), 0);
    // Eccentric orbit
    // start.v = vec(0,0.8 * 2.0 * M_PI * au / (sec_per_year * v_unit));
}

/* Calculate total energy at a given state. This is a function which gets a state as an argument 
   and returns a double (e.g. the total energy of the state) */
double calculateTotalEnergy(state &here) {
    double mass = msun / m_unit;                   /* Mass of sun in internal units
                                                    (will be 1 in the chosen units) */
    double pot = here.m * mass * G / here.x.abs(); /* Potential energy */
    double kin = 0.5 * here.m * here.v.abs2();     /* Kinetic energy */

    return (kin - pot);                               /* Here we return the total energy */
}

void calculateAcceleration(state &here) {
    double r = here.x.abs();
    double sol_mass = msun / m_unit;  //Mass of sun in internal units (will be 1 in the chosen units)
    here.a = here.x * (-G * sol_mass / r / r / r);
}

void updateVelocity(state &here) {
    here.v += here.a * DT_FACTOR;
}

void updatePosition(state &here) {
    here.x += here.v * DT_FACTOR;
}


int main(int argc, const char **argv) {
    state current;

    // Lets output at the beginning some useful information ...
    cout << "# L = " << l_unit << endl;
    cout << "# T = " << t_unit << endl;
    cout << "# M = " << m_unit << endl;
    cout << "# V = " << v_unit << endl;
    cout << "# G = " << G << endl;

    // Lets initialize the system. We defined this procedure before
    initialize(current);

    // This is loop which is done until integration of t reaches tmax
    const double torbit = sec_per_year / t_unit;
    const double tmax = NOrbits * torbit;
    while (current.t < tmax) {
        // Lets output time , position and total energy of the current time
        cout << current.t << " " << current.x << " " << current.v << " " << calculateTotalEnergy(current) << endl;

        // Now we calculate the acceleration of the current state
        calculateAcceleration(current);

        /* We first update the position of the current state.
       Remember, here the velocity is used, so if we would first
       update the velocity this would be different ! */
        updatePosition(current);

        // Now we update the velocity of the current state
        updateVelocity(current);

        // Finally we update the time t
        current.t += DT_FACTOR;
    }

    return 0;
}