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

#include "BehSequences.h"
#include "../headers/RoboCupGameControlData.h"
#include "../headers/Sensors.h"

//We send the kick command for this long just to be safe since our frame
//rate is now faster than motions.
const ulong kick_cmd_time = SecToTime(0.3);

/*============================================================*/

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

GetNearBall::GetNearBall(){
  get_ball = new BeGetBall();
  gnb = new BeGetNearBall();

  event_mgr = NULL;
  fs = NULL;
};

GetNearBall::~GetNearBall(){
  if(get_ball!=NULL){
    delete get_ball;
    get_ball = NULL;
  }
  if(gnb != NULL){
    delete gnb;
    gnb = NULL;
  }

}

double GetNearBall::operator()(FeatureSet *features, MotionCommand *command) {

  //(*get_ball)(features, &gb_info, command);
  
  //pprintf(TextOutputStream,"%f %f\n", features->pos_uncert_sMaj,features->pos_uncert_sMin);
  
  if(features->team_msg_mgr->countActiveTeammates() == 0){
    gnb->setTargetBounds(-1500.0, -1000.0,
			  1500.0,  1000.0);
  }else if(features->team_msg_mgr->getSupporterRole() == 1){
    gnb->setTargetBounds(sign(features->defend_goal_const.x)*1500.0, -1000.0,
			 0, 1000.0);
  }else{
    gnb->setTargetBounds(sign(features->score_goal_const.x)*1500.0, -1000.0,
			 0, 1000.0);
  }
  
  (*gnb)(features, command);

  return 1.0;
}

double GetNearBall::status(FeatureSet *features){

  if(features->near_ball && features->see_ball_med)
    return 1.0;
  else
    return 0.0;

}

void GetNearBall::reset(ulong time){
  if(event_mgr == NULL)
    setupEventMgr();

  get_ball->reset(time);
  gnb->reset(time);

  start_time = time;
}

bool GetNearBall::initConnections(){
  return true;
}

bool GetNearBall::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

const MotionCommand *GetNearBall::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

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

SearchBallClose::SearchBallClose(){
  fbn = new FindBallNear();

  event_mgr = NULL;
  fs = NULL;
};

SearchBallClose::~SearchBallClose(){
  if(fbn!=NULL){
    delete fbn;
    fbn = NULL;
  }
}

double SearchBallClose::operator()(FeatureSet *features, MotionCommand *command) {

  (*fbn)(features, command);

  return 1.0;
}

double SearchBallClose::status(FeatureSet *features){
  if(features->see_ball_med)
    return 1.0;
  if(features->current_time - start_time > timeout)
    return -1.0;
  else
    return 0.0;

}

void SearchBallClose::reset(ulong time){
  if(event_mgr == NULL)
    setupEventMgr();

  fbn->reset(time);
  
  start_time = time;
}

bool SearchBallClose::initConnections(){
  return true;
}

bool SearchBallClose::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

const MotionCommand *SearchBallClose::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

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

char const * const KickClear::state_names[KickClear::NumStates] = {
  "APPROACH","AIM","HOLD_BALL","KICK"};


KickClear::KickClear(){
  fsm.init(state_names,NumStates,APPROACH,16,16);

  approach = new BeApproachBall();
  turn = new BeTurn();
};

KickClear::~KickClear(){
  if(turn != NULL){
    delete turn;
    turn = NULL;
  }
  if(approach != NULL){
    delete approach;
    approach = NULL;
  }
}

double KickClear::operator()(FeatureSet *features, MotionCommand *command) {

  command->setBinLEDs(LED_TOP_WHITE);

  bool done=false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case APPROACH:
      approach_retval = (*approach)(features, command);

      if(approach_retval == 1.0){
	TRANS_CONT(fsm,STAND,5,"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.0;
      command->head_pan  = 0.0;
      command->head_tilt2 = 2.0;

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

      done = true;
      break;

    case AIM: {
      if(fsm.isNewState()){
	//dont want to time out in the turn
	start_time = features->current_time;

	kick = selectKick(features->world_model->score_goal(),CLEAR_MASK,true,false);
	turn_angle = norm_angle(features->world_model->score_goal().angle() -
                                kick.angle + features->my_angle);
      }

      turn_retval = turn->turnBallToAngle(features, command, turn_angle, RAD(10.0));
      if(turn_retval == 0.0){

	//if we didnt need to turn, then we dont have the ball against our chest
	//because the head is not down, so a lot of kicks will fail.  Hold the ball
	//briefly to get it against the chest.
	if(fsm.isNewState()){
	  TRANS_CONT(fsm,HOLD_BALL,5,"no turn,pull ball closer");
	}
	//otherwise we are done turning and can kick
	TRANS_CONT(fsm,KICK,6,"kick");	
      }

      done = true;
    } break;

    case HOLD_BALL:
      command->motion_cmd = MOTION_STAND_NEUTRAL;
      command->head_cmd = HEAD_ANGLES;
      command->head_tilt = -1.1;
      command->head_pan  = 0.0;
      command->head_tilt2 = 1.1;

      if(fsm.timeInState() > 20000){
	TRANS_CONT(fsm,KICK,5,"hold done");	
      }

      done = true;
      break;
    case KICK:
      command->motion_cmd = kick.kick_name;

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

  return 1.0;
}

double KickClear::status(FeatureSet *features){

  if(fsm.getState() == APPROACH){
    if(!features->near_ball){
      return -1.0;
    }
    if(approach_retval == -1){
      features->approach_failed = true;
      return -1.0;
    }
  }
  else if(fsm.getState() == AIM){
    if(features->see_ball_low){
      return -1.0;
    }
    if(turn_retval == -1){
      features->approach_failed = true;
      return -1.0;
    }
  }else if(fsm.getState() == KICK){
    if(fsm.timeInState() > kick_cmd_time){
      return 1.0;
    }
  }

  if(features->current_time - start_time > timeout){
    return -1.0;
  }

  return 0.0;
}

void KickClear::reset(ulong time){
  fsm.setState(APPROACH,0,"Reset",time);

  if(event_mgr == NULL)
    setupEventMgr();

  approach->reset(time);
  approach->setApproachType(BeApproachBall::APPROACH_CENTER,
			    BeApproachBall::CENTER,time);
  turn->reset(time);

  approach_retval = 0;
  turn_retval = 0;
  start_time = time;
}

bool KickClear::initConnections(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);

  return true;
}

bool KickClear::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}
const MotionCommand *KickClear::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

static const bool DebugKickToPoint = false;

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

char const * const KickToPoint::state_names[KickToPoint::NumStates] = {
  "APPROACH","STAND","AIM"};


KickToPoint::KickToPoint(){
  fsm.init(state_names,NumStates,APPROACH,16,16);

  approach = new BeApproachBall();
  turn = new BeTurn();
};

KickToPoint::~KickToPoint(){
  if(turn != NULL){
    delete turn;
    turn = NULL;
  }
  if(approach != NULL){
    delete approach;
    approach = NULL;
  }
}

double KickToPoint::operator()(FeatureSet *features, MotionCommand *command) {

  vector2d target_ego = target;
  target_ego -= features->my_position_body;
  target_ego = target_ego.rotate(-features->my_angle);
  
  bool done=false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case APPROACH:

      approach_retval = (*approach)(features, command);

      if(approach_retval == 1.0){
	TRANS_CONT(fsm,STAND,5,"have ball");
      }

      if(features->have_ball_left && 
	 fabs(target_ego.angle()) < 0.1){
	command->motion_cmd = MOTION_KICK_ARM_DIVE_L;
	kicked = true;
      }
      if(features->have_ball_right && 
	 fabs(target_ego.angle()) < 0.1){
	command->motion_cmd = MOTION_KICK_ARM_DIVE_R;
	kicked = true;
      }
      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.15;
      command->head_pan  = 0.0;
      command->head_tilt2 = 0.5;

      if(fsm.isNewState()){
	//reset the start time because we are making progress
	//and dont want to time out in a turn
	start_time = features->current_time;
      }

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

      done = true;
      break;

    case AIM:
      if(fsm.isNewState()){
        if(DebugKickToPoint)
          pprintf(TextOutputStream, 
                  "Kicking: global(%d,%d) ego(%d,%d) angle %f\n", 
                  (int) target.x,
                  (int) target.y,
                  (int) target_ego.x,
                  (int) target_ego.y,
                  target_ego.angle());

	kick = selectKick(target_ego, kick_mask, kickThroughTarget, false);

        if(DebugKickToPoint)
          pprintf(TextOutputStream,"my position (%f %f %f), kick used = %d\n",
                  features->my_position.position.mean.x, 
                  features->my_position.position.mean.y,
                  features->my_angle, 
                  kick.kick_name);

        if(DebugKickToPoint)
          pprintf(TextOutputStream,"uncertainty: sMaj %f, sMin %f, angle %f\n",
                  features->pos_uncert_sMaj, features->pos_uncert_sMin,
                  features->my_position.heading.sMaj);

	start_time = features->current_time;

	//use the world model to kick
	turn_angle = norm_angle(target_ego.angle() -
                                kick.angle + features->my_angle);
      }

      turn_retval = turn->turnBallToAngle(features, command, turn_angle, RAD(3.0));
      if(turn_retval == 0.0){
        if(DebugKickToPoint)
          pprintf(TextOutputStream,"turned to angle %f, angle to target = %f\n",
                  features->my_angle, target_ego.angle());
	TRANS_CONT(fsm,KICK,5,"lined up to kick");
      }
      
      done = true;
      break;
    case KICK:

      command->motion_cmd = kick.kick_name;

      done = true;
      break;

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

  return 1.0;
}

double KickToPoint::status(FeatureSet *features){
  if(kicked){
    pprintf(TextOutputStream,"x1\n");
    return 1.0;
  }
  if(!features->valid_ball_hyp){
    pprintf(TextOutputStream,"x2\n");
    return -1.0;
  }
  if(fsm.getState() == APPROACH){
    if(!features->near_ball){
      pprintf(TextOutputStream,"x3\n");
      return -1.0;

    }
    if(features->current_time - features->ball_last_seen_med > SecToTime(1.0)){
      pprintf(TextOutputStream,"x4\n");
      return -1.0;
    }
    if(approach_retval == -1){
      pprintf(TextOutputStream,"x5\n");
      features->approach_failed = true;
      return -1.0;
    }
  }else if(fsm.getState() == AIM){
    pprintf(TextOutputStream,"x6\n");
    if(features->see_ball_low){
      return -1.0;
    }
    if(turn_retval == -1){
      pprintf(TextOutputStream,"x7\n");
      features->approach_failed = true;
      return -1.0;
    }
  }else if(fsm.getState() == KICK){
    if(fsm.timeInState() > kick_cmd_time){
      pprintf(TextOutputStream,"x8\n");
      return 1.0;
    }
  }
  if(features->current_time - start_time > timeout){
    pprintf(TextOutputStream,"x9\n");
    return -1.0;
  }

  return 0.0;
}

void KickToPoint::setTarget(vector2d target, bool kickThroughTarget) {
  this->target = target;
  this->kickThroughTarget = kickThroughTarget;
}

void KickToPoint::setKickMask(int mask){
  this->kick_mask = mask;
}

void KickToPoint::reset(ulong time){
  fsm.setState(APPROACH,0,"Reset",time);

  if(event_mgr == NULL)
    setupEventMgr();
  approach->reset(time);
  approach->setApproachType(BeApproachBall::APPROACH_CENTER,
			    BeApproachBall::CENTER,time);
  turn->reset(time);

  kick_mask = SHOOT_MASK;
  kicked = false;
  approach_retval = 0;
  turn_retval = 0;
  start_time = time;
}

bool KickToPoint::initConnections(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);

  return true;
}

bool KickToPoint::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

const MotionCommand *KickToPoint::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

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

char const * const ShootOnGoal::state_names[ShootOnGoal::NumStates] = {
  "SHOOT" };


ShootOnGoal::ShootOnGoal(){
  fsm.init(state_names,NumStates,SHOOT,16,16);
  kickToPoint = new KickToPoint();
};

ShootOnGoal::~ShootOnGoal(){
  if (kickToPoint != NULL) {
    delete kickToPoint;
    kickToPoint = NULL;
  }
}

double ShootOnGoal::operator()(FeatureSet *features, MotionCommand *command) {
  vector2d target = features->score_goal_const;
  if(fabs(features->my_position.position.mean.x) > halfLength &&
     sign(features->my_position.position.mean.x) == sign(features->score_goal_const.x)){
    target.x = features->my_position.position.mean.x*1.2;
  }

  /*features->checkBallObst(160.0);

  if(features->obst_left_of_ball && 
    features->obst_right_of_ball &&
    (features->ball_near_wall || features->high_loc_uncertainty)){
    kickToPoint->setKickMask(SHOOT_HEAD_ONLY);
  }else*/ 
  if(features->near_front_wall){
    kickToPoint->setKickMask(CORNER_MASK);
  }else{
    kickToPoint->setKickMask(SHOOT_MASK);
  }

  kickToPoint->setTarget(target, true);
  return (*kickToPoint)(features, command);
}

double ShootOnGoal::status(FeatureSet *features){
  return kickToPoint->status(features);
}

void ShootOnGoal::reset(ulong time){
  fsm.setState(SHOOT,0,"Reset",time);
  if(event_mgr == NULL)
    setupEventMgr();
  kickToPoint->reset(time);
}

bool ShootOnGoal::initConnections(){
  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);
  return true;
}

bool ShootOnGoal::setupEventMgr(){
  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  return true;
}

const MotionCommand *ShootOnGoal::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  if(fs!=NULL){
    lfs = fs->get(time);
    (*this)(lfs,&out_command);
  }
  return &out_command;
}

REGISTER_EVENT_PROCESSOR(ShootOnGoal,ShootOnGoal::beh_name,ShootOnGoal::create);
/*============================================================*/

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

char const * const PassToTeammate::state_names[PassToTeammate::NumStates] = {
  "SHOOT" };


PassToTeammate::PassToTeammate(){
  fsm.init(state_names,NumStates,SHOOT,16,16);
  kickToPoint = new KickToPoint();
};

PassToTeammate::~PassToTeammate(){
  if (kickToPoint != NULL) {
    delete kickToPoint;
    kickToPoint = NULL;
  }
}

double PassToTeammate::operator()(FeatureSet *features, MotionCommand *command) {
  vector2d target = features->pass_receiver_pos_global;
  kickToPoint->setTarget(target, true);
  kickToPoint->setKickMask(CORNER_MASK);
  double result = (*kickToPoint)(features, command);
  if (kickToPoint->status(features) == 1.0) {
    if (isNewSequence) {
      command->sound_cmd       = SOUND_NOTE;
      command->sound_frequency = 1200;
      command->sound_duration  = SecToTime(2.0);
      isNewSequence = false;
    }
    features->team_msg_mgr->setHavePassedRecently(true);
  }
  return result;
}

double PassToTeammate::status(FeatureSet *features){
  return kickToPoint->status(features);
}

void PassToTeammate::reset(ulong time){
  fsm.setState(SHOOT,0,"Reset",time);
  if(event_mgr == NULL)
    setupEventMgr();
  kickToPoint->reset(time);
  isNewSequence = true;
}

bool PassToTeammate::initConnections(){
  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);
  return true;
}

bool PassToTeammate::setupEventMgr(){
  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  return true;
}

const MotionCommand *PassToTeammate::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  if(fs!=NULL){
    lfs = fs->get(time);
    (*this)(lfs,&out_command);
  }
  return &out_command;
}

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

/*============================================================*/

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

char const * const InterceptBall::state_names[InterceptBall::NumStates] = {
  "INTERCEPT" };


InterceptBall::InterceptBall(){
  fsm.init(state_names,NumStates,INTERCEPT,16,16);
  done = false;
  sequenceTime = 0;
};

InterceptBall::~InterceptBall(){
}

double InterceptBall::operator()(FeatureSet *features, 
                                 MotionCommand *command) {
  if (sequenceTime < 5) {
    std::pair<vector2d, double> ballIntercept = 
      features->world_model->getBallIntercept(10);
    vector2d interceptPos = ballIntercept.first;
    //double interceptTime = ballIntercept.second;
    double yint = interceptPos.y;
    
    if (fabs(yint) < 150.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;
    }

    sequenceTime++;
  }
  if (sequenceTime >= 5 && features->motion_state != Motion::STATE_KICKING) {
    done = true;
  }
  return 0.0;
}

double InterceptBall::status(FeatureSet *features){
  if (done) {
    return 1.0;
  }
  if(!features->valid_ball_hyp){
    return -1.0;
  }
  else {
    return 0.0;
  }
}

void InterceptBall::reset(ulong time){
  fsm.setState(INTERCEPT,0,"Reset",time);
  sequenceTime = 0;
  done = false;
}

bool InterceptBall::initConnections(){
  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);
  return true;
}

bool InterceptBall::setupEventMgr(){
  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  return true;
}

const MotionCommand *InterceptBall::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  if(fs!=NULL){
    lfs = fs->get(time);
    (*this)(lfs,&out_command);
  }
  return &out_command;
}

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

/*============================================================*/

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

char const * const KickNearObstacle::state_names[KickNearObstacle::NumStates] = {
  "PUNCH","CIRCLE"};

KickNearObstacle::KickNearObstacle(){
  fsm.init(state_names,NumStates,PUNCH,16,16);
  approach = new BeApproachBall();
  turn = new BeTurn();
  track_objects = new BeTrackObjects();
  should_punch = false;
};

KickNearObstacle::~KickNearObstacle(){
  if(turn != NULL){
    delete turn;
    turn = NULL;
  }
  if(approach != NULL){
    delete approach;
    approach = NULL;
  }
  if(track_objects != NULL){
    delete track_objects;
    track_objects = NULL;
  }
    
}

double KickNearObstacle::operator()(FeatureSet *features, MotionCommand *command) {

  vector2d walk_rel;
  bool done=false;

  ball_far = (features->ball_distance > 600);

  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case PUNCH:

      if(!should_punch && 
	 //along the side wall
	 ((fabs(features->my_position.position.mean.y) > halfWidth-500.0 &&
	  features->world_model->score_goal().x > 0) ||
	 //by our own goal
	 (fabs(features->defend_goal_const.x - features->my_position.position.mean.x) < 500.0 &&
	  features->world_model->defend_goal().x < 0) ||
	 //by opponent goal
	 (fabs(features->score_goal_const.x - features->my_position.position.mean.x) < 500.0 &&
	  features->world_model->score_goal().x > 0 &&
	  fabs(features->world_model->score_goal().angle()) < RAD(10.0)))){
	should_punch = true;
      }

      if(!should_punch){
	start_time_circle = features->current_time;
	TRANS_CONT(fsm,CIRCLE,5,"BadPunchPosition");	
      }
      if(features->obst_right_of_ball && !features->obst_left_of_ball){
	command->setFaceLEDs(LED_VLINE_RIGHT, LED_BRIGHT,MODE_A);
	command->setFaceLEDs(LED_K, LED_BRIGHT,MODE_A);
	approach->setApproachType(BeApproachBall::APPROACH_SIDE,BeApproachBall::RIGHT,features->current_time);
      }else if(features->obst_left_of_ball && !features->obst_right_of_ball){
	command->setFaceLEDs(LED_VLINE_LEFT, LED_BRIGHT,MODE_A);
	command->setFaceLEDs(LED_K, LED_BRIGHT,MODE_A);
	approach->setApproachType(BeApproachBall::APPROACH_SIDE,BeApproachBall::LEFT,features->current_time);
      }else{
	pprintf(TextOutputStream,"KickNearObstacle: if the ball isnt next to a wall, why am I here?\n");
      }
  
      if(features->have_ball_left){
	command->motion_cmd = MOTION_KICK_ARM_DIVE_L;
	command->setFaceLEDs(LED_VLINE_LEFT, LED_BRIGHT,MODE_A);
	kicked = true;
      }else if(features->have_ball_right){
	command->motion_cmd = MOTION_KICK_ARM_DIVE_R;
	command->setFaceLEDs(LED_VLINE_RIGHT, LED_BRIGHT,MODE_A);
	kicked = true;
      }else{
	if(features->ball_vector.x > 0 && features->ball_vector.x < 70 && fabs(features->ball_vector.y) < 50){
	    if(features->world_model->score_goal().angle() > 0)
	      command->motion_cmd = MOTION_KICK_SWING_L;
	    else
	      command->motion_cmd = MOTION_KICK_SWING_R;
	    kicked = true;
	  }
	  else{
	    (*approach)(features, command);
	  }
      }
      done = true;
      break;

    case CIRCLE:
      if(features->world_model->score_goal().x > 0){
	kicked = true;	
      }

      (*track_objects)(features, command, false, true);

      if(fsm.timeInState() < 50000){
	should_punch = false;
	command->motion_cmd = MOTION_WALK_TROT_FAST;
	walk_rel = features->ball_vector.norm() * MAX_DX * 0.3;
	command->vx = walk_rel.x;
	command->vy = 0;
	command->va = walk_rel.angle();
	approach->setApproachType(BeApproachBall::APPROACH_CENTER,BeApproachBall::CENTER,features->current_time);
	turn_side = (features->world_model->defend_goal().angle() > 0);
	if(features->uniform_color == TEAM_COLOR_BLUE){
	  if(fabs(features->my_angle) < M_PI/2.0){
	    turn_thresh = M_PI/2.0;
	  }else{
	    turn_thresh = 2.8;
	  }
	}
	else{
	  if(fabs(features->my_angle) > M_PI/2.0){
	    turn_thresh = M_PI/2.0;
	  }else{
	    turn_thresh = 0.4;
	  }
	}
      }
      else{
	if((features->uniform_color == TEAM_COLOR_BLUE && fabs(features->my_angle) < turn_thresh) || 
	   (features->uniform_color == TEAM_COLOR_RED && fabs(features->my_angle) > turn_thresh)){

	  command->motion_cmd = MOTION_WALK_TROT_FAST;
	  walk_rel = features->ball_vector.rotate(turn_side ? 1.6 : -1.6);
	  walk_rel = walk_rel.norm() * MAX_DX;
	  command->vx = walk_rel.x; 
	  command->vy = walk_rel.y;
	  command->va = features->ball_angle + sign(features->ball_angle)*0.3;
	}
	else{
	  kicked = true;
	}
      }
      done = true;
      break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();

  return 1.0;
}

double KickNearObstacle::status(FeatureSet *features){
  if(!features->valid_ball_hyp){
    return -1.0;
  }
  if(fsm.getState() == CIRCLE && 
     (features->current_time - start_time_circle > circle_timeout ||
      ball_far || kicked || !features->saw_ball_recently)){
    return -2.0;
  }
  if(kicked)
    return 1.0;
  if(ball_far ||
     !features->saw_ball_recently ||
     features->current_time - start_time > timeout)
    return -1.0;
  else 
    return 0.0;
}

void KickNearObstacle::reset(ulong time){
  fsm.setState(PUNCH,0,"Reset",time);

  if(event_mgr == NULL)
    setupEventMgr();

  turn->reset(time);
  
  kicked = false;
  turn_side = false;
  ball_far = false;
  should_punch = false;
  start_time = time;
  start_time_circle = time;
  approach_retval = 0;
  turn_thresh = M_PI/2.0;
}

bool KickNearObstacle::initConnections(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);

  return true;
}

bool KickNearObstacle::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

const MotionCommand *KickNearObstacle::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

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

char const * const Dribble::state_names[Dribble::NumStates] = {
  "APPROACH","DRIBBLE", "WALK"};


Dribble::Dribble(){
  fsm.init(state_names,NumStates,APPROACH,16,16);

  approach = new BeApproachBall();
  turn = new BeTurn();
  dribble = new BeDribble();
};

Dribble::~Dribble(){
  if(turn != NULL){
    delete turn;
    turn = NULL;
  }
  if(approach != NULL){
    delete approach;
    approach = NULL;
  }
  if(dribble != NULL){
    delete dribble;
    dribble = NULL;
  }
}

double Dribble::operator()(FeatureSet *features, MotionCommand *command) {

  bool done=false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case APPROACH:
      (*approach)(features, command);

      if(features->have_ball_center){
	TRANS_CONT(fsm,DRIBBLE,5,"have ball");
      }
      done = true;
      break;
    case DRIBBLE:
      if(fsm.isNewState()){
	//dont want to time out here
	start_time = features->current_time;
	//target_pt.set(features->my_position.position.mean.x, 0);
	target_pt.set(sign(features->my_position.position.mean.x)*penaltyRegionLengthOffset, 0);
      }
      
      dribble_retval = (*dribble)(features, command, target_pt);
      if(dribble_retval != 0.0){
	dribble_done = true;
      }

      done = true;
      break;
    case WALK:
      command->motion_cmd = MOTION_WALK_TROT_FAST;
      command->vx = MaxDX;
      command->vy = 0;
      command->va = 0;
      command->head_cmd = HEAD_LOOKAT;
      command->head_lookat.set(300, 0, 0);

      if(fsm.timeInState() > 1000000){
	dribble_done = true;
      }
      

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

  return 1.0;
}

double Dribble::status(FeatureSet *features){
//   if(features->current_time - start_time > timeout){
//     pprintf(TextOutputStream,"timeout in BehSeq\n");
//     return -1.0;
//   }
  if(!features->valid_ball_hyp){
    return -1.0;
  }
  if(fsm.getState() == APPROACH){
    if(!features->near_ball ||
       approach_retval == -1)
      return -1.0;
  }
  return dribble_done;

}

void Dribble::reset(ulong time){
  fsm.setState(APPROACH,0,"Reset",time);

  if(event_mgr == NULL)
    setupEventMgr();

  approach->reset(time);
  approach->setApproachType(BeApproachBall::APPROACH_CENTER,
			    BeApproachBall::CENTER,time);
  turn->reset(time);
  dribble->reset(time);

  dribble_done = false;
  approach_retval = 0;
  start_time = time;
}

bool Dribble::initConnections(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);

  return true;
}

bool Dribble::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}
const MotionCommand *Dribble::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

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

char const * const WalkDribble::state_names[WalkDribble::NumStates] = {
  "APPROACH_SIDE","WALK"};

WalkDribble::WalkDribble(){
  fsm.init(state_names,NumStates,APPROACH_SIDE,16,16);
  track_objects = new BeTrackObjects();
  gpoint.set(185.0,75.0);
  side_set = false;
};

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

double WalkDribble::operator()(FeatureSet *features, MotionCommand *command) {

  vector2d walk_rel;
  bool done=false;

  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case APPROACH_SIDE:
      /* Choose appropriate foot to align ball with */
      if(!side_set){
	if(features->ball_vector.y > 0){
	  gpoint.set(185.0,75.0);
	}else{
	  gpoint.set(185.0,-75.0);
	}
	side_set = true;
      }
      /* Calculate vector to walk to */
      command->motion_cmd = MOTION_WALK_TROT_FAST;
      walk_rel.set(features->ball_vector.x-gpoint.x,features->ball_vector.y-gpoint.y);
      walk_rel = walk_rel.norm() * MAX_DX;

      /* Do quick scan of area if ball is not seen */
      if(features->current_time - features->ball_last_seen_low > 300000){
	command->head_cmd = HEAD_SCAN_DRIBBLE;
	command->vx = 0;
	command->vy = 0;
	command->va = 0;
      }
      /* Walk so as to align foot with ball */
      else{
	(*track_objects)(features, command, false, true);
	command->vx = walk_rel.x;
	command->vy = 0;
	command->va = walk_rel.angle();
      }

      /* Walk straight to punch ball forward */
      if(features->ball_vector.x > 100 && features->ball_distance < 230){
	TRANS_CONT(fsm,WALK,5,"WalkForward");
      }
      done = true;
      break;

    case WALK:
      /* Position head so as to see the ball spit out */
      if(fsm.timeInState() < 150000){
	command->head_cmd = HEAD_ANGLES;
	command->head_tilt = 0.016110;
	command->head_pan  = bound(features->ball_angle,-0.347819,0.347819);
	command->head_tilt2 = -0.349065;
      }
      /* Look directly at ball */
      else{
	(*track_objects)(features, command, false, true);
      }

      /* Walk forward for a short period */
      if(fsm.timeInState() < 500000){
        command->motion_cmd = MOTION_WALK_TROT_FAST;
 	command->vx = MAX_DX;
 	command->vy = 0;
 	command->va = 0;
      }
      else{
	side_set = false;
	TRANS_CONT(fsm,APPROACH_SIDE,5,"Approach");
      }
      done = true;
      break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();

  return 1.0;
}

double WalkDribble::status(FeatureSet *features){
  if(!features->valid_ball_hyp)
    return -1.0;
  if(features->ball_distance > 550)
    return 1.0;
  if(!features->saw_ball_recently ||
     !features->shouldDribble())
    return -1.0;
  else 
    return 0.0;
}

void WalkDribble::reset(ulong time){
  fsm.setState(APPROACH_SIDE,0,"Reset",time);

  if(event_mgr == NULL)
    setupEventMgr();

  gpoint.set(185.0,75.0);
  side_set = false;
}

bool WalkDribble::initConnections(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);

  return true;
}

bool WalkDribble::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

const MotionCommand *WalkDribble::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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


/*============================================================*/

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

char const * const TurnWithBall::state_names[TurnWithBall::NumStates] = {
  "APPROACH","AIM","KICK"};

const ulong twb_holding_time = SecToTime(2.5);
const ulong twb_stand_time   = SecToTime(0.4);

TurnWithBall::TurnWithBall(){
  fsm.init(state_names,NumStates,APPROACH,16,16);

  approach = new BeApproachBall();
};

TurnWithBall::~TurnWithBall(){
  if(approach != NULL){
    delete approach;
    approach = NULL;
  }
}

double TurnWithBall::operator()(FeatureSet *features, MotionCommand *command) {

  bool done=false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case APPROACH:
      approach_retval = (*approach)(features, command);

      if(approach_retval == 1.0){
	num_saw_goal_frames = 0;
	TRANS_CONT(fsm,AIM,5,"have ball");
      }
      done = true;
      break;

    case AIM: {
      if(fsm.isNewState()){
	//dont want to time out in the turn
	start_time = features->current_time;
	direction = 1.0 * sign(features->world_model->score_goal().angle());
      }
      
      if(direction > 0)
      	command->motion_cmd = MOTION_TURN_WITH_BALL_L;
      else
	command->motion_cmd = MOTION_TURN_WITH_BALL_R;

      if(features->saw_score_goal_confidence > .6 ){
	num_saw_goal_frames++;
	pprintf(TextOutputStream,"goal at %f %f\n",
		features->score_goal->loc.x,
		features->score_goal->loc.y);
      }

      if(num_saw_goal_frames > 0){

	// If the ball is on the goal score side of the field
	if (sign(features->world_ball_vector.x)==sign(features->score_goal_const.x)) { 
	  // If we're in the penalty box
	  if (fabs(features->world_ball_vector.x) > penaltyRegionLengthOffset) {
	    // We're in the goal penalty box, so do the arm dive
	    kick = selectKick(features->world_model->score_goal(),1<<(Motion::MOTION_KICK_ARM_DIVE_L-300) ,true,true);
	    // Is it even appropriate to try to do the bump kick???
	    //	    kick = selectKick(features->world_model->score_goal(),1<<(Motion::MOTION_KICK_BUMP-300) ,true,true);
	  } else {
	    // We're not in the goal penalty box, so do the arm dive
	    kick = selectKick(features->world_model->score_goal(),1<<(Motion::MOTION_KICK_ARM_DIVE_L-300) ,true,true);
	  }
	} else {
	  // We're on the other side of the field, so do the hard forward kick
	  kick = selectKick(features->world_model->score_goal(),1<<(Motion::MOTION_KICK_FOREWARD-300) ,true,true); 
	}
	TRANS_CONT(fsm,KICK,5,"saw goal");	
      }

      if(fsm.timeInState() > twb_holding_time){
	holding = true;
	
	if(features->current_time - features->last_saw_marker > fsm.timeInState()){
	  TRANS_CONT(fsm,ABORT,6,"timout, no markers");
	}else{
	  //	  kick = selectKick(features->world_model->score_goal(),SHOOT_MASK,true,true);
	  //	  TRANS_CONT(fsm,KICK,6,"timout, saw marker");
	  TRANS_CONT(fsm,ABORT,6,"timout, saw marker");
	}
      }

      done = true;
    } break;

    case KICK:
      command->motion_cmd = kick.kick_name;

      done = true;
      break;

    case ABORT:
      command->motion_cmd = MOTION_WALK_TROT;
      command->vx = 0;
      command->vy = 0;
      command->va = 0;
      command->head_cmd = HEAD_SCAN_MARKERS;

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

  return 1.0;
}

double TurnWithBall::status(FeatureSet *features){

  if(fsm.getState() == APPROACH){
    if(!features->near_ball){
      return -1.0;
    }
    if(approach_retval == -1){
      features->approach_failed = true;
      return -1.0;
    }
  }
  else if(fsm.getState() == AIM){
    if(features->see_ball_low){
      return -1.0;
    }
  }else if(fsm.getState() == KICK){
    //longer kick time here because it takes a while to stand up
    if(fsm.timeInState() > kick_cmd_time*3.0){
      return 1.0;
    }
  }else if(fsm.getState() == ABORT){
    if(fsm.timeInState() > 500000){
      return -2.0;
    }
  }

  if(features->current_time - start_time > timeout){
    return -1.0;
  }

  return 0.0;
}

void TurnWithBall::reset(ulong time){
  fsm.setState(APPROACH,0,"Reset",time);

  if(event_mgr == NULL)
    setupEventMgr();

  approach->reset(time);
  approach->setApproachType(BeApproachBall::APPROACH_CENTER,
			    BeApproachBall::CENTER,time);

  approach_retval = 0;
  num_saw_goal_frames = 0;
  holding = false;
  start_time = time;
  direction = 1.0;
}

bool TurnWithBall::initConnections(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);

  return true;
}

bool TurnWithBall::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}
const MotionCommand *TurnWithBall::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

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

char const * const WallTurn::state_names[WallTurn::NumStates] = {
  "APPROACH","TURN"};

WallTurn::WallTurn(){
  fsm.init(state_names,NumStates,APPROACH,16,16);
  track_objects = new BeTrackObjects();
  turned = false;
};

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

double WallTurn::operator()(FeatureSet *features, MotionCommand *command) {

  vector2d walk_rel;
  bool done=false;

  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case APPROACH:
      walk_rel = features->ball_vector;
      if(walk_rel.x > 0 && walk_rel.x < 120 && fabs(walk_rel.y) < 50){
	TRANS_CONT(fsm,TURN,5,"Turn");
      }
      if(features->ball_vector.x < 220 && fabs(features->ball_vector.y) < 60){
	command->head_cmd = HEAD_ANGLES;
	command->head_tilt = -0.5;
	command->head_pan  = 0;
	command->head_tilt2 = -2.0;
      }else{
	(*track_objects)(features, command, false, true);
      }
      command->motion_cmd = MOTION_WALK_TROT_FAST;
      walk_rel = features->ball_vector - (features->ball_vector.norm()*100.0);
      command->vx = walk_rel.x;
      command->vy = 0;
      command->va = walk_rel.angle()*2.0;
      done = true;
      break;

    case TURN:
      if(fsm.isNewState()){
	if(features->uniform_color == TEAM_COLOR_BLUE){
	  if(features->my_position.position.mean.y > 0){
	    if(features->my_angle < -M_PI/3.0 && features->my_angle > -2.0*M_PI/3.0){
	      command->motion_cmd = MOTION_KICK_HEAD_HARD_R;
	    }else{
	      command->motion_cmd = MOTION_KICK_HEAD_HARD_L;
	    }
	  }else{
	    if(features->my_angle > M_PI/3.0 && features->my_angle < 2.0*M_PI/3.0){
	      command->motion_cmd = MOTION_KICK_HEAD_HARD_L;
	    }else{
	      command->motion_cmd = MOTION_KICK_HEAD_HARD_R;
	    }
	  }
	}
	else{
	  if(features->my_position.position.mean.y > 0){
	    if(features->my_angle < -M_PI/3.0){
	      command->motion_cmd = MOTION_KICK_HEAD_HARD_L;
	    }else{
	      command->motion_cmd = MOTION_KICK_HEAD_HARD_R;
	    }
	  }else{
	    if(features->my_angle > M_PI/3.0){
	      command->motion_cmd = MOTION_KICK_HEAD_HARD_R;
	    }else{
	      command->motion_cmd = MOTION_KICK_HEAD_HARD_L;
	    }
	  }
	}
      }
      else{
	turned = true;
      }

//       if(fsm.timeInState() < 200000){
// 	command->head_cmd = HEAD_ANGLES;
// 	command->head_tilt = -1.3;
// 	command->head_pan  = -1.5;
// 	command->head_tilt2 = -2.0;
// 	command->motion_cmd = MOTION_WALK_TROT;
// 	command->vx = 0;
//  	command->vy = 0;
//  	command->va = MAX_DA * 0.8;
//       }
//       else 

//       command->head_cmd = HEAD_ANGLES;
//       command->head_tilt = -0.4;
//       command->head_pan  = features->ball_angle;
//       command->head_tilt2 = -2.0;
// 	if(fsm.timeInState() < 1000000){
// 	command->motion_cmd = MOTION_WALK_TROT;
//  	command->vx = 0;
//  	command->vy = 0;
//  	command->va = MAX_DA * 0.8;
//       }
//       else{
// 	turned = true;
//       }

//       command->head_cmd = HEAD_ANGLES;
//       command->head_tilt = -0.4;
//       command->head_pan  = features->ball_angle;
//       command->head_tilt2 = -2.0;
//       if(fsm.timeInState() < 300000){
// 	command->motion_cmd = MOTION_STAND_WALL_TURN_R;
//       }
//       else if(fsm.timeInState() < 1000000){
// 	command->motion_cmd = MOTION_WALK_DRIBBLE;
// 	//command->motion_cmd = MOTION_WALK_WALL_TURN;
//  	command->vx = 0;
//  	command->vy = 0;
//  	command->va = -MAX_DA/2.0;
//       }
//       else{
// 	turned = true;
//       }
      done = true;
      break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();

  return 1.0;
}

double WallTurn::status(FeatureSet *features){
  if(!features->valid_ball_hyp){
    return -1.0;
  }
  if(turned){
    return 1.0;
  }
  if(!features->saw_ball_recently ||
     !features->near_ball ||
     features->current_time - start_time > timeout){
    return -1.0;
  }
  
  return 0.0;
}

void WallTurn::reset(ulong time){
  fsm.setState(APPROACH,0,"Reset",time);

  if(event_mgr == NULL)
    setupEventMgr();

  start_time = time;

  turned = false;
}

bool WallTurn::initConnections(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);

  return true;
}

bool WallTurn::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

const MotionCommand *WallTurn::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

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

char const * const WallTurnSoft::state_names[WallTurnSoft::NumStates] = {
  "APPROACH","TURN"};

WallTurnSoft::WallTurnSoft(){
  fsm.init(state_names,NumStates,APPROACH,16,16);
  track_objects = new BeTrackObjects();
  turned = false;
};

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

double WallTurnSoft::operator()(FeatureSet *features, MotionCommand *command) {

  vector2d walk_rel;
  bool done=false;

  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case APPROACH:
      walk_rel = features->ball_vector;
      if(walk_rel.x > 0 && walk_rel.x < 120 && fabs(walk_rel.y) < 50){
	TRANS_CONT(fsm,TURN,5,"Turn");
      }
      if(features->ball_vector.x < 220 && fabs(features->ball_vector.y) < 60){
	command->head_cmd = HEAD_ANGLES;
	command->head_tilt = -0.5;
	command->head_pan  = 0;
	command->head_tilt2 = -2.0;
      }else{
	(*track_objects)(features, command, false, true);
      }
      command->motion_cmd = MOTION_WALK_TROT_FAST;
      walk_rel = features->ball_vector - (features->ball_vector.norm()*100.0);
      command->vx = walk_rel.x;
      command->vy = 0;
      command->va = walk_rel.angle()*2.0;
      done = true;
      break;

    case TURN:
      if(fsm.isNewState()){
	if(features->uniform_color == TEAM_COLOR_BLUE){
	  if(features->my_position.position.mean.y > 0){
	    if(features->my_angle < -M_PI/3.0 && features->my_angle > -2.0*M_PI/3.0){
	      command->motion_cmd = MOTION_KICK_HEAD_R;
	    }else{
	      command->motion_cmd = MOTION_KICK_HEAD_L;
	    }
	  }else{
	    if(features->my_angle > M_PI/3.0 && features->my_angle < 2.0*M_PI/3.0){
	      command->motion_cmd = MOTION_KICK_HEAD_L;
	    }else{
	      command->motion_cmd = MOTION_KICK_HEAD_R;
	    }
	  }
	}
	else{
	  if(features->my_position.position.mean.y > 0){
	    if(features->my_angle < -M_PI/3.0){
	      command->motion_cmd = MOTION_KICK_HEAD_L;
	    }else{
	      command->motion_cmd = MOTION_KICK_HEAD_R;
	    }
	  }else{
	    if(features->my_angle > M_PI/3.0){
	      command->motion_cmd = MOTION_KICK_HEAD_R;
	    }else{
	      command->motion_cmd = MOTION_KICK_HEAD_L;
	    }
	  }
	}
      }
      else{
	turned = true;
      }
      done = true;
      break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();

  return 1.0;
}

double WallTurnSoft::status(FeatureSet *features){
  if(!features->valid_ball_hyp){
    return -1.0;
  }
  if(turned){
    return 1.0;
  }
  if(!features->saw_ball_recently ||
     !features->near_ball ||
     features->current_time - start_time > timeout){
    return -1.0;
  }
  
  return 0.0;
}

void WallTurnSoft::reset(ulong time){
  fsm.setState(APPROACH,0,"Reset",time);

  if(event_mgr == NULL)
    setupEventMgr();

  start_time = time;

  turned = false;
}

bool WallTurnSoft::initConnections(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(fs_ep);

  return true;
}

bool WallTurnSoft::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

const MotionCommand *WallTurnSoft::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

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

StandScanMarkers::StandScanMarkers(){
  event_mgr = NULL;
  fs = NULL;
};

StandScanMarkers::~StandScanMarkers(){
}

double StandScanMarkers::operator()(FeatureSet *features, MotionCommand *command) {

  command->motion_cmd = MOTION_STAND_NEUTRAL;
  command->head_cmd = HEAD_SCAN_MARKERS_HIGH;

  return 1.0;
}

double StandScanMarkers::status(FeatureSet *features){
  if(features->current_time - start_time > timeout)
    return 1.0;
  else
    return 0.0;

}

void StandScanMarkers::reset(ulong time){
  if(event_mgr == NULL)
    setupEventMgr();

  start_time = time;
}

bool StandScanMarkers::initConnections(){
  return true;
}

bool StandScanMarkers::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

const MotionCommand *StandScanMarkers::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

char const * const FrontWall::beh_name = "FrontWall";
char const * const FrontWall::state_names[FrontWall::NumStates] = {
    "PUSH",
    "BUMP" };

FrontWall::FrontWall()
{
  fsm.init(state_names,NumStates,PUSH,16,16);
  track_objects = new BeTrackObjects();
}

FrontWall::~FrontWall(){
}

// note: all code is for the left side of the goal currently
double FrontWall::operator()(FeatureSet *features, MotionCommand *command) {

  bool done=false;

  command->setBinLEDs(LED_EARS_RED);

  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case BUMP: 
      {
        static int kick;
        if (fsm.isNewState()) {
          pprintf(TextOutputStream,"bump\n");
          track_objects->reset(features->current_time);

          if (fabs(features->position.x) > 300) { // FIXME half goal width
            kick = MOTION_KICK_BUMP;
          }
          else {
            switch (rand() % 4) {
            case 0:
            case 1:
              kick = MOTION_KICK_BUMP;
              break;
            case 2:
              kick = MOTION_KICK_ARM_DIVE_R;
              break;
            case 3:
              kick = MOTION_KICK_ARM_DIVE_L;
              break;
            }
          }
        }

        command->motion_cmd = kick;

        if (fsm.timeInState() > SecToTime(0.2) && 
            features->motion_state != Motion::STATE_KICKING) {
          TRANS_CONT(fsm, PUSH, 5, "done with bump");
        }
        (*track_objects)(features,command,false,true);
        done = true;
        break;
      }
    case PUSH: 
      {
        if (fsm.isNewState()) {
          pprintf(TextOutputStream,"push\n");
          track_objects->reset(features->current_time);
        }
        if (fsm.timeInState() > SecToTime(2.0) &&
            features->have_ball) {
            //            fabs(features->position.x) < penaltyRegionHalfWidth + 100) {
//             (features->ball_on_chest ||
//              (features->ball_vector.x < 150 &&
//               fabs(features->ball_vector.y) < 20.0))) {
          TRANS_CONT(fsm, BUMP, 5, "timed out with ball on chest");
        }
        vector2d target; // relative coords
        vector2d target_angle_pt;
        double score_sign = sign(features->score_goal_const.x);
        if (features->have_ball || features->ball_on_chest) { // FIXME: side
          target = features->world_model->score_goal();
          target_angle_pt = 
            target + features->getRelativePosn(vector2d(score_sign * 150, 0));
        }
        else {
          target = features->ball_vector; // FIXME: if wrong side fail
          target_angle_pt = target;
        }

        command->motion_cmd = MOTION_WALK_TROT_FAST;
        command->vx = target.x;
        command->vy = 0;
        command->va = target_angle_pt.angle();

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

  return 1.0;
}

double FrontWall::status(FeatureSet *features){
  if (!features->near_ball) {
    return -1.0;
  }
  if (!(features->current_time - features->ball_last_seen_med < SecToTime(1.0)
        && features->valid_ball_hyp)) {
    return -1.0;
  }
  if (!features->ball_on_front_wall) {
    return -1.0;
  }
  if (!features->facing_score_goal) {
    return -1.0;
  }
  if (!features->facing_ball) {
    return -1.0;
  }
//   if (!features->ball_between_self_and_goal_y) {
//     return -1.0;
//   }
  // Otherwise continue. We never return success, because success only
  // happens if we score.
  return 0.0;
}

void FrontWall::reset(ulong time){
  track_objects->reset(time);
}

bool FrontWall::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);
  return true;
}

bool FrontWall::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

const MotionCommand *FrontWall::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/

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

Default::Default(){
};

Default::~Default(){
}

double Default::operator()(FeatureSet *features, MotionCommand *command) {
  
  command->motion_cmd = MOTION_STAND_NEUTRAL;
  
  return 1.0;
}

double Default::status(FeatureSet *features){
  return 0.0;
}

void Default::reset(ulong time){
}

bool Default::initConnections(){
  return true;
}

bool Default::setupEventMgr(){
  event_mgr = EventManager::getManager();

  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  fs_ep = event_mgr->getEventProcessor(fs_id);
  fs = (FeatureSet *)(fs_ep);
  
  return true;
}

const MotionCommand *Default::get(ulong time){
  FeatureSet *lfs=NULL; // local feature set
  mzero(out_command);
  
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;

}


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

/*============================================================*/