/**
 * model.c
 * @brief particle motion simulator
 * @author Michael Roth <xrothm00@stud.fit.vutbr.cz>
 * @version 2.0
 * @since 0.1
*/

// include required headers
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>

#include "physics.h"
#include "model.h"

int main() {
    // init the starting parameters
    double x0, y0, z0, tx0, ty0;
    x0 = y0 = z0 = ty0 = 0;
    tx0 = 0;

    // calculate the starting t_r
    double tr0 = sqrt(pow(tx0, 2) + pow(ty0, 2) + 1);

    // init the homogenous magnetic field
    double Bx0, By0, Bz0; 
    Bx0 = Bz0 = 0; // [T]
    By0 = 1; // [T]

    // properties of the simulated particle
    double v0 = LIGHT_SPEED * 0.01;
    double m0 = ELECTRON_M0; // [kg]
    
    // charge and momentum of simulated particle
    double q = -1 * ELEMENTARY_CHARGE; // [C]
    double p = lorenz_factor(v0) * m0; // [kg*m/s]

    // calculate the q/p value for an electron entering the detector parallel to the z axis with speed of 0.9c
    double qp0 = q / p; // q / (1 / p) [C*kg*m/s]

    // init the state vector
    double st[N_INTEGRATORS] = {x0, y0, tx0, ty0, qp0, tr0, Bx0, By0, Bz0};

    // define the default step and terminating z-coordinate
    double step = DEFAULT_STEP;
    double z_max = 1; // [m]

    // setup the output .csv file 
    FILE *f = fopen("sim_Zmodel_electron_5e-1c_c.csv", "w");
    if(f == NULL) return -1;

    fprintf(f, "x,y,z,t_x,t_y,q/p,t_r,B_x,B_y,B_z\n");

    // run the simulation
    simulation_loop(st, step, z0, z_max, f, rk4);

    // close the output file
    fclose(f);
} // end function main()

void simulation_loop(double st[], double default_step, double z0, double zmax, FILE *traj_f, step_callback num_method) {

    // step initialization
    double step = default_step;

    // initialize the z variable
    double z = z0;

    while(z < zmax) {

        // log the state of the simulation
        fprintf(traj_f,
            "%10e,%10e,%10e,%10e,%10e,%10e,%10e,%10e,%10e,%10e\n",
            st[I_X], st[I_Y], z, st[I_TX], st[I_TY], st[I_QP], st[I_TR], st[I_BX], st[I_BY], st[I_BZ]);
        
        // numeric step and time advance
        num_method(st, step);
        z += step;

        if(z + step > zmax) step = zmax - z;
    } // end while
} // end function simulation_loop()

void dynamic(double st[], double in[]) {
    
    // derivatives of the position
    in[I_X] = st[I_TX]; // dx/dz = tx 
    in[I_Y] = st[I_TY]; // dy/dz = ty

    // derivative of t_r // TODO see if this is correct
    // in[I_TR] = sqrt(pow(st[I_TX], 2) + pow(st[I_TY], 2) + 1);
    in[I_TR] = 0;

    double tr = sqrt(pow(st[I_TX], 2) + pow(st[I_TY], 2) + 1);
    st[I_TR] = tr;

    // derivatives of the trajectory angles
    in[I_TX] = LIGHT_SPEED * st[I_QP] * tr * (     st[I_TY] * (st[I_BZ] + st[I_TX] * st[I_BX]) - (1 + pow(st[I_TX], 2)) * st[I_BY]);
    in[I_TY] = LIGHT_SPEED * st[I_QP] * tr * (-1 * st[I_TX] * (st[I_BZ] + st[I_TY] * st[I_BY]) + (1 + pow(st[I_TY], 2)) * st[I_BX]); 

    // these integrators evaluate to 0, as the values are currently set to be constant 
    in[I_QP] = 0;
    in[I_BX] = 0;
    in[I_BY] = 0;
    in[I_BZ] = 0;
} // end function dynamic()

void euler(double st[], double step) {
    // define the integrator vector
    double in[N_INTEGRATORS];

    // calculate the integrators
    dynamic(st, in);

    for(int i = 0; i < N_INTEGRATORS; i++) {
        st[i] = st[i] + step * in[i];
    } // end for
} // end function euler()

void rk4(double st[], double step) {
    // define the integrator vectors
    double in[N_INTEGRATORS];
    double start_st[N_INTEGRATORS];

    // define the coeficient vectors
    double k1[N_INTEGRATORS];
    double k2[N_INTEGRATORS];
    double k3[N_INTEGRATORS];
    double k4[N_INTEGRATORS];

    for(int i = 0; i < N_INTEGRATORS; i++) {
        start_st[i] = st[i]; // save the starting state
    } // end for

    // calculate the first integration
    dynamic(st, in);

    for(int i = 0; i < N_INTEGRATORS; i++) {
        k1[i] = st[i]; // save the first integration result
        st[i] = start_st[i] + k1[i] * 0.5 * step; // prepare the k2
    } // end for

    // calculate the second integration
    dynamic(st, in);

    for(int i = 0; i < N_INTEGRATORS; i++) {
        k2[i] = in[i]; // save the k2 result
        st[i] = start_st[i] + k2[i] * 0.5 * step; // prepare the k3
    } // end for

    // calculate the third integration
    dynamic(st, in);

    for(int i = 0; i < N_INTEGRATORS; i++) {
        k3[i] = in[i]; // save the k3 result
        st[i] = start_st[i] + k3[i] * step; // prepare the k4
    } // end for

    // calculate the last integration
    dynamic(st, in);

    for(int i = 0; i < N_INTEGRATORS; i++) {
        k4[i] = in[i];
        st[i] = start_st[i] + (k1[i] + 2.0 * (k2[i] + k3[i]) + k4[i]) * step / 6.0;
    } // end for
} // end function rk4()

double lorenz_factor(double v) {
    return 1.0 / (1.0 - pow(v, 2) / pow(LIGHT_SPEED, 2));
} // end function lorenz_factor()