/*
   File:        hill.c
   Author:      Andrew W. Moore
   Created:     Fri Oct 16th 1992
   Description: The old car-on-hill 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 */

/*********************** CUT-OUT-ABLE ***********************/

/*
        acceleration of the car in terms of the position of the car and
        the horizontal force supplied by the car. (The car weighs one kg.
        Dynamics are independent of velocity). Simulation of the car can
        then be achieved by Euler integration:

          x(t+h) = x(t) + h x'(t) + 0.5 + x''(t) * h * h
          x'(t+h) = x'(t) + h x''(t)
          x''(t) = vdot_of(x(t),f(t))
 
     Copyright (c) Andrew W. Moore, 1990
*/

#define TIME_STEP 0.015

#define XLO -1.0
#define XHI 1.0

#define MAXVEL 2.0

#define YLO -MAXVEL
#define YHI MAXVEL

#define STARTX -0.31
#define STARTY 0.05

#define GOALX 0.6
#define GOALY 0.0

#define MAX_ACTION 4.0

float f1(x)
float x;
{
  return( x * (x + 1.0) );
}

float f1_dashed(x)
float x;
{
  return( 2.0 * x + 1.0 );
}

float Aaa = 1.0;
float Bbb = 5.0;
float Ccc = 0.0;

float f2(x)
float x;
{
  return( Aaa * x / sqrt( 1.0 + Bbb * x * x ) );
}

float f2_dashed(x)
float x;
{
  float alpha = sqrt(1.0 + Bbb * x * x);
  return( Aaa / (alpha * alpha * alpha) );
}

float f(x)
float x;
{
  if ( x < Ccc )
    return(f1(x));
  else
    return(f2(x));
}

float f_dashed(x)
float x;
{
  if ( x < Ccc )
    return( f1_dashed(x) );
  else
    return( f2_dashed(x) );
}

float xtov(x)
float x;
{
  return(512.0 * (x - XLO)/(XHI - XLO));
}

float vtox(v)
float v;
{
  return(XLO + (XHI - XLO) * v/512.0);
}

float ytov(y)
float y;
{
  return(512.0 * (y - YLO)/(YHI - YLO));
}

float vtoy(v)
float v;
{
  return(YLO + (YHI - YLO) * v/512.0);
}

/*
   GRAPHICS: DRAWING THE CAR AND THE HILL.
   Graphics are not an essential part of the simulation.

     You will need to write a couple of low-level graphics functions.

       ag_line(x1,y1,x2,y2) x1,y1,x2,y2 all floats draws
       a line between (x1,y1) and (x2,y2) on a canvas with
       bottom left hand corner (0.0 , 0.0) and top right corner (512.0,512.0)

       ag_dot(x,y) draws a dot at x,y
       ag_circle(x,y,r) draws a circle at x,y radius r
*/

float draw_f()
{
  int i;
  int di = 1;
  float sf = 4.0;

  for ( i = 0 ; i < 512 ; i += di )
  {
    float agxlo = (float) i;
    float agxhi = (float) (i + di);
    float xlo = vtox(agxlo);
    float xhi = vtox(agxhi);
    printf("f(%g) = %g\n",xlo,f(xlo));
    ag_line(agxlo,
            ytov(sf * f(xlo)),
            agxhi,
            ytov(sf * f(xhi))
           );
  }
}

float Dc_x,Dc_y,Dc_t;

void dc_line(x1,y1,x2,y2)
float x1,y1,x2,y2;
{
  ag_line(Dc_x + x1 * cos(Dc_t) - y1 * sin(Dc_t),
          Dc_y + x1 * sin(Dc_t) + y1 * cos(Dc_t),
          Dc_x + x2 * cos(Dc_t) - y2 * sin(Dc_t),
          Dc_y + x2 * sin(Dc_t) + y2 * cos(Dc_t)
         );
}

void dc_disc(x1,y1,r)
float x1,y1,r;
{
  ag_disc(Dc_x + x1 * cos(Dc_t) - y1 * sin(Dc_t),
          Dc_y + x1 * sin(Dc_t) + y1 * cos(Dc_t),
          r
         );
}

void draw_car(x,y,t)
float x,y,t;
{
  float x1 = -40.0;
  float x2 = -20.0;
  float x3 = 10.0;
  float x4 = 20.0;
  float x5 = 40.0;
  float y1 = 0.0;
  float y2 = 8.0;
  float y3 = 24.0;
  float y4 = 40.0;

  Dc_x = x;
  Dc_y = y;
  Dc_t = t;

  dc_line(x1,y4,x3,y4);
  dc_line(x3,y4,x3,y3);
  dc_line(x3,y3,x5,y3);
  dc_line(x5,y3,x5,y2);
  dc_line(x5,y2,x1,y2);
  dc_line(x1,y2,x1,y4);

/* Forward 
  {
    int i;
    for ( i = 0 ; i <= 4 ; i++ )
      dc_line(x1 - (y2 - y1) * 0.5 * ((float) ( 15 - i )),
              y4 + (y2 - y4) * ((float) i)/5.0,
              x1 - (y2 - y1),
              y4 + (y2 - y4) * ((float) i)/5.0
             );
  }
*/

/* Backward           
  {
    int i;
    for ( i = 0 ; i <= 4 ; i++ )
      dc_line(x5 + (y2 - y1) * 0.5 * ((float) ( 15 - i )),
              y4 + (y2 - y4) * ((float) i)/5.0,
              x5 + (y2 - y1),
              y4 + (y2 - y4) * ((float) i)/5.0
             );
  }
*/

  dc_disc(x2,y2,(y2-y1));
  dc_disc(x4,y2,(y2-y1));
}

float draw_f_and_car(pos)
float pos;
{
  int i;
  int di = 1;
  float sf = 4.0;
  float ag_x_car = xtov(pos);
  float ag_y_car = ytov(sf * f(pos));
  float ag_grad = f_dashed(pos) * sf * (XHI - XLO) / (YHI - YLO);
  float ag_theta = atan2(ag_grad,1.0);

  draw_car(ag_x_car,ag_y_car,ag_theta);

  printf("pos = %g\n",pos );
  printf("f(pos)= %g\n",f(pos));
  printf("f_dashed(pos)= %g\n",f_dashed(pos));
  printf("ag_x_car= %g\n",ag_x_car);
  printf("ag_y_car= %g\n",ag_y_car);
  printf("ag_grad= %g\n",ag_grad);
  printf("ag_theta= %g\n",ag_theta);

  for ( i = 0 ; i < 512 ; i += di )
  {
    float agxlo = (float) i;
    float agxhi = (float) (i + di);
    float xlo = vtox(agxlo);
    float xhi = vtox(agxhi);
    if ( Verbosity > 45.0 )
      printf("f(%g) = %g\n",xlo,f(xlo));
    ag_line(agxlo,
            ytov(sf * f(xlo)),
            agxhi,
            ytov(sf * f(xhi))
           );
  }
}

void test_f()
{
  while ( TRUE )
  {
    draw_f();
    printf("Aaa = %g, Bbb = %g, Ccc = %g\n",Aaa,Bbb,Ccc);
    Aaa = input_float("New value of Aaa> ");
    Bbb = input_float("New value of Bbb> ");
    Ccc = input_float("New value of Ccc> ");
  }
}

void apply_accn(rx,rv,a)
float *rx,*rv,a;
{
  *rx = ( *rx + *rv * TIME_STEP + 0.5 * a * TIME_STEP * TIME_STEP );
  *rv = *rv + a * TIME_STEP;
}

float Big_mass = 1.0;
float Big_g = 9.81;

float vdot_of(x,f)
/*
   PRE:  XLO <= x <= XHI, -MAXACTION <= f <= MAXACTION
   POST: Returns the horizontal acceleration (d-squared x by d-(t-squared))
         of the car.
*/
float x,f;
{
  float q = f_dashed(x);
  float p = 1.0 + q * q;
  float fcomp = f / (Big_mass * sqrt(p));
  float gcomp = Big_g * q / p;
  return(fcomp - gcomp);
}

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

#define RUNNING_MODE 0
#define GOAL_MODE 1
#define CRASH_MODE 2

#define X_GAIN 1e4
#define V_GAIN 1e5

void hill_running_next_worst(wld,wst,wac,next_wst)
world *wld;
worst *wst;
float *wac;
worst *next_wst;
{
  float x = wst -> nstate[0];
  float v = wst -> nstate[1];
  float f = wac[0];
  float a = vdot_of(x,f);

  apply_accn(&x,&v,a);

  next_wst -> mode = wst -> mode;
  next_wst -> nstate[0] = x;
  next_wst -> nstate[1] = v;
}

void hill_running_local_control(wld,wst,goal_wst,wac)
world *wld;
worst *wst,*goal_wst;
float *wac;
{
  float aim[MAX_DIM];
  float x_goal,x_now,v_goal,v_now;
  float f;

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

  x_goal = aim[0];
  v_goal = aim[1];

  x_now = wst->nstate[0];
  v_now = wst->nstate[1];

  f = X_GAIN * (x_goal - x_now) + V_GAIN * (v_goal - v_now);
  f = real_max(-MAX_ACTION,f);
  f = real_min(MAX_ACTION,f);

  wac[0] = f;
}

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

bool hill_running_should_remove_trans(wld,msp,hy_me,hy_neigh)
world *wld;
mode_spec *msp;
hype *hy_me,*hy_neigh;
{
  bool result = FALSE;
  int vel_comp = 1;
  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,0,LO) &&
             hype_has_limit(hy_neigh,0,HI) &&
             hype_ref(hy_me,0,LO) >= hype_ref(hy_neigh,0,HI) - 0.01;
  }
  else if ( negative_vel )
  {
    result = hype_has_limit(hy_me,0,HI) &&
             hype_has_limit(hy_neigh,0,LO) &&
             hype_ref(hy_me,0,HI) <= hype_ref(hy_neigh,0,LO) + 0.01;
  }

  return(result);
}

void hill_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 hill_draw_structure(gp,wld,wst)
gproj *gp;
world *wld;
worst *wst;
{
}

/*
#define GOAL_WIDTH 0.1
*/
#define GOAL_WIDTH 0.25

mode_spec *make_hill_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);
  hype *crash_zone = create_hype(msp->state.dim);
  worst *goal_wst = create_worst(GOAL_MODE,0);
  worst *crash_wst = create_worst(CRASH_MODE,0);
  msp -> name = "running";

  hype_update(msp->state.bound,0,LO,XLO);
  hype_update(msp->state.bound,0,HI,XHI);
  hype_update(msp->state.bound,1,LO,YLO);
  hype_update(msp->state.bound,1,HI,YHI);
  m_and_s_from_hype(msp->state.bound,msp->middle,msp->scales);

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

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

  copy_hype(msp->state.bound,crash_zone);

  hype_update(goal_zone,0,LO,GOALX-(GOAL_WIDTH/2.0));
  hype_update(goal_zone,0,HI,GOALX+(GOAL_WIDTH/2.0));
  hype_update(goal_zone,1,LO,GOALY-(GOAL_WIDTH/2.0) * MAXVEL);
  hype_update(goal_zone,1,HI,GOALY+(GOAL_WIDTH/2.0) * MAXVEL);

  msp -> mode_transitions =
    add_to_mtrans(create_out_hype_region(crash_zone),
                  crash_wst,
                  msp->mode_transitions
                 );

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

  msp -> next_worst = hill_running_next_worst;
  msp -> local_control = hill_running_local_control;
  msp -> draw_worst = hill_running_draw_worst;
  msp -> is_stuck = hill_running_is_stuck;
  msp -> should_remove_trans = hill_running_should_remove_trans;
  msp -> data = NULL;

  return(msp);
}

mode_spec *make_hill_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 = hill_goal_draw_worst;

  return(msp);
}

mode_spec *make_hill_crash_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);
  hype *next_zone = create_hype(msp->state.dim);
  int run_mode_num = mode_number_called(wld,"running");
  mode_spec *run_mode = mode_ref(wld,run_mode_num);
  worst *restart_wst = create_worst(run_mode_num,run_mode->state.dim);

  msp -> name = "crash";

  copy_floats(run_mode->middle,restart_wst->nstate,run_mode->state.dim);

  msp -> mode_transitions =
    add_to_mtrans(create_in_hype_region(next_zone),
                  restart_wst,
                  msp->mode_transitions
                 );

  return(msp);
}

void load_hill(wld,wname,argc,argv)
world *wld;
char *wname;
int argc;
char *argv[];
{
  init_empty_mode_spec_array(wld,3);
  printf("mode array done\n");
  wld -> modes[RUNNING_MODE] = make_hill_running_mode(wld,wname,argc,argv);
  printf("run_mode done\n");
  wld -> modes[GOAL_MODE] = make_hill_goal_mode(wld,wname,argc,argv);
  printf("goal mode done\n");
  wld -> modes[CRASH_MODE] = make_hill_crash_mode(wld,wname,argc,argv);
  printf("crash mode done\n");
  wld -> goal = create_worst(GOAL_MODE,state_dim(wld,GOAL_MODE));
  printf("goal done\n");
  wld -> draw_structure = hill_draw_structure;
}