#include <math.h>
#include <stdio.h>

#include "vect.h"

typedef struct {
    t_vect s;
    t_vect v;
} t_state;

typedef struct {
    t_vect ds;
    t_vect dv;
} t_deriv;

t_vect acceleration(t_state *state, float t) {
    float k = 10;
    float b = 1;

    /* result */
    t_vect r;

    /* -k*state.s - b*state.v */
    r = u_vect_mul(state->s, k);
    r = u_vect_sub(r, u_vect_mul(state->v, b));

    return(r);
}

t_deriv evaluate(t_state *ref, float t) {
    /* result */
    t_deriv r = {
        ref->v,
        acceleration(ref, t)
    };

    return(r);
}

t_deriv evaluate_deriv(t_state *ref,
    float t, float dt, t_deriv *deriv) {

    t_state state = {
        /* ref.s + deriv.ds*dt */
        u_vect_add(ref->s, u_vect_mul(deriv->ds, dt)),

        /* ref.v + deriv.dv*dt */
        u_vect_add(ref->v, u_vect_mul(deriv->dv, dt))
    };

    /* result */
    t_deriv r = {
        state.s,
        acceleration(&state, t+dt)
    };

    return(r);
}

void integrate(t_state *state, float t, float dt) {
    t_deriv a = evaluate(state, t);
    t_deriv b = evaluate_deriv(state, t, 0.5*dt, &a);
    t_deriv c = evaluate_deriv(state, t, 0.5*dt, &b);
    t_deriv d = evaluate_deriv(state, t, 1.0*dt, &c);

    t_vect dsdt;

    /* dsdt = 1/6*(a.ds + 2*(b.ds + c.ds) + d.ds) */
    dsdt = u_vect_add(b.ds, c.ds);
    dsdt = u_vect_mul(dsdt, 2.0);
    dsdt = u_vect_add(dsdt, d.ds);
    dsdt = u_vect_add(dsdt, a.ds);
    dsdt = u_vect_mul(dsdt, 1.0/6.0);

    t_vect dvdt;

    /* dvdt = 1/6*(a.dv + 2*(b.dv + c.dv) + d.dv) */
    dvdt = u_vect_add(b.dv, c.dv);
    dvdt = u_vect_mul(dvdt, 2.0);
    dvdt = u_vect_add(dvdt, d.dv);
    dvdt = u_vect_add(dvdt, a.dv);
    dvdt = u_vect_mul(dvdt, 1.0/6.0);

    /* state.s = state.s + dsdt*dt */
    state->s = u_vect_add(state->s, u_vect_mul(dsdt, dt));

    /* state.v = state.v + dvdt*dt */
    state->v = u_vect_add(state->v, u_vect_mul(dvdt, dt));
}

int main(int argc, char **argv) {
    t_state cur = {
        { 0, 0, 0 },
        { 100, 0, 0 }
    };

    float t = 0;
    float dt = 1e-3;

    while(fabs(t)<1) {
        printf("%.3f, %.3f, %.3f\n",
            cur.s.x, cur.s.y, cur.s.z);
            integrate(&cur, t, dt);
            t = t+dt;
    }

    return(0);
}