/*
   File:        rmaze.c
   Author:      Andrew W. Moore
   Created:     Sun Apr 25th 1993
   Description: Maze with limited rotation

   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 "geom.h"      /* Simple 2-d geometry structures and operations */
#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 */

#define LENGTH 0.4
#define MOVE_DISTANCE 0.01
#define RUNNING_MODE 0
#define GOAL_MODE 1
#define MAX_DTHETA ((2.0 * PI)/40.0)

typedef struct rmaze_data_struct
{
  lines *lines;
  float length;
} rmaze_data;

void rmaze_running_next_worst(wld,wst,wac,next_wst)
world *wld;
worst *wst;
float *wac;
worst *next_wst;
{
  rmaze_data *rmd = (rmaze_data *) mode_ref(wld,wst->mode) -> data;
  lines *ls = rmd -> lines;
  circle ci;
  point d;
  float theta = wst->nstate[2];
  float dtheta = wac[2];

  point_from_spec(wst->nstate[0],wst->nstate[1],&ci.middle);
  ci.radius = rmd->length/2.0;
  point_from_spec(wac[0],wac[1],&d);

  if ( can_line_move(&ci,theta,&d,dtheta,ls) )
  {
    int i;
    mode_spec *msp = mode_ref(wld,wst->mode);
    floats_add(wst->nstate,wac,msp->state.dim,next_wst->nstate);
    for ( i = 0 ; i < msp->state.dim ; 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]);
    }
  }
  else
    copy_worst(wst,next_wst,state_dim(wld,wst->mode));

  next_wst -> mode = wst -> mode;
  if ( Verbosity > 19.0 )
    fprintf_worst(stdout,"next_wst = ",next_wst,wld,"\n");
}

void rmaze_running_local_control(wld,wst,goal_wst,wac)
world *wld;
worst *wst,*goal_wst;
float *wac;
{
  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);

  floats_subtract(aim,wst->nstate,state_dim(wld,wst->mode),wac);
  normalize_to_legality(wld,wst->mode,wac);
}

void rmaze_running_draw_worst(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  rmaze_data *rmd = (rmaze_data *) mode_ref(wld,wst->mode) -> data;
  line l;
  circle ci;
  if ( Verbosity > 19.0 )
    fprintf_worst(stdout,"drawing = ",wst,wld,"\n");
  point_from_spec(wst->nstate[0],wst->nstate[1],&ci.middle);
  ci.radius = rmd->length/2.0;
  line_from_circle(&ci,wst->nstate[2],&l);

  if ( Verbosity > 19.0 )
    printf("line = (%g,%g,%g,%g)\n",l.start.x,l.start.y,l.end.x,l.end.y);
  gp_draw_line(gp,l.start.x,l.start.y,l.end.x,l.end.y);
  gp_draw_disc(gp,l.start.x,l.start.y,3.0);
}
   
bool rmaze_running_is_stuck(wld,wh)
world *wld;
worst_hist *wh;
{
  bool result;
  if ( wh -> length < 2 )
    result = FALSE;
  else
  {
    worst *w1 = wh -> recent -> worst;
    worst *w2 = wh -> recent -> before -> worst;
    point p1,p2;
    point_from_spec(w1->nstate[0],w1->nstate[1],&p1);
    point_from_spec(w2->nstate[0],w2->nstate[1],&p2);
    result = distance_between_points(&p1,&p2) < MOVE_DISTANCE/10.0;
    if ( result )
      result = fabs(w1->nstate[2] - w2->nstate[2]) < 0.5 * MAX_DTHETA;
  }

  return(result);
}
   
void rmaze_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 rmaze_draw_structure(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  rmaze_data *rmd = (rmaze_data *) mode_ref(wld,RUNNING_MODE) -> data;
  lines *ls = rmd -> lines;
  geom_base gb;
  float mid[MAX_DIM];
  mode_spec *run_mode = mode_ref(wld,RUNNING_MODE);
  gb.bottom_left.x = gp->x.lo;
  gb.bottom_left.y = gp->y.lo;
  gb.top_right.x = gp->x.hi;
  gb.top_right.y = gp->y.hi;
  graphics_thick_lines(&gb,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);
}

mode_spec *make_rmaze_running_mode(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  mode_spec *msp = create_empty_mode_spec(3,3);
  hype *goal_zone = create_hype(msp->state.dim);
  worst *goal_wst = create_worst(GOAL_MODE,0);
  rmaze_data *rmd = AM_MALLOC(rmaze_data);
  int i;

  msp -> name = "running";

  hype_update(msp->state.bound,0,LO,0.0);
  hype_update(msp->state.bound,0,HI,1.0);
  hype_update(msp->state.bound,1,LO,0.0);
  hype_update(msp->state.bound,1,HI,1.0);
  hype_update(msp->state.bound,2,LO,0.0);
  hype_update(msp->state.bound,2,HI,PI);
  m_and_s_from_hype(msp->state.bound,msp->middle,msp->scales);

  hype_update(msp->action.bound,0,LO,-MOVE_DISTANCE);
  hype_update(msp->action.bound,0,HI,MOVE_DISTANCE);
  hype_update(msp->action.bound,1,LO,-MOVE_DISTANCE);
  hype_update(msp->action.bound,1,HI,MOVE_DISTANCE);
  hype_update(msp->action.bound,2,LO,-MAX_DTHETA);
  hype_update(msp->action.bound,2,HI,MAX_DTHETA);

  msp->splittable_attributes[0] = TRUE;
  msp->splittable_attributes[1] = TRUE;
  msp->splittable_attributes[2] = TRUE;

  hype_update(goal_zone,0,LO,0.82);
  hype_update(goal_zone,0,HI,0.92);
  hype_update(goal_zone,1,LO,0.62);
  hype_update(goal_zone,1,HI,0.72);
  hype_update(goal_zone,2,LO,0.0);
  hype_update(goal_zone,2,HI,PI);

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

  msp -> next_worst = rmaze_running_next_worst;
  msp -> local_control = rmaze_running_local_control;
  msp -> draw_worst = rmaze_running_draw_worst;
  msp -> is_stuck = rmaze_running_is_stuck;

  printf("Setup rmd\n");
  rmd -> lines = load_lines(argv[2]);
  printf("lines okay\n");
  rmd -> length = LENGTH;

  i = index_of_arg("-length",argc,argv);
  if ( i >= 0 && i < argc - 1 )
    rmd -> length = atof(argv[i+1]);

  printf("Arged okay\n");

  msp -> data = (char *) rmd;

  return(msp);
}

mode_spec *make_rmaze_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 = rmaze_goal_draw_worst;

  return(msp);
}

void load_rmaze(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  if ( argc < 3 )
  {
    printf("%s rmaze <lines-file> [options] [-length <line-length>]\n",
           argv[0]
          );
    my_error("load_rmaze(): argv[2] should be the lines definition file.\n");
  }
  else
  {
    init_empty_mode_spec_array(wld,2);
    printf("mode array done\n");
    wld -> modes[RUNNING_MODE] = make_rmaze_running_mode(wld,wname,argc,argv);
    printf("run_mode done\n");
    wld -> modes[GOAL_MODE] = make_rmaze_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 = rmaze_draw_structure;
  }
}