#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "integration.h"
#include "logger.h"

/*
t0 – the start value t0 of t (input)
t1 – the final value of t (input)
dt – the increment in t (input)
y0[N] – the values of the functions y_n(t) at t0 (input)
y1[N] – the values of the functions y_n(t) at t1 (output)
N – the dimension of y (input)
int f(double t, double * y, double * dy, void * params) – computes
the vector of derivatives dy[N] of y[N], given t, y[N] and params
void * params – a pointer to a struct containing any parameters needed
by f (input)
All three functions return an estimate of y(t1) in y1.
All functions return 0 upon success, or -1 when a computational error
occurs (overflow, NaN or such)
*/

int Euler(double t0, double t1, double dt, double *y0, double *y1, int N,
        f_ptr f, void *params) {
    int i;
    double dy[N], t;

    // Initialize with first values (specified)
    for( i = 0; i < N; i++ )
        y1[i] = y0[i];

    logger(t0, N, y0);

    // Estimate for each interval
    for( t = t0; t < t1; t = t + dt > t1 ? t1 : t + dt ) {
        if( f(t, y1, dy, params) ) {
            puts("Error calculating slope.");
            return -1;
        }

        for( i = 0; i < N; i++ )
            y1[i] = y1[i] + dy[i] * dt;

        // Value can be printed/logged here
        logger(t, N, y1);
    }

    return 0;
}

int RungeKutta2(double t0, double t1, double dt, double *y0, double *y1, int N,
        f_ptr f, void *params) {
    int i;
    double dy1[N], dy2[N], t;

    // Initialize with first values (specified)
    for( i = 0; i < N; i++ )
        y1[i] = y0[i];

    logger(t0, N, y0);

    // Estimate for each interval
    for( t = t0; t < t1; t = t + dt > t1 ? t1 : t + dt ) {
        if( f(t, y1, dy1, params) || f(t + dt, y1, dy2, params) ) {
            puts("Error calculating slope.");
            return -1;
        }

        for( i = 0; i < N; i++ )
            y1[i] = y1[i] + .5 * (dy1[i] + dy2[i]) * dt;

        // Value can be printed/logged here
        logger(t, N, y1);
    }

    return 0;
}

int RungeKutta4(double t0, double t1, double dt, double *y0, double *y1, int N,
        f_ptr f, void *params) {
    int i;
    double y[N], dy[N], e1[N], e2[N], e3[N], e4, t, half_dt = .5 * dt;

    // Initialize with first values (specified)
    for( i = 0; i < N; i++ )
        y1[i] = y0[i];

    logger(t0, N, y0);

    // Estimate for each interval
    for( t = t0; t < t1; t = t + dt > t1 ? t1 : t + dt ) {
        if( f(t, y1, dy, params) ) {
            puts("Error calculating first estimate.");
            return -1;
        }

        for( i = 0; i < N; i++ ) {
            e1[i] = dy[i] * dt;
            y[i] = y1[i] + .5 * e1[i];
        }

        if( f(t + half_dt, y1, dy, params) ) {
            puts("Error calculating second estimate.");
            return -1;
        }

        for( i = 0; i < N; i++ ) {
            e2[i] = dy[i] * dt;
            y[i] = y1[i] + .5 * e2[i];
        }

        if( f(t + half_dt, y1, dy, params) ) {
            puts("Error calculating third estimate.");
            return -1;
        }

        for( i = 0; i < N; i++ ) {
            e3[i] = dy[i] * dt;
            y[i] = y0[i] + e3[i];
        }

        if( f(t + dt, y1, dy, params) ) {
            puts("Error calculating fourth estimate.");
            return -1;
        }

        for( i = 0; i < N; i++ ) {
            e4 = dy[i] * dt;
            y1[i] = y1[i] + (e1[i] + 2 * (e2[i] + e3[i]) + e4) / 6;
        }

        // Value can be printed/logged here
        logger(t, N, y1);
    }

    return 0;
}
