/*
   File:        arm.c
   Author:      Andrew W. Moore
   Created:     Thu Apr 29th 1993
   Description: Kinematic multijoint arm.

   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 RUNNING_MODE 0
#define GOAL_MODE 1

#define DEFAULT_DIM 2.0
#define MAX_DTHETA ((2.0 * PI)/30.0)
#define MAX_THETA (PI - MAX_DTHETA)
#define DEFAULT_JOINT_LENGTH 0.7
#define DEFAULT_START_X 0.1
#define DEFAULT_START_Y 0.1

bool self_intersecting(ls)
lines *ls;
{
  bool result = FALSE;
  for ( ; !result && ls != NULL && ls -> next != NULL ; ls = ls -> next )
    result = line_intersects_lines(&ls->line,ls->next->next);
  return(result);
}

lines *make_joining_lines(ls1,ls2)
lines *ls1,*ls2;
/*
   PRE: old and new same length
*/
{
  lines *result = NULL;
  line l;

  if ( ls1 != NULL )
  {
    if ( ls2 == NULL )
      my_error("ovfubenueb");
    else
    {
      line_from_points(&ls1->line.start,&ls2->line.start,&l);
      result = add_to_lines(&l,result);

      while ( ls1 != NULL )
      {
        if ( ls2 == NULL )
          my_error("ovfubenueb2");
        else
        {
          line_from_points(&ls1->line.end,&ls2->line.end,&l);
          result = add_to_lines(&l,result);
        }
        ls1 = ls1 -> next;
        ls2 = ls2 -> next;
      }
      if ( ls2 != NULL ) my_error("oghibjr");
    }
  }
  return(result);
}

bool move_legal(old,new,barriers)
lines *old,*new,*barriers;
/*
   returns TRUE only if the new configuration does not intersect with itself
                     the lines between the corresponding ends of the old and
                     new positions dont intersect with any barriers and
                     the new position does not intersect with any barriers

   PRE: old and new same length
*/
{
  bool result = TRUE;
  if ( result )
    result = !self_intersecting(new);
  if ( result )
    result = !line_sets_intersect(new,barriers);
  if ( result )
  {
    lines *joins = make_joining_lines(old,new);
    result = !line_sets_intersect(joins,barriers);
    free_lines(joins);
  }
  return(result);
}

typedef struct arm_data_struct
{
  lines *barriers;
  float joint_length;
  int num_joints;
  float start_x,start_y;
} arm_data;

lines *state_to_lines(ad,nstate)
arm_data *ad;
float *nstate;
{
  lines *result = NULL;
  int i;
  float x = ad->start_x;
  float y = ad->start_y;
  float theta = 0.0;

  for ( i = 0 ; i < ad->num_joints ; i++ )
  {
    line l;
    point_from_spec(x,y,&l.start);
    theta += nstate[i];
    point_from_spec(x + ad->joint_length * cos(theta),
                    y + ad->joint_length * sin(theta),
                    &l.end
                   );
    result = add_to_lines(&l,result);
    x = l.end.x;
    y = l.end.y;
  }

  return(result);
}

worst Old_worst;
bool Old_worst_valid = FALSE;
bool Old_worst_different = FALSE;

void arm_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);
  arm_data *ad = (arm_data *) msp -> data;
  lines *old = state_to_lines(ad,wst->nstate);
  lines *new;
  int i;

  copy_worst(wst,&Old_worst,ad->num_joints);
  Old_worst_valid = TRUE;

  floats_add(wst->nstate,wac,ad->num_joints,next_wst->nstate);

  for ( i = 0 ; i < ad->num_joints ; 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]);
  }

  new = state_to_lines(ad,next_wst->nstate);

  if ( !move_legal(old,new,ad->barriers) )
  {
    copy_worst(wst,next_wst,state_dim(wld,wst->mode));
    Old_worst_different = FALSE;
  }
  else
    Old_worst_different = TRUE;

  free_lines(old);
  free_lines(new);

  next_wst -> mode = wst -> mode;

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

void arm_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);
}

/*
#define FANCY_JOINTS
*/

void arm_draw(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  arm_data *ad = (arm_data *) mode_ref(wld,wst->mode) -> data;
  lines *ls = state_to_lines(ad,wst->nstate);
  geom_base gb;
  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_lines(&gb,ls);

#ifdef FANCY_JOINTS
  {
    lines *ls2;
    for ( ls2 = ls ; ls2 != NULL ; ls2 = ls2 -> next )
    {
      circle c;
      graphics_point(&gb,&ls2->line.start);
      copy_point(&ls2->line.start,&c.middle);
      c.radius = 0.015;
      graphics_circle(&gb,&c);
    }
  }
#endif
  free_lines(ls);
}
   
void arm_running_draw_worst(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  if ( Old_worst_different )
  {
    if ( Old_worst_valid )
    {
      arm_draw(gp,wld,wst); 
      ag_pen(1.0);
      arm_draw(gp,wld,&Old_worst);
      ag_pen(0.0);
    }

    arm_draw(gp,wld,wst); 
  }
  else if ( !Old_worst_valid )
    arm_draw(gp,wld,wst);
}
   
int Time_to_refresh = 30;

void very_crap_arm_running_draw_worst(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  if ( Old_worst_different || !Old_worst_valid )
  {
    Time_to_refresh -= 1;
    if ( Time_to_refresh <= 0 )
    {
      extern void arm_draw_structure();
      ag_on("");
      Time_to_refresh = 30;
      arm_draw_structure(gp,wld,wst);
    }

    arm_draw(gp,wld,wst); 
  }
}
   
worst Very_old_worst;
bool Very_old_worst_valid = FALSE;

void crap_arm_running_draw_worst(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  if ( Old_worst_different )
  {
    mode_spec *msp = mode_ref(wld,RUNNING_MODE);
    arm_data *ad = (arm_data *) msp -> data;
    if ( Old_worst_valid )
    {
      if ( Very_old_worst_valid )
      {
        ag_pen(1.0);
        arm_draw(gp,wld,&Very_old_worst);
        ag_pen(0.0);
      }
    }

    copy_worst(&Old_worst,&Very_old_worst,ad->num_joints);
    Very_old_worst_valid = TRUE;
    arm_draw(gp,wld,wst); 
  }
  else if ( !Old_worst_valid )
    arm_draw(gp,wld,wst);
}
   
bool arm_running_is_stuck(wld,wh)
world *wld;
worst_hist *wh;
{
  bool result;
  if ( wh -> length < 2 )
    result = FALSE;
  else
  {
    float diff[MAX_DIM];
    floats_subtract(wh -> recent -> worst -> nstate,
                    wh -> recent -> before -> worst -> nstate,
                    state_dim(wld,RUNNING_MODE),
                    diff
                   );

    result = floats_magnitude_sqd(diff,state_dim(wld,RUNNING_MODE)) <
             real_square(MAX_DTHETA/30.0) * state_dim(wld,RUNNING_MODE);
  }

  return(result);
}

void arm_goal_draw_worst(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  worst run_wst;
  mode_spec *run_mode = mode_ref(wld,RUNNING_MODE);

  run_wst.mode = RUNNING_MODE;
  middle_of_region(run_mode->mode_transitions->region,run_wst.nstate);
  arm_running_draw_worst(gp,wld,&run_wst);
}

void arm_draw_structure(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
  arm_data *ad = (arm_data *) mode_ref(wld,RUNNING_MODE) -> data;
  lines *ls = ad -> barriers;
  geom_base gb;
  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);
  if ( mode_ref(wld,wst->mode)->draw_worst != NULL )
    mode_ref(wld,wst->mode)->draw_worst(gp,wld,wst);
}

mode_spec *make_arm_running_mode(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  int index = index_of_arg("-dim",argc,argv);
  int dim = (index < 0 || index >= argc-1) ? DEFAULT_DIM : atoi(argv[index+1]);
  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);
  arm_data *ad = AM_MALLOC(arm_data);
  int i;
  float base_angle = real_min(PI/2.0,MAX_THETA);
  float goal_nstate[MAX_DIM];

  msp -> name = "running";

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

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

    msp->splittable_attributes[i] = TRUE;

    goal_nstate[i] = (i == 0) ? base_angle : 0.0;
  }

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

  for ( i = 0 ; i < dim ; i++ )
  {
    hype_update(goal_zone,i,LO,goal_nstate[i] - 2.0 * MAX_DTHETA);
    hype_update(goal_zone,i,HI,goal_nstate[i] + 2.0 * MAX_DTHETA);
  }

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

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

  msp -> next_worst = arm_running_next_worst;
  msp -> local_control = arm_running_local_control;
  msp -> draw_worst = arm_running_draw_worst;
  msp -> is_stuck = arm_running_is_stuck;

  printf("Setup ad\n");
  ad -> barriers = load_lines(argv[2]);
  ad -> num_joints = dim;
  ad -> joint_length = DEFAULT_JOINT_LENGTH / dim;
  ad -> start_x = DEFAULT_START_X;
  ad -> start_y = DEFAULT_START_Y;

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

  i = index_of_arg("-base",argc,argv);
  if ( i >= 0 && i < argc - 2 )
  {
    ad -> start_x = atof(argv[i+1]);
    ad -> start_y = atof(argv[i+2]);
  }

  msp -> data = (char *) ad;

  return(msp);
}

mode_spec *make_arm_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 = arm_goal_draw_worst;

  return(msp);
}

void load_arm(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  if ( argc < 3 )
  {
    printf("%s arm <lines-file> [options] [arm-options]\n",
           argv[0]
          );
    printf("Arm options:\n");
    printf("-dim <number of joints>\n");
    printf("-length <joint length>\n");
    printf("-base <arm base x> <arm base y>\n");
    printf("-goal <theta_1> <theta_2> .. <theta_dim>\n");
 
    my_error("load_arm(): 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_arm_running_mode(wld,wname,argc,argv);
    printf("run_mode done\n");
    wld -> modes[GOAL_MODE] = make_arm_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 = arm_draw_structure;
  }
}