/* 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.
  ========================================================================= */



#define DESTROY_ALL_HUMANS 1

// TODO
//
// FUNCTIONALITY / BUG FIXES
//
// make clearing distance shorter?
//
// draw fouls
//
// stuck detector - doing something intelligent
//
// special dive direction when perp to goal, so we don't slap or
// head-butt the ball toward goal.
//
// use turnballtoangle if we're against a wall, since turning with the
// ball won't work well
//
// longer timeout when aiming and inside the goal box
//
// canInterceptBall() returning a double instead of a bool?
//
// CODE BEAUTIFICATION PROJECT
//
// pull all configurable goalie stuff into one struct or code block
//
// put utility functions in the actual Goalie class


#include "Goalie.h"
#include "../Main/RobotMain.h"
#include "../Motion/RobotConstants.h"

extern bool UseCorners;

char const * const Goalie::beh_name = "Goalie";
char const * const Goalie::state_names[Goalie::NumStates] = {
    "WAITING",
    "BLOCKING",
    "POSITIONING", 
    "RETURNING",
    "APPROACH",
    "STAND",
    "AIM",
    "HOLD_BALL"
}; 

/* ======================================================================= */
/* ===============================================================> Goalie */
/* ======================================================================= */

Goalie::Goalie()
{
  fsm.init(state_names,NumStates,WAITING,16,16);

  track_objects = new BeTrackObjects;
  pri_attacker = new BePrimaryAttacker;
  pri_attacker->setupEventMgr();
  goto_point = new BeGotoPointGlobal();
  turn = new BeTurn();
  approach = new BeApproachBall();
  stuck_time = 0UL;
  approach_last_failed = 0UL;
  ball_been_far_recently = true;
  ball_far_time = 0UL;
}

Goalie::~Goalie()
{
  if(track_objects){
    track_objects = new BeTrackObjects;
    track_objects = NULL;
  }

  if(pri_attacker){
    pri_attacker = new BePrimaryAttacker;
    pri_attacker = NULL;
  }
}

void Goalie::reset(ulong timestamp)
{
  track_objects->reset(timestamp);
  pri_attacker->reset(timestamp);
  goto_point->reset(timestamp);
  turn->reset(timestamp);
  approach->reset(timestamp);
  approach_last_failed = 0UL;
  ball_been_far_recently = true;
  ball_far_time = 0UL;
}

bool isLost(FeatureSet* features) {
  //double angle_dev = features->my_position.heading.ang_dev();
  double pos_dev = features->my_position.position.dist_dev();
  // FIXME: have a sloppy and a tight version?
  return pos_dev > 100.0;
}

// get the best ball. pay attention to teammate estimates only if
// you've not seen it for 5 seconds
vector2d getBallGlobal(FeatureSet* features) {
  //return features->world_ball_vector;
  vector2d ball = features->world_ball_vector;
//   if (features->current_time - features->ball_last_seen_med > SecToTime(3.0)) {
//     features->getBestBallPosn(ball);
//   }
  return ball;
}

vector2d getBallLocal(FeatureSet* features) {
  return features->getRelativePosn(getBallGlobal(features));
}

bool Goalie::shouldBlock(FeatureSet* features) {
  return 
    ball_been_far_recently &&
    GVector::distance(features->position, 
                      features->defend_goal_const) < 750.0 &&
    features->canInterceptBallSloppy(MOTION_BLOCK_GOALIE_L);
}

vector2d defendLine(FeatureSet *features, 
                    vector2d left_endpoint,
                    vector2d right_endpoint) {
  double fieldWidth = 2 * halfWidth;
  vector2d ball = getBallGlobal(features);

  // weight is the fraction of the right endpoint to use
  double weight = (ball.y + halfWidth) / fieldWidth;
  
  vector2d target = 
    right_endpoint * weight + 
    left_endpoint * (1 - weight);

  static int hb = 0;
  if (hb % 30 == 0) {
    //pprintf(TextOutputStream,"target: (%f,%f)\n", target.x, target.y);
  }
  hb++;

  return target;
}

/* 
   We want to guard the goal on a circle that intersects the goal
   edges.  To find the position to guard, we find the intersection of
   the line between the ball and the circle center and the circle.  If
   this intersection is behind the goal line, clip to the goal
   line. The returned value is in world coords.
*/
vector2d calcTarget(FeatureSet* features) {
  vector2d ball = getBallGlobal(features);

  // The circle we guard on is set up to go through the points
  //   (goalie_guard_line, goalie_guard_width) and 
  //   (goalie_guard_line, -goalie_guard_width).
  //
  // We make the line be a little bit in front of the actual goal line,
  // mostly to keep the goalie from becoming stuck on the corner of the
  // goal.
  //
  // You can't tune these parameters arbitrarily. They must correlate
  // so that the circle intersects the goal edge.
  //const double stand_in_front_dist = 100.0;
  int weird_goalie = 0;
  double stand_in_front_dist = 50.0;
  double goalie_guard_line = halfLength - stand_in_front_dist;
  if (weird_goalie != 0) {
    goalie_guard_line = penaltyRegionLengthOffset - stand_in_front_dist;
  }
  double goalie_guard_width = goalHalfWidth + 100;
  vector2d goalie_circle_center(goalie_guard_line + 1.75 * goalDepth, 
                                weird_goalie * penaltyRegionHalfWidth); 
  double goalie_circle_radius = 660.0;
  if (weird_goalie != 0) {
    goalie_circle_radius *= 2;
    goalie_guard_width *= 2;
  }

  double guard_x;
  
  if (features->defend_goal_color == DEFEND_YELLOW_GOAL){
    guard_x = -goalie_guard_line;
    goalie_circle_center = vector2d(guard_x - 1.75 * goalDepth, 
                                    weird_goalie * penaltyRegionHalfWidth); 
  } 
  else {
    guard_x =  goalie_guard_line;
    goalie_circle_center = vector2d(guard_x + 1.75 * goalDepth, 
                                    weird_goalie * penaltyRegionHalfWidth); 
  }


  // The corners are the positions the goalie should go to if it isn't
  // possible to stand between the ball position and the back of the
  // goal because this point is off the field.
  vector2d pos_corner(guard_x, +(goalie_guard_width - 50));
  vector2d neg_corner(guard_x, -(goalie_guard_width - 50));

  double dist = GVector::distance(ball, goalie_circle_center);

  vector2d target = 
    (ball - goalie_circle_center)*(goalie_circle_radius/dist) + 
    goalie_circle_center;

  if (target.y > goalie_guard_width){
    target = pos_corner;
  }
  if (target.y < -goalie_guard_width){
    target = neg_corner;
  }

  // if lost, walk forward some
  // FIXME: do we still need this?
  if (features->my_position.position.dist_dev() > 100.0) {
    vector2d target_ego = features->getRelativePosn(target);
    target_ego += vector2d(200,0);
    target = features->getGlobalPosn(target_ego);
  }

  return target;
}

double calcTargetAngle(FeatureSet* features) {
  // So we're setting block_dir as a linear combination of ball_dir
  // and base_dir. If we've seen the ball recently, we weight ball_dir
  // more heavily. As the time we've lost the ball approaches 5
  // seconds, we totally use base_dir.
  double ball_lost_time = 
    features->current_time - features->ball_last_seen_med;
  vector2d base_dir(sign(features->score_goal_const.x), 0);
  base_dir = base_dir.rotate(-features->my_position.heading.mean.angle());
  
  vector2d ball_ego = getBallLocal(features);     
  vector2d ball_dir;
  
  if (features->valid_ball_hyp) {
    ball_dir = ball_ego.norm();
  }
  else {
    ball_dir = base_dir;
  }
  
  double ball_lost_time_minus = ball_lost_time - SecToTime(5.0);
  if (ball_lost_time_minus < 0) {
    ball_lost_time_minus = 0;
  }
  double f = min(ball_lost_time_minus / SecToTime(5), 1.0);
  vector2d block_dir = ball_dir*(1-f) + base_dir*f;
  //vector2d block_dir = ball_dir;
  return norm_angle(block_dir.angle());
}

bool Goalie::shouldClear(FeatureSet* features, 
                         bool is_clearing) {
  if (features->current_time - approach_last_failed < SecToTime(3.0)) {
    return false;
  }

  if (features->current_time -  features->ball_last_seen_med > 
      SecToTime(1.0)) {
    return false;
  }

  const double goalie_clear_dist      = 750.0;
  const double goalie_clear_done_dist = 950.0;
  const double goalie_max_y_ball_pos  = 700;

  // This probably has no effect at this value since vision never
  // reports distances this close.  It might have an effect due to the
  // world model though.
  const double goalie_near_ball      = 150;

  // We only use the world model ball here, because clearing a
  // teammate-reported ball is potentially incredibly stupid :)
  vector2d ball = features->world_ball_vector;

  // The distance from the ball to the center of the goal line. We
  // want to clear as long as the ball is closer than a certain
  // distance to the center of the goal.
  double bd = GVector::distance(features->defend_goal_const, ball);
 
  // we don't want to start clearing if the ball is too close to the
  // side of the field to clear safely
  bool ball_too_far_to_side = 
    (fabs(ball.y) > goalie_max_y_ball_pos) &&
    (features->ball_distance > goalie_near_ball);

  if (is_clearing) {
    return bd < goalie_clear_done_dist && !ball_too_far_to_side;
  }
  else {
    return 
      bd < goalie_clear_dist && 
      !ball_too_far_to_side;
  }    
  return is_clearing;
}

// FIXME: near target should depend more on global x, relative to
// global y.
std::pair<bool, bool> nearTarget(FeatureSet* features, 
                                 vector2d target_loc_ego,
                                 double target_angle_ego) {
  std::pair<bool, bool> retval; // first element is "tight", second "sloppy"
  retval.first = 
    target_loc_ego.length() < 80.0 && 
    fabs(target_angle_ego) < RAD(10);
  retval.second = 
    target_loc_ego.length() < 100.0 && 
    fabs(target_angle_ego) < RAD(20);
  return retval;
}

// taken from BeNavToPoint, with modifications
bool cellIsClear(OccGridEntry* cell) {
  if(cell->obs > 0.2 && 
     cell->evidence > 0.3){
    return false;
  }
  return true;
}


// taken from BeNavToPoint, with modifications
bool angleIsClear(FeatureSet *features, double a) {
  int num_recs = 3;
  double rec_length = 200.0;
  double rec_width = 200.0;
  vector2f rec_size(rec_length, rec_width);
  bool use_black_obstacles = false;


  // Directional unit vector towards angle
  vector2f dir(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 (int i = 0; i < num_recs; i++) {	// Query num_rec rectangles
    vector2f len = dir * ((i * rec_length) + rec_length / 2.0); 
    OccGridEntry cell;
    features->queryLocalModel(&cell, dir, len, rec_size, use_black_obstacles);
    if(!cellIsClear(&cell)){
      return false;
    }
  }
  return true;
}

bool frontIsClear(FeatureSet* features) {
  return 
    angleIsClear(features, RAD(-10.0)) &&
    angleIsClear(features, RAD(- 5.0)) &&
    angleIsClear(features, RAD(  0.0)) &&
    angleIsClear(features, RAD(  5.0)) &&
    angleIsClear(features, RAD( 10.0));
}

bool isStuck(FeatureSet* features) {
  static ulong last_unstuck_time = 0;
  if (features->stuck_info.fraction_stuck <= .5) {
    last_unstuck_time = features->current_time;
  }
  return features->current_time - last_unstuck_time > SecToTime(0.5);
}

/*
  Goalie will position between ball and back of goal; chase it out and
  return once it's been pushed far enough away from the goal.
*/
double Goalie::operator()(FeatureSet *features, 
			  Motion::MotionCommand *command) {

  // In theory, we only need to set this once. However, I'm not sure
  // if it's safe to do it in the constructor. I *think* RobotMain gets
  // created first 'cuz we only have 1 static instance of it, but just
  // in case we'll set it here. Every single frame.
  features->team_msg_mgr->i_am_goalie = true;
  UseCorners = true;
  
  vector2d ball_world = getBallGlobal(features);
  features->world_model->sendWMDebug(ball_world.x, 
                                     ball_world.y, 
                                     0, 
                                     0x4D, 
                                     0x00, 
                                     0x88);

  vector2d target_loc_world = calcTarget(features);
//   double defend_sign = sign(features->defend_goal_const.x);
//   vector2d target_loc_world = 
//     defendLine(features,
//                features->defend_goal_const + 
//                vector2d(-defend_sign * 130.0, 
//                         -defend_sign * goalHalfWidth),
//                features->defend_goal_const + 
//                vector2d(-defend_sign * 130.0, 
//                         defend_sign * goalHalfWidth));
  vector2d target_loc_ego = features->getRelativePosn(target_loc_world);
  double target_angle_ego = calcTargetAngle(features);

  std::pair<bool, bool> near_target = 
    nearTarget(features, target_loc_ego, target_angle_ego);
  bool near_target_tight = near_target.first;
  bool near_target_sloppy = near_target.second;
  
  std::pair<vector2d, double> ball_intercept = 
    features->world_model->getBallIntercept(5);
  vector2d intercept_pos_ego = ball_intercept.first;
  //double intercept_time = ball_intercept.second;

  bool ball_approaching = features->ball_trj.approach_vel > 200.0; 
  bool ball_close_to_us = features->ball_vector.length() < 500.0;

  //bool stuck = isStuck(features);
  bool stuck = false;

  // we will only dive if we've seen the ball be far away sometime
  // recently
  if (GVector::distance(features->position, 
                        features->world_ball_vector) > 500.0) {
    ball_far_time = features->current_time;
    ball_been_far_recently = true;
  }
  if (features->current_time - ball_far_time > SecToTime(5.0)) {
    ball_been_far_recently = false;
  }

  // we do special head motions during a block, so we don't call
  // TrackObjects if we're blocking - the head is handled specially in
  // the BLOCKING state.
  if (fsm.getState() != BLOCKING) {
    // if we don't know where we are, look at markers
    if (isLost(features)) {
      (*track_objects)(features,command,true,false);
      //double pos_dev = features->my_position.position.dist_dev();
      //pprintf(TextOutputStream,"lost, pos_dev = %f\n", pos_dev);
    }
    // if ball very close to us, look at ball
    else if (ball_close_to_us) {
      (*track_objects)(features,command,false,true);
      //pprintf(TextOutputStream,"ball close, dist = %f\n", features->ball_vector.length());
    }
    // if ball coming toward you, look at ball
    else if (ball_approaching) {      
      (*track_objects)(features,command,false,true);
      //pprintf(TextOutputStream,"ball approaching, app vel = %f\n", features->ball_trj.approach_vel);
    }
    // look at markers if stuck
    else if (stuck) {
      (*track_objects)(features,command,true,false);
    }
    else {
      // look at both markers and ball
      (*track_objects)(features,command,true,true);
    }
  }

  bool done=false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case WAITING: {      
      if (fsm.isNewState()) {
        pprintf(TextOutputStream, "wait\n");
	features->team_msg_mgr->goalie_clearing = false;
      }
      
      if (shouldBlock(features)) {
        TRANS_CONT(fsm,BLOCKING,5,"BlockShot");
      }
      else if (shouldClear(features, false)) {
        TRANS_CONT(fsm,APPROACH,6,"BallClose");
      }
      else if (!near_target_sloppy) {
        TRANS_CONT(fsm,POSITIONING,7,"OutOfPosition");
      }
      
      if (false) {
      //if (ball_approaching && fabs(intercept_pos_ego.y) > 150.0) {
	command->motion_cmd = 
          (intercept_pos_ego.y > 0.0) ? MOTION_STAND_RIGHT : MOTION_STAND_LEFT;
      }
      else {
	command->motion_cmd = MOTION_STAND_NEUTRAL;
      }
    
      done = true;
      break;
    }
      
    case POSITIONING: {
      if (fsm.isNewState()) {
        pprintf(TextOutputStream, "positioning\n");
	features->team_msg_mgr->goalie_clearing = false;
      }
      
      if (shouldBlock(features)) {
        TRANS_CONT(fsm,BLOCKING,5,"BlockShot");
      }        
      else if (shouldClear(features, false)) {
        TRANS_CONT(fsm,APPROACH,6,"ShouldClear");
      }
      else if (near_target_tight){
        TRANS_CONT(fsm,WAITING,7,"InPosition");
      }

      command->motion_cmd = MOTION_WALK_TROT;      
      command->vx = target_loc_ego.x;
      command->vy = target_loc_ego.y;
      command->va = target_angle_ego;
     
      done = true;
      break;      
    }

    case RETURNING: {
      if (fsm.isNewState()) {
        pprintf(TextOutputStream, "returning\n");
	features->team_msg_mgr->goalie_clearing = false;
      }

      if (shouldBlock(features)) {
        TRANS_CONT(fsm,BLOCKING,5,"BlockShot");
      }     
      else if (shouldClear(features, false)) {
        TRANS_CONT(fsm,APPROACH,6,"ShouldClear");
      }
      // we use near_target_sloppy here and rely on POSITIONING to get
      // us the rest of the way back to the circle. Doing this allows
      // the goalie to dive again when it's reasonably close to where
      // it wants to be.
      else if (near_target_sloppy) {
        TRANS_CONT(fsm,POSITIONING,6,"GettingClose");
      }
      // otherwise we timeout after a while to ensure that we can
      // block or clear again if needed.
      else if (fsm.timeInState() > SecToTime(3.0)) {
        TRANS_CONT(fsm,POSITIONING,7,"ReturnTimeout");
      }

      goto_point->setWalk(MOTION_WALK_TROT);
      goto_point->setTarget(target_loc_world);
      goto_point->setHeadingEgo(target_angle_ego);
      (*goto_point)(features, command);
      
      done = true;
      break;
    }

    case BLOCKING: {
      // -1 if diving left, 1 if right, 0 if center
      static int dive_direction; 
      ball_been_far_recently = false;

      if (fsm.isNewState()) {
        pprintf(TextOutputStream, "block\n");
	features->team_msg_mgr->goalie_clearing = true;
      }
      if (fsm.timeInState() > SecToTime(0.2) && 
          features->motion_state != Motion::STATE_KICKING) {
        track_objects->reset(features->current_time);
        //(*track_objects)(features,command,false,true);
        TRANS_CONT(fsm,POSITIONING,5,"ReturningAfterBlock"); // RETURN
      }
      
      if (fsm.isNewState()) {
        // We don't currently use the center dive at all. I don't like
        // it because sometimes we do it when we should have done one
        // of the other two dives, and end up missing the ball as it
        // slips past our barely-outstretched arms.

        if (fabs(intercept_pos_ego.y) < 0.0) {
          dive_direction = 0;
          command->motion_cmd = MOTION_BLOCK_GOALIE_C;
        }
        else if (intercept_pos_ego.y > 0.0) {
          dive_direction = -1;
          command->motion_cmd = MOTION_BLOCK_GOALIE_L;
        }
        else if (intercept_pos_ego.y < 0.0) {
          dive_direction = 1;
          command->motion_cmd = MOTION_BLOCK_GOALIE_R;
        }
      }
      else { // not a new FSM state
        // go down with head sideways
        if (fsm.timeInState() < SecToTime(0.8)) {
          command->head_cmd   = HEAD_ANGLES;
          command->head_tilt  = ers7_constants.head_tilt_min;
          command->head_tilt2 = 0; //ers7_constants.head_tilt2_min;
          command->head_pan   = ers7_constants.head_pan_min * dive_direction;
        }
        // move head up for sweep
        else if (fsm.timeInState() < SecToTime(1.0)) {
          command->head_cmd   = HEAD_ANGLES;
          command->head_tilt  = RAD(-35);
          command->head_tilt2 = 0; //ers7_constants.head_tilt2_min;
          command->head_pan   = ers7_constants.head_pan_min * dive_direction;
        }
        // sweep from one side to the other
        else if (fsm.timeInState() < SecToTime(1.5)) {
          command->head_cmd   = HEAD_ANGLES;
          command->head_tilt  = RAD(-35);//ers7_constants.head_tilt_min;
          command->head_tilt2 = 0; //ers7_constants.head_tilt2_min;
          command->head_pan   = ers7_constants.head_pan_max * dive_direction;
        }
        // look for ball
        else {
          (*track_objects)(features,command,false,true);
        }
      }
      done = true;
      break;           
    }

    case APPROACH: {
      if (fsm.isNewState()) {
        pprintf(TextOutputStream, "approach\n");
	features->team_msg_mgr->goalie_clearing = true;
        approach->reset(features->current_time);
        approach->setApproachType(BeApproachBall::GOALIE,
                                  BeApproachBall::CENTER,
                                  0);
      }

      //if (false) {
      if (fsm.timeInState() < SecToTime(0.5) &&
          shouldBlock(features)) {
        approach->sleep();
        TRANS_CONT(fsm,BLOCKING,5,"BlockShot");
      }
      else if (fsm.timeInState() > SecToTime(5.0)) {
        pprintf(TextOutputStream,"clear timed out\n");
        approach->sleep();
        approach_last_failed = features->current_time;
        TRANS_CONT(fsm,POSITIONING,6,"ClearTimeout"); // RETURN
      }
      // if the ball is far away from the goal, and we should be in
      // the goal then return
      else if(!shouldClear(features, true)) {
        approach->sleep();
        TRANS_CONT(fsm,POSITIONING,7,"BallCleared");
      }      
      // if we've lost track of the ball, return
      else if (features->current_time -  features->ball_last_seen_med > 
               SecToTime(1.0)) {
        approach->sleep();
        TRANS_CONT(fsm,POSITIONING,8,"BallLost");
      }

      // call approach
      double approach_retval = (*approach)(features, command);

      if(approach_retval < 0.0) {
        approach_last_failed = features->current_time;
        if (!near_target_sloppy) {
          TRANS_CONT(fsm, POSITIONING, 11, "ApproachFailed"); // RETURN
        }
        else {
          TRANS_CONT(fsm, POSITIONING, 12, "ApproachFailed");
        }
      }
      // stand in place for a little bit to gain control of the ball
      else if(approach_retval > 0.0){
        approach->sleep();
        TRANS_CONT(fsm,STAND,10,"have ball");
      }

      done = true;
      break;
    }
    case STAND: 
      //have to stand before turning to switch parameters
      command->motion_cmd = MOTION_STAND_NEUTRAL;
      command->head_cmd = HEAD_ANGLES;
      command->head_tilt = -1.1;
      command->head_pan  = 0.0;
      command->head_tilt2 = 0.78;

      if(fsm.timeInState() > 10000){
	TRANS_CONT(fsm,AIM,5,"done standing");
      }

      done = true;
      break;

    case AIM: {
      static int turn_direction; // -1 left, 1 right
      static ulong front_blocked_time;
      static bool facing_score_goal;

      if(fsm.isNewState()){
        pprintf(TextOutputStream, "aim\n");
        turn->reset(features->current_time);
	features->team_msg_mgr->goalie_clearing = true;
      }

      if (fsm.isNewState()) {
        turn_direction = sign(features->goal_angle) > 0 ? -1 : 1;
        front_blocked_time = features->current_time;
        facing_score_goal = false;
      }

      facing_score_goal = fabs(features->goal_angle) < RAD(30.0);

      bool turnWithBall = false;

      if (turnWithBall) {
        if (features->goal_angle > RAD(30.0)) {
          turn_direction = -1;
        }
        if (features->goal_angle < RAD(-30.0)) {
          turn_direction = 1;
        }
        
        if (turn_direction == -1) {
          command->motion_cmd = MOTION_TURN_WITH_BALL_L;
        }
        else {
          command->motion_cmd = MOTION_TURN_WITH_BALL_R;
        }

        if (!frontIsClear(features)) {
          front_blocked_time = features->current_time;
        }

        bool front_clear = 
          features->current_time - front_blocked_time > SecToTime(0.15);
        
        if (!front_clear) {
          command->setBinLEDs(LED_EARS_RED);
        }

        if (fabs(features->goal_angle) < RAD(30.0) && front_clear) {
          TRANS_CONT(fsm, HOLD_BALL, 5, "good angle for clear");
        }
      
        if (features->see_ball_med) {
          approach_last_failed = features->current_time;
          TRANS_CONT(fsm, POSITIONING, 8, "don't have the ball");
        }

        if (fsm.timeInState() > SecToTime(2.0)) { 
          if (facing_score_goal) {
            TRANS_CONT(fsm, HOLD_BALL, 6, "timed out, facing score_goal");
          }
          else {
            approach_last_failed = features->current_time;
            TRANS_CONT(fsm, POSITIONING, 7, "timed out, not facing score_goal");
          }
        }
      }
      else {
        //make sure we are facing away from our goal.  Must check the
        //defense goal since we have a chance of seeing it
        double retval = turn->turnBallToAngle(features, 
                                              command, 
                                              features->score_goal_const.angle(),
                                              RAD(70.0));
        if (retval == 0.0) {
          TRANS_CONT(fsm,HOLD_BALL,9,"pull ball closer");
        }
        if (retval == -1.0) {
          approach_last_failed = features->current_time;
          TRANS_CONT(fsm,POSITIONING,10,"lost ball in turn");
        }
      }
      
      done = true;
      break;
    }

    case HOLD_BALL:
      if (fsm.isNewState()) {
        pprintf(TextOutputStream, "holdball\n");
	features->team_msg_mgr->goalie_clearing = true;
      }

      command->motion_cmd = MOTION_STAND_NEUTRAL;
      command->head_cmd = HEAD_ANGLES;
      command->head_tilt = -1.3;
      command->head_pan  = 0.0;
      command->head_tilt2 = 0.78;

      // wait for 500ms to ensure that the ball is held well
      if(fsm.timeInState() >= SecToTime(0.0) &&
         fsm.timeInState() <= SecToTime(0.3)) {
        // Decide which kick to use - we want the leg closest to the
        // goal to be the one that gets stuck out. Strangely, DIVE_L
        // usually sticks out the right arm, and vice versa.
        if (features->defend_goal_color == DEFEND_YELLOW_GOAL) {
          if (features->my_position.heading.mean.angle() < 0) {
            command->motion_cmd = MOTION_KICK_FOREWARD; // DIVE L
          }
          else {
            command->motion_cmd = MOTION_KICK_FOREWARD;
          }
        }
        else {
          if (features->my_position.heading.mean.angle() < 0) {
            command->motion_cmd = MOTION_KICK_FOREWARD; // R
          }
          else {
            command->motion_cmd = MOTION_KICK_FOREWARD;//L;
          }
        }
      }

      // wait until the kick is done, then transition
      if (fsm.timeInState() > SecToTime(0.5) && 
          features->motion_state != Motion::STATE_KICKING) {
        track_objects->reset(features->current_time);
        //(*track_objects)(features,command,false,true);
        TRANS_CONT(fsm,POSITIONING,5,"ReturnAfterDive"); // RETURN
      }

      done = true;
      break;
    }
  }
  
  //pprintf(TextOutputStream,"end of goalie\n");

  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();

  return 1.0;
}

uchar *Goalie::encodeAllNames(ulong time,uchar *buf,int buf_size)
{
  uchar *bufp;
  
  bufp = 
    BehaviorEncodeNames::encodeAllNames<FSM,State>(buf,
						   beh_id,
						   beh_name,
						   NumStates,
						   state_names,
						   &fsm);
  
  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,2);
  bufp = track_objects->encodeAllNames(time,bufp,buf_size - (bufp - buf));
  bufp = pri_attacker ->encodeAllNames(time,bufp,buf_size - (bufp - buf));
  
  return bufp;
}

uchar *Goalie::encodeTrace(uchar *buf,int buf_size)
{
  uchar *bufp;
  
  bufp = BehaviorEncodeTrace::encodeTrace<FSM,State>(buf,beh_id,&fsm);
  
  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,2);
  bufp = track_objects->encodeTrace(bufp,buf_size - (bufp - buf));
  bufp = pri_attacker ->encodeTrace(bufp,buf_size - (bufp - buf));
  
  return bufp;
}

Goalie *Keeper=NULL;

bool Goalie::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *ep_ptr;

  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  ep_ptr = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(ep_ptr);

  sensor_id = event_mgr->getEventProcessorId("SensorData");
  ep_ptr = event_mgr->listenEventProcessor(beh_name, sensor_id);
  sd = (SensorData *)(ep_ptr);

  return true;
}

bool Goalie::setupEventMgr(){
  return true;
};

// The behavior system will call this function to find out
// what motions the robot should execute this frame.
const MotionCommand *Goalie::get(ulong time)
{
  FeatureSet *lfs=NULL; // local feature set

  mzero(out_command);

  if(fs!=NULL && sd!=NULL){
    lfs = fs->get(time);

    (*this)(lfs, &out_command);
  }
  
  return &out_command;
}


void
BehaviorSystem::goalie(FeatureSet *features) {
  static Goalie *goalie = NULL;
  if(goalie==NULL)
    goalie = new Goalie();
  (*goalie)(features,command);
}

REGISTER_EVENT_PROCESSOR(Goalie,Goalie::beh_name,Goalie::create);