/* This file is part of MINPANTU.
   See file `COPYRIGHT' for pertinent copyright notices.

   Functions defining and solving the differential equations
   describing the motion of the balls. */


#include <math.h>
#include <assert.h>
#include "defs.h"
#include "motion.h"


/* See file `defs.h' for the explanation of tags `A', `B', and `C' on
   the constants below. */

#define BALL_MIN_DISTANCE 3	/* Well, almost any small value is ok. */
#define HIT_BALL_REPULSION_COEFF 2500000 /* A */
#define GOAL_BALL_REPULSION_COEFF 1000000 /* A */
#define BASE_REPULSION_COEFF 2000000 /* A */
#define EDGE_REPULSION_COEFF 100000 /* A */
/* The following three constanst belong to class `B' since changing
   them risks making the heuristics in function `probability'
   inoperative. */
#define BASE_ROTATION_COEFF 60000 /* B */
#define BALL_FRICTION_SPEED_LIMIT 120 /* B */
#define BALL_FRICTION_COEFF -3	/* B */


/* Compute the derivate of `state' into `Dstate'. */

void derive_state(double *state, double *Dstate)
{
  int i, j;

  /* For all objects the location is the derivate of the speed. */
  for (i = 0; i < NUMBER_OF_BALLS; i++) {
    BALL_X_LOCATION(Dstate, i) = BALL_X_SPEED(state, i);
    BALL_Y_LOCATION(Dstate, i) = BALL_Y_SPEED(state, i);
  }

  /* The derivate of speed is accelleration, which is solved from the
     equation `F = m a', and `F' is given by the sum of frictions,
     repulsions, attractions, and rotating forces between the balls,
     bases, and the edges of the playing field.  The accellerations
     are summed up, component by component, in the following loops. */
  
  for (i = 0; i < NUMBER_OF_BALLS; i++)
    BALL_X_SPEED(Dstate, i) = BALL_Y_SPEED(Dstate, i) = 0;

  /* First compute the repulsion and attraction between the balls.
     This is a rather time critical loop in the program, particularly
     if there are many balls. */
  for (i = 1; i < NUMBER_OF_BALLS; i++)
    for (j = 0; j < i; j++) {
      double F, x_distance, y_distance, distance_sqr;
      double distance, unit_x_distance, unit_y_distance;

      /* Compute various measures for the distance between the balls
         `i' and `j'. */
      x_distance = BALL_X_LOCATION(state, i) - BALL_X_LOCATION(state, j);
      y_distance = BALL_Y_LOCATION(state, i) - BALL_Y_LOCATION(state, j);
      distance_sqr = x_distance * x_distance + y_distance * y_distance;

      /* If the distance is less than `BALL_MIN_DISTANCE', then use
	 `BALL_MIN_DISTANCE'.  This avoids division by zero (and reals
	 too close to zero) later. */
      if (distance_sqr < BALL_MIN_DISTANCE * BALL_MIN_DISTANCE)
	distance_sqr = BALL_MIN_DISTANCE * BALL_MIN_DISTANCE;

      /* Compute the unit distances, used later to project the forces
         affecting the balls to coordinates. */
      distance = sqrt(distance_sqr);
      unit_x_distance = x_distance / distance;
      unit_y_distance = y_distance / distance;

      /* Compute the repulsion force, which grows to the second power
         of the inverse distance. */

      /* If the two balls `i' and `j' are both goal balls, their
         repulsive force is different from what it is when either ball
         is a hit ball. */
      if (IS_GOAL_BALL(i) && IS_GOAL_BALL(j))
	F = GOAL_BALL_REPULSION_COEFF / distance_sqr;
      else
	F = HIT_BALL_REPULSION_COEFF / distance_sqr;
      
      /* Add the acceleration caused by the components of the forces
         to the speeds. */
      BALL_X_SPEED(Dstate, i) += F * unit_x_distance;
      BALL_Y_SPEED(Dstate, i) += F * unit_y_distance;
      BALL_X_SPEED(Dstate, j) -= F * unit_x_distance;
      BALL_Y_SPEED(Dstate, j) -= F * unit_y_distance;
    }

  /* Second, edges of the playing field repulse the balls, linearly
     w.r.t. the inverse of the distance from the edge. */
  for (i = 0; i < NUMBER_OF_BALLS; i++) {
    double distance;

    /* Repulsion of the left edge. */
    distance = BALL_X_LOCATION(state, i) - XMIN;
    if (distance < BALL_MIN_DISTANCE)
      distance = BALL_MIN_DISTANCE;
    BALL_X_SPEED(Dstate, i) += EDGE_REPULSION_COEFF / distance;

    /* Repulsion from the right edge. */
    distance = XMAX - BALL_X_LOCATION(state, i);
    if (distance < BALL_MIN_DISTANCE)
      distance = BALL_MIN_DISTANCE;
    BALL_X_SPEED(Dstate, i) -= EDGE_REPULSION_COEFF / distance;

    /* Repulsion from the bottom edge. */
    distance = BALL_Y_LOCATION(state, i) - YMIN;
    if (distance < BALL_MIN_DISTANCE)
      distance = BALL_MIN_DISTANCE;
    BALL_Y_SPEED(Dstate, i) += EDGE_REPULSION_COEFF / distance;

    /* Repulsion from the top edge. */
    distance = YMAX - BALL_Y_LOCATION(state, i);
    if (distance < BALL_MIN_DISTANCE)
      distance = BALL_MIN_DISTANCE;
    BALL_Y_SPEED(Dstate, i) -= EDGE_REPULSION_COEFF / distance;
  }

  /* Third, base areas induce a rotating force field for all goal
     balls. */
  for (i = 0; i < NUMBER_OF_BALLS; i++)
    if (IS_GOAL_BALL(i)) {
      double x_distance, y_distance, distance;
      double F, unit_x_distance, unit_y_distance;
      
      x_distance = BALL_X_LOCATION(state, i) - PLAYER0_BASE_X;
      y_distance = BALL_Y_LOCATION(state, i) - PLAYER0_BASE_Y;
      distance = sqrt(x_distance * x_distance + y_distance * y_distance);
      if (distance < BALL_MIN_DISTANCE)
	distance = BALL_MIN_DISTANCE;
      unit_x_distance = x_distance / distance;
      unit_y_distance = y_distance / distance;
      F = BASE_ROTATION_COEFF / distance;
      if (i % 2 == 0)
	F = -F;
      BALL_X_SPEED(Dstate, i) += F * unit_y_distance;
      BALL_Y_SPEED(Dstate, i) -= F * unit_x_distance;

      x_distance = BALL_X_LOCATION(state, i) - PLAYER1_BASE_X;
      y_distance = BALL_Y_LOCATION(state, i) - PLAYER1_BASE_Y;
      distance = sqrt(x_distance * x_distance + y_distance * y_distance);
      if (distance < BALL_MIN_DISTANCE)
	distance = BALL_MIN_DISTANCE;
      unit_x_distance = x_distance / distance;
      unit_y_distance = y_distance / distance;
      F = BASE_ROTATION_COEFF / distance;
      if (i % 2 == 0)
	F = -F;
      BALL_X_SPEED(Dstate, i) -= F * unit_y_distance;
      BALL_Y_SPEED(Dstate, i) += F * unit_x_distance;
    }

  /* Fourth, the player's base repulses the the same player's hit
     ball.  This is so that the players couldn't nest in their bases,
     only defending approaching goal balls. */
  for (i = 0; i < NUMBER_OF_BALLS; i++)
    if (IS_HIT_BALL(i)) {
      double x_distance, y_distance, distance, distance_sqr;
      double F, unit_x_distance, unit_y_distance;

      x_distance = BALL_X_LOCATION(state, i) - base[i].x;
      y_distance = BALL_Y_LOCATION(state, i) - base[i].y;
      distance_sqr = x_distance * x_distance + y_distance * y_distance;
      if (distance_sqr < BALL_MIN_DISTANCE * BALL_MIN_DISTANCE)
	distance_sqr = BALL_MIN_DISTANCE * BALL_MIN_DISTANCE;
      distance = sqrt(distance_sqr);
      unit_x_distance = x_distance / distance;
      unit_y_distance = y_distance / distance;
      F = BASE_REPULSION_COEFF / distance_sqr;
      BALL_X_SPEED(Dstate, i) += F * unit_x_distance;
      BALL_Y_SPEED(Dstate, i) += F * unit_y_distance;
    }

  /* Fifth, cause some friction.  Does not affect slowly moving
     balls. */
  for (i = 0; i < NUMBER_OF_BALLS; i++) {
    double x_speed, y_speed, speed, unit_x_speed, unit_y_speed, F;
    x_speed = BALL_X_SPEED(state, i);
    y_speed = BALL_Y_SPEED(state, i);
    speed = sqrt(x_speed * x_speed + y_speed * y_speed);
    if (speed > BALL_FRICTION_SPEED_LIMIT) {
      F = BALL_FRICTION_COEFF * (speed - BALL_FRICTION_SPEED_LIMIT);
      unit_x_speed = x_speed / speed;
      unit_y_speed = y_speed / speed;
      BALL_X_SPEED(Dstate, i) += F * unit_x_speed;
      BALL_Y_SPEED(Dstate, i) += F * unit_y_speed;
    }
  }
}


/* Compute the derivate of `state' into `Dstate', but forget the
   effect of hit balls and interactions between all balls.

   This routine is used as a more efficient surrogate for
   `derive_state' when trying to find out the "positional values" of
   goal balls, i.e. whether the goal ball is likely to pose a threat
   by passing through a player's base.  The positional values are
   estimated by integrating the motion-describing ODE's far in the
   future, but because the places of hit balls are unpredictable, we
   can leave their effect out, and integrate more efficiently. */

void sloppy_derive_state(double *state, double *Dstate)
{
  int i;

  /* Locations are the derivates of speeds. */
  for (i = 0; i < NUMBER_OF_BALLS; i++) {
    BALL_X_LOCATION(Dstate, i) = BALL_X_SPEED(state, i);
    BALL_Y_LOCATION(Dstate, i) = BALL_Y_SPEED(state, i);
  }

  for (i = 0; i < NUMBER_OF_BALLS; i++)
    BALL_X_SPEED(Dstate, i) = BALL_Y_SPEED(Dstate, i) = 0;

  /* The effect of edges of the playing field on the goal balls. */
  for (i = 0; i < NUMBER_OF_BALLS; i++) 
    if (IS_GOAL_BALL(i)) {
      double distance;
      
      /* Repulsion of the left edge. */
      distance = BALL_X_LOCATION(state, i) - XMIN;
      if (distance < BALL_MIN_DISTANCE)
	distance = BALL_MIN_DISTANCE;
      BALL_X_SPEED(Dstate, i) += EDGE_REPULSION_COEFF / distance;
      
      /* Repulsion from the right edge. */
      distance = XMAX - BALL_X_LOCATION(state, i);
      if (distance < BALL_MIN_DISTANCE)
	distance = BALL_MIN_DISTANCE;
      BALL_X_SPEED(Dstate, i) -= EDGE_REPULSION_COEFF / distance;
      
      /* Repulsion from the bottom edge. */
      distance = BALL_Y_LOCATION(state, i) - YMIN;
      if (distance < BALL_MIN_DISTANCE)
	distance = BALL_MIN_DISTANCE;
      BALL_Y_SPEED(Dstate, i) += EDGE_REPULSION_COEFF / distance;
      
      /* Repulsion from the top edge. */
      distance = YMAX - BALL_Y_LOCATION(state, i);
      if (distance < BALL_MIN_DISTANCE)
	distance = BALL_MIN_DISTANCE;
      BALL_Y_SPEED(Dstate, i) -= EDGE_REPULSION_COEFF / distance;
    }

  /* Third, base areas induce a rotating force field for all goal
     balls. */
  for (i = 0; i < NUMBER_OF_BALLS; i++)
    if (IS_GOAL_BALL(i)) {
      double x_distance, y_distance, distance;
      double F, unit_x_distance, unit_y_distance;
      
      x_distance = BALL_X_LOCATION(state, i) - PLAYER0_BASE_X;
      y_distance = BALL_Y_LOCATION(state, i) - PLAYER0_BASE_Y;
      distance = sqrt(x_distance * x_distance + y_distance * y_distance);
      if (distance < BALL_MIN_DISTANCE)
	distance = BALL_MIN_DISTANCE;
      unit_x_distance = x_distance / distance;
      unit_y_distance = y_distance / distance;
      F = BASE_ROTATION_COEFF / distance;
      if (i % 2 == 0)
	F = -F;
      BALL_X_SPEED(Dstate, i) += F * unit_y_distance;
      BALL_Y_SPEED(Dstate, i) -= F * unit_x_distance;

      x_distance = BALL_X_LOCATION(state, i) - PLAYER1_BASE_X;
      y_distance = BALL_Y_LOCATION(state, i) - PLAYER1_BASE_Y;
      distance = sqrt(x_distance * x_distance + y_distance * y_distance);
      if (distance < BALL_MIN_DISTANCE)
	distance = BALL_MIN_DISTANCE;
      unit_x_distance = x_distance / distance;
      unit_y_distance = y_distance / distance;
      F = BASE_ROTATION_COEFF / distance;
      if (i % 2 == 0)
	F = -F;
      BALL_X_SPEED(Dstate, i) -= F * unit_y_distance;
      BALL_Y_SPEED(Dstate, i) += F * unit_x_distance;
    }

  /* Friction. */
  for (i = 0; i < NUMBER_OF_BALLS; i++)
    if (IS_GOAL_BALL(i)) {
      double x_speed, y_speed, speed, unit_x_speed, unit_y_speed, F;
      x_speed = BALL_X_SPEED(state, i);
      y_speed = BALL_Y_SPEED(state, i);
      speed = sqrt(x_speed * x_speed + y_speed * y_speed);
      if (speed > BALL_FRICTION_SPEED_LIMIT) {
	F = BALL_FRICTION_COEFF * (speed - BALL_FRICTION_SPEED_LIMIT);
	unit_x_speed = x_speed / speed;
	unit_y_speed = y_speed / speed;
	BALL_X_SPEED(Dstate, i) += F * unit_x_speed;
	BALL_Y_SPEED(Dstate, i) += F * unit_y_speed;
      }
    }
}


/* Perform a step in the classic fourth-order Runge-Kutta integration:

   Given the equation y' = f'(x,y), where y and f can be
   vector-valued, the integration step of length h involves the
   following steps:

     k1 = h * f'(x, y)
     k2 = h * f'(x + h/2, y+k1/2)
     k3 = h * f'(x + h/2, y+k2/2)
     k4 = h * f'(x + h, y + k3)
     y = y + k1/6 + k2/3 + k3/3 + k4/6
     x = x + h

   See any basic numerical analysis school book for a reference,
   e.g. Press, Flannery, Teukolsky, Vetterling, "Numerical Recipes in
   C" for very nice descriptions and very bad C-code. */

void integrate_step(double *state,
		    void (*derivator)(double *, double *),
		    double step_size)
{
  int i;
  double k1[BALL_STATE_VECTOR_LENGTH];
  double k2[BALL_STATE_VECTOR_LENGTH];
  double k3[BALL_STATE_VECTOR_LENGTH];
  double k4[BALL_STATE_VECTOR_LENGTH];
  double tmp[BALL_STATE_VECTOR_LENGTH];
  double Dtmp[BALL_STATE_VECTOR_LENGTH];

  /* Below, some divisions by a constant `C' have been transformed to
     multiplication with `(1.0/C)' in hope that the compiler will
     generate slightly more efficient code. */

  derivator(state, Dtmp);
  for (i = 0; i < BALL_STATE_VECTOR_LENGTH; i++)
    k1[i] = step_size * Dtmp[i];

  for (i = 0; i < BALL_STATE_VECTOR_LENGTH; i++)
    tmp[i] = state[i] + 0.5 * k1[i];
  derivator(tmp, Dtmp);
  for (i = 0; i < BALL_STATE_VECTOR_LENGTH; i++)
    k2[i] = step_size * Dtmp[i];

  for (i = 0; i < BALL_STATE_VECTOR_LENGTH; i++)
    tmp[i] = state[i] + 0.5 * k2[i];
  derivator(tmp, Dtmp);
  for (i = 0; i < BALL_STATE_VECTOR_LENGTH; i++)
    k3[i] = step_size * Dtmp[i];

  for (i = 0; i < BALL_STATE_VECTOR_LENGTH; i++)
    tmp[i] = state[i] + k3[i];
  derivator(tmp, Dtmp);
  for (i = 0; i < BALL_STATE_VECTOR_LENGTH; i++)
    k4[i] = step_size * Dtmp[i];

  for (i = 0; i < BALL_STATE_VECTOR_LENGTH; i++)
    state[i] += 
      (1.0/6) * k1[i] + (1.0/3) * k2[i] + (1.0/6) * k3[i] + (1.0/6) * k4[i];
}