#include <stdlib.h>
#include <stdio.h>
#include <string.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);
}

/* Mirrors a vector off another, stores results in vector. */
void vect_reflect(Vec2* r, Vec2* v, Vec2* n) {
  double dot;

  dot       = vect_dot(v, n);
  r->x      = v->x - ((2. * dot) * n->x);
  r->y      = v->y - ((2. * dot) * n->y);
  r->mod    = MOD(r->x, r->y);
  r->angle  = MOD(r->x, r->y);

}

/**
 * @brief Vector dot product.
 *    @param a Vector 1 for dot product.
 *    @param b Vector 2 for dot product.
 *    @return Dot product of vectors.
 */
double vect_dot(Vec2* a, Vec2* b) {
  return a->x * b->x + a->y * b->y;
}
  
/* ================
 * SOLID!
 * ================
 */
#if defined(FREEBSD)
/**
 * @brief Update the solids position using a euler integration.
 *
 * Simple 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 double dt) {
  double px, py, vx, vy, ax, ay;

  /* 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;
  if(obj->dir < 0.)      obj->dir += 2*M_PI;

  /* Initial positions. */
  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. */
    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;
  }

  /* Update position and velocity. */
  vect_cset(&obj->vel, vx, vy);
  vect_cset(&obj->pos, px, py);
}
#endif /* defined(FREEBSD) */

/**
 * @brief Runge-Kutta method of updating a solid based on it's
 * acceleration.
 *
 * 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)
 *
 * Main advantage comes thanks to the fact that Lephisto is on
 * a 2D plane. Therefore RK chops it up in chunks and actually
 * creates a tiny curve instead of aproximating the curve for 
 * a tiny staight line.
 */

#if !defined(FREEBSD)
#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;

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

  /* Initial positions and velocity. */
  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);
}
#endif /* !defined(FREEBSD) */

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

  /* Set direction velocity. */
  dest->dir_vel = 0.;

  /* Set force. */
  vectnull(&dest->force);

  /* Set direction. */
  dest->dir = dir;
  if((dest->dir > 2.*M_PI) || (dest->dir < 0.))
    dest->dir = fmod(dest->dir, 2*M_PI);

  /* Set velocity. */
  if(vel == NULL) vectnull(&dest->vel);
  else vectcpy(&dest->vel, vel);

  /* Set position. */
  if(pos == NULL) vectnull(&dest->pos);
  else vectcpy(&dest->pos, pos);

/*
 * FreeBSD seems to have a bug with optimizations in rk4_update causing it to
 * eventually become NaN.
 */
#if defined(FREEBSD)
  dest->update = simple_update;
#else
  dest->update = rk4_update;
#endif
}

/* 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);
}