/*
   File:        hilldyn.c
   Author:      Andrew W. Moore
   Created:     Mon May 17 15:47:32 EDT 1993
   Description: Dynamics constrained to surface of (N-1)d hill

   Copyright (C) 1992, Andrew W. Moore
*/

#include <stdio.h>
#include <math.h>
#include "ambs.h"      /* Very basic operations */
#include "amgr.h"      /* Basic (0,512)x(0,512) Graphics window */
#include "amma.h"      /* Fast, non-fragmenting, memory management */
#include "amar.h"      /* Obvious operations on 1-d arrays */
#include "maxdim.h"    /* The MAX_DIM declaration */
#include "hilldyn.h"   /* Dynamics constrained to surface of (N-1)d hill */

void hilldyn_accn(hd,gradx,xpos,f,accn)
hilldyn *hd;
float *gradx;
float *xpos,*f,*accn;
/*
   xpos is dim-dimensional (thpough it is legal to pass it a (dim+1)-d vector.
   f    is dim+1 dimensional
   accn is dim+1 dimensional

   Calculate the acceleration of a point at (dim+1)vector coordinates
     ( xpos , height(xpos) )
   When subjected to a force f, and constrained to remain on the manifold
   defined by the height function.

   The result is stored in accn.
*/
{
  float alpha[MAX_DIM];  /* dim-d */
  float height_accn = 0.0;
  float sum_grad_sqd = 0.0;
  int i;

  if ( Verbosity > 20.0 )
    fprintf_floats(stdout,"gradx = ",gradx,hd->dim,"\n");

  for ( i = 0 ; i < hd->dim ; i++ )
    alpha[i] = f[i] + gradx[i] * (f[hd->dim] - hd->g);

  if ( Verbosity > 20.0 )
    fprintf_floats(stdout,"alpha = ",alpha,hd->dim,"\n");

  for ( i = 0 ; i < hd->dim ; i++ )
  {
    height_accn += gradx[i] * alpha[i];
    sum_grad_sqd += gradx[i] * gradx[i];
  }

  height_accn /= (hd->mass * (1.0 + sum_grad_sqd));

  for ( i = 0 ; i < hd->dim ; i++ )
    accn[i] = alpha[i] / hd->mass - gradx[i] * height_accn;

  accn[hd->dim] = height_accn;

  if ( Verbosity > 10.0 )
    fprintf_floats(stdout,"accn = ",accn,hd->dim+1,"\n");
}

void total_lie_accn(hd,gradx,xpos,f,accn)
hilldyn *hd;
float *gradx;
float *xpos,*f,*accn;
/*
   xpos is dim-dimensional (thpough it is legal to pass it a (dim+1)-d vector.
   f    is dim+1 dimensional
   accn is dim+1 dimensional

   Calculate the acceleration of a point at (dim+1)vector coordinates
     ( xpos , height(xpos) )
   When subjected to a force f, and constrained to remain on the manifold
   defined by the height function.

   The result is stored in accn.
*/
{
  float mag_grad = sqrt(floats_magnitude_sqd(gradx,hd->dim));
  float scale = ( mag_grad < 0.05 ) ? 0.0 : -(hd->g) / mag_grad;

  floats_scalar_multiply(gradx,hd->dim,scale,accn);
  floats_add(accn,f,hd->dim,accn);
  floats_scalar_multiply(accn,hd->dim,1.0 / hd->mass,accn);
  accn[hd->dim] = 0.0;

  if ( Verbosity > 10.0 )
    fprintf_floats(stdout,"accn = ",accn,hd->dim+1,"\n");
}

void hilldyn_step(hd,xstate,f,h)
hilldyn *hd;
float *xstate; /* (2 * dim) - dimensional */
float *f;      /* (dim + 1) - dimensional */
float h;
/*
   Given particle at position  ( xstate[0] , .. xstate[dim-1]  )
   and at velocity             ( xstate[dim+0] , .. xstate[2*dim-1] )
   (ignoring height here)
    What will be its position and velocity after h seconds, assuming
    constant acceleration equal to the exact acceleration which should
    occur at xstate?

    Answer is copied into xstate.

     vnew = vold + h * accn
     xnew = xold + h * vold + 1/2 * accn * h^2
*/
{
  float gradx[MAX_DIM];
  float accn[MAX_DIM+1]; /* dim+1 dimensional */
  float fric_force[MAX_DIM+1];
  int i;

  copy_floats(f,fric_force,hd->dim+1);
  for ( i = 0 ; i < hd->dim ; i++ )
    fric_force[i] -= hd->friction * xstate[i+hd->dim];

  hd->height_derivative_fn(xstate,hd->dim,hd->height_fn_data,gradx);

/*
  hilldyn_accn(hd,gradx,xstate,fric_force,accn);
*/
  total_lie_accn(hd,gradx,xstate,fric_force,accn);

  for ( i = 0 ; i < hd->dim ; i++ )
  {
    float vold = xstate[i+hd->dim];
    float xold = xstate[i];
    float vnew = vold + h * accn[i];
    float xnew = xold + h * vold + h * h * accn[i] / 2.0;

    xstate[i+hd->dim] = vnew;
    xstate[i] = xnew;
  }

/* Now velocities must be modified to account for gradient */
/*
  for ( i = 0 ; i < hd->dim ; i++ )
    xstate[i+hd->dim] /= sqrt(1.0 + real_square(gradx[i]));
*/

  for ( i = 0 ; i < hd->dim ; i++ )
  {
    int vi = i + hd->dim;
    xstate[i] = real_max(-hd->max_pos,xstate[i]);
    xstate[i] = real_min(hd->max_pos,xstate[i]);
    xstate[vi] = real_max(-hd->max_vel,xstate[vi]);
    xstate[vi] = real_min(hd->max_vel,xstate[vi]);
  }
}

void hilldyn_next_state(hd,xstate,f,h,xnext)
hilldyn *hd;
float *xstate; /* (2 * dim) - dimensional */
float *f;      /* (dim + 1) - dimensional */
float h;
float *xnext;
/*
   Given particle at position  ( xstate[0] , .. xstate[dim-1]  )
   and at velocity             ( xstate[dim+0] , .. xstate[2*dim-1] )
   (ignoring height here)
    What will be its position and velocity after h seconds? The answer is
    copied into xnext. The answer is obtained by a form of euler integration
    in which a small time step hsmall is chosen so that the movement between
    each integration step will be roughly less than or equal to
    hd->max_move_dist in length.
*/
{
  bool finished = FALSE;
  float min_h = h / 20.0;

  copy_floats(xstate,xnext,2 * hd->dim);

  while ( !finished )
  {
    float speed = 0.0;
    float remain_dist;
    float smallh;
    int i;

    for ( i = 0 ; i < hd->dim ; i++ )
      speed += real_square(xnext[i+hd->dim]);

    speed = sqrt(speed);

    remain_dist = speed * h;

    finished = remain_dist < hd->max_move_dist;
    smallh = ( finished ) ? h : real_max(min_h,hd->max_move_dist / speed);

    hilldyn_step(hd,xnext,f,smallh);
    h -= smallh;
  }
}