/*
* Copyright (c) 2025 Konrad Gębik
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program.  If not, see <http://www.gnu.org/licenses/>
*/

#include <iostream>
#include "Simulation.hpp"
#include "Vector.hpp"

Vector Simulation::getPosition() const {
    return {position.x, position.y, position.z};
}

Vector Simulation::getVelocity() const {
    return {velocity.x, velocity.y, velocity.z};
}

double Simulation::getTime() const {
    return time;
}

double Simulation::getEnergy() const {
    double m = mass1 + mass2, mprod = mass1 * mass2;
    double energy = 0;
    energy += mprod / m * getVelocity().normSq() / 2 - mprod / getPosition().norm();
    energy -= mprod * (mass1*mass1*mass1 + mass2*mass2*mass2) / (m*m*m*m) * getVelocity().normSq()*getVelocity().normSq();
    energy += m * mprod / getPosition().normSq() / 2;
    return energy;
}

Vector Simulation::PNapprox(double time, Vector pos, Vector vel) {
    double m = mass1 + mass2, eta = mass1*mass2 / (m*m);
    double r = pos.norm(), v = vel.norm();
    double rdot = pos.dotProduct(vel) / r;
    Vector value;

    value = -1*m/(r*r*r)*pos;  

    if (PNorder >= 1) {
        double APN = 3/2*eta*rdot*rdot + (4 + 2*eta)*m/r - (1 + 3*eta)*v*v;
        double BPN = 4 - 2*eta;
        if (PNorder >= 2) {
            APN += -eta*(3 - 4*eta)*v*v*v*v + eta*(13 - 4*eta)*v*v*m/r/2 + 3*eta*(3 - 4*eta)*v*v*rdot*rdot/2 + (2 + 25*eta + 2*eta*eta)*rdot*rdot*m/r 
                   -15*eta*(1 - 3*eta)*rdot*rdot*rdot*rdot/8 - 3*(12 + 29*eta)*m*m/r/r/4;
            BPN += eta*(15 + 4*eta)*v*v/2 - 3*eta*(3 + 2*eta)*rdot*rdot/2 - (4 + 41*eta + 8*eta*eta)*m/r/2;
        }
        value += (APN/r*pos + rdot*BPN*vel)*m/r/r;
    }

    return value;
}

void Simulation::simulate() {
    Vector k1vel = PNapprox(time, position, velocity);
    Vector k1pos = velocity;

    Vector k2vel = PNapprox(time + deltaTime / 2, position + deltaTime / 2 * k1pos,  velocity + deltaTime / 2 * k1vel);
    Vector k2pos = velocity + deltaTime / 2 * k1vel;

    Vector k3vel = PNapprox(time + deltaTime / 2, position + deltaTime / 2 * k2pos,  velocity + deltaTime / 2 * k2vel);
    Vector k3pos = velocity + deltaTime / 2 * k2vel;

    Vector k4vel = PNapprox(time + deltaTime, position + deltaTime * k3pos, velocity + deltaTime * k3vel);
    Vector k4pos = velocity + deltaTime * k3vel;

    position += deltaTime / 6 * (k1pos + 2 * k2pos + 2 * k3pos + k4pos);
    velocity += deltaTime / 6 * (k1vel + 2 * k2vel + 2 * k3vel + k4vel);
    time += deltaTime;
}