/** @file PenningTrap.cpp
 *
 *  @author Cory Alexander Balaton (coryab)
 *  @author Janita Ovidie Sandtrøen Willumsen (janitaws)
 *
 *  @version 0.1
 *
 *  @brief The implementation of the PenningTrap class.
 *
 *  @bug No known bugs
 * */

#include "PenningTrap.hpp"
#include "constants.hpp"
#include "typedefs.hpp"
#include "utils.hpp"

PenningTrap::PenningTrap(double B_0, std::function<double(double)> V_0,
                         double d, double t)
{
    this->B_0 = B_0;
    this->V_0 = V_0;
    this->d = d;
    this->t = t;
}

PenningTrap::PenningTrap(unsigned int i, double B_0,
                         std::function<double(double)> V_0, double d, double t)
    : PenningTrap::PenningTrap(B_0, V_0, d)
{
    vec_3d r, v;
    for (size_t j = 0; j < i; j++) {
        r = vec_3d().randn() * .1 * this->d;
        v = vec_3d().randn() * .1 * this->d;
        this->add_particle(Particle(1., 40., r, v));
    }
}

PenningTrap::PenningTrap(std::vector<Particle> particles, double B_0,
                         std::function<double(double)> V_0, double d, double t)
    : PenningTrap::PenningTrap(B_0, V_0, d)
{
    this->particles = particles;
}

vec_3d PenningTrap::v_func(unsigned int i, unsigned int j, double dt)
{
    switch (i) {
    case 0:
        return .5 * dt * this->k_v[0][j];
    case 1:
        return .5 * dt * this->k_v[1][j];
    case 2:
        return dt * this->k_v[2][j];
    case 3:
        return (dt / 6.) * (this->k_v[0][j].eval() + this->k_v[1][j].eval() +
                            this->k_v[2][j].eval() + this->k_v[3][j].eval());
    default:
        std::cout << "Not valid!" << std::endl;
        abort();
    }
}

vec_3d PenningTrap::r_func(unsigned int i, unsigned int j, double dt)
{
    switch (i) {
    case 0:
        return .5 * dt * this->k_r[0][j];
    case 1:
        return .5 * dt * this->k_r[1][j];
    case 2:
        return dt * this->k_r[2][j];
    case 3:
        return (dt / 6.) * (this->k_r[0][j].eval() + this->k_r[1][j].eval() +
                            this->k_r[2][j].eval() + this->k_r[3][j].eval());
    default:
        std::cout << "Not valid!" << std::endl;
        abort();
    }
}

void PenningTrap::add_particle(Particle particle)
{
    this->particles.push_back(particle);
}

vec_3d PenningTrap::external_E_field(vec_3d r)
{
    r(2) *= -2.;
    double f = this->V_0(this->t) / (this->d * this->d);

    return f * r;
}

vec_3d PenningTrap::external_B_field(vec_3d r)
{
    return vec_3d{0., 0., this->B_0};
}

vec_3d PenningTrap::force_on_particle(unsigned int i, unsigned int j)
{
    Particle p_j = this->particles[j];
    // Calculate the difference between the particles' position
    vec_3d res = this->particles[i].r_vec - p_j.r_vec;

    // Get the distance between the particles
    double norm = arma::norm(res, 2);

    // Multiply res with p_j's charge divided by the norm cubed

    return vec_3d(res * p_j.q / (norm * norm * norm));
}

vec_3d PenningTrap::total_force_external(unsigned int i)
{
    Particle p = this->particles[i];

    if (arma::norm(p.r_vec) > this->d) {
        return vec_3d{0., 0., 0.};
    }

    vec_3d force =
        p.q * (this->external_E_field(p.r_vec) +
               arma::cross(p.v_vec, this->external_B_field(p.r_vec)));

    return force;
}

vec_3d PenningTrap::total_force_particles(unsigned int i)
{
    Particle p = this->particles[i];

    vec_3d res;

    for (size_t j = 0; j < this->particles.size(); j++) {
        if (i == j) {
            continue;
        }

        res += this->force_on_particle(i, j);
    }

    return vec_3d(res * K_E * (p.q / p.m));
}

vec_3d PenningTrap::total_force(unsigned int i)
{
    return this->total_force_external(i) - this->total_force_particles(i);
}

void PenningTrap::evolve_RK4(double dt, bool particle_interaction)
{

    std::vector<Particle> original_particles = this->particles;
    std::vector<Particle> tmp_particles = this->particles;

    vec_3d (PenningTrap::*force)(unsigned int);
    if (particle_interaction) {
        force = &PenningTrap::total_force;
    }
    else {
        force = &PenningTrap::total_force_external;
    }

    size_t size = this->particles.size();

    this->k_v = sim_arr(4, sim_cols(size));
    this->k_r = sim_arr(4, sim_cols(size));

    for (size_t i = 0; i < 4; i++) {
#pragma omp parallel for
        for (size_t j = 0; j < this->particles.size(); j++) {
            this->k_v[i][j] = (this->*force)(j) / this->particles[j].m;
            this->k_r[i][j] = this->particles[j].v_vec;

            Particle *p = &tmp_particles[j];

            p->v_vec = original_particles[j].v_vec + this->v_func(i, j, dt);
            p->r_vec = original_particles[j].r_vec + this->r_func(i, j, dt);
        }
        this->particles = tmp_particles;
    }
    this->t += dt;
}

void PenningTrap::evolve_forward_euler(double dt, bool particle_interaction)
{
    std::vector<Particle> new_state = this->particles;

    Particle *p;

    vec_3d (PenningTrap::*force)(unsigned int);
    if (particle_interaction) {
        force = &PenningTrap::total_force;
    }
    else {
        force = &PenningTrap::total_force_external;
    }

#pragma omp parallel for private(p)
    for (size_t i = 0; i < this->particles.size(); i++) {
        p = &new_state[i];
        p->v_vec += dt * (this->*force)(i) / p->m;
        p->r_vec += dt * this->particles[i].v_vec;
    }

    this->particles = new_state;
    this->t += dt;
}

sim_arr PenningTrap::simulate(double time, unsigned int steps,
                              std::string method, bool particle_interaction)
{
    double dt = time / (double)steps;

    sim_arr res(this->particles.size(), sim_cols(steps));

    void (PenningTrap::*func)(double, bool);
    if (method == "rk4") {
        func = &PenningTrap::evolve_RK4;
    }
    else if (method == "euler") {
        func = &PenningTrap::evolve_forward_euler;
    }
    else {
        std::cout << "Not a valid method!" << std::endl;
        abort();
    }

    for (size_t j = 0; j < steps; j++) {
        for (size_t i = 0; i < this->particles.size(); i++) {
            res[i][j] = this->particles[i].r_vec;
        }
        (this->*func)(dt, particle_interaction);
    }

    return res;
}

void PenningTrap::write_simulation_to_dir(std::string path, double time,
                                          unsigned int steps, std::string method,
                                          bool particle_interaction)
{
    if (path.back() != '/') {
        path += '/';
    }
    if (mkpath(path, 0777) != 0) {
        std::cout << "Hello" << std::endl;
        return;
    }

    sim_arr res = this->simulate(time, steps, method, particle_interaction);

    std::ofstream ofile;

#pragma omp parallel for private(ofile)
    for (size_t i = 0; i < this->particles.size(); i++) {
        ofile.open(path + "particle_" + std::to_string(i) + ".txt");
        for (vec_3d &vec : res[i]) {
            ofile << vec(0) << "," << vec(1) << "," << vec(2) << "\n";
        }
        ofile.close();
    }
}

double PenningTrap::fraction_of_particles_left(double time, unsigned int steps, std::string method, bool particle_interaction)
{
    sim_arr res = this->simulate(time, steps, method, particle_interaction);

    int particles_left = 0;

    for (Particle p : this->particles) {
        if (arma::norm(p.r_vec) < this->d) {
            particles_left++;
        }
    }

    return (double) particles_left / (double) this->particles.size();
}