/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

// approach 

// new kick


#include <string>

#include "FeatureSet.h"
#include "../Localization/GLocalization.h"
#include "../Localization/LocalizationInterface.h"
#include "../Main/RobotMain.h"
#include "../Main/MainObject.h"
#include "../Main/SystemEvent.h"
#include "../WorldModel/summarize.h"

FeatureSet::FeatureSet(void)
{
  memset(&current_time,0,((char *)this)+sizeof(FeatureSet) - (char *)&current_time);

  robot_is_paused = true;

  locOutStream = NULL;
  poseBroadcastCounter = 0;

  ep_sensors = NULL;
  vision = NULL;
  localization_ep = NULL;
  robot_velocity_ep = NULL;
  prev_motion_state = 0;

  was_primary_last = false;
  last_role_change_time = 0UL;
  time_in_role = 0UL;
  last_supporter_role = -1;

  for(int i = 0; i < kick_hist_len; i++){
    kick_hist[i] = 0;
  }

  near_front_wall = false;

  if(strcmp(model,"ERS210")==0) {
    have_ball_threshold = have_ball_threshold_ers7;
  } else {
    have_ball_threshold = have_ball_threshold_ers7;
  }
}

/* initializeStreams()
 * This initializes the streams that can be used for debugging output.
 * It needs to be modified so that the output is both the position and
 *   the heading gaussians.
 */
void FeatureSet::initializeStreams(PacketStreamCollection *out_psc) 
{
  int loc_id;

  loc_id = out_psc->allocateStream(sizeof(Gaussian2)*10,10);
  locOutStream = out_psc->getStream(loc_id);
  locOutStream->type = SPOutEncoder::LocalizationLead;
  locOutStream->binary = true;
}
  
void
FeatureSet::update(ulong timestamp, int r_id,
                   int goal_color, int jersey_color,
		   bool goalie,
                   WorldModel *modeller,
                   BotModel *botModeller,
                   ObjectInfo *object_info,
                   RadialObjectMap *radial_map_param,
		   LModel *lmodel,
                   TeamMsgMgr *_team_msg_mgr,
                   vector2d _neck_offset,
		   StuckInfo _stuck_info,
		   VisionInterface::HeadVelocity *head_vel,
                   bool _robot_is_paused, short mot_state)
{
  current_time = timestamp;

  vision->get(current_time);

  camera_loc = vision->get_camera_loc();
  camera_dir = vision->get_camera_dir();

  robot_id = RobotMain::GetObject().getRobotID();
  robot_name = RobotMain::GetObject().getRobotName();

  team_msg_mgr = _team_msg_mgr;
  world_model = modeller;

  my_position = *(localization_ep->get(current_time));

  /* Send a localization update.  This gets sent from updateSensors because
   *   updateSensors is always called after updateMotion 
   */
  if(config.spoutConfig.dumpLocalization) {
    if(++poseBroadcastCounter >= config.spoutConfig.dumpLocalization) {
      if(locOutStream!=NULL) {
	static uchar buf[7*sizeof(float)];
        int out_size=0;
	out_size=encoder.encodeLocalization(buf,&my_position);
	locOutStream->writeBinary(buf,out_size);
      }else{
        pprintf(TextOutputStream,"NELOS");
      }
      poseBroadcastCounter = 0;
    }
  } /* End localize update broadcast */

  neck_offset = _neck_offset;
  my_angle = my_position.heading.mean.angle();
  my_position_body = my_position.position.mean - neck_offset.rotate(my_angle);

  velocity = robot_velocity_ep->get(current_time);

  defend_goal_color = goal_color;

  uniform_color = jersey_color;

  sensors = ep_sensors->get(current_time);

  saw_ball_confidence = object_info->ball.confidence;
    
  if(saw_ball_confidence>see_threshold_high) {
    ball_last_seen_high = current_time;
    ball_last_seen_med = current_time;
    ball_last_seen_low = current_time;
    see_ball_high = true;
    see_ball_med = true;
    see_ball_low = true;
  } else if(saw_ball_confidence>see_threshold_med) {
    
    ball_last_seen_med = current_time;
    ball_last_seen_low = current_time;
    see_ball_high = false;
    see_ball_med = true;
    see_ball_low = true;
  } else if(saw_ball_confidence>see_threshold_low) {
    ball_last_seen_low = current_time;
    see_ball_high = false;
    see_ball_med = false;
    see_ball_low = true;
  } else {
    see_ball_high = false;
    see_ball_med = false;
    see_ball_low = false;
  }

  /*
  ball_vector = modeller->ball_posn();
  ball_vector3d.set(ball_vector.x,ball_vector.y,BallRadius);

  // ball_vector is in ego centric coords
  vector2d fwd = vector2d(1, 0);
  fwd = fwd.rotate(my_position.heading.mean.angle());
  vector2d perp = vector2d(-fwd.y, fwd.x);
  world_ball_vector = my_position.position.mean +
    fwd*ball_vector.x + perp*ball_vector.y;
  */

  //Gaussian2 ball_gaus;
  //Gaussian2 world_ball_gaus;

  valid_ball_hyp = modeller->ball_tracker_observation(ball_gaus);
  if(valid_ball_hyp){
    ball_vector = ball_gaus.mean;
    ball_vector3d.set(ball_vector.x,ball_vector.y,BallRadius);

    modeller->ball_tracker_observation_global(world_ball_gaus);
    world_ball_vector = world_ball_gaus.mean;
    
    ulong last_ball_obs;
    world_model->ball_tracker_observation_time(last_ball_obs,0,false);
    time_since_last_ball_obs = current_time - last_ball_obs;

  }else{
    ball_vector = getRelativePosn(world_ball_vector);
    ball_vector3d.set(ball_vector.x,ball_vector.y,BallRadius);
    time_since_last_ball_obs = current_time;
  }

  ball_distance = ball_vector.length();

  ball_stddev[0] = modeller->ball_estimate().sx;
  ball_stddev[1] = modeller->ball_estimate().sy;
  ball_error_vector = vector2d(ball_stddev[0],
                               ball_stddev[1]);

  // what if ball_vector is (0, 0)?
  if(ball_distance==0)
    ball_angle = 0;
  else
    ball_angle = ball_vector.angle();
    
  ball_angle = ball_angle + 1;
  ball_angle = ball_angle - 1;

  ball_trj = modeller->getBallTrajectory(5);

  if(isnan(ball_angle))
    ball_angle = 0;

  vision_info = object_info;
  radial_map = radial_map_param;
  local_model = lmodel;
  if(goal_color == DEFEND_CYAN_GOAL) {
    score_goal  = &vision_info->yellow_goal;
    defend_goal = &vision_info->cyan_goal;
  } else {
    score_goal  = &vision_info->cyan_goal;
    defend_goal = &vision_info->yellow_goal;
  }

  // Do we see a marker?
  for(int i=0; i<6; i++) {
    if(object_info->marker[i].confidence > see_threshold_med)
      last_saw_marker = current_time;
  }
  // Do we see goal
  for(int i=YELLOW_GOAL; i<BALL; i++) {
    VObject *vobj = ((VObject *)vision_info)+i;
    if(vobj->confidence > see_threshold_med)
      last_saw_goal = current_time;
  }

  saw_score_goal_confidence = score_goal->confidence;
  saw_defend_goal_confidence = defend_goal->confidence;

  if(goal_color == DEFEND_CYAN_GOAL) {
    score_goal_const.set(-halfLength, 0);
    defend_goal_const.set(halfLength, 0);
  } else {
    score_goal_const.set(halfLength, 0);
    defend_goal_const.set(-halfLength, 0);
  }
#if 0    
  score_goal_left_angle  = score_goal_wm_start[0].mean.angle();
  score_goal_right_angle = score_goal_wm_start[1].mean.angle();

  score_goal_mean = (score_goal_wm_start[0].mean +
                     score_goal_wm_start[1].mean)/2;
  score_goal_mean_global = (score_goal_wm_global_start[0].mean +
			    score_goal_wm_global_start[1].mean)/2;
  defend_goal_mean = (defend_goal_wm_start[0].mean +
                      defend_goal_wm_start[1].mean)/2;
  defend_goal_mean_global = (defend_goal_wm_global_start[0].mean +
			     defend_goal_wm_global_start[1].mean)/2;
#endif
  
  /* Calculate whether or not an ally is in front of us. */
  const double confidenceThreshold = 10;
  const double InFrontAng = M_PI/6;
  const double InFrontMinDist = -100;
  const double InFrontMaxDist = 400;
  const double InFrontDy = 100;
        
  double blue_confidence = 0;
  double red_confidence = 0;
      
  blue_confidence += 
    botModeller->BotInRangeRect(BLUE_BOT_NOIR_MASK, current_time,
                                InFrontMinDist, InFrontMaxDist, 
                                -InFrontDy, InFrontDy);
  red_confidence += 
    botModeller->BotInRangeRect(RED_BOT_NOIR_MASK, current_time,
                                InFrontMinDist, InFrontMaxDist, 
                                -InFrontDy, InFrontDy);
  blue_confidence += 
    botModeller->BotInRangePolar(BLUE_BOT_NOIR_MASK, current_time,
                                 -InFrontAng, InFrontAng,
                                 InFrontMinDist, InFrontMaxDist);
  red_confidence += 
    botModeller->BotInRangePolar(RED_BOT_NOIR_MASK, current_time,
                                 -InFrontAng, InFrontAng,
                                 InFrontMinDist, InFrontMaxDist);
  blue_confidence += 
    2*botModeller->BotInRangeRect(BLUE_BOT_IR_MASK, current_time,
				  InFrontMinDist, InFrontMaxDist, 
				  -InFrontDy, InFrontDy);
  red_confidence += 
    2*botModeller->BotInRangeRect(RED_BOT_IR_MASK, current_time,
				  InFrontMinDist, InFrontMaxDist, 
				  -InFrontDy, InFrontDy);
  blue_confidence += 
    2*botModeller->BotInRangePolar(BLUE_BOT_IR_MASK, current_time,
				   -InFrontAng, InFrontAng,
				   InFrontMinDist, InFrontMaxDist);
  red_confidence += 
    2*botModeller->BotInRangePolar(RED_BOT_IR_MASK, current_time,
				   -InFrontAng, InFrontAng,
				   InFrontMinDist, InFrontMaxDist);
    
  if(uniform_color==TEAM_COLOR_RED) {
    ally_in_front = (red_confidence > confidenceThreshold);
    opponent_in_front = (blue_confidence > confidenceThreshold);
  } else {
    ally_in_front = (blue_confidence > confidenceThreshold);
    opponent_in_front = (red_confidence > confidenceThreshold);
  }
    
  in_own_half = (sign(my_position.position.mean.x)==
		 sign(defend_goal_const.x));

  stuck_info = _stuck_info;

  stuck_low = stuck_med = stuck_high = false;
  if(stuck_info.fraction_stuck > stuck_threshold_low)
    stuck_low = true;
  if(stuck_info.fraction_stuck > stuck_threshold_med)
    stuck_med = true;
  if(stuck_info.fraction_stuck > stuck_threshold_high)
    stuck_high = true;

  robot_is_paused = _robot_is_paused;

  motion_state = mot_state;
  if(motion_state != prev_motion_state &&
     prev_motion_state == Motion::STATE_KICKING){
    store_kick_time();
  }
  prev_motion_state = motion_state;
  

  ball_near_robot = world_model->robotInRect(ball_vector, 1000.0,
						1000.0, my_angle) > .5; 

  /* ========================================> Decision Tree */


  //position stuff
  position = my_position.position.mean;
  angle = my_position.heading.mean.angle();
  pos_uncert_sMaj = my_position.position.sMaj;
  pos_uncert_sMin = my_position.position.sMin;

  high_loc_uncertainty = pos_uncert_sMaj > 150.0;

  goal_angle = world_model->score_goal().angle();

  def_mid_pos_thresh = -sign(score_goal_const.x)*200;//-sign(score_goal_const.x) * halfWidth/2.0;
  mid_fwd_pos_thresh = sign(score_goal_const.x) * halfWidth/1.5;
  side_pos_thresh = halfWidth/2.0;

  if( sign(position.x) == sign(def_mid_pos_thresh) && 
      fabs(position.x) > fabs(def_mid_pos_thresh))
    field_region_x = DEF;
  else if( sign(position.x) == sign(mid_fwd_pos_thresh) && 
	   fabs(position.x) > fabs(mid_fwd_pos_thresh))
    field_region_x = FWD;
  else
    field_region_x = MID;

  if(fabs(position.y) < side_pos_thresh)
    field_region_y = CENTER;
  else
    field_region_y = SIDE;

  /*HACK- what time do we really want?*/
  if(current_time - ball_last_seen_med > 1000000L){
    saw_ball_recently = false;
  }else{
    saw_ball_recently = true;
  }
  
  //having the ball stuff
  if(saw_ball_recently && valid_ball_hyp){

    if(ball_vector.x > 0 && 
       ball_vector.x < have_ball_threshold && 
       fabs(ball_vector.y) < 40){
      have_ball_center = true;
    }else if(ball_vector.x > 0 && ball_vector.x < 195 && 
	     ball_vector.y > 55 && ball_vector.y < 90){
      have_ball_left = true;
    }else if(ball_vector.x > 0 && ball_vector.x < 195 && 
	     ball_vector.y < -55 && ball_vector.y > -90){
      have_ball_right = true;
    }else{
      have_ball_center = have_ball_left = have_ball_right = false;
    }
  }else{
    have_ball_center = have_ball_left = have_ball_right = false;
  }
  
  have_ball = have_ball_center || have_ball_left || have_ball_right;

  
  if(ball_distance < 400 && valid_ball_hyp){
    near_ball = true;
  }else{
    near_ball = false;
  }  
  
  if(fabs(world_model->score_goal().angle()) < M_PI/2.0){
    facing_own_half = false;
  } else {
    facing_own_half = true;
  }

  // in_front_corner now reports whether the *ball* is in the front
  // corner (not us.)
  // |                      |
  // |                      |
  // |\                    /| ^
  // | \                  / | |
  // |  \         500mm  /  | |  850mm (forms a right triangle)
  // |   \       <----->/   | | 
  // +-------+      +-------+ v
  //         | goal |
  //         +------+
  double score_goal_dx = fabs(world_ball_vector.x - 
                              score_goal_const.x);
  double score_goal_dy = fabs(world_ball_vector.y);
  if(score_goal_dy - score_goal_dx > 500.0) {
    in_front_corner = true;
  }else{
    in_front_corner = false;
  }


  if(//along the side wall
     (fabs(my_position.position.mean.y) > halfWidth-300.0) ||
     //by a goal
     (fabs(my_position.position.mean.x) > halfLength-300.0 )){
    near_wall = true;
  }else{
    near_wall = false;
  }

  if(//along the side wall
     (fabs(world_ball_vector.y) > halfWidth-300.0) ||
     //by a goal
     (fabs(world_ball_vector.x) > halfLength-300.0 )){
    ball_near_wall = true;
  }else{
    ball_near_wall = false;
  }

  // true iff the y-coord of the ball is between our own y-coord and
  // the goal.
  facing_ball = fabs(ball_vector.angle()) < RAD(20);
  facing_score_goal = fabs(world_model->score_goal().angle()) < RAD(20);

  ball_on_front_wall = fabs(score_goal_const.x - world_ball_vector.x) < 350.0;

  // if well-localized, determine whether we're in the front
  if (my_position.position.dist_dev() < 100.0) {
    if (fabs(score_goal_const.x - position.x) < 350) {
      near_front_wall = true;
    }
    else {
      near_front_wall = false;
    }
  }

  //check if something is against the sensor and that its not
  //our own head.  Values for the head angles came from using
  //Limpdog
  if(world_model->ball_on_chest_prob > .5 &&
     !(sensors->getFrame(0)->headAngles[0] < RAD(-60.0) &&
       fabs(sensors->getFrame(0)->headAngles[1]) < RAD(80.0) &&
       sensors->getFrame(0)->headAngles[2] < RAD(10.0)) &&
     fabs(vision->head_vel.tilt) < 3.5 && fabs(vision->head_vel.pan) < 3.5 &&
     !see_ball_med){
    ball_on_chest = true;
  }else{
    ball_on_chest = false;
  }

  //always set this flag to false.  If the approach
  //does fail, it will be set to true in BehSequences and
  //then evaluated in the decision tree. 
  approach_failed = false;

  game_state = RobotMain::GetObject().robot_state;
  kickoff = RobotMain::GetObject().have_kickoff;
  
  // Set the uniform ID of the robot that would be most likely to be
  // able to receive a pass from us
  {
    pass_receiver = 0;
    pass_receiver_pos_global = score_goal_const;
    double our_distance_from_goal = 
      GVector::distance(my_position.position.mean, score_goal_const);
    double distanceToClosest = 1e10;
    for (int i = 1; i <= 3; i++) {
      if (team_msg_mgr->teammateReadyForPass(i)) {
        //pprintf(TextOutputStream, "teammate %d ready for pass\n", i);
        vector2d pos = team_msg_mgr->getTeammatePosn(i).mean;
        // for passing to closest teammate
        //double dist = GVector::distance(pos, my_position.position.mean);
        // for passing to the teammate closest to the goal
        double dist_from_goal = GVector::distance(pos, score_goal_const);
        if (dist_from_goal < distanceToClosest &&
            dist_from_goal + 500.0 < our_distance_from_goal) {
          distanceToClosest = dist_from_goal;
          pass_receiver = i;
          pass_receiver_pos_global = pos;
        }
      }
    }
  }

  for (int i = 1; i <= 3; i++) {
    if (team_msg_mgr->teammateHasPassedRecently(i)) {
      teammate_pass_time = current_time;
    }
  }
  teammate_passed_recently = 
    current_time - teammate_pass_time < SecToTime(10.0);

  if (was_primary_last != team_msg_mgr->isPrimaryAttacker() ||
      last_supporter_role != team_msg_mgr->getSupporterRole()) {
    was_primary_last= team_msg_mgr->isPrimaryAttacker();
    last_supporter_role = team_msg_mgr->getSupporterRole();
    last_role_change_time = current_time;
  }

  time_in_role = current_time - last_role_change_time;
}

// Determines if blocking is a good idea.  We look to see where the
// ball would be if we blocked, vs. where the ball would be 0.1
// seconds after it passed us, and will decide that blocking is OK
// if the "blocked" position is closer to score_goal than the
// "un-blocked" position.
bool FeatureSet::shouldInterceptIfPossible() {
  vector2d my_pos = my_position.position.mean;


  bool should_block_if_possible = false;

  std::pair<vector2d, double> intercept = 
    world_model->getBallIntercept(10);
  vector2d ball_future_blocked = getGlobalPosn(intercept.first);
  double intercept_time = intercept.second;

  vector2d ball_future_unblocked = 
    getGlobalPosn
    (world_model->estimateBallPosn(intercept_time + 0.1, 10));
    
  if (intercept_time >= 0.0 &&
      intercept_time <= 3.0) {
    double blocked_dist = GVector::distance(ball_future_blocked, 
                                            score_goal_const);
    double unblocked_dist = GVector::distance(ball_future_unblocked, 
                                              score_goal_const);
    // if the two values are very close, prefer not to block.
    if (blocked_dist + 10.0 < unblocked_dist) {
      should_block_if_possible = true;
    }
  }

  // Extra paranoid check - if we're near the score_goal, we will only
  // block if we're on the "far" side of the ball's y-trajectory. It
  // looks *really bad* if we accidentally block a ball that's heading
  // for the opponents' goal :)
  if (GVector::distance(my_pos, score_goal_const) < 400.0) {
    double dy = ball_future_unblocked.y - world_ball_vector.y;
    if (sign(my_pos.y) != sign(dy)) {
      should_block_if_possible = false;
    }
  }
  return should_block_if_possible;
}

// the passing challenge has some different requirements. We want to
// use a nice, smooth, trajectory estimation, 'cuz we know that the
// ball is gonna be nice and unoccluded as it heads towards us. On the
// other hand, we want to dive even if we think the ball's out of
// range or if it's not moving too fast... just because diving looks
// cooler than walking up and grabbing it. 
bool FeatureSet::canInterceptBallPassingChallenge(int which_dive) {
  double trj_uncertainty_max = sq(30);
  double intercept_early_time = 1.0;
  double vel_thresh = 200.0;

  return canInterceptBallImpl(which_dive, 
                              trj_uncertainty_max,
                              intercept_early_time,
                              vel_thresh,
                              1.5,
                              15);
}

bool FeatureSet::canInterceptBallSloppy(int which_dive) {
  // This is the maximum chi^2 uncertainty we allow in the trajectory
  // estimation. If the trajectory has a higher chi^2 we will always
  // return false.
  double trj_uncertainty_max = sq(30); 

  // We calculate the time at which the ball is expected to cross the
  // line perpendicular to us. intercept_early_time tells us how much
  // earlier we want to initiate the dive.
  double intercept_early_time = 1.5; // sec

  // If the ball's not moving at least this fast, assume that we don't
  // need to dive to intercept it (it will be faster and more reliable
  // to just walk toward it.)
  double vel_thresh = 400.0; // mm/sec

  return canInterceptBallImpl(which_dive, 
                              trj_uncertainty_max,
                              intercept_early_time,
                              vel_thresh,
                              1.5,
                              5);
}

bool FeatureSet::canInterceptBall(int which_dive) {
  // This is the maximum chi^2 uncertainty we allow in the trajectory
  // estimation. If the trajectory has a higher chi^2 we will always
  // return false.
  double trj_uncertainty_max = sq(20); // FIXME: check this

  // We calculate the time at which the ball is expected to cross the
  // line perpendicular to us. intercept_early_time tells us how much
  // earlier we want to initiate the dive.
  double intercept_early_time = 1.25; // sec

  // If the ball's not moving at least this fast, assume that we don't
  // need to dive to intercept it (it will be faster and more reliable
  // to just walk toward it.)
  double vel_thresh = 400.0; // mm/sec

  return canInterceptBallImpl(which_dive, 
                              trj_uncertainty_max,
                              intercept_early_time,
                              vel_thresh,
                              1.0,
                              10);
}

bool FeatureSet::canInterceptBallImpl(int which_dive,
                                      double trj_uncertainty_max,
                                      double intercept_early_time,
                                      double vel_thresh,
                                      double catch_width_mult,
                                      int min_hist_len) {
  // If the ball's intercept isn't at least this close to us, we won't
  // try to intercept. This value depends on the kick we want to use.
  double catch_width;
  switch (which_dive) {
  case MOTION_BLOCK_GOALIE_L:
  case MOTION_BLOCK_GOALIE_R:
  case MOTION_BLOCK_SUPP_L:
  case MOTION_BLOCK_SUPP_R:
    catch_width = 250.0;
    break;

  case MOTION_BLOCK_SUPP:
    catch_width = 120.0;
    break;
    
  default:
    catch_width = 0.0;
    break;
  }
  catch_width *= catch_width_mult;

  std::pair<vector2d, double> ballIntercept = 
    world_model->getBallIntercept(min_hist_len);
  vector2d interceptPos = ballIntercept.first;
  double interceptTime = ballIntercept.second;

  bool result = 
    ball_trj.chi2 <= trj_uncertainty_max &&
    ball_trj.velocity.length() > vel_thresh &&
    interceptTime >= 0.25 && 
    interceptTime <= intercept_early_time && 
    valid_ball_hyp &&
    ball_distance <= 1000.0 &&  
    fabs(interceptPos.y) <= catch_width;

  if (result) {
    pprintf(TextOutputStream, "chi2 = %f, vel = %f, time = %f\n", 
            ball_trj.chi2, 
            ball_trj.velocity.length(),
            interceptTime);
  }

  return result;
}

/* Returns true if we have a valid ball hypothesis, and the most
   likely hypothesis is within the given distance. */
bool FeatureSet::ballClose(double distance) {
  return valid_ball_hyp && ball_distance <= distance;
}

int FeatureSet::getBestBallPosn(vector2d& retval) {
  // 0 = bug: ball_choice didn't get set in this function
  // 1 = neither ball is valid (but we choose local)
  // 2 = local is best
  // 3 = teammate is best
  static ulong last_switch_time = 0L;
  static int last_ball_choice = 0;
  static bool using_teammate_ball = false;
  static ulong last_called_time = 0L;
  static vector2d last_ball_location(0,0);

  // To switch to a new teammate ball, our confidence must be at least
  // switch_better_multiple times higher than the current confidence.
  const double switch_better_multiple = 1.25;

  // If an estimate hasn't been updated within at least this long, we
  // drop it entirely.
  const double drop_estimate_timeout = SecToTime(1.0);

  // We won't switch to a teammate's hypothesis (or switch between
  // different teammate hypotheses) unless we've been using the
  // current hypothesis for at least this long.
  const ulong switch_hysteresis_time = SecToTime(1.0);

  // if we've already been asked for a ball this frame, just return
  // the cached one.
  // FIXME BUG: won't ever return 4 from here, but 3 instead. 
  if (last_called_time == current_time) {
    retval = last_ball_location;
    return last_ball_choice;
  }

  int ball_choice = 0;
  WorldModel* wm = world_model;
  vector2d ball;

  // Get the confidence of the local ball.
  Gaussian2 local_obs;
  double local_conf = 0.0;
  if (wm->ball_tracker_observation_global(local_obs)) {
    //local_conf = local_obs.eval(local_obs.mean);
    local_conf = 1 / local_obs.dist_dev();
  }
  if (current_time - ball_last_seen_low > drop_estimate_timeout) {
    local_conf = 0.0;
  }

  // Get the confidence of the highest-confidence teammate ball.
  Gaussian2 teammate_best_obs;
  double teammate_best_conf = 0.0; 
  ulong teammate_best_tracker_id = 0;
  if (wm->teammate_ball_tracker_observation_global(teammate_best_obs)) {
    //teammate_best_conf = teammate_best_obs.eval(teammate_obs.mean);
    teammate_best_conf = 1 / teammate_best_obs.dist_dev();
    if (!wm->get_teammate_ball_tracker_ID_from_hypothesis(teammate_best_tracker_id)) {
      pprintf(TextOutputStream,"BUG: couldn't get ball tracker ID from a valid hypothesis\n");
    }
    ulong obs_time = 0UL;
    if (!wm->teammate_ball_tracker_observation_time(obs_time)) {
      pprintf(TextOutputStream, "BUG: couldn't get observation time from a valid hypothesis\n");
    }
    if (current_time - obs_time > drop_estimate_timeout) {
      teammate_best_conf = 0.0;
    }
  }
 
  bool teammate_switch_ok = 
    current_time - last_switch_time > switch_hysteresis_time;
  //current_time - teammate_last_worse >= SecToTime(0.5);
  bool local_switch_ok = true;
  //current_time - local_last_worse >= SecToTime(0.5);

  // There are 2 different ways of handling which ball to choose,
  // depending on whether we are currently using a teammate ball or
  // not.
  if (using_teammate_ball) {
    // if no ball is valid, we always use the local one
    if (local_conf < 1e-9 && 
        teammate_best_conf < 1e-9) {
      ball_choice = 1;
      ball = world_ball_vector;
    }
    // if local ball is at all better than the current teammate ball,
    // switch to it immediately
    else if (local_switch_ok &&
             local_conf > teammate_best_conf) {
      ball_choice = 2;
      ball = local_obs.mean;
    }
    // otherwise we keep using the current teammate hypothesis.
    else {
      ball_choice = 3;
      ball = teammate_best_obs.mean;
    }
  }
  else { // not using teammate ball
    // if no ball is valid, we always use the local one
    if (local_conf < 1e-9 && teammate_best_conf < 1e-9) {
      ball_choice = 1;
      ball = world_ball_vector;
    }
    // if teammate ball is much better than local ball, switch to it
    else if (teammate_switch_ok && teammate_best_conf > local_conf * switch_better_multiple) {
      ball_choice = 3;
      ball = teammate_best_obs.mean;
    }
    // otherwise continue using the local ball
    else {
      ball_choice = 2;
      ball = local_obs.mean;
    }
  }

  // debugging output
  if (false) {
    static int hb = 0;
    if (ball_choice != last_ball_choice) {
      pprintf(TextOutputStream,"-----------------------------------------\n");
      pprintf(TextOutputStream,
              "previously using teammate ball: %d\n", 
              using_teammate_ball);
      switch (ball_choice) {
      case 0:
        pprintf(TextOutputStream, "BUG: chose no ball\n");
        break;
      case 1:
        pprintf(TextOutputStream, 
                "using local ball (no valid observations)\n");
        break;
      case 2:
        pprintf(TextOutputStream, 
                "using local ball (better than teammate)\n");
        break;
      case 3:
        pprintf(TextOutputStream, 
                "using teammate ball (better than local)\n");
        break;
      }
    }

    if (hb % 1 == 0 || ball_choice != last_ball_choice) {      
      pprintf(TextOutputStream,
              "local: %e teammate_best: %e\n", 
              local_conf,
              teammate_best_conf);      
    }
    hb++;
  }  

  // update our static variables
  
  // ball_choice equal to 3 or 4 corresponds to using a teammate ball
  // of some sort.
  using_teammate_ball = ball_choice == 3 || ball_choice == 4;

  // update vars used for hysteresis
  if (ball_choice != last_ball_choice) {
    last_switch_time = current_time;
  }

  // update last_ball_choice - if our choice was 4, change it to 3 to
  // indicate that we are currently tracking a teammate ball.
  last_ball_choice = ball_choice;
  if (last_ball_choice == 4) {
    last_ball_choice = 3;
  }

  // And, some vars for the caching functionality.
  last_ball_location = ball;
  last_called_time = current_time;

  retval = ball;
  return ball_choice;
}


int FeatureSet::getBestBallPosnPrimary(vector2d& retval) {
  // 0 = bug: ball_choice didn't get set in this function
  // 1 = neither ball is valid (but we choose local)
  // 2 = local is best
  // 3 = teammate is best
  static ulong last_switch_time = 0L;
  static int last_ball_choice = 0;
  static bool using_teammate_ball = false;
  static ulong last_called_time = 0L;
  static vector2d last_ball_location(0,0);

  // To switch to a new teammate ball, our confidence must be at least
  // switch_better_multiple times higher than the current confidence.
  const double switch_better_multiple = 1.25;

  // If an estimate hasn't been updated within at least this long, we
  // drop it entirely.
  const double drop_estimate_timeout = SecToTime(3.0);

  // We won't switch to a teammate's hypothesis (or switch between
  // different teammate hypotheses) unless we've been using the
  // current hypothesis for at least this long.
  const ulong switch_hysteresis_time = SecToTime(1.0);

  // if we've already been asked for a ball this frame, just return
  // the cached one.
  // FIXME BUG: won't ever return 4 from here, but 3 instead. 
  if (last_called_time == current_time) {
    retval = last_ball_location;
    return last_ball_choice;
  }

  int ball_choice = 0;
  WorldModel* wm = world_model;
  vector2d ball;

  // Get the confidence of the local ball.
  Gaussian2 local_obs;
  double local_conf = 0.0;
  if (wm->ball_tracker_observation_global(local_obs)) {
    // if we see a ball at all, set its confidence artifically high so
    // that we warp to it immediately.
    local_conf = 1 / local_obs.dist_dev();
    if (see_ball_med) {
      local_conf = 1.0 * switch_better_multiple;
    }
  }
  if (current_time - ball_last_seen_low > drop_estimate_timeout) {
    local_conf = 0.0;
  }

  // Get the confidence of the highest-confidence teammate ball.
  Gaussian2 teammate_best_obs;
  double teammate_best_conf = 0.0; 
  ulong teammate_best_tracker_id = 0;
  if (wm->teammate_ball_tracker_observation_global(teammate_best_obs)) {
    //teammate_best_conf = teammate_best_obs.eval(teammate_obs.mean);
    teammate_best_conf = 1 / teammate_best_obs.dist_dev();
    if (!wm->get_teammate_ball_tracker_ID_from_hypothesis(teammate_best_tracker_id)) {
      pprintf(TextOutputStream,"BUG: couldn't get ball tracker ID from a valid hypothesis\n");
    }
    ulong obs_time = 0UL;
    if (!wm->teammate_ball_tracker_observation_time(obs_time)) {
      pprintf(TextOutputStream, "BUG: couldn't get observation time from a valid hypothesis\n");
    }
    if (current_time - obs_time > drop_estimate_timeout) {
      teammate_best_conf = 0.0;
    }
  }
  
  bool teammate_switch_ok = 
    current_time - last_switch_time > switch_hysteresis_time;
  bool local_switch_ok = true;

  // There are 2 different ways of handling which ball to choose,
  // depending on whether we are currently using a teammate ball or
  // not.
  if (using_teammate_ball) {
    // if no ball is valid, we always use the local one
    if (local_conf < 1e-9 && 
        teammate_best_conf < 1e-9) {
      ball_choice = 1;
      ball = world_ball_vector;
    }
    // if local ball is at all better than the current teammate ball,
    // switch to it immediately
    else if (local_switch_ok &&
             local_conf > teammate_best_conf) {
      ball_choice = 2;
      ball = local_obs.mean;
    }
    // otherwise we keep using the current teammate hypothesis.
    else {
      ball_choice = 3;
      ball = teammate_best_obs.mean;
    }
  }
  else { // not using teammate ball
    // if no ball is valid, we always use the local one
    if (local_conf < 1e-9 && teammate_best_conf < 1e-9) {
      ball_choice = 1;
      ball = world_ball_vector;
    }
    // if teammate ball is much better than local ball, switch to it
    else if (teammate_switch_ok && teammate_best_conf > local_conf * switch_better_multiple) {
      ball_choice = 3;
      ball = teammate_best_obs.mean;
    }
    // otherwise continue using the local ball
    else {
      ball_choice = 2;
      ball = local_obs.mean;
    }
  }

  // debugging output
  if (false) {
    static int hb = 0;
    if (ball_choice != last_ball_choice) {
      pprintf(TextOutputStream,"-----------------------------------------\n");
      pprintf(TextOutputStream,
              "previously using teammate ball: %d\n", 
              using_teammate_ball);
      switch (ball_choice) {
      case 0:
        pprintf(TextOutputStream, "BUG: chose no ball\n");
        break;
      case 1:
        pprintf(TextOutputStream, 
                "using local ball (no valid observations)\n");
        break;
      case 2:
        pprintf(TextOutputStream, 
                "using local ball (better than teammate)\n");
        break;
      case 3:
        pprintf(TextOutputStream, 
                "using teammate ball (better than local)\n");
        break;
      }
    }

    if (hb % 1 == 0 || ball_choice != last_ball_choice) {      
      pprintf(TextOutputStream,
              "local: %e teammate_best: %e\n", 
              local_conf,
              teammate_best_conf);      
    }
    hb++;
  }  

  // update our static variables
  
  // ball_choice equal to 3 or 4 corresponds to using a teammate ball
  // of some sort.
  using_teammate_ball = ball_choice == 3 || ball_choice == 4;

  // update vars used for hysteresis
  if (ball_choice != last_ball_choice) {
    last_switch_time = current_time;
  }

  // update last_ball_choice - if our choice was 4, change it to 3 to
  // indicate that we are currently tracking a teammate ball.
  last_ball_choice = ball_choice;
  if (last_ball_choice == 4) {
    last_ball_choice = 3;
  }

  // And, some vars for the caching functionality.
  last_ball_location = ball;
  last_called_time = current_time;

  retval = ball;
  return ball_choice;
}


/*Check for obstacles next to the ball*/
void FeatureSet::checkBallObst(double clen)
{
  if(!near_ball || !valid_ball_hyp){
    obst_left_of_ball = false;
    obst_right_of_ball = false;
    ball_near_obst = false;
    return;
  }

  if(false){
    static const double length = 300;
    static const double width  = 150;
    vector2f size(length,width);

    OccGridEntry cell;
    vector2f ball = vec2d2f(ball_vector);
    vector2f dir  = ball.norm();
    vector2f perp = dir.perp()*(width/2);

    queryLocalModel(&cell, dir, ball+perp, size);
    obst_left_of_ball = (cell.obs > 0.1 && cell.evidence > 1.0);

    queryLocalModel(&cell, dir, ball-perp, size);
    obst_right_of_ball = (cell.obs > 0.1 && cell.evidence > 1.0);

  }else{
    obst_left_of_ball  = checkObsAngle(ball_vector.angle()+0.26,clen);
    obst_right_of_ball = checkObsAngle(ball_vector.angle()-0.26,clen);
  }

  ball_near_obst = obst_left_of_ball || obst_right_of_ball;

  if(obst_right_of_ball && obst_left_of_ball)
    ball_near_obst = false;
}

/**
 * Checks for obstacles at the given angle.
 * Returns true if there are obstacles at the angle
 * false if it is clear to walk at that angle.
 */
bool FeatureSet::checkObsAngle(double a, double clen){
  /* Initializations */
  int i, max = 3;
  vector2f dir;			//Directional unit vector towards angle
  vector2f len;			//Center of local model rectangles
  double length = clen;		//Length of one rectangle
  double width  = 80.0;		//Width of one rectangle
  OccGridEntry cell;

  /* Rotate vector to angle */
  dir.set(100.0, 0.0);		//Initial state of vector
  dir = dir.rotate(a);		//Rotate to point to angle
  dir = dir.norm();		//Make it a unit vector

  /* Query rectangles in that direction */
  for(i=2; i<max; i++){					//Query 3 rectangles
    len = dir * (double(i*length) + length/2.0);	//Calculate center of rectangle
    queryLocalModel(&cell, dir, len,		//Specify unit vector and center
		    vector2f(300,width));	//Rectangle is 200mm long, 300mm wide
    
    /* If cell is not clear */
    if(cell.obs > 0.1 && 
       cell.evidence > 1.0){
      return !(cell.ball_conf > 0.1);   //Obstacle found
    }
  }
  
  return false;		//No obstacles were found at angle
}



/* Converts rel_vec, a vector in relative coordinates
   to a global position based the position and angle of
   the robot (as contained in the feature set). */
vector2d
FeatureSet::getGlobalPosn(vector2d rel_vec)
{
  vector2d retval;
  vector2d add;
  vector2d fwd = vector2d(1, 0);
  fwd = fwd.rotate(my_position.heading.mean.angle());
    
  vector2d perp = vector2d(-fwd.y, fwd.x);
    
  retval = my_position.position.mean;
  add = 
    fwd*rel_vec.x +
    perp*rel_vec.y;
  retval += add;
    
  return retval;
}

/* Converts an object location in global coordinates to
   a relative vector given where we currently are [think we are].
*/
vector2d
FeatureSet::getRelativePosn(vector2d global_vec)
{
  // passed by value so this is safe.
  global_vec = global_vec - my_position.position.mean;
  global_vec = global_vec.rotate(-my_position.heading.mean.angle());

  return global_vec;
}

vector3d
FeatureSet::getRelativePosn(vector3d global_vec)
{
  vector3d dog_posn = vector3d(my_position.position.mean.x,
                               my_position.position.mean.y,
                               0);
  
  global_vec = global_vec - dog_posn;
  global_vec = global_vec.rotate_z(-my_position.heading.mean.angle());

  return global_vec;
}
  
/* Returns true if rel_vec points toward the goal
   we're trying to score into. (toward is defined
   as rel_vec has a positive component going in
   that direction. NOTE THAT rel_vec IS A RELATIVE
   HEADING BASED ON THE DOG'S CURRENT POSITION.
*/
bool
FeatureSet::pointsToScoreGoal(vector2d rel_vec)
{    
  vector2d global_vec = getGlobalPosn(rel_vec);
    
  return ((global_vec - score_goal_const).length() <
          (my_position.position.mean - 
           score_goal_const).length());
}
  
bool
FeatureSet::pointInDefenseBox(vector2d global_pt)
{
  if(defend_goal_color==DEFEND_CYAN_GOAL) {
    return (global_pt.x > penaltyRegionLengthOffset && 
	    fabs(global_pt.y) < penaltyRegionHalfWidth);
  } else {
    return (global_pt.x < -penaltyRegionLengthOffset &&
	    fabs(global_pt.y) < penaltyRegionHalfWidth);
  }
}


/* Returns true if the point is far enough into the
   defense box that the attacker should avoid it.
*/
bool
FeatureSet::attackerAvoidPoint(vector2d global_pt)
{
  return pointInDefenseBox(global_pt);
}

/* Errs on the side of caution - if we don't know we say
   no so that we don't give up looking for it.
*/
bool
FeatureSet::isBallInDefenseBox(void)
{
  vector2d ego_ball_loc(0, 0);
  vector2d global_ball_loc(0, 0);
    
  /* Do we see the ball? */
  // FIXME: replace magic # with a constant
  if(current_time - ball_last_seen_med < 2000000L &&
     valid_ball_hyp) {
    ego_ball_loc.set(vision_info->ball.loc.x,
                     vision_info->ball.loc.y);
    global_ball_loc =
      getGlobalPosn(ego_ball_loc);
  } else {
    global_ball_loc = world_ball_vector;
  }
    
  return attackerAvoidPoint(global_ball_loc);
}

double
FeatureSet::cosAngleCameraToPoint(vector3d pt)
{
  vector3d cam_to_pt;
  cam_to_pt = pt - camera_loc;
  cam_to_pt.normalize();

  return cam_to_pt.dot(camera_dir);
}

bool
FeatureSet::isPointInFront(vector2d point)
{
  vector2d dog_vec;
  vector2d target_vec;
  vector2d cam_vec;
  
  // I'm defining theta as the angle to the target in the plane
  // of the field.
  double cos_theta;

  
  dog_vec.set(my_position.position.mean.x,
	      my_position.position.mean.y);
  
  target_vec = point - dog_vec;
  if(target_vec.length() > 0)
    target_vec.normalize();
  else
    target_vec.set(1, 0);
  
  cam_vec = my_position.heading.mean;
  
  // these vectors are already normalized
  cos_theta = target_vec.dot(cam_vec);
  
  // Is the point behind us?
  if(cos_theta < -0.2)  //ROBOCUP, used to be < 0
    return false;
  else
    return true;
}

bool
FeatureSet::isPointInFrontLocal(vector2d point)
{
  vector2d dog_vec;
  vector2d target_vec;
  vector2d cam_vec;
  
  // I'm defining theta as the angle to the target in the plane
  // of the field.
  double cos_theta;

  
  dog_vec.set(my_position.position.mean.x,
	      my_position.position.mean.y);
  
  target_vec = point;
  if(target_vec.length() > 0)
    target_vec.normalize();
  else
    target_vec.set(1, 0);
  
  // these vectors are already normalized
  cos_theta = target_vec.x;
  
  // Is the point behind us?
  if(cos_theta < -0.2)  //ROBOCUP, used to be < 0
    return false;
  else
    return true;
}

bool FeatureSet::shouldDribble()
{
  if(!valid_ball_hyp){
    return false;
  }

  /* Ball has rolled past us */
  if((ball_vector.x < 120 && fabs(ball_vector.y) > 130) ||
     (fabs(ball_angle) > 0.75 && ball_distance > 300)){
    return false;
  }

  //Determines whether or not the robot should dribble
  vector2d ball_to_goal = world_model->score_goal() - ball_vector;
  return (fabs(ball_to_goal.angle()) < 0.4 && world_model->score_goal().length() > 1000);
}

bool FeatureSet::shouldNotKick(void)
{
  /* Put the marker IDs into local coordinates.
     Left and right are named from the perspective of a 
     robot facing the goal which we're ATTACKING.
  */
  int LeftDefend, RightDefend,
    LeftMiddle, RightMiddle, LeftAttack,
    RightAttack;

  if(uniform_color==TEAM_COLOR_BLUE) {
    LeftDefend = MARKER_POC;
    RightDefend = MARKER_COP;
    LeftMiddle = MARKER_POG;
    RightMiddle = MARKER_GOP;
    LeftAttack = MARKER_POY;
    RightAttack = MARKER_YOP;
  } else {
    LeftDefend = MARKER_YOP;
    RightDefend = MARKER_POY;
    LeftMiddle = MARKER_GOP;
    RightMiddle = MARKER_POG;
    LeftAttack = MARKER_COP;
    RightAttack = MARKER_POC;
  }

  /* Do we see our own goal? */
  if(score_goal->confidence > 0.4)
    return true;

  /* Do we see either of the markers that live
     beside the goal that we defend? 
  */
  if(vision_info->marker[LeftDefend].confidence > 0.4 ||
     vision_info->marker[RightDefend].confidence > 0.4)
    return true;

  /* Are we on their half of the field? If so, do we see either
     of the markers in the middle?
  */
  if((sign(my_position.position.mean.x)==
     sign(score_goal_const.x)) && 
     (vision_info->marker[LeftMiddle].confidence > 0.4 ||
      vision_info->marker[RightMiddle].confidence > 0.4))
    return true;

  return false;
}


void FeatureSet::store_kick_time(){
  for(int i = kick_hist_len-1; i > 0; i--){
    kick_hist[i] = kick_hist[i-1];
  }
  kick_hist[0] = current_time;
  pprintf(TextOutputStream,"storing a kick\n");
}

ulong FeatureSet::get_kick_time(int idx){
  return kick_hist[idx];
}

// direction - unit vector in x direction relative to robot
// cell_center - center of query relative to robot
// range - major,minor size of query in basis reference frame
void FeatureSet::queryLocalModel(OccGridEntry *cell,vector2f direction, vector2f cell_center, vector2f range, bool black_obs)
{
  local_model->calc_occupancy(cell,direction,cell_center,range,black_obs);
}

bool FeatureSet::cellIsClear(OccGridEntry cell)
{
  if(cell.obs > 0.4 && 
     cell.evidence > 1.0 && 
     ((cell.red_robot_conf + cell.unknown_obs_conf) > .4  || (cell.blue_robot_conf + cell.unknown_obs_conf) > .4) &&
     (cell.red_robot_conf > 0.1 || cell.blue_robot_conf > 0.1)){
    return false;
  }
  return true;
}

std::string FeatureSet::name("FeatureSet");

EventProcessor *FeatureSet::create()
{
  static FeatureSet *fs=new FeatureSet;

  return fs;
}

bool FeatureSet::initConnections()
{
  EventManager *event_mgr;
  uint ep_id,sd_id,vis_id,loc_id,vel_id;

  event_mgr = EventManager::getManager();
  ep_id = event_mgr->getEventProcessorId(CameraFrameSource::name);
  event_mgr->listenEventProcessor(name,ep_id);

  sd_id = event_mgr->getEventProcessorId(SensorData::name);
  ep_sensors = dynamic_cast<SensorData *>(event_mgr->listenEventProcessor(name,sd_id));

  vis_id = event_mgr->getEventProcessorId(Vision::name);
  vision = dynamic_cast<Vision *>(event_mgr->listenEventProcessor(name,vis_id));

  loc_id = event_mgr->getEventProcessorId("Localizer");
  localization_ep = dynamic_cast<LocalizationEP *>(event_mgr->listenEventProcessor(name,loc_id));

  vel_id = event_mgr->getEventProcessorId(MotionUpdateVelocitySummary::name);
  robot_velocity_ep = dynamic_cast<MotionUpdateVelocitySummary *>(event_mgr->listenEventProcessor(name,vel_id));

  return true;
}

bool FeatureSet::update(ulong time,const EventList *events)
{
  return true;
}

FeatureSet *FeatureSet::get(ulong time)
{
  RobotMain::GetObject().behave.updateFeatureSet();

  return this;
}

REGISTER_EVENT_PROCESSOR(FeatureSet,FeatureSet::name,FeatureSet::create);