/*
   File:        tmaze.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 "amgr.h"      /* Basic (0,512)x(0,512) Graphics window */
#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 MOVE_DISTANCE 0.04
#define RUNNING_MODE 0
#define GOAL_MODE 1

typedef struct tmaze_data_struct
{
  float center[MAX_DIM];
  float radius;
} tmaze_data;

tmaze_data *tmaze_data_of(wld)
world *wld;
{
  char *result = mode_ref(wld,RUNNING_MODE) -> data;
  return( (tmaze_data *) result );
}

void tmaze_running_next_worst(wld,wst,wac,next_wst)
world *wld;
worst *wst;
float *wac;
worst *next_wst;
{
  tmaze_data *tmd = tmaze_data_of(wld);
  mode_spec *msp = mode_ref(wld,RUNNING_MODE);
  float rsq  = real_square(tmd->radius);
  float diff[MAX_DIM];
  int i;

  if ( Verbosity > 40.0 )
  {
    fprintf_worst(stdout,"trnw wst = ",wst,wld,"\n");
    fprintf_floats(stdout,"trnw wac = ",wac,state_dim(wld,wst->mode),"\n");
  }

  floats_add(wst->nstate,wac,state_dim(wld,wst->mode),next_wst->nstate);

  if ( Verbosity > 40.0 )
  {
    fprintf_floats(stdout,"trnw next nstate = ",next_wst->nstate,state_dim(wld,wst->mode),"\n");
  }

  floats_subtract(next_wst->nstate,tmd->center,state_dim(wld,wst->mode),diff);
  if ( floats_magnitude_sqd(diff,state_dim(wld,wst->mode)) < rsq )
    copy_floats(wst->nstate,next_wst->nstate,state_dim(wld,wst->mode));

  for ( i = 0 ; i < state_dim(wld,wst->mode) ; 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]);
  }
  next_wst -> mode = wst -> mode;
  if ( Verbosity > 40.0 )
    fprintf_worst(stdout,"trnw next_wst = ",wst,wld,"\n");
}

void tmaze_running_local_control(wld,wst,goal_wst,wac)
world *wld;
worst *wst,*goal_wst;
float *wac;
{
  float aim[MAX_DIM];
  float mag;
  if ( wst->mode == goal_wst -> mode )
    copy_floats(goal_wst->nstate,aim,state_dim(wld,wst->mode));
  else
  {
    printf("wst -> mode = %d\n",wst->mode);
    middle_of_region(mode_ref(wld,wst->mode)->mode_transitions->region,aim);
  }

  if ( Verbosity > 30.0 )
    printf("tmaze lcon about to subtract\n");

  floats_subtract(aim,wst->nstate,state_dim(wld,wst->mode),wac);
  mag = sqrt(floats_magnitude_sqd(wac,state_dim(wld,wst->mode)));
  if ( mag > MOVE_DISTANCE )
    floats_scalar_multiply(wac,state_dim(wld,wst->mode),MOVE_DISTANCE/mag,wac);
}

void tmaze_running_draw_worst(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  int i;
  char buff[1000];
  float x1,y1,x2,y2;
  gp_draw_floats_dot(gp,wst->nstate);
  
  sprintf(buff,"");
  for ( i = 0 ; i < state_dim(wld,wst->mode) ; i++ )
  {
    char buff2[1000];
    sprintf(buff2,"%s%4.2f",buff,wst->nstate[i]);
    sprintf(buff,"%s%s",buff2,(i==state_dim(wld,wst->mode)-1)?"":" ");
  }

  x1 = wst->nstate[gp->x.comp] + 0.05;
  y1 = wst->nstate[gp->y.comp] - 0.05;
  x2 = x1 + strlen(buff) / 70.0;
  y2 = y1 + 0.06;

  ag_pen(1.0);
  gp_draw_box(gp,x1,y1,x2,y2);
  ag_pen(0.0);
  gp_draw_rectangle(gp,x1,y1,x2,y2);
  gp_draw_string(gp,x1+0.02,y1+0.02,buff);
  gp_draw_line(gp,wst->nstate[gp->x.comp],wst->nstate[gp->y.comp],x1,y2);
}
   
bool tmaze_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;
    float diff[MAX_DIM];
    float thresh = real_square(MOVE_DISTANCE/10.0);
    floats_subtract(w1->nstate,w2->nstate,state_dim(wld,w1->mode),diff);
    result = floats_magnitude_sqd(diff,state_dim(wld,w1->mode)) < thresh;
  }

  return(result);
}
   
void tmaze_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 tmaze_draw_structure(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  tmaze_data *tmd = tmaze_data_of(wld);
  gp_draw_floats_circle(gp,tmd->center,512.0 * tmd->radius);
}

mode_spec *make_tmaze_running_mode(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  int i;
  int dim = atoi(argv[2]);
  mode_spec *msp = create_empty_mode_spec(dim,dim);
  hype *goal_zone = create_hype(msp->state.dim);
  worst *goal_wst = create_worst(GOAL_MODE,0);
  tmaze_data *tmd = AM_MALLOC(tmaze_data);

  msp -> name = "running";

  for ( i = 0 ; i < dim ; i++ )
  {
    hype_update(msp->state.bound,i,LO,0.0);
    hype_update(msp->state.bound,i,HI,1.0);
  }

  for ( i = 0 ; i < dim ; i++ )
  {
    hype_update(msp->action.bound,i,LO,-MOVE_DISTANCE);
    hype_update(msp->action.bound,i,HI,MOVE_DISTANCE);
  }

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

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

  for ( i = 0 ; i < dim ; i++ )
  {
    hype_update(goal_zone,i,LO,0.05);
    hype_update(goal_zone,i,HI,0.15);
  }

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

  msp -> next_worst = tmaze_running_next_worst;
  msp -> local_control = tmaze_running_local_control;
  msp -> draw_worst = tmaze_running_draw_worst;
  msp -> is_stuck = tmaze_running_is_stuck;

  set_floats_constant(tmd->center,dim,0.5);
  tmd->radius = atof(argv[3]);

  msp -> data = (char *) tmd;

  return(msp);
}

mode_spec *make_tmaze_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 = tmaze_goal_draw_worst;

  return(msp);
}

void load_tmaze(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  if ( argc < 4 )
    my_error("load_tmaze(): argv[2] dimension. argv[3] obstacle radius\n");
  else
  {
    init_empty_mode_spec_array(wld,2);
    printf("mode array done\n");
    wld -> modes[RUNNING_MODE] = make_tmaze_running_mode(wld,wname,argc,argv);
    printf("run_mode done\n");
    wld -> modes[GOAL_MODE] = make_tmaze_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 = tmaze_draw_structure;
  }
}