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

// TODO
//
// avoid rect between the primary and ball

#include "BehaviorPacketEncoder.h"
#include "BeSupportingAttacker.h"

#include <math.h>
#include "../headers/Geometry.h"
#include "../headers/Reporting.h"

#include "state_machine.h"

char const * const BeSupportingAttacker::beh_name = "BeSupportingAttacker";

char const * const BeSupportingAttacker::state_names[BeSupportingAttacker::NumStates] = {
  "POSITION", "STAND", "CATCH", "ATTACK"};

BeSupportingAttacker::BeSupportingAttacker()
{
  fsm.init(state_names,NumStates,POSITION,16,16);

  track_objects = new BeTrackObjects();
  goto_point = new BeGotoPointGlobal();
  navto_point = new BeNavToPointGlobal();
  primary_attacker = new BePrimaryAttacker();
  //need to setupEventMgr since we're using the ()operator
  primary_attacker->setupEventMgr();
  be_behind_ball = false;
  side_to_wait_on = 1;
  last_catch_time = 0;
  avoiding_defense_box = false;
  avoiding_opponent_defense_box = false;
  avoiding_obstacles = true;
  facing_ball = false;
  last_avoiding_primary = 0;
  far_from_target = false;
  last_primary_avoid_direction = 1;
  last_role = RoleCommand::COM_DEF_SUPP;
  role_change_time = 0UL;
}

BeSupportingAttacker::~BeSupportingAttacker()
{
  if(track_objects!=NULL) {
    delete track_objects;
    track_objects = NULL;
  }
}

void
BeSupportingAttacker::reset(ulong timestamp)
{
  fsm.setState(POSITION,0,"Reset",timestamp);
  goto_point->reset(timestamp);
  navto_point->reset(timestamp);
  primary_attacker->reset(timestamp);
  track_objects->reset(timestamp);
  avoiding_defense_box = false;
  avoiding_opponent_defense_box = false;
  avoiding_obstacles = true;
  far_from_target = false;
  facing_ball = false;
  be_behind_ball = false;
  last_role = RoleCommand::COM_DEF_SUPP;
  role_change_time = timestamp;
}

bool BeSupportingAttacker::primaryInScrum(FeatureSet* features) {
  return false; // FIXME: disabled, this function has never really
                // worked

  static ulong start_scrum_time = 0UL;
  static double start_scrum_xpos = 0.0;

  vector2d my_pos = features->my_position.position.mean;
  vector2d primary_pos = features->team_msg_mgr->getPrimaryLoc();
  // here we don't use teammate balls just because, if there really
  // is a scrum, the teammate estimates from the primary are really
  // far off from reality.
  vector2d ball = features->world_ball_vector;

  double scrum_timeout = SecToTime(10.0);
  double near_edge = 200.0;
  double scrum_no_progress_distance = 500.0;
  double primary_near_ball_distance = 500000.0; // FIXME

  // if the ball has moved a ways since starting, don't call it a
  // scrum
  // if the ball isn't close to the edge, don't call it a scrum
  if ((start_scrum_time != 0UL && 
       fabs(ball.x - start_scrum_xpos) > scrum_no_progress_distance) ||
      (fabs(ball.y) < halfWidth - near_edge)) {
    pprintf(TextOutputStream,"not in scrum\n");
    start_scrum_time = 0UL;
    start_scrum_xpos = 0.0;
    return false;
  }
  // otherwise we're in a scrum
  else {
    if (start_scrum_time == 0UL) {
      pprintf(TextOutputStream,"just starting scrum... ");

      // if we're starting, the primary also has to be near the ball
      if (GVector::distance(ball, primary_pos) < primary_near_ball_distance) {
        pprintf(TextOutputStream,"and primary near ball\n");
        start_scrum_time = features->current_time;
        start_scrum_xpos = ball.x;
      }
      else {
        pprintf(TextOutputStream,"but primary not near ball\n");
        return false;
      }
    }

    if (features->current_time - start_scrum_time > scrum_timeout) {
      pprintf(TextOutputStream,"in scrum for real\n");
      return true;
    }
    pprintf(TextOutputStream,"in scrum, but not for long enough yet\n");
    return false;
  }
}

bool BeSupportingAttacker::shouldBeOpportunistic(FeatureSet* features) {
  static ulong last_opportunistic_time = 0UL;
  if (features->current_time - last_opportunistic_time < SecToTime(15.0)) {
    return false;
  }
  if (!features->see_ball_med) {
    return false;
  }
  if (features->isBallInDefenseBox()) {
    return false;
  }

  vector2d my_pos = features->my_position.position.mean;
  vector2d primary_pos = features->team_msg_mgr->getPrimaryLoc();
  vector2d ball = getBallPosn(features);

  if (fabs(features->getRelativePosn(ball).angle()) > RAD(30)) {
    return false;
  }

  // be opportunistic if we're 1.5x closer to the ball than primary,
  // and if the distance between us and primary is at least 250mm.
  if (GVector::distance(my_pos, ball) * 1.5 < 
      GVector::distance(primary_pos, ball) &&
      GVector::distance(my_pos, primary_pos) > 250.0 &&
      GVector::distance(my_pos, ball) > 500.0) {
    last_opportunistic_time = features->current_time;
    return true;
  }
  return false;
}

double gety(vector2d primary, vector2d goal_point, double xpos) {
  double dx = primary.x - goal_point.x;
  double frac = (primary.x - xpos) / dx;
  return (primary.y - goal_point.y) * frac;
}

int BeSupportingAttacker::calcPrimaryAvoidanceSide(FeatureSet *features) {
  double width = 600.0;
  
  bool avoiding_recently = 
    features->current_time - last_avoiding_primary < SecToTime(3.0);

  if (avoiding_recently) {
    width = 800.0;
  }

  double score_sign = sign(features->score_goal_const.x);
  vector2d primary = features->team_msg_mgr->getPrimaryLoc();
  // pretend that the primary is further back than he is, so that we
  // avoid him in a wider berth
  primary.x -= 500 * score_sign;
  vector2d primaryleft(primary.x + 300, primary.y);
  vector2d primaryright(primary.x - 300, primary.y);
  vector2d center = features->score_goal_const;
  vector2d left(center.x, center.y + width); //* sign(center.x));
  vector2d right(center.x, center.y - width); // * sign(center.x));
  vector2d me = features->my_position.position.mean;
  
  // if we're further back than the primary, don't avoid
  if (me.x * score_sign < primary.x * score_sign) {
    return 0;
  }

  double ly = gety(primaryleft, left, me.x);
  //double cy = gety(primary, center, me.x);
  double ry = gety(primaryright, right, me.x);        

  int direction = 0;
  // if we're in the cone, flee!
  if (ly <= me.y && me.y <= ry) {
    if (fabs(primary.y) > 500 && avoiding_recently) {
      if (primary.y > 0) {
        direction = -1;
      }
      else {
        direction = 1;
      }
    }
    else {
      direction = last_primary_avoid_direction;
    }
  }

  // if the primary isn't in the front half of the field, we will
  // happily cross between him and the goal.
  // FIXME: removed this
//   if (fabs(primary.x - features->score_goal_const.x) > halfLength) {
//     direction = 0;
//   }

  if (direction != 0) {
    last_primary_avoid_direction = direction;
  }
  if (direction != 0) {
    last_avoiding_primary = features->current_time;
  }
  return direction;
}

vector2d BeSupportingAttacker::getBallPosn(FeatureSet *features) {
  vector2d ball;
  features->getBestBallPosn(ball);
  return ball;
}

int BeSupportingAttacker::getSupporterRole(FeatureSet* features) {
  int supporter_role = features->team_msg_mgr->getSupporterRole();
  if (supporter_role != last_role) {
    role_change_time = features->current_time;
  }
  last_role = supporter_role;
  return supporter_role;
}

vector2d BeSupportingAttacker::calcTargetForRole(FeatureSet* features) {
  switch (getSupporterRole(features)) {
    case RoleCommand::COM_DEF_SUPP: {
      return getDefensePosn(features);
      break;
    }
      
    case RoleCommand::COM_OFF_SUPP:
    case RoleCommand::COM_OFF_SUPP_LONG: {
      return getOffensePosn(features);
      break;
    }
      
    default:
      pprintf(TextOutputStream, "BUG - UNKNOWN SUPPORTER ROLE\n");
      return vector2d(0,0);
  }
}

vector2d BeSupportingAttacker::calcTargetForScrum
(FeatureSet* features, vector2d target, MotionCommand *command) {
  if (!primaryInScrum(features)) {
    return target;
  }

  vector2d my_pos = features->my_position.position.mean;
  vector2d ball;
  features->getBestBallPosn(ball);
  vector2d primary_pos = features->team_msg_mgr->getPrimaryLoc();

  // if scrum very close to defend goal, don't do anything special
  if (fabs(ball.x - features->defend_goal_const.x) < 1000) {
    return target;
  }

  target.x = (ball.x - features->defend_goal_const.x) / 2.0 + 
    features->defend_goal_const.x;
  target.y = halfWidth * sign(ball.y); 
  return target;
}

vector2d BeSupportingAttacker::calcTargetAvoidingTeammates
(FeatureSet* features, vector2d target, MotionCommand *command) {

  static ulong start_time_stuck = 0UL;

  // FIXME: disabled this
  return target;

  // defensive supporter always needs to get into position, to avoid
  // breakaways. 
  if (getSupporterRole(features) == RoleCommand::COM_DEF_SUPP) {
    return target;
  }

  vector2d my_pos = features->my_position.position.mean;  

  // If near the primary, just stand still
  if (features->team_msg_mgr->getPrimaryDist() < 250.0) {
    return my_pos;
  }

  // If the other supporter is near us, and we're in the right half of
  // the field, stand still.
  vector2d supp_pos = features->team_msg_mgr->getOtherSuppLoc();
  vector2d supp_pos_rel = features->getRelativePosn(supp_pos);

  // FIXME: stuck detection currently disabled
  if (features->stuck_info.fraction_stuck <= 1.0){
    start_time_stuck = features->current_time;
  }

  if (supp_pos_rel.length() < 250.0 || 
      features->current_time - start_time_stuck > SecToTime(2.0)) {
    command->setBinLEDs(LED_EARS_PURPLE);
    switch (getSupporterRole(features)) {
    case RoleCommand::COM_OFF_SUPP:
    case RoleCommand::COM_OFF_SUPP_LONG:
      if (fabs(features->score_goal_const.x - my_pos.x) < halfLength + 500) {
        return my_pos;
      }
      break;

    case RoleCommand::COM_DEF_SUPP:
      if (fabs(features->defend_goal_const.x - my_pos.x) < halfLength + 500) {
        return my_pos;
      }
      break;

    default:
      pprintf(TextOutputStream, "BUG - UNKNOWN SUPPORTER ROLE\n");
      break;
    }
    return my_pos;
  }

//   // if we're near a teammate, and in our zone, just stand still
//   vector2d nearest_teammate(0,0); // global coords
//   double nearest_teammate_dist = 1e10;
//   int nearest_teammate_id = -1;
//   for (int i = 1; i <= 3; i++) {
//     vector2d teammate_pos = features->team_msg_mgr->getTeammatePosn(i).mean;
//     if (GVector::distance(teammate_pos, vector2d(0,0)) > 1.0) {
//       double dist = GVector::distance(teammate_pos, my_pos);
//       if (dist < nearest_teammate_dist) {
//         nearest_teammate_dist = dist;
//         nearest_teammate = teammate_pos;
//       }
//     }
//   }

//   if (nearest_teammate_dist < 250.0) {
//     avoiding_teammate = true;
//   }
//   if (nearest_teammate_dist > 500.0) {
//     avoiding_teammate = false;
//   }
  
//   // FIXME: got rid of this, either get rid of dead code or add it
//   // back in
//     avoiding_teammate = false;
  
//     if (avoiding_teammate) {
//       //command->addBinLEDs(LED_EARS_RED);
//       vector2d teammate_local = features->getRelativePosn(nearest_teammate);
//       target = my_pos + -teammate_local.norm() * 1000.0;
//     }

  // repulsion idea: if I actually were at my target, in what
  // direction (and how far) would I move to keep away from teammates?
  
//   vector2d repulsion(0,0);
//
//   for (int i = 1; i <= 3; i++) {
//     vector2d teammate = features->team_msg_mgr->getTeammatePosn(i).mean;
//     // We need to check and make sure it's valid data - we may
//     // not be getting network messages from this teammate.
//     if (GVector::distance(teammate, vector2d(0,0)) > 1.0) {
//       vector2d teammate_rel = teammate - target;
//       double target_to_teammate_angle = norm_angle(teammate_rel.angle());
//       double repulsion_angle = norm_angle(target_to_teammate_angle + M_PI);
//       double dist = teammate_rel.length();
//       double repulsion_weight = 0.0;
//       if (dist < 250) { // FIXME magic num
//         repulsion_weight = 1.0;
//       }
//       else if (dist < 1250) { // FIXME magic num
//         repulsion_weight = 1.0 - (dist - 250) / 1000;
//       }
//       vector2d repulsion_component(repulsion_weight, 0);
//       repulsion_component.rotate(repulsion_angle);
//       repulsion += repulsion_component;
//     }
//   }
  
//   return target + repulsion * 500.0; // FIXME magic num
  return target;
}

vector2d BeSupportingAttacker::calcTargetAvoidingPrimary
(FeatureSet *features, vector2d target) {
  vector2d my_pos = features->my_position.position.mean;  
  // if we're between the primary and the goal, get out of the way!
  int avoidance_dir = calcPrimaryAvoidanceSide(features);
  if (avoidance_dir != 0 && 
      getSupporterRole(features) != RoleCommand::COM_DEF_SUPP) {
    target.x = my_pos.x;
    target.y = my_pos.y + halfWidth * avoidance_dir;   
  }
  return target;
}

vector2d BeSupportingAttacker::calcTargetAvoidingDefenseBox
(FeatureSet *features, vector2d target) {
  vector2d my_pos = features->my_position.position.mean;  
  // if we're going to enter the defense box... don't.
  if (fabs(my_pos.x - features->defend_goal_const.x) < 750.0 && 
      fabs(my_pos.y) < 750.0) {
    avoiding_defense_box = true;
  }

  if (fabs(my_pos.x - features->defend_goal_const.x) > 800.0 || 
      fabs(my_pos.y) > 800.0) {
    avoiding_defense_box = false;
  }

  if (avoiding_defense_box) {
    // if we want to go somewhere inside the rectangle, set our target
    // x to be outside the rectangle.
    target.x = features->defend_goal_const.x - 
      850.0 * sign(features->defend_goal_const.x);
    target.y = my_pos.y + 100.0 * sign(target.y - my_pos.y); 
  }
  return target;
}

vector2d BeSupportingAttacker::calcTargetAvoidingOpponentDefenseBox
(FeatureSet *features, vector2d target) {
  vector2d my_pos = features->my_position.position.mean;  
  // if we're going to enter the opponent's defense box... don't. We
  // can get called for goalie pushing even if we're just standing
  // nearish their goalie.
  if (fabs(my_pos.x - features->score_goal_const.x) < 750.0 && 
      fabs(my_pos.y) < 750.0) {
    avoiding_opponent_defense_box = true;
  }

  if (fabs(my_pos.x - features->score_goal_const.x) > 800.0 || 
      fabs(my_pos.y) > 800.0) {
    avoiding_opponent_defense_box = false;
  }

  if (avoiding_opponent_defense_box) {
    // if we want to go somewhere inside the rectangle, set our target
    // x to be outside the rectangle.
    target.x = features->score_goal_const.x - 
      850.0 * sign(features->score_goal_const.x);
    target.y = my_pos.y + 100.0 * sign(target.y - my_pos.y); 
  }
  return target;
}

void BeSupportingAttacker::configureWalks(FeatureSet* features, 
                                          vector2d target) {
  vector2d my_pos = features->my_position.position.mean;
  vector2d ball = getBallPosn(features);
  //bool avoiding_primary_recently = 
  //  features->current_time - last_avoiding_primary < SecToTime(1.0);
  // FIXME there may be more cases where we want to avoid obstacles
  //avoiding_obstacles = far_from_target;
  avoiding_obstacles = false;

  goto_point->setTarget(target);
  navto_point->setTargetGlobal(target);

  // If we're avoiding primary, face the location we want to go to,
  // not the ball. This helps us get there faster.
  // FIXME: removed this
  if (false) {
  //if (avoiding_primary_recently) {
    // angle to target, global
    double delta = norm_angle((target - my_pos).angle());
    goto_point->setWalk(MOTION_WALK_TROT_FAST);
    goto_point->setHeadingGlobal(delta);
  }
  // otherwise if we need to walk fast, either face straight toward our
  // target or straight away from our target
  //else if (far_from_target) {
  else if (false) { // FIXME
    // angle to target, global
    double delta = norm_angle((target - my_pos).angle());
    // angle away from target
    double negdelta = norm_angle(delta + M_PI);
    // angle to ball, global
    double ball_angle = norm_angle((ball - my_pos).angle());
    // use the angle away from target if it's closer to our current
    // heading
    goto_point->setWalk(MOTION_WALK_TROT_FAST);
    if (fabs(norm_angle(ball_angle - delta)) > RAD(95)) {
      goto_point->setHeadingGlobal(negdelta);
    }
    else if (fabs(norm_angle(ball_angle - delta)) < RAD(85)) {
      goto_point->setHeadingGlobal(delta);
    }
    else {
      goto_point->setHeadingGlobal(ball_angle);
    }
  }
  // otherwise face the ball
  else {
    vector2d delta = ball - my_pos;
    goto_point->setWalk(MOTION_WALK_TROT_FAST);
    goto_point->setHeadingGlobal(norm_angle(delta.angle()));
  }
}

double
BeSupportingAttacker::operator()(FeatureSet *features,
				 MotionCommand *command)
{ 
  static EventTimeReporter reporter("supporter",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  bool done = false;
  // The supporter will choose to track the ball if the ball is moving
  // at least this fast.
  static const double ball_fixate_vel = 150.0;

  // We wait at least this long between successive attempts at
  // catching.
  double catch_delay = SecToTime(3.0);
 
  const bool block = true; //turn blocking on/off

  vector2d my_pos = features->my_position.position.mean;
  //double my_heading = features->my_position.heading.mean.angle();
  vector2d primary_pos = features->team_msg_mgr->getPrimaryLoc();
  //bool in_front_of_primary = 
  //  my_pos.x * score_sign > primary_pos.x * score_sign;

  // Calculate a position to go to. Both the target and the ball are
  // in global coordinates.
  vector2d ball = getBallPosn(features);

  vector2d target = calcTargetForRole(features);
  //target = calcTargetForScrum(features, target, command);
  //target = calcTargetAvoidingTeammates(features, target, command);
  target = calcTargetAvoidingPrimary(features, target);
  target = calcTargetAvoidingDefenseBox(features, target);
  target = calcTargetAvoidingOpponentDefenseBox(features, target);

  // show the ball and target in the GUI
  if (getSupporterRole(features) == RoleCommand::COM_DEF_SUPP) {
    features->world_model->sendWMDebug(ball.x, ball.y, 0, 0x4D, 0x00, 0x88);
  }
  else {
    features->world_model->sendWMDebug(ball.x, ball.y, 0, 0x83, 0x4C, 0xAC);
  }
  features->world_model->sendWMDebug(target.x, target.y, 0, 0x00, 0xFF, 0x00);


  // DO NOT EDIT TARGET BELOW THIS POINT.
  // We precompute a bunch of values based on the target below, and
  // changing the target afterwards would make all the values wrong.

  // we only want to avoid obstacles if:
  // we are far from our target location
  // both the target location and the ball are roughly in front of us
  double distance_from_target = GVector::distance(my_pos, target);
  vector2d target_delta = target - my_pos;
  //double target_da = norm_angle(target_delta.angle() - my_heading);
  bool near_target_sloppy = distance_from_target < 300.0;
  bool near_target_tight = distance_from_target < 150.0;
  double ball_da = features->getRelativePosn(ball).angle();
 
  if (fabs(ball_da) > RAD(20)) {
    facing_ball = false;
  }
  if (fabs(ball_da) < RAD(10)) {
    facing_ball = true;
  }

  if (distance_from_target > 1000) {
    far_from_target = true;
  }
  if (distance_from_target < 750) {
    far_from_target = false;
  }
  
  configureWalks(features, target);

  bool should_block_if_possible = features->shouldInterceptIfPossible();

  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0) {
    switch(fsm.getState()) {
    case POSITION:   
      // check for transition to catching state
      if (block && 
          last_catch_time + catch_delay < features->current_time) {
        if (should_block_if_possible &&
            features->canInterceptBall(MOTION_BLOCK_SUPP_L)) {
          TRANS_CONT(fsm, CATCH, 6, "blocking");
        }
      }
      
      if (shouldBeOpportunistic(features)) {
        TRANS_CONT(fsm, ATTACK, 8, "opportunistic");
      }

      // check for transition to standing state
      if (near_target_tight) {
        TRANS_CONT(fsm, STAND, 7, "standing");
      }

      if (avoiding_obstacles) {
       (*navto_point)(features, command);
      }
      else {
        (*goto_point)(features, command);
      }
      done = true;
      break;
      
    case STAND: {

      // check for transition to catching state
      if (block && 
          last_catch_time + catch_delay < features->current_time) {
        if (features->teammate_passed_recently &&
            features->canInterceptBallSloppy(MOTION_BLOCK_SUPP_L)) {
          TRANS_CONT(fsm, CATCH, 5, "blocking");
        }
        if (should_block_if_possible &&
            features->canInterceptBall(MOTION_BLOCK_SUPP_L)) {
          TRANS_CONT(fsm, CATCH, 6, "blocking");
        }
      }

      if (shouldBeOpportunistic(features)) {
        TRANS_CONT(fsm, ATTACK, 8, "opportunistic");
      }

      // check for transition to positioning state
      if (!near_target_sloppy) {
        TRANS_CONT(fsm, POSITION, 5, "not near target");
      }

      // rotate if needed
      if (facing_ball) {
        command->motion_cmd = MOTION_STAND_NEUTRAL;
      }
      else {
        command->motion_cmd = MOTION_WALK_TROT;
        command->vx = 0.0;
        command->vy = 0.0;
        command->va = ball_da * 2;
      }
      done = true;
      break;
    }

    case CATCH: {
      if (fsm.isNewState()) {
        last_catch_time = features->current_time;
      }
      else if (fsm.timeInState() > SecToTime(0.2) &&
               features->motion_state != Motion::STATE_KICKING) {
        TRANS_CONT(fsm, POSITION, 5, "completed catch");
      }
      
      std::pair<vector2d, double> ballIntercept = 
        features->world_model->getBallIntercept(10);
      vector2d interceptPos = ballIntercept.first;
      double yint = interceptPos.y;
      
      if (fabs(yint) < 100.0) {
        command->motion_cmd = MOTION_BLOCK_SUPP;
      } 
      else if (yint > 0.0) {
        command->motion_cmd = MOTION_BLOCK_SUPP_L;
      }
      else if (yint < 0.0) {
        command->motion_cmd = MOTION_BLOCK_SUPP_R;
      }
      
      done = true;
      break;
    }

    case ATTACK: 
      {
        double attacker_timeout = SecToTime(3.0);
        
        if (fsm.isNewState()) {
          primary_attacker->reset(features->current_time);
        }
        if (features->current_time - features->ball_last_seen_med > 
                 SecToTime(0.5)) {
          primary_attacker->sleep();
          TRANS_CONT(fsm, POSITION, 5, "lost ball");
        }
        if (fsm.timeInState() > attacker_timeout) {
          primary_attacker->sleep();
          TRANS_CONT(fsm, POSITION, 6, "attacker timed out");
        }
        
        (*primary_attacker)(features, command);
                
        command->setBinLEDs(LED_EARS_RED);

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

  
  if(features->ball_trj.velocity.length() > ball_fixate_vel) {
    //track the ball if it's moving quickly
    (*track_objects)(features,command,false,true);
  }else {
    // track ball and markers
    (*track_objects)(features, command);
  }
  

  // If our teammate just passed, look direct at where the ball ought
  // to be
  if (features->teammate_passed_recently) {
    if (features->saw_ball_recently) {
      (*track_objects)(features,command,false,true);
    }
    else{
      command->head_cmd = HEAD_SCAN_POINT;
      vector2d headtarget = getBallPosn(features);
      headtarget = features->getRelativePosn(headtarget);
      command->head_lookat.set(headtarget.x,headtarget.y,0.0);
    }
  }

  return calcActivation(features);
}

double BeSupportingAttacker::calcActivation(FeatureSet *features)
{
  // Is the ball in the defense box?
  if(features->isBallInDefenseBox())
    return 1.0;
  
  if(!features->team_msg_mgr->isPrimaryAttacker())
    return 1.0;
  else
    return 0.0;
}

uchar *BeSupportingAttacker::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,1);
  bufp = track_objects->encodeAllNames(time,bufp,buf_size - (bufp - buf));

  return bufp;
}

uchar *BeSupportingAttacker::encodeTrace(uchar *buf,int buf_size)
{
  uchar *bufp;

  bufp = BehaviorEncodeTrace::encodeTrace<FSM,State>(buf,beh_id,&fsm);

  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,1);
  bufp = track_objects->encodeTrace(bufp,buf_size - (bufp - buf));

  return bufp;
}

vector2d BeSupportingAttacker::getDefensePosn(FeatureSet *features) {
  static bool primary_in_offensive_half = false;
  vector2d my_pos = features->my_position.position.mean;
  vector2d primary_pos = features->team_msg_mgr->getPrimaryLoc();
  //vector2d supporter_pos = features->team_msg_mgr->getOtherSuppLoc();
  
  double score_sign = sign(features->score_goal_const.x);
  
  if (fabs(primary_pos.x - features->score_goal_const.x) < halfLength + 800) {
    primary_in_offensive_half = true;
  }
  if (fabs(primary_pos.x - features->score_goal_const.x) > halfLength + 1200) {
    primary_in_offensive_half = false;
  }

  bool in_front_of_primary = 
    my_pos.x * score_sign > primary_pos.x * score_sign &&
    primary_in_offensive_half;

  // Calculate a position to go to. Both the target and the ball are
  // in global coordinates.
  vector2d target(0,0);
  
  vector2d ball = getBallPosn(features);
  
  double depth_clip = penaltyRegionLengthOffset - 300.0;
  double defensive_supporter_width = 450.0;

  if (primary_in_offensive_half) {
    // if in front of the primary attacker, walk around him first.
    // we try to squeeze between the primary and the far wall.
    if (in_front_of_primary) {
      target.x = my_pos.x - 500 * score_sign;
      if (fabs(primary_pos.y) > 500) {
        if (primary_pos.y > 0) {
          side_to_wait_on = -1;
        }
        else {
          side_to_wait_on = 1;
        }
      } else {
        side_to_wait_on = sign(my_pos.y);
      }
      target.y = (primary_pos.y + (side_to_wait_on * halfWidth)) / 2.0;
    } else { // primary in offensive half, not in front of primary
      // wait halfway between ball and the goal.
      target.x = (ball.x + features->defend_goal_const.x) / 2.0;
      target.y = ball.y / 2;

      // special case: if the primary's offensive, and the ball is far
      // to the side, and the ball is on the offensive side, go stand
      // next to the correct wall.
      if (fabs(ball.y) > halfWidth - 400.0 &&
          fabs(ball.x - features->score_goal_const.x) < halfLength) {
        target.y = (halfWidth - 200.0) * sign(ball.y);
      }
    }
    if (fabs(target.x - features->defend_goal_const.x) > halfLength - 750) {
      target.x = -750 * score_sign;
    }
  
    if (fabs(target.x) > depth_clip) {
      target.x = depth_clip * sign(target.x);
    }
  }
  else {
    // side_to_wait_on is the same side of where the primary is.
    // wait behind the primary and on the same side.
    if (primary_pos.y > defensive_supporter_width) {
      side_to_wait_on = 1;
    }
    else if (primary_pos.y < -defensive_supporter_width) {
      side_to_wait_on = -1;
    }
    else {
      side_to_wait_on = sign(my_pos.y);
    }
    target.x = primary_pos.x + sign(features->score_goal_const.x) * 300.0;
    target.y = defensive_supporter_width * side_to_wait_on;
  }

  // Weight the target x by the length of time we've been in this
  // role. If we've just changed roles, we prefer to be closer to the
  // middle
//   double weight =
//     ((double) (features->current_time - role_change_time)) / SecToTime(15.0);
//   if (weight > 1) {
//     weight = 1;
//   }
// FIXME: this is also disabled at the moment
  double weight = 1;

  target.x = 
    (target.x * weight) + 
    500.0 * sign(features->defend_goal_const.x) * (1-weight);

  // absolute final check: if we've not seen the ball for more than 5
  // seconds, return to a neutral point near our own goal, to prevent
  // possible breakaways.
  if (features->current_time - features->ball_last_seen_med > SecToTime(5.0)) {
    double xpos = penaltyRegionLengthOffset - 500;
    xpos *= sign(features->defend_goal_const.x);
    target.set(xpos, 0);
  }
  return target;
}

vector2d BeSupportingAttacker::getOffensePosn(FeatureSet *features) {
  vector2d my_pos = features->my_position.position.mean;
  vector2d primary_pos = features->team_msg_mgr->getPrimaryLoc();
  //vector2d supporter_pos = features->team_msg_mgr->getOtherSuppLoc();
  
  // Calculate a position to go to. Both the target and the ball are
  // in global coordinates.
  vector2d target(0,0);
  
  vector2d ball = getBallPosn(features);
  
  // offensive supporter picks a y-position based on the current
  // position of the ball, and an x-position partway between the
  // ball and score_goal.
  double offensive_supporter_width = 500.0;
  double depth_clip = penaltyRegionLengthOffset - 300;
  
  // if we're on defense, play in front of the ball.
  if (fabs(ball.x - features->defend_goal_const.x) < halfLength - 300.0) {
    be_behind_ball = false;
  }
  // if we're on offense, play behind the ball.
  if (fabs(ball.x - features->score_goal_const.x) < halfLength - 300.0) {
    be_behind_ball = true;
  }

  if (be_behind_ball) {
    target.x = ball.x + sign(features->defend_goal_const.x) * 250.0;
  }
  else {
    target.x = ball.x + sign(features->score_goal_const.x) * 250.0;
  }

  if (fabs(target.x) > depth_clip) {
    target.x = depth_clip * sign(target.x);
  }
  // clip to the mid line, so that we never want to be in the front
  // half of the field
  if (fabs(target.x - features->score_goal_const.x) > halfLength) {
    target.x = 0.0;
  }
  
  // Weight the target x by the length of time we've been in this
  // role. If we've just changed roles, we prefer to be closer to the
  // middle.
  //
  // FIXME: took this weighting out as well, to be consistent with the
  // getDefensePosn() function. 
//   double weight =
//     ((double) (features->current_time - role_change_time)) / SecToTime(15.0);
//   if (weight > 1) {
//     weight = 1;
//   }
//   target.x = 
//     (target.x * weight) + 
//     halfLength / 2 * sign(features->score_goal_const.x) * (1-weight);

//   side_to_wait_on is the opposite side of where the primary is...
//   if (primary_pos.y > offensive_supporter_width / 2) {
//     side_to_wait_on = -1;
//   }
//   else if (primary_pos.y < -offensive_supporter_width / 2) {
//     side_to_wait_on = 1;
//   }

  if (ball.y > offensive_supporter_width / 2) {
    side_to_wait_on = 1;
  }
  else if (ball.y < -offensive_supporter_width / 2) {
    side_to_wait_on = -1;
  }

  // but if the primary is far to the defensive half of the field,
  // prefer to be on the same side as him
  target.y = offensive_supporter_width * side_to_wait_on;
  if (fabs(primary_pos.x - features->defend_goal_const.x) < halfLength - 500) {
    target.y = -target.y;
  }

  return target;
}