/*
   File:        field.c
   Author:      Andrew W. Moore
   Created:     Fri Oct 16th 1992
   Description: The old car-on-field problem

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

#include <stdio.h>
#include <math.h>
#include "ambs.h"      /* Very basic operations */
#include "amma.h"      /* Fast, non-fragmenting, memory management */
#include "amar.h"      /* Obvious operations on 1-d arrays */
#include "amgr.h"      /* Simple ag_ graphics */
#include "maxdim.h"    /* The MAX_DIM declaration */
#include "gpro.h"      /* Graphics projections kd->2d space */
#include "hype.h"      /* Hyper-rectangles from ../kdtr */
#include "region.h"    /* K-d region Data Structure */
#include "wrld.h"      /* Spec. of the World Control problem */
#include "whis.h"      /* History of worsts */

/*

    equation of motion in a potential field with N point charges.
    The object in motion has 1 unit of positive charge. The other
    charges have location c_i and charge q_i (+ or -) respectively.

    an external uniform force can be applied to the charge to control it.

                ______
                \
      x'' = F +  \        q_i
                 /   -------------
                /    ( x - c_i )^2
                -------

          x(t+h) = x(t) + h x'(t) + 0.5 + x''(t) * h * h
          x'(t+h) = x'(t) + h x''(t)
*/

#define TIME_STEP 0.02

#define MAX_POS 1.0
#define MAX_VEL 4.0

#define MAX_ACTION 4.0

void field_apply_accn(rx,rv,a)
float *rx,*rv,a;
{
  *rx = ( *rx + *rv * TIME_STEP + 0.5 * a * TIME_STEP * TIME_STEP );
  *rv = *rv + a * TIME_STEP;
}

typedef struct charge_struct
{
  float charge;
  float center[MAX_DIM];
} charge;

#define MAX_CHARGES 40

typedef struct field_struct
{
  int sdim;  /* Dimension of state space = 2 * pdim */
  int pdim;  /* Dimension of physical space */
  int size;  /* Number of charges */
  float pgain,vgain;   /* Position and velocity gains */

  charge charges[MAX_CHARGES];
} field;

void load_field_contents(fld,fname)
field *fld;
char *fname;
/* Format:
<pdim>
<pgain> <vgain>
<charge_0> <center_0>[0] .. <center_0>[pdim-1]
<charge_1> <center_1>[0] .. <center_1>[pdim-1]
  .
  .
*/
{
  FILE *s = safe_fopen(fname,"r");
  if ( 3 != fscanf(s,"%d %f %f",&(fld->pdim),&(fld->pgain),&(fld->vgain)) )
    my_error("load_field_contents syntax error");
  else
  {
    bool finished = FALSE;
    int i;

    fld -> sdim = 2 * fld->pdim;

    for ( i = 0 ; !finished && i < MAX_CHARGES ; i++ )
    {
      int j;
      charge *cg = &(fld->charges[i]);
      finished = 1 != fscanf(s,"%f",&cg->charge);
      for ( j = 0 ; !finished && j < fld->pdim ; j++ )
      {
        float x;
        if ( 1 != fscanf(s,"%f",&x) ) my_error("load_field_contents loop");
        cg->center[j] = x;
      }
    }
    fld -> size = i;
  }
  fclose(s);
}

field *create_and_load_field(fname)
char *fname;
/* Format: See above */
{
  field *fld = AM_MALLOC(field);
  load_field_contents(fld,fname);
  return(fld);
}

float field_charge(fld,chnum)
field *fld;
int chnum;
{
  if ( chnum < 0 || chnum > fld->size )
    my_error("isybvcis");
  else
    return(fld->charges[chnum].charge);
}

float *field_center(fld,chnum)
field *fld;
int chnum;
{
  if ( chnum < 0 || chnum > fld->size )
    my_error("isybvcis");
  else
    return(fld->charges[chnum].center);
}

void compute_accn(fld,nstate,wac,accn)
field *fld;
float *nstate,*wac,*accn;
{
  int chnum;
  copy_floats(wac,accn,fld->pdim);
  for ( chnum = 0 ; chnum < fld->size ; chnum++ )
  {
    float min_mag = MAX_POS/10.0;
    float diff[MAX_DIM];
    float mag_sqd,scale,mag;
    floats_subtract(field_center(fld,chnum),nstate,fld->pdim,diff);
    mag_sqd = floats_magnitude_sqd(diff,fld->pdim);
    mag = sqrt(mag_sqd);
    mag_sqd = real_max(mag_sqd,min_mag * min_mag);
      /* To prevent things being too wild near center */

    mag = real_max(mag,0.001);
      /* To prevent numerical overflow */

    scale = field_charge(fld,chnum) / (mag_sqd * mag);
    floats_scalar_multiply(diff,fld->pdim,scale,diff);
    floats_add(accn,diff,fld->pdim,accn);
  }
}

/****************** NOW THE WRLD-RELATED PART ******************/

#define RUNNING_MODE 0
#define GOAL_MODE 1

void field_running_next_worst(wld,wst,wac,next_wst)
world *wld;
worst *wst;
float *wac;
worst *next_wst;
{
  mode_spec *msp = mode_ref(wld,RUNNING_MODE);
  field *fld = (field *) msp -> data;
  float accn[MAX_DIM];
  int i;

  compute_accn(fld,wst->nstate,wac,accn);
 
  next_wst -> mode = wst -> mode;
  for ( i = 0 ; i < fld->pdim ; i++ )
  {
    float x = wst->nstate[i];
    float v = wst->nstate[i+fld->pdim];
    field_apply_accn(&x,&v,accn[i]);
    next_wst->nstate[i] = x;
    next_wst->nstate[i+fld->pdim] = v;
  }

  for ( i = 0 ; i < fld->sdim ; i++ )
  {
    next_wst->nstate[i] = 
      real_max(msp->middle[i]-msp->scales[i],next_wst->nstate[i]);
    next_wst->nstate[i] = 
      real_min(msp->middle[i]+msp->scales[i],next_wst->nstate[i]);
  }
}

void field_running_local_control(wld,wst,goal_wst,wac)
world *wld;
worst *wst,*goal_wst;
float *wac;
{
  field *fld = (field *) mode_ref(wld,wst->mode) -> data;
  int i;
  float aim[MAX_DIM];
  float h = TIME_STEP;

  if ( wst->mode == goal_wst -> mode )
    copy_floats(goal_wst->nstate,aim,state_dim(wld,wst->mode));
  else
    middle_of_region(mode_ref(wld,wst->mode)->mode_transitions->region,aim);

  for ( i = 0 ; i < fld->pdim ; i++ )
    wac[i] = fld->pgain * (aim[i] - wst->nstate[i]) / (h * h) +
             fld->vgain * (aim[i+fld->pdim] - wst->nstate[i+fld->pdim]) / h;

  normalize_to_legality(wld,wst->mode,wac);
}

void field_running_draw_worst(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  gp_draw_floats_dot(gp,wst->nstate);
}
   
bool field_running_is_stuck(wld,wh)
world *wld;
worst_hist *wh;
{
  return ( wh -> length > 98 );
}

bool field_running_should_remove_trans(wld,msp,hy_me,hy_neigh)
world *wld;
mode_spec *msp;
hype *hy_me,*hy_neigh;
/*
   Okay, if for any physical dimension i:
             the velocity with hy_me is all the same sign
             and all paths from hy_me to hy_neigh are in the opposite
                 physical direction

         then its hopeless to even attempt to travel from hy_me to hy_neigh
*/
{
  field *fld = (field *) mode_ref(wld,RUNNING_MODE) -> data;
  bool result = FALSE;
  int i;
  for ( i = 0 ; !result && i < fld->pdim ; i++ )
  {
    int vel_comp = i + fld->pdim;
    bool positive_vel = !hype_low_limit_is_below(hy_me,vel_comp,0.0);
    bool negative_vel = !hype_high_limit_is_above(hy_me,vel_comp,0.0);

    if ( positive_vel && negative_vel )
      my_error("+ and -");

    if ( positive_vel )
    {
      result = hype_has_limit(hy_me,i,LO) &&
               hype_has_limit(hy_neigh,i,HI) &&
               hype_ref(hy_me,i,LO) >= hype_ref(hy_neigh,i,HI) - 0.0001;
    }
    else if ( negative_vel )
    {
      result = hype_has_limit(hy_me,i,HI) &&
               hype_has_limit(hy_neigh,i,LO) &&
               hype_ref(hy_me,i,HI) <= hype_ref(hy_neigh,i,LO) + 0.0001;
    }
  }

  return(result);
}

void field_goal_draw_worst(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  gp_draw_region_border(gp,
                        mode_ref(wld,RUNNING_MODE)->mode_transitions->region
                       );
}

void field_draw_structure(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  field *fld = (field *) mode_ref(wld,RUNNING_MODE) -> data;
  int chnum;
  for ( chnum = 0 ; chnum < fld->size ; chnum++ )
  {
    float center[MAX_DIM];
    float r_pixels = 20.0 * fabs(field_charge(fld,chnum));

    set_floats_constant(center,2,0.0); /* Zero out first two components
                                          in case pdim is less than two */
    copy_floats(field_center(fld,chnum),center,fld->pdim);
 
    if ( field_charge(fld,chnum) > 0.0 )
      gp_draw_floats_circle(gp,center,r_pixels);
    else
      gp_draw_floats_disc(gp,center,r_pixels);
  }
}

mode_spec *make_field_running_mode(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  field *fld = create_and_load_field(argv[2]);
  mode_spec *msp = create_empty_mode_spec(fld->sdim,fld->pdim);
  hype *goal_zone = create_hype(msp->state.dim);
  worst *goal_wst = create_worst(GOAL_MODE,0);
  float goal_nstate[MAX_DIM];
  int i;

  msp -> name = "running";

  for ( i = 0 ; i < fld->pdim ; i++ )
  {
    hype_update(msp->state.bound,i,LO,-MAX_POS);
    hype_update(msp->state.bound,i,HI,MAX_POS);
    hype_update(msp->state.bound,i+fld->pdim,LO,-MAX_VEL);
    hype_update(msp->state.bound,i+fld->pdim,HI,MAX_VEL);

    hype_update(msp->action.bound,i,LO,-MAX_ACTION);
    hype_update(msp->action.bound,i,HI,MAX_ACTION);
  }

  for ( i = 0 ; i < fld->sdim ; i++ )
  {
    msp->splittable_attributes[i] = TRUE;
    goal_nstate[i] = (i < fld->pdim) ? 0.6666666666 : 0.0;
  }

  i = index_of_arg("-goal",argc,argv);
  if ( i >= 0 && i + fld->sdim < argc )
  {
    int j;
    for ( j = 0 ; j < fld->sdim ; j++ )
      goal_nstate[j] = atof(argv[i+j+1]);
  }

  m_and_s_from_hype(msp->state.bound,msp->middle,msp->scales);

  for ( i = 0 ; i < fld->pdim ; i++ )
  {
    int vi = i + fld->pdim;
    hype_update(goal_zone,i,LO,goal_nstate[i] - msp->scales[i]/10.0);
    hype_update(goal_zone,i,HI,goal_nstate[i] + msp->scales[i]/10.0);

    hype_update(goal_zone,vi,LO,goal_nstate[vi] - msp->scales[vi]);
    hype_update(goal_zone,vi,HI,goal_nstate[vi] + msp->scales[vi]);
  }

  msp -> mode_transitions =
    add_to_mtrans(create_in_hype_region(goal_zone),
                  goal_wst,
                  msp->mode_transitions
                 );

  msp -> next_worst = field_running_next_worst;
  msp -> local_control = field_running_local_control;
  msp -> draw_worst = field_running_draw_worst;
  msp -> is_stuck = field_running_is_stuck;
  msp -> should_remove_trans = field_running_should_remove_trans;

  msp -> data = (char *) fld;
  return(msp);
}

mode_spec *make_field_goal_mode(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
/*
    PRE: Running mode has been created
*/
{
  mode_spec *msp = create_empty_mode_spec(0,0);
  msp -> name = "goal";

  msp -> draw_worst = field_goal_draw_worst;

  return(msp);
}

void load_field(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  if ( argc < 3 )
  {
    printf("%s field <charge-and-charges-file> [options] [arm-options]\n",
           argv[0]
          );
    printf("Field options:\n");
    printf("-goal <x_0> <x_1> .. <x_pdim> <v_0> <v_1> .. <v_pdim> \n");
 
    my_error("load_field(): argv[2] should be the charge file.\n");
  }
  else
  {
    init_empty_mode_spec_array(wld,2);
    printf("mode array done\n");
    wld -> modes[RUNNING_MODE] = make_field_running_mode(wld,wname,argc,argv);
    printf("run_mode done\n");
    wld -> modes[GOAL_MODE] = make_field_goal_mode(wld,wname,argc,argv);
    printf("goal mode done\n");
    wld -> goal = create_worst(GOAL_MODE,state_dim(wld,GOAL_MODE));
    printf("goal done\n");
    wld -> draw_structure = field_draw_structure;
  }
}