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

#include "physics.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif

// Set the vector value using cartesian coords.
void vect_cset(Vec2* v, double x, double y) {
  v->x = x;
  v->y = y;
  v->mod = MOD(x,y);
  v->angle = ANGLE(x, y);
}

// Set the vector value using polar coords.
void vect_pset(Vec2* v, double mod, double angle) {
  v->mod = mod;
  v->angle = angle;
  v->x = v->mod*cos(v->angle);
  v->y = v->mod*sin(v->angle);
}

// Copy vector source to destination.
void vectcpy(Vec2* dest, const Vec2* src) {
  dest->x = src->x;
  dest->y = src->y;
  dest->mod = src->mod;
  dest->angle = src->angle;
}

// Null a vector.
void vectnull(Vec2* v) {
  v->x = v->y = v->mod = v->angle = 0.;
}

// ==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.
// ========================================================
#if 0 // Simply commenting this out to avoid silly warnings.
static void simple_update(Solid* obj, const double dt) {
  // Make sure angle doesn't flip.
  obj->dir += obj->dir_vel/360.*dt;
  if(obj->dir > 2*M_PI)   obj->dir -= 2*M_PI;
  if(obj->dir < 0.)      obj->dir += 2*M_PI;
  
  double px, py, vx, vy;
  px = VX(obj->pos);
  py = VY(obj->pos);
  vx = VX(obj->vel);
  vy = VY(obj->vel);

  if(obj->force.mod) { // Force applied on an object.
    double ax, ay;
    ax = VX(obj->force)/obj->mass;
    ay = VY(obj->force)/obj->mass;

    vx += ax*dt;
    vy += ay*dt;

    px += vx*dt + 0.5*ax * dt*dt;
    py += vy*dt + 0.5*ay * dt*dt;

    obj->vel.mod = MOD(vx, vy);
    obj->vel.angle = ANGLE(vx, vy);
  } else {
    px += vx*dt;
    py += vy*dt;
  }
  obj->pos.mod = MOD(px, py);
  obj->pos.angle = ANGLE(px, py);
}
#endif

// ==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_MIN_H 0.01 // Minimal pass we want.
static void rk4_update(Solid* obj, const double dt) {
  // Make sure angle doesn't flip.
  obj->dir += obj->dir_vel/360.0*dt;
  if(obj->dir > 2*M_PI)   obj->dir -= 2*M_PI;
  if(obj->dir < 0.0)      obj->dir += 2*M_PI;

  int N = (dt > RK4_MIN_H) ? (int)(dt/RK4_MIN_H) : 1;
  double h = dt / (double)N; // Step.

  double px, py, vx, vy;
  px = VX(obj->pos);
  py = VY(obj->pos);
  vx = VX(obj->vel);
  vy = VY(obj->vel);

  if(obj->force.mod) {  // Force applied on object.
    int i;
    double ix, iy, tx, ty; // Initial and temp cartesian vector values.

    double ax, ay;
    ax = VX(obj->force)/obj->mass;
    ay = VY(obj->force)/obj->mass;
    for(i = 0; i < N; i++) {
      // X component.
      tx = ix = vx;
      tx += 2*ix + h*tx;
      tx += 2*ix + h*tx;
      tx += ix + h*tx;
      tx *= h/6;

      px += tx;
      vx += ax*h;

      // Y component.
      ty = iy = vy;
      ty += 2*(iy + h/2*ty);
      ty += 2*(iy + h/2*ty);
      ty += iy +h*ty;
      ty *= h/6;

      py += ty;
      vy += ay*h;
    }
    vect_cset(&obj->vel, vx, vy);
  } else {
    px += dt*vx;
    py += dt*vy;
  }
  vect_cset(&obj->pos, px, py);
}

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

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

  if(vel == NULL) vectnull(&dest->vel);
  else vectcpy(&dest->vel, vel);
  
  if(pos == NULL) vectnull(&dest->pos);
  else vectcpy(&dest->pos, pos);

  dest->update = rk4_update;
}

// Create a new solid.
Solid* solid_create(const double 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;
}