#include <math.h>
#include <stdlib.h>
#include <assert.h>

#include "physics.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif

// Pretty efficient, no need for sine table!!
#define SIN(dir)(sinf(dir))
#define COS(dir)(cosf(dir))

// ==Update method.========================================
// d^2 x(t) / d t^2 = a, a = constant (acceleration)
// x'(0) = v, x(0) = p
//
// d x(t) / d t = a*t + v, v = constant (initial velocity)
// x(t) = a/2*t + v*t + p, p = constant (initial position)
//
// Since dt isn't actually differential this gives us an
// error, so watch out with big values for dt.
// ========================================================

static void simple_update(Solid* obj, const FP dt) {
  if(obj->force) {
    Vec2 acc;
    acc.x = obj->force / obj->mass * COS(obj->dir);
    acc.y = obj->force / obj->mass * SIN(obj->dir);

    obj->pos.x += acc.x * dt;
    obj->vel.y += acc.y * dt;

    obj->pos.x += obj->vel.x * dt + 0.5 * acc.x / obj->mass * dt * dt;
    obj->pos.y += obj->vel.y * dt + 0.5 * acc.y / obj->mass * dt * dt;
  } else {
    obj->pos.x += obj->vel.x * dt;
    obj->pos.y += obj->vel.y * dt;
  }
}

// ==Runge-Kutta 4th method.===============================
// d^2 x(t) / d t^2 = a, a = constant(acceleration)
// x'(0) = v, x(0) = p
// x'' = f(t, x, x') = (x', a)
//
// x_ {n+1} = x_n + h/6 (k1 + 2*k2 + 3*k3 + k4)
//  h = (b-a)/2
//  k1 = f(t_n, X_n), X_n = (x_n, x'_n)
//  k2 = f(t_n + h/2, X_n + h/2*k1)
//  k3 = f(t_n + h/2, X_n + h/2*k2)
//  k4 = f(t_n + h,   X_n + h*k3)
//
// x_{n+1} = x_n + h/6x'_n + 3*h*a, 4*a)
// ========================================================

#define RK4_N 4
static void rk4_update(Solid* obj, const FP dt) {
  FP h = dt / RK4_N; // Step.

  if(obj->force) {  // Force applied on object.
    int i;
    Vec2 initial, tmp;

    Vec2 acc;
    acc.x = obj->force / obj->mass * COS(obj->dir);
    acc.y = obj->force / obj->mass * SIN(obj->dir);

    for(i = 0; i < RK4_N; i++) {
      // X component.
      tmp.x  = initial.x = obj->vel.x;
      tmp.x += 2*initial.x + h*tmp.x;
      tmp.x += 2*initial.x + h*tmp.x;
      tmp.x += initial.x + h*tmp.x;
      tmp.x *= h/6;

      obj->pos.x += tmp.x;
      obj->vel.x += acc.x*h;

      // Y component.
      tmp.y  = initial.y = obj->vel.y;
      tmp.y += 2*(initial.y + h/2*tmp.y);
      tmp.y += 2*(initial.y + h/2*tmp.y);
      tmp.y += initial.y + h*tmp.y;
      tmp.y *= h/6;

      obj->pos.y += tmp.y;
      obj->pos.y += acc.y*h;
    }
  } else {
    obj->pos.x += dt*obj->vel.x;
    obj->pos.y += dt*obj->vel.y;
  }
}

// Initialize a new solid.
void solid_init(Solid* dest, const FP mass, const Vec2* vel, const Vec2* pos) {
  dest->mass = mass;

  dest->force = 0;
  dest->dir   = 0;

  if(vel == NULL)
    dest->vel.x = dest->vel.y = 0.0;
  else {
    dest->vel.x = vel->x;
    dest->vel.y = vel->y;
  }

  if(pos == NULL)
    dest->pos.x = dest->pos.y = 0.0;
  else {
    dest->pos.x = pos->x;
    dest->pos.y = pos->y;
  }

  dest->update = rk4_update;
}

// Create a new solid.
Solid* solid_create(const FP mass, const Vec2* vel, const Vec2* pos) {
  Solid* dyn = MALLOC_L(Solid);
  assert(dyn != NULL);
  solid_init(dyn, mass, vel, pos);
  return dyn;
}

// Free an existing solid.
void solid_free(Solid* src) {
  free(src);
  src = NULL;
}