/*
   File:        slider.c
   Author:      Andrew W. Moore
   Created:     Fri Oct 16th 1992
   Description: The old car-on-slider 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 */
#include "geom.h"      /* Simple 2-d geometry structures and operations */
#include "contours.h"  /* Drawing contours of functions and 2d arrays */
#include "hilldyn.h"   /* Dynamics constrained to surface of (N-1)d hill */
#include "cones.h"     /* Operations on 2d cones and lists thereof */

#define TIME_STEP 0.03

#define MAX_ACTION 2.0

typedef struct slider_struct
{
  int sdim;  /* Dimension of state space = 2 * pdim */
  int pdim;  /* Dimension of physical space */
  float pgain,vgain;   /* Position and velocity gains */
  lines *cone_lines;
  hilldyn hilldyn;
  lines *contours;
  float time_step;
} slider;

alist *default_slider_alist()
{
  alist *al = NULL;

  al = alist_add(al,"cone_radius",0.3);
  al = alist_add(al,"cone_height",10.0);
  al = alist_add(al,"lo_height",-0.01);
  al = alist_add(al,"parts",5.0);
  al = alist_add(al,"draw_parts",39.0);
  al = alist_add(al,"friction",0.01);
  al = alist_add(al,"mass",1.0);
  al = alist_add(al,"max_move_dist",0.005);
  al = alist_add(al,"g",15.0);
  al = alist_add(al,"time_step",0.03);
  al = alist_add(al,"pgain",500.0);
  al = alist_add(al,"vgain",180.0);
  al = alist_add(al,"max_vel",3.0);

  return(al);
}

void load_slider_contents(sld,fname,params_fname)
slider *sld;
char *fname;
char *params_fname;
{
  alist *al = default_slider_alist();
  geom_base gb;
  extern float Cone_height,Cone_radius;
  FILE *s = (params_fname == NULL) ? NULL : fopen(params_fname,"r");

  if ( s != NULL )
  {
    char key[100];
    float value;

    while ( 2 == fscanf(s,"%s %f",key,&value) )
      al = alist_add(al,key,value);

    fclose(s);
  }

  fprintf_alist(stdout,"",al,"\n");

  Cone_radius = alist_lookup(al,"cone_radius");
  Cone_height = alist_lookup(al,"cone_height");

  sld -> pdim = 2;
  sld -> sdim = 2 * sld->pdim;
  sld -> pgain = alist_lookup(al,"pgain");
  sld -> vgain = alist_lookup(al,"vgain");
  sld -> cone_lines = load_lines(fname);

  make_cone_hilldyn(&sld->hilldyn,
                    sld->cone_lines,
                    alist_lookup(al,"friction"),
                    alist_lookup(al,"mass"),
                    alist_lookup(al,"max_move_dist"),
                    alist_lookup(al,"g")
                   );

  printf("Computing contours...\n");

  point_from_spec(-1.0,-1.0,&gb.bottom_left);
  point_from_spec(1.0,1.0,&gb.top_right);

  sld -> contours = cone_contours(&gb,
                                  sld -> cone_lines,
                                  alist_lookup(al,"lo_height"),
                                  alist_lookup(al,"cone_height"),
                                  (int) alist_lookup(al,"parts"),
                                  (int) alist_lookup(al,"draw_parts")
                                 );

  printf("...Computed contours\n");

  sld -> time_step = alist_lookup(al,"time_step");
  sld -> hilldyn.max_vel = alist_lookup(al,"max_vel");
}

slider *create_and_load_slider(fname,params_fname)
char *fname;
char *params_fname;
/* Format: See above */
{
  slider *sld = AM_MALLOC(slider);
  load_slider_contents(sld,fname,params_fname);
  return(sld);
}

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

#define RUNNING_MODE 0
#define GOAL_MODE 1

void slider_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);
  slider *sld = (slider *) msp -> data;
  cone_hilldyn_next_state(&sld->hilldyn,wst->nstate,wac,sld->time_step,
                          next_wst->nstate
                         );
  next_wst -> mode = wst -> mode;
}

void slider_running_local_control(wld,wst,goal_wst,wac)
world *wld;
worst *wst,*goal_wst;
float *wac;
{
  slider *sld = (slider *) mode_ref(wld,wst->mode) -> data;
  int i;
  float aim[MAX_DIM];

  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 < sld->pdim ; i++ )
    wac[i] = sld->pgain * (aim[i] - wst->nstate[i])  +
             sld->vgain * (aim[i+sld->pdim] - wst->nstate[i+sld->pdim]);

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

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

bool slider_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
*/
{
  slider *sld = (slider *) mode_ref(wld,RUNNING_MODE) -> data;
  bool result = FALSE;
  int i;
  for ( i = 0 ; !result && i < sld->pdim ; i++ )
  {
    int vel_comp = i + sld->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 slider_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 slider_draw_structure(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  slider *sld = (slider *) mode_ref(wld,RUNNING_MODE) -> data;
  lines *ls = sld -> contours;
  float dumx,dumy;

  float mid[MAX_DIM];
  mode_spec *run_mode = mode_ref(wld,RUNNING_MODE);

  draw_lines(ls);

  middle_of_region(run_mode->mode_transitions->region,mid);
  gp_draw_string(gp,mid[gp->x.comp],mid[gp->y.comp],"G");
  if ( mode_ref(wld,wst->mode)->draw_worst != NULL )
    mode_ref(wld,wst->mode)->draw_worst(gp,wld,wst);

  (void) ag_get_xy(&dumx,&dumy);
}

mode_spec *make_slider_running_mode(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  char *params_fname = (argc < 4) ? NULL : argv[3];
  slider *sld = create_and_load_slider(argv[2],params_fname);
  mode_spec *msp = create_empty_mode_spec(sld->sdim,sld->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 < sld->pdim ; i++ )
  {
    hype_update(msp->state.bound,i,LO,-1.0);
    hype_update(msp->state.bound,i,HI,1.0);
    hype_update(msp->state.bound,i+sld->pdim,LO,-sld->hilldyn.max_vel);
    hype_update(msp->state.bound,i+sld->pdim,HI,sld->hilldyn.max_vel);

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

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

  i = index_of_arg("-goal",argc,argv);
  if ( i >= 0 && i + sld->sdim < argc )
  {
    int j;
    for ( j = 0 ; j < sld->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 < sld->pdim ; i++ )
  {
    int vi = i + sld->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 = slider_running_next_worst;
  msp -> local_control = slider_running_local_control;
  msp -> draw_worst = slider_running_draw_worst;
  msp -> is_stuck = slider_running_is_stuck;
  msp -> should_remove_trans = slider_running_should_remove_trans;

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

mode_spec *make_slider_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 = slider_goal_draw_worst;

  return(msp);
}

void load_slider(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  if ( argc < 3 )
  {
    printf("%s slider <charge-and-charges-file> [params-file] [options] [arm-options]\n",
           argv[0]
          );
    printf("Slider options:\n");
    printf("-goal <x_0> <x_1> .. <x_pdim> <v_0> <v_1> .. <v_pdim> \n");
 
    my_error("load_slider(): 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_slider_running_mode(wld,wname,argc,argv);
    printf("run_mode done\n");
    wld -> modes[GOAL_MODE] = make_slider_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 = slider_draw_structure;
  }
}