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

#include "lephisto.h"
#include "log.h"
#include "physics.h"

// ================
// MISC
// ================
double angle_diff(const double ref, double a) {
  double d;
  if(a < M_PI) a += 2*M_PI;
  d = fmod((a-ref), 2*M_PI);
  return (d <= M_PI) ? d : d - 2*M_PI;
}

void limit_speed(Vec2* vel, const double speed, const double dt) {
  double vmod;
  vmod = VMOD(*vel);
  if(vmod > speed) // Should not go faster.
    vect_pset(vel, (vmod-speed)*(1.-dt*3.) + speed, VANGLE(*vel));
}

// ================
// VEC2
// ================

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

// Create a minimal vector, only valid for blitting.
void vect_csetmin(Vec2* v, const double x, const double y) {
  v->x = x;
  v->y = y;
}

// Set the vector value using polar coords.
void vect_pset(Vec2* v, const double mod, const 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.;
}

// Get the direction pointed to by two vectors (from ref to v).
double vect_angle(const Vec2* ref, const Vec2* v) {
  return ANGLE(v->x - ref->x, v->y - ref->y);
}

void vect_cadd(Vec2* v, const double x, const double y) {
  v->x += x;
  v->y += y;
  v->mod = MOD(v->x, v->y);
  v->angle = ANGLE(v->x, v->y);
}


// ================
// SOLID!
// ================

// ==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 += M_PI/360.*obj->dir_vel*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 = obj->pos->x;
  py = obj->pos->y;
  vx = obj->vel->x;
  vy = obj->vel->y;

  if(obj->force.mod) { // Force applied on an object.
    double ax, ay;
    ax = obj->force->x/obj->mass;
    ay = obj->force->y/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) {
  int i, N;                       // For iteration and pass calculation.
  double h, px, py, vx, vy;       // Pass and position/velocity values.
  double ix, iy, tx, ty, ax, ay;  // Initial and temp cartesian vector values.
  // Make sure angle doesn't flip.
  obj->dir += M_PI/180.*obj->dir_vel*dt;
  if(obj->dir >= 2.*M_PI) obj->dir -= 2*M_PI;
  else if(obj->dir < 0.)  obj->dir += 2*M_PI;

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

  px = obj->pos.x;
  py = obj->pos.y;
  vx = obj->vel.x;
  vy = obj->vel.y;

  if(obj->force.mod) {  // Force applied on object.
    // Movement quantity theorem : m*a = \sum f.
    ax = obj->force.x / obj->mass;
    ay = obj->force.y / 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 {
    // Euler method -> p = v*t + 0.5*a*t^2 (no accel, so no error).
    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 double dir,
                const Vec2* pos, const Vec2* vel) {
  dest->mass = mass;

  dest->dir_vel = 0.;

  vect_cset(&dest->force, 0., 0.);
  dest->dir = dir;
  if((dest->dir > 2.*M_PI) || (dest->dir < 0.))
    dest->dir = fmod(dest->dir, 2*M_PI);

  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 double dir,
                    const Vec2* pos, const Vec2* vel) {
  Solid* dyn = MALLOC_L(Solid);
  if(dyn == NULL) ERR("Out of memory");
  solid_init(dyn, mass, dir, pos, vel);
  return dyn;
}

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