/*
   File:        smaze.c
   Author:      Andrew W. Moore
   Created:     Sun Sep 20 16:24:56 EDT 1992
   Description: The simplest kind of continuous maze

   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 RADIUS 0.006
#define MOVE_DISTANCE 0.005
#define RUNNING_MODE 0
#define GOAL_MODE 1

/*
Noise level 1 means interesting wobbliness in lines.
0 means standard deterministic.
*/

#define NOISE_LEVEL 0.0

void smaze_running_next_worst(wld,wst,wac,next_wst)
world *wld;
worst *wst;
float *wac;
worst *next_wst;
{
  lines *ls = (lines *) mode_ref(wld,wst->mode) -> data;
  circle ci;
  point v;
  float theta = wac[0] + NOISE_LEVEL * range_random(-1.0,1.0);
  POSFLOAT time_to_collide;

  ci.middle.x = wst->nstate[0];
  ci.middle.y = wst->nstate[1];
  ci.radius = RADIUS;
  v.x = cos(theta);
  v.y = sin(theta);

  time_to_collide = earliest_meet_lines(&ci,&v,ls);

  if ( PF_INFINITE(time_to_collide) || time_to_collide > MOVE_DISTANCE )
    time_to_collide = MOVE_DISTANCE;

  points_add_scalar_mult(&ci.middle,&v,time_to_collide,&ci.middle);

  next_wst -> mode = wst -> mode;
  next_wst -> nstate[0] = ci.middle.x;
  next_wst -> nstate[1] = ci.middle.y;
}

void smaze_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);

  wac[0] = atan2(aim[1] - wst->nstate[1], aim[0] - wst->nstate[0]);
}

#define DRAW_EVERY 2
int Time_since_drawn = 0;

void smaze_running_draw_worst(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{ 
  if ( Time_since_drawn <= 0 )
  {
    gp_draw_floats_dot(gp,wst->nstate);
    Time_since_drawn= DRAW_EVERY;
  }
  else
    Time_since_drawn -= 1;
}
   
bool smaze_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;
  }

  return(result);
}
   
void smaze_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 smaze_draw_structure(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  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,(lines *)run_mode->data);
  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_smaze_running_mode(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  mode_spec *msp = create_empty_mode_spec(2,1);
  hype *goal_zone = create_hype(msp->state.dim);
  worst *goal_wst = create_worst(GOAL_MODE,0);
  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);
  m_and_s_from_hype(msp->state.bound,msp->middle,msp->scales);

  hype_update(msp->action.bound,0,LO,0.0);
  hype_update(msp->action.bound,0,HI,2.0 * PI);

  set_bools_constant(msp->splittable_attributes,msp->state.dim,TRUE);

#define SPECIAL_CASE

#ifndef SPECIAL_CASE
  hype_update(goal_zone,0,LO,0.80);
  hype_update(goal_zone,0,HI,0.85);
  hype_update(goal_zone,1,LO,0.80);
  hype_update(goal_zone,1,HI,0.85);
#else
  hype_update(goal_zone,0,LO,0.90);
  hype_update(goal_zone,0,HI,0.95);
  hype_update(goal_zone,1,LO,0.90);
  hype_update(goal_zone,1,HI,0.95);
#endif

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

  msp -> next_worst = smaze_running_next_worst;
  msp -> local_control = smaze_running_local_control;
  msp -> draw_worst = smaze_running_draw_worst;
  msp -> is_stuck = smaze_running_is_stuck;

  msp -> data = (char *) load_lines(argv[2]);

  return(msp);
}

mode_spec *make_smaze_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 = smaze_goal_draw_worst;

  return(msp);
}

void load_smaze(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  if ( argc < 3 )
    my_error("load_smaze(): 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_smaze_running_mode(wld,wname,argc,argv);
    printf("run_mode done\n");
    wld -> modes[GOAL_MODE] = make_smaze_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 = smaze_draw_structure;
  }
}