/**
 * @brief motion of a single particle in a homogenous magnetic field simulator 
 * @author Michael Roth <xrothm00@stud.fit.vutbr.cz>
 * @version 1.0.5
 * @since 0.1
*/

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>

#include "physics.h"
#include "cont_sim.h"

void calculate_gqm(Particle * p) {

    // calculate the speed of particle
    double abs_v = sqrt(pow(p->st[I_VX], 2) + pow(p->st[I_VY], 2) + pow(p->st[I_VZ], 2));

    // calculate the relativistic mass factor
    p->gamma = 1 / sqrt(1 - pow(abs_v, 2) / pow(LIGHT_SPEED, 2));

    // set the gqm constant to speed up
    p->gqm = p->charge / (p->gamma * p->m_0);

} // end calculate_gqm()

void simulation_loop(Particle p, double default_step, FILE *traj_f, FILE *log_f,  step_callback num_method) {

    // step initialization
    double step = default_step;

    while(p.time < p.end_time) {

        // print the state of the simulation
        fprintf(traj_f, 
            "%10e, %10e, %10e, %10e, %10e, %10e, %10e, %d\n", 
            p.time, p.st[I_X], p.st[I_Y], p.st[I_Z], p.st[I_VX], p.st[I_VY], p.st[I_VZ], p.id);
        
        // save state and time
        double st_old[N_INTEGRATORS];
        memcpy(st_old, p.st, sizeof st_old);
        double t_old = p.time;

        // numeric step and time advance
        num_method(p.st, p.gqm, step);
        p.time += step;

        // check if the state condition has changed
        if(state_condition(p.st, st_old)) {
            // check fi maximum precision has been reached
            if(step <= MIN_STEP) {
                // state event: log the intersects with detector plane to a separate file
                if(log_f != NULL) {
                    fprintf(log_f,
                            "%d, %10e, %10e, %10e, %10e, %10e, %10e, %10e, %10e\n", 
                            p.id, 
                            p.time, 
                            p.st[I_X], 
                            p.st[I_Y], 
                            p.st[I_Z], 
                            p.st[I_VX], 
                            p.st[I_VY], 
                            p.st[I_VZ], 
                            p.charge);
                } // end if

                // reset the step size
                step = default_step;
            } else {
                // revert the state back one step
                p.time = t_old;
                memcpy(p.st, st_old, sizeof st_old);

                // shorten the step to match the precision
                step /= 2;

                if(step < MIN_STEP) {
                    step = MIN_STEP;
                } // end if
            } // end if
        } // end if

        // adjust the last step, if needed
        if(p.time + step > p.end_time) step = p.end_time - p.time;

    } // end for
} // end simulation_loop()

int state_condition(double st[], double st_old[]) {

    // detectors are 300, 400, 500, 600, 700, 
    // 800, 900 and 1000 mm away from the target
    int detectors[N_DETECTOR_PLANES] = {300, 400, 500, 600, 700, 800, 900, 1000};
    
    // check all planes in [mm], if the particle went through
    for(int i = 0; i < N_DETECTOR_PLANES; i++) {
        if(((int) (st[I_X] * 1000) >= detectors[i])
        && ((int) (st_old[I_X] * 1000) < detectors[i])) {
            return CONDITION_TRIGGERED;        
        } else if(((int) (st[I_X] * 1000) <= detectors[i])
        && ((int) (st_old[I_X] * 1000) > detectors[i])) {
            return CONDITION_TRIGGERED;
        } // end if
    } // end for
    
    return NO_CONDITION_TRIGGERED;

} // end state_condition()

void dynamic(double st[], double in[], double gqm) {
    
    // gqm = q / (m_0 / sqrt(1 - (v^2 / c^2)))

	// calculate the position vector
    in[I_X] = st[I_VX];
    in[I_Y] = st[I_VY];
    in[I_Z] = st[I_VZ];

	// calculate the velocity vector
    in[I_VX] = (B_Z * st[I_VY] - B_Y * st[I_VZ]) * gqm;
    in[I_VY] = (B_X * st[I_VZ] - B_Z * st[I_VX]) * gqm;
    in[I_VZ] = (B_Y * st[I_VX] - B_X * st[I_VY]) * gqm;

} // end dynamic()

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

    dynamic(st, in, gqm);

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

void rk4(double st[], double gqm, double step) {
    
    // define the state vectors
    double in[N_INTEGRATORS];
    double k1[N_INTEGRATORS];
    double k2[N_INTEGRATORS];
	double k3[N_INTEGRATORS];
    double start_st[N_INTEGRATORS];

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

    dynamic(st, in, gqm); // calculate k1

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

    dynamic(st, in, gqm); // calculate k2

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

	dynamic(st, in, gqm); // calculate k3

	for(int i = 0; i < N_INTEGRATORS; i++) {
		k3[i] = in[i]; // save calculated k3

		// prepare k4
		st[i] = start_st[i] + (k3[i] * step);
	} // end for

	dynamic(st, in, gqm); // calculate k4

	for(int i = 0; i < N_INTEGRATORS; i++) {
		// calculate the result state
		// st = 1/6 * step * (k1 + 2 * (k2 + k3) + k4), k4 = in
		st[i] = start_st[i] + (step / 6) * (k1[i] + 2 * (k2[i] + k3[i]) + in[i]);
	} // end for

} // end rk4()