/* 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 "Behaviors.h"
#include "BeLowLevel.h"
#include "state_machine.h"
#include "../headers/CircBufPacket.h"
#include "../headers/Dictionary.h"
#include "../headers/FileSystem.h"
#include "../headers/Sensors.h"
#include "../Logger/LoggerInterface.h"
#include "../Main/globals.h"
#include "../Main/RobotMain.h"
#include "../Motion/Kinematics.h"

#include "MiscBehaviors.h"

void
BehaviorSystem::camera(FeatureSet *FS)
{
  logCallChain(CALL_camera);

#if 0
  static const int frame_delay = 10;

  static bool wrote_log = false;

  static bool log_button_down=false;
  static bool front_head_button_down=false;
  static int fhb_down_cnt=-frame_delay;
  static bool cur_fhb_down=false;
  static bool old_fhb_down=false;
      
  front_head_button_down = ((sensor_data->button & HeadFrontButton) != 0);
  log_button_down = ((sensor_data->button & LogButton) != 0);

  double tilt_norm,pan_norm; // range from [-1,1]

  tilt_norm =  sensor_data->getFrame(0)->tailAngles[TailTiltReading]/RAD(22);
  pan_norm  = -sensor_data->getFrame(0)->tailAngles[TailPanReading ]/RAD(22);

  command->tail_cmd = TAIL_FOLLOW;
  command->head_cmd = HEAD_ANGLES;
  command->head_tilt = .5*(head_tilt_min + head_tilt_max + (head_tilt_max-head_tilt_min)*tilt_norm);
  command->head_pan  = .5*(head_pan_min  + head_pan_max  + (head_pan_max -head_pan_min )*pan_norm );
  command->head_roll = 0.0;

  fhb_down_cnt += (front_head_button_down ? 1 : -1);
  fhb_down_cnt = bound(fhb_down_cnt,-frame_delay,frame_delay);

  cur_fhb_down = (fhb_down_cnt == frame_delay);

  if(cur_fhb_down) {
    if(old_fhb_down) {
      command->led.cmd = setbits(command->led.cmd,
                             LED_LOWER_LEFT  | LED_LOWER_RIGHT  |
                             LED_MIDDLE_LEFT | LED_MIDDLE_RIGHT |
                             LED_UPPER_LEFT  | LED_UPPER_RIGHT  |
                             LED_TAIL_RED    | LED_TAIL_BLUE);
    } else {
      VisionInterface::SendRLEImage(RobotMain::GetObject().getVision());
      VisionInterface::SendRawImage(RobotMain::GetObject().getVision());

      old_fhb_down = true;
    }
  } else {
    if(fhb_down_cnt == -frame_delay)
      old_fhb_down = false;
  }

  if(wrote_log) {
    command->led.cmd = setbits  (command->led.cmd,
                             LED_LOWER_LEFT   |
                             LED_MIDDLE_RIGHT |
                             LED_UPPER_LEFT   |
                             LED_TAIL_RED);
    command->led.cmd = clearbits(command->led.cmd,
                             LED_LOWER_RIGHT |
                             LED_MIDDLE_LEFT |
                             LED_UPPER_RIGHT |
                             LED_TAIL_BLUE);
    command->head_cmd = HEAD_SCAN_MARKERS;
  }

  if(log_button_down && !wrote_log && LogControlStream!=NULL) {
    LogControl control;
    control.command = LogControl::WriteLog;
    LogControlStream->writeBinary((const uchar *)&control,(int)sizeof(control));
    wrote_log = true;
  }

#endif
}

// ===== ChaseBall implementation =========================================== //

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

char const * const ChaseBall::state_names[ChaseBall::NumStates] = {
  "GOTO_BALL",
  "HEAD_SEARCH_BALL",
  "SPIN_SEARCH_BALL",
  "WALK_SEARCH_BALL",
  "KICK"};

const int ChaseBall::kick_ids[ChaseBall::NumKicks] = {
  MOTION_KICK_DIVE,
  MOTION_KICK_BUMP,
  MOTION_KICK_FOREWARD,
  MOTION_KICK_HEAD_HARD_L,
  MOTION_KICK_SWING_R
};

static const ulong BallLostTime   =  250000UL;
static const ulong HeadSearchTime = 2000000UL;
static const double WalkSearchDist =  500.0;
static const ulong WalkSearchTime = (ulong)(1.0e6 * WalkSearchDist / MaxDX);

ChaseBall::ChaseBall()
{
  LookAtObject = new BeLookAtObject();
  LookAtObjectInfo = new BeLookAtObjectInfo();

  LookAtObject->setFixateTimeNoReset(0UL);
  LookAtObject->setTargetNoReset(VisionInterface::BALL);

  fsm.init(state_names,NumStates,HEAD_SEARCH_BALL,16,16);

  fs_id = ~0;
  fs = NULL;

  kick_index = 0;

  SpinSearchTime = (ulong)(2*M_PI*1.0e6 / MaxDA);
}

ChaseBall::~ChaseBall()
{
}

const ulong cb_wait_time = SecToTime(2.0);

void
ChaseBall::reset(ulong time) {
  fsm.setState(HEAD_SEARCH_BALL,0,"Reset",time);
  SpinSearchTime = (ulong)(2*M_PI*1.0e6 / MaxDA);
}

double
ChaseBall::operator()(FeatureSet *FS, MotionCommand *command) {
  vector2d ball;
  double ball_angle;

  ball       = FS->ball_vector;
  ball_angle = atan2a(ball.y,ball.x + 65); // 65mm is roughly the distance from center to neck

  if(FS->vision_info->ball.confidence > 0.4) {
    // ball.x = FS->vision_info->ball.loc.x;
    // ball.y = FS->vision_info->ball.loc.y;
    ball_side_last_seen = (int)sign_nz(ball.y);
  }

  (*LookAtObject)(FS,LookAtObjectInfo,command);

  bool done=false;
  fsm.startLoop(FS->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
      case GOTO_BALL:
        {
          if(fabs(ball.y)<50 && ball.x<125){
            TRANS_CONT(fsm,KICK,5,"BallKickable");
          }

          if(fsm.timeInState() > BallLostTime){
            TRANS_CONT(fsm,HEAD_SEARCH_BALL,6,"BallLost");
          }

          command->motion_cmd = MOTION_WALK_TROT;
          command->vx = ball.x;
          command->vy = ball.y;
          command->va = ball_angle;
          done = true;
        } break;
      case HEAD_SEARCH_BALL:
        {
          if(FS->see_ball_med){
            TRANS_CONT(fsm,GOTO_BALL,5,"SeeBall");
          }
          
          if(fsm.timeInState() > HeadSearchTime){
            TRANS_CONT(fsm,SPIN_SEARCH_BALL,6,"Timeout");
          }
          
          command->motion_cmd = MOTION_STAND_NEUTRAL;
          done = true;
        } break;
      case SPIN_SEARCH_BALL:
        {
          if(!fsm.isNewState()){
	    SpinSearchTime = (ulong)(2*M_PI*1.0e6 / MaxDA * (1.0 + (rand() / (double)RAND_MAX)));
	  }

          if(FS->see_ball_med){
            TRANS_CONT(fsm,GOTO_BALL,5,"SeeBall");
          }

          if(fsm.timeInState() > SpinSearchTime){
            TRANS_CONT(fsm,WALK_SEARCH_BALL,6,"Timeout");
          }

          command->motion_cmd = MOTION_WALK_TROT;
          command->vx = 0.0;
          command->vy = 0.0;
          command->va = ball_side_last_seen*MaxDA;
          command->head_cmd = HEAD_SCAN_BALL_TURN;
          done = true;
        } break;
      case WALK_SEARCH_BALL:
        {
          if(FS->see_ball_med) {
            TRANS_CONT(fsm,GOTO_BALL,5,"SeeBall");
          }

          if(fsm.timeInState() > WalkSearchTime){
            TRANS_CONT(fsm,SPIN_SEARCH_BALL,6,"Timeout");
          }

          command->motion_cmd = MOTION_WALK_TROT;
          command->vx = MaxDX;
          command->vy = 0.0;
          command->va = 0.0;
          done = true;
        } break;
      case KICK:
        {
          if(!fsm.isNewState()){
	    command->motion_cmd = MOTION_STAND_NEUTRAL;

	    if(fsm.timeInState() > cb_wait_time)
	      TRANS_CONT(fsm,HEAD_SEARCH_BALL,5,"Done");
          } else {
	    command->motion_cmd = kick_ids[kick_index];
	    kick_index = (kick_index + 1) % NumKicks;
	  }

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

bool ChaseBall::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *fs_ep;

  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 ChaseBall::setupEventMgr(){
  return true;
};

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

const MotionCommand *ChaseBall::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(ChaseBall,ChaseBall::beh_name,ChaseBall::create);

void BehaviorSystem::chaseBall(FeatureSet *FS) {
  logCallChain(CALL_chaseBall);
  
  static ChaseBall *chase_ball = NULL;
  if(chase_ball==NULL){
    chase_ball = new ChaseBall();
    //chase_ball->initConnections();
  }

  //(*chase_ball)(FS,command);
  const MotionCommand *out_command;
  out_command = chase_ball->get(FS->current_time);
  *command = *out_command;
}

// ===== Getup implementation =============================================== //

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

Getup::Getup()
{
}

Getup::~Getup()
{
}

void
Getup::reset(ulong time) {
}

double
Getup::operator()(FeatureSet *FS, MotionCommand *command) {
  int cmd;

  switch(RobotMain::GetObject().posture.getPosture()){
    case PostureDetector::PosFallFront:
      cmd = Motion::MOTION_GETUP_FRONT;
      break;
    case PostureDetector::PosFallBack:
      cmd = Motion::MOTION_GETUP_BACK;
      break;
    case PostureDetector::PosFallLeft:
      cmd = Motion::MOTION_GETUP_LEFT;
      break;
    case PostureDetector::PosFallRight:
      cmd = Motion::MOTION_GETUP_RIGHT;
      break;
    default:
      cmd = -1;
      break;
  }

  if(cmd != -1){
    command->motion_cmd = cmd;
  }

  return 1.0;
}

bool Getup::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *fs_ep;

  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 Getup::setupEventMgr(){
  return true;
};

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

const MotionCommand *Getup::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(Getup,Getup::beh_name,Getup::create);

// ===== Kick test ============================================== //

static const int KicksToTest[]={
  MOTION_GOAL_HAPPY1,
  MOTION_KICK_DIVE       ,
  MOTION_KICK_BUMP       ,
  MOTION_KICK_FOREWARD   ,
  MOTION_KICK_HEAD_L     ,
  MOTION_KICK_HEAD_SOFT_L,
  MOTION_KICK_HOLD       ,
  MOTION_GETUP_BACK   ,
  MOTION_GETUP_FRONT  ,
  MOTION_GETUP_LEFT};

static const int NumKicksToTest = sizeof(KicksToTest)/sizeof(KicksToTest[0]);

KickTest::KickTest()
{
  LookAtObject = new BeLookAtObject();
  LookAtObjectInfo = new BeLookAtObjectInfo();

  LookAtObject->setFixateTimeNoReset(0UL);
  LookAtObject->setTargetNoReset(VisionInterface::BALL);

  reset();
}

KickTest::~KickTest()
{
}

void
KickTest::reset() {
  CurrentState = HEAD_SEARCH_BALL;
  Timer = 0UL;
  SpinSearchTime = (ulong)(2*M_PI*1.0e6 / MaxDA);
  CurrentKickIdx = 0;
}

double
KickTest::operator()(FeatureSet *FS, MotionCommand *Command) {
  static const int frame_delay = 5;
  static bool sel_down_cur[2]={false       ,false       };
  static int  sel_down_cnt[2]={-frame_delay,-frame_delay};
  static bool cur_sel_down[2]={false       ,false       };
  static bool old_sel_down[2]={false       ,false       };
  bool sel_down_edge[2]={false,false};
  bool sel_up_edge  [2]={false,false};
  bool sel_down     [2]={false,false};
  bool sel_up       [2]={false,false};

  sel_down_cur[0] = ((FS->sensors->button & SelNextButton) != 0);
  sel_down_cur[1] = ((FS->sensors->button & SelPrevButton) != 0);

  for(int i=0; i<2; i++) {
    sel_down_cnt[i] += (sel_down_cur[i] ? 1 : -1);
    sel_down_cnt[i] = bound(sel_down_cnt[i],-frame_delay,frame_delay);
    cur_sel_down[i] = (cur_sel_down[i] ? (sel_down_cnt[i] != -frame_delay) : (sel_down_cnt[i] == frame_delay));
    
    if(cur_sel_down[i]) {
      if(old_sel_down[i]) {
        sel_down[i] = true;
      } else {
        sel_down_edge[i] = true;
      }
    } else {
      if(old_sel_down[i]) {
        sel_up_edge[i] = true;
      } else {
        sel_up[i] = true;
      }
    }
                       
    old_sel_down[i] = cur_sel_down[i];
  }

  if(sel_down_edge[0])
    CurrentKickIdx = (CurrentKickIdx+1)%NumKicksToTest;
  if(sel_down_edge[1])
    CurrentKickIdx = (CurrentKickIdx+NumKicksToTest-1)%NumKicksToTest;

  Command->led.cmd = (unsigned short)CurrentKickIdx;

  vector2d ball;
  double ball_angle;

  ball       = FS->ball_vector;
  ball_angle = FS->ball_angle;
  if(FS->vision_info->ball.confidence > 0.4) {
    ball.x = FS->vision_info->ball.loc.x;
    ball.y = FS->vision_info->ball.loc.y;
    ball_angle = atan2a(ball.y,ball.x);
  }

  (*LookAtObject)(FS,LookAtObjectInfo,Command);

  int n = NumStates;
  bool done = false;
  while(n-- > 0 && !done) {
    switch(CurrentState) {
    case GOTO_BALL:
      {
        vector2d walk_dir;

        if(fabs(ball.y)<50 && ball.x<65){
          CurrentState = KICK;
          continue;
        }

        if(FS->current_time - FS->ball_last_seen_med > BallLostTime) {
          CurrentState = HEAD_SEARCH_BALL;
          Timer = FS->current_time;
          continue;
        }

        walk_dir = ball;
        walk_dir.x -= 20;

        Command->motion_cmd = MOTION_WALK_TROT;
        Command->vx = walk_dir.x;
        Command->vy = walk_dir.y;
        Command->va = walk_dir.angle();
        done = true;
      } break;
    case HEAD_SEARCH_BALL:
      {
        if(FS->see_ball_med) {
          CurrentState = GOTO_BALL;
          Timer = FS->current_time;
          continue;
        }

        if(FS->current_time - Timer > HeadSearchTime) {
          CurrentState = SPIN_SEARCH_BALL_INIT;
          Timer = FS->current_time;
          continue;
        }

        Command->motion_cmd = MOTION_STAND_NEUTRAL;
        done = true;
      } break;
    case SPIN_SEARCH_BALL_INIT:
      {
        SpinSearchTime = (ulong)(2*M_PI*1.0e6 / MaxDA * (1.0 + (rand() / (double)RAND_MAX)));

        CurrentState = SPIN_SEARCH_BALL;
      } break;
    case SPIN_SEARCH_BALL:
      {
        if(FS->see_ball_med) {
          CurrentState = GOTO_BALL;
          Timer = FS->current_time;
          continue;
        }

        if(FS->current_time - Timer > SpinSearchTime) {
          CurrentState = WALK_SEARCH_BALL;
          Timer = FS->current_time;
          continue;
        }

        Command->motion_cmd = MOTION_WALK_TROT;
        Command->vx = 0.0;
        Command->vy = 0.0;
        Command->va = MaxDA;
        Command->head_cmd = HEAD_SCAN_BALL_TURN;
        done = true;
      } break;
    case WALK_SEARCH_BALL:
      {
        if(FS->see_ball_med) {
          CurrentState = GOTO_BALL;
          Timer = FS->current_time;
          continue;
        }

        if(FS->current_time - Timer > WalkSearchTime) {
          CurrentState = SPIN_SEARCH_BALL_INIT;
          Timer = FS->current_time;
          continue;
        }

        Command->motion_cmd = MOTION_WALK_TROT;
        Command->vx = MaxDX;
        Command->vy = 0.0;
        Command->va = 0.0;
        done = true;
      } break;
    case KICK:
      {
        Command->motion_cmd = KicksToTest[CurrentKickIdx];
        done = true;

        CurrentState = HEAD_SEARCH_BALL;
      };
    default:
      return 0.0;
    }
  }
  
  return (done ? 1.0 : 0.0);
}

void BehaviorSystem::kickTest(FeatureSet *FS) {
  logCallChain(CALL_test);
  
  static KickTest *kick_test = NULL;
  if(kick_test==NULL)
    kick_test = new KickTest();

  (*kick_test)(FS,command);
}

// ===== Static Kick Test ============================================== //

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

char const * const StaticKickTest::state_names[NumStates] = {
  "STAND",
  "KICK"};

StaticKickTest::StaticKickTest()
{
  LookAtObject = new BeLookAtObject();
  LookAtObjectInfo = new BeLookAtObjectInfo();

  LookAtObject->setFixateTimeNoReset(0UL);
  LookAtObject->setTargetNoReset(VisionInterface::BALL);

  fsm.init(state_names,NumStates,STAND,16,16);

  fs_id = ~0;
  fs = NULL;
}

StaticKickTest::~StaticKickTest()
{
}

void
StaticKickTest::reset(ulong time) {
  fsm.setState(STAND,0,"Reset",time);
}

double
StaticKickTest::operator()(FeatureSet *FS, MotionCommand *command) {
  vector2d ball;
  double ball_angle;

  ball       = FS->ball_vector;
  ball_angle = FS->ball_angle;
  if(FS->vision_info->ball.confidence > 0.4) {
    ball.x = FS->vision_info->ball.loc.x;
    ball.y = FS->vision_info->ball.loc.y;
    ball_angle = atan2a(ball.y,ball.x);
  }

  (*LookAtObject)(FS,LookAtObjectInfo,command);

  command->head_cmd = HEAD_LOOKAT;
  command->head_lookat.set(200.0, 0.0, 0.0);

  bool done=false;
  fsm.startLoop(FS->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
      case STAND:
        {
          if(FS->sensors->button & ChinButton){
            TRANS_CONT(fsm,KICK,5,"KickButton");
          }
          
          command->motion_cmd = MOTION_STAND_NEUTRAL;
          command->tail_cmd = TAIL_AIM;
          command->tail_pan  =   0;
          command->tail_tilt = 100;
          done = true;
        } break;
      case KICK:
        {
          if(!fsm.isNewState())
            TRANS_CONT(fsm,STAND,5,"Done");

          command->motion_cmd = MOTION_GOAL_HAPPY1;
          command->tail_cmd = TAIL_AIM;
          command->tail_pan  =   0;
          command->tail_tilt = 100;
	  
	  done = true;
	} break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();
  
  return 1.0;
}

bool StaticKickTest::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *fs_ep;

  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 StaticKickTest::setupEventMgr(){
  return true;
};

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

const MotionCommand *StaticKickTest::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(StaticKickTest,StaticKickTest::beh_name,StaticKickTest::create);

//========================== Motion calibration code ===================== //

class BeTestMotion
{
public:
  static const int beh_id;

  static char const * const beh_name;

  static const int NumStates=5;
  enum State { GOTO_CENTER, RESET_FEET, LOCALIZE, RUN_MOTION, CONTROLLED_STOP };

  static char const * const state_names[NumStates];

  typedef FiniteStateMachine<State,ulong> FSM;

private:
  FSM fsm;
  bool ran_motion;
  ulong timer;

  vector2d pre_loc;
  double pre_angle;
  vector2d post_loc;
  double post_angle;

  int move_frac_idx;
  int angle_idx;

  double dx,dy,da;

  BeGotoPointGlobal *goto_point;

public:
  BeTestMotion();

  double operator()(FeatureSet *features,MotionCommand *command);
};

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

char const * const BeTestMotion::state_names[BeTestMotion::NumStates] = {
  "GOTO_CENTER",
  "RESET_FEET",
  "LOCALIZE",
  "RUN_MOTION",
  "CONTROLLED_STOP"};


const int BeTestMotion::beh_id          = 20;

static const ulong tm_time_to_run_motion = SecToTime(4.0);
static const ulong tm_time_to_reset_feet = SecToTime(1.3);
static const ulong tm_time_to_stop       = SecToTime(1.0);
static const ulong tm_time_to_localize_extra = SecToTime(4.0);
static const double tm_localized_pos = 100.0;
static const double tm_localized_ang = 0.1;

static const int tm_num_angles=16;

struct MoveDescription {
  double frac_linear,frac_angular;
  bool all_angles;
};

static const MoveDescription tm_move_fracs[]={
  {0.0, -1.0,  false}, // 16
  {0.0, 1.0,   false}, // 09
  {0.0, -0.5,  false}, // 13
  {0.0, 0.5,   false}, // 06
  {0.0, -0.25, false}, // 10
  {0.0, 0.25,  false}, // 03
  {0.0, 0.0,   false}, // 00
  {1.0, 0.0,   true},  // 02
  {0.5, 0.0,   true},  // 01
  {0.5, 0.25,  true},  // 04
  {1.0, 0.25,  true},  // 05
  {0.5, 0.5,   true},  // 07
  {1.0, 0.5,   true},  // 08
  {0.5, -0.25, true},  // 11
  {1.0, -0.25, true},  // 12
  {0.5, -0.5,  true},  // 14
  {1.0, -0.5,  true},  // 15
};
  

BeTestMotion::BeTestMotion()
{
  fsm.init(state_names,NumStates,GOTO_CENTER,16,16);
  ran_motion = false;
  move_frac_idx = 0;
  angle_idx = 0;

  goto_point = new BeGotoPointGlobal;

  dx = dy = da = 0.0;
}

double BeTestMotion::operator()(FeatureSet *features,MotionCommand *command)
{
  bool done=false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
      case GOTO_CENTER:
        {
          double target_angle = atan2a(halfWidth,halfLength);
          vector2d target_pos(0.0,0.0);
          
          if(GVector::distance(features->my_position.position.mean,target_pos) < 100.0 &&
             fabs(features->my_position.heading.mean.angle() - target_angle) < 0.1){
            TRANS_CONT(fsm,RESET_FEET,5,"AtCenter");
          }
          
          ran_motion = false;
          
          if(fsm.isNewState()){
            goto_point->setTarget(V2COMP(target_pos),target_angle);
          }
          
          (*goto_point)(features,command);
          command->head_cmd = Motion::HEAD_SCAN_MARKERS;
        }

        done=true;
        break;
      case RESET_FEET:
        if(fsm.timeInState() > tm_time_to_reset_feet){
          TRANS_CONT(fsm,LOCALIZE,5,"Timeout");
        }

        command->motion_cmd = Motion::MOTION_WALK_TROT;
        command->vx = 0.0;
        command->vy = 0.0;
        command->va = 0.0;
        command->head_cmd = Motion::HEAD_SCAN_MARKERS;

        done=true;
        break;
      case LOCALIZE:
        if(fsm.isNewState()){
          timer=0UL;
        }

        if(timer==0UL &&
           features->my_position.position.sMaj < tm_localized_pos &&
           features->my_position.heading.sMaj < tm_localized_ang){
          timer = fsm.timeInState() + tm_time_to_localize_extra;
        }

        if(timer > 0UL && fsm.timeInState() > timer){
          if(ran_motion){
            post_loc = features->my_position.position.mean;
            post_angle = features->my_position.heading.mean.angle();

            pprintf(TextOutputStream,"MT: motion: dx=%g dy=%g da=%g pre (%g,%g) <%g post (%g,%g) <%g neck %g\n",
                    dx,dy,da,
                    V2COMP(pre_loc), pre_angle,
                    V2COMP(post_loc),post_angle,
                    features->neck_offset.x);

            TRANS_CONT(fsm,GOTO_CENTER,6,"Localized");
          }else{
            pre_loc = features->my_position.position.mean;
            pre_angle = features->my_position.heading.mean.angle();
            TRANS_CONT(fsm,RUN_MOTION,5,"Localized");
          }
        }

        command->motion_cmd = Motion::MOTION_STAND_NEUTRAL;
        command->head_cmd = Motion::HEAD_SCAN_MARKERS;

        done=true;
        break;
      case RUN_MOTION:
        if(fsm.timeInState() > tm_time_to_run_motion){
          if(tm_move_fracs[move_frac_idx].all_angles){
            angle_idx++;
            if(angle_idx >= tm_num_angles){
              angle_idx = 0;
              move_frac_idx = (move_frac_idx+1) % (sizeof(tm_move_fracs)/sizeof(tm_move_fracs[0]));
            }
          }else{
            move_frac_idx = (move_frac_idx+1) % (sizeof(tm_move_fracs)/sizeof(tm_move_fracs[0]));
            angle_idx=0;
          }

          TRANS_CONT(fsm,CONTROLLED_STOP,5,"Timeout");
        }

        ran_motion = true;

        if(fsm.isNewState()){
          vector2d walk_dir;
          double angle;
          double linear_speed;
          double angular_speed;
          
          angle = 2*M_PI * angle_idx / (double)tm_num_angles;
          walk_dir.set(cos(angle),sin(angle));
          
          angular_speed = tm_move_fracs[move_frac_idx].frac_angular;
          linear_speed  = tm_move_fracs[move_frac_idx].frac_linear;
          linear_speed *= (1.0 - fabs(angular_speed));

          dx = linear_speed  * walk_dir.x *MaxDX;
          dy = linear_speed  * walk_dir.y *MaxDY;
          da = angular_speed * MaxDA;
        }

        command->motion_cmd = Motion::MOTION_WALK_TROT;
        command->vx = dx;
        command->vy = dy;
        command->va = da;
        command->head_cmd = Motion::HEAD_SCAN_MARKERS;

        done=true;
        break;
      case CONTROLLED_STOP:
        if(fsm.timeInState() > tm_time_to_stop){
          TRANS_CONT(fsm,LOCALIZE,5,"Timeout");
        }

        command->motion_cmd = Motion::MOTION_WALK_TROT;
        command->vx = 0.0;
        command->vy = 0.0;
        command->va = 0.0;
        command->head_cmd = Motion::HEAD_SCAN_MARKERS;

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

void BehaviorSystem::motion_test(FeatureSet *FS) {
  static BeTestMotion *test_motion = NULL;
  if(test_motion==NULL)
    test_motion = new BeTestMotion();

  (*test_motion)(FS,command);
}

//========================== Localization test/challenge =====================

double optimalOrdering(const vector2d pointList[5], const vector2d &point0, 
		       double angle0, int ordering[5]) {
  int bestpoints[5];
  double newangle, diffangle;
  double diffpos;
  double difftime, newtime;
  double besttime = -1.0;
  double oldangle;
  vector2d list[5];
  vector2d newvector;
  
  /* An optimizing compiler should be able to optimize this... */
  /* This function might be better generated by another program.. at least
     for the arbitrary case. */

  memcpy(list, pointList, sizeof(list));

  for (int a = 0; a < 5; a++) {
    newvector = list[a] - point0;
    newangle = atan2(newvector.y, newvector.x);
    diffpos = newvector.length();
    diffangle = min(fabs(norm_angle(newangle - angle0)),
		    fabs(norm_angle(angle0 - newangle)));
    difftime = diffangle/MaxDA + diffpos/MaxDX;
    {
      oldangle = newangle;
      newtime = difftime;
      for (int b = 0; b < 5; b++) {
	if (b == a) continue;
	newvector = list[b] - list[a];
	newangle = atan2(newvector.y, newvector.x);
	diffpos = newvector.length();
	diffangle = min(fabs(norm_angle(newangle - oldangle)),
			fabs(norm_angle(oldangle - newangle)));
	difftime = diffangle/MaxDA + diffpos/MaxDX;
	{
	  double savtime = newtime;
	  double savangle = oldangle;
	  newtime += difftime;
	  oldangle = newangle;
	  for (int c = 0; c < 5; c++) {
	    if (c == a || c == b) continue;
	    newvector = list[c] - list[b];
	    newangle = atan2(newvector.y, newvector.x);
	    diffpos = newvector.length();
	    diffangle = min(fabs(norm_angle(newangle - oldangle)),
			    fabs(norm_angle(oldangle - newangle)));
	    difftime = diffangle/MaxDA + diffpos/MaxDX;
	    {
	      double savtime = newtime;
	      double savangle = oldangle;
	      newtime += difftime;
	      oldangle = newangle;
	      for (int d = 0; d < 5; d++) {
		if (d == a || d == b || d == c) continue;
		newvector = list[d] - list[c];
		newangle = atan2(newvector.y, newvector.x);
		diffpos = newvector.length();
		diffangle = min(fabs(norm_angle(newangle - oldangle)),
				fabs(norm_angle(oldangle - newangle)));
		difftime = diffangle/MaxDA + diffpos/MaxDX;
		{
		  double savtime = newtime;
		  double savangle = oldangle;
		  newtime += difftime;
		  oldangle = newangle;
		  for (int e = 0; e < 5; e++) {
		    if (e == a || e == b || e == c || e == d) continue;
		    newvector = list[e] - list[d];
		    newangle = atan2(newvector.y, newvector.x);
		    diffpos = newvector.length();
		    diffangle = min(fabs(norm_angle(newangle - oldangle)),
				    fabs(norm_angle(oldangle - newangle)));
		    difftime = diffangle/MaxDA + diffpos/MaxDX;
		    newtime += difftime;
		    if (newtime < besttime || besttime < 0.0) {
		      besttime = newtime;
		      bestpoints[0] = a;
		      bestpoints[1] = b;
		      bestpoints[2] = c;
		      bestpoints[3] = d;
		      bestpoints[4] = e;
		    }
		  }
		  oldangle = savangle;
		  newtime = savtime;
		}
	      }
	      oldangle = savangle;
	      newtime = savtime;
	    }
	  }
	  oldangle = savangle;
	  newtime = savtime;
	}
      }
    }
  }
  memcpy(ordering, bestpoints, sizeof(bestpoints));
  return besttime;
}

char *  LocalizationTestFileName;
const double dxCutOff   = 20;
const double xDevCutOff = 200;
const double dyCutOff   = 20;
const double yDevCutOff = 200;
const double daCutOff   = .1;
const double aDevCutOff = .3;
ulong const pauseTime = SecToTime(3.1);

static const double blc_get_more_info_time    = SecToTime(3.0);
static const ulong blc_head_scan_marker_time = SecToTime(3.0);
static const ulong blc_head_scan_ball_time   = SecToTime(3.0);
static const ulong blc_head_period = blc_head_scan_marker_time + blc_head_scan_ball_time;

static const vector2d global_localize_loc(1100.0,0.0);
static const double global_localize_angle = 0.0;

BeGotoPointGlobal *GotoPoint = NULL;

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

char const * const LocalizationTest::state_names[LocalizationTest::NumStates] = {
  "INIT",
  "ORDER_POINTS",
  "START_GLOBAL_LOCALIZE",
  "GLOBAL_LOCALIZE",
  "GOTO_WAYPOINT",
  "INPLACE_GLOBAL_LOCALIZE",
  "WAG_TAIL"};


LocalizationTest::LocalizationTest()
{
  goto_point = new BeGotoPointGlobal();
  track_objects = new BeTrackObjects();

  fsm.init(state_names,NumStates,GOTO_WAYPOINT,16,16);

  LocalizationTestFileName = "/MS/config/points.cfg";
    
  global_localizing=false;
  goal_loc.set(0.0,0.0);
}

LocalizationTest::~LocalizationTest()
{
}

void LocalizationTest::reset()
{
  fsm.setState(INIT,0,"Reset",GetTime());
  global_localizing=false;
}

void LocalizationTest::read_waypoints(vector3d **waypoints,int *num_waypoints)
{
  HFS::FILE *waypoint_file;
  char buf[512];

  waypoint_file = HFS::fopen(LocalizationTestFileName,"rt");
  if(waypoint_file == NULL){
    pprintf(TextOutputStream,"unable to open '%s'\n",LocalizationTestFileName);
    *waypoints = NULL;
    *num_waypoints = 0;
    return;
  }
  
  int line_cnt=0;
  while(HFS::fgets(buf,sizeof(buf),waypoint_file)!=NULL){
    int x,y;
    if(sscanf(buf,"%d %d\n",&x,&y)==2){
      line_cnt++;
    }
  }

  HFS::fclose(waypoint_file);

  *waypoints = new vector3d[line_cnt];

  waypoint_file = HFS::fopen(LocalizationTestFileName,"rt");
  if(waypoint_file == NULL){
    pprintf(TextOutputStream,"unable to open '%s' on second time\n",LocalizationTestFileName);
    delete[] (*waypoints);
    *waypoints = NULL;
    *num_waypoints = 0;
    return;
  }
  
  int line_idx=0;
  while(HFS::fgets(buf,sizeof(buf),waypoint_file)!=NULL){
    int xi,yi;
    if(sscanf(buf,"%d %d\n",&xi,&yi)==2){
      double x,y;

      x = -xi*10.0;
      y = -yi*10.0;

      (*waypoints)[line_idx].set(x,y,0.0);
      line_idx++;
    }
  }

  HFS::fclose(waypoint_file);

  *num_waypoints = line_idx;
}

double LocalizationTest::operator()(FeatureSet *fs,MotionCommand *command)
{
  const bool looping = true;
  const bool useForDebug = false;
  static bool inited = false;
  static ulong outsideTime = 0;
  static ulong startTime = 0;

  (*track_objects)(fs,command,true,false);
  //ulong phase_time;
  //phase_time = 
  //  fs->current_time -
  //  (fs->current_time / blc_head_period) * blc_head_period;
  //if(phase_time < blc_head_scan_marker_time){
  //  command->head_cmd = HEAD_SCAN_MARKERS;
  //}else{
  //  command->head_cmd = HEAD_SCAN_BALL;
  //}

  if(fs->robot_is_paused){
    outsideTime = fs->current_time;
    startTime = fs->current_time;
    return 1.0;
  }
  
  static vector3d *waypoints = NULL;
  static bool *fixed_angles = NULL;
  static int num_waypoints = 0;
  static int cur_waypoint_idx = 0;

  if(waypoints==NULL || num_waypoints==0){
    // read in the waypoints
    read_waypoints(&waypoints,&num_waypoints);
    fixed_angles = new bool[num_waypoints];
    for(int i=0; i<num_waypoints; i++)
      fixed_angles[i] = false;
  }
  
  command->motion_cmd = MOTION_WALK_TROT;
  command->vx = 0.0;
  command->vy = 0.0;
  command->va = 0.0;

  bool done=false;
  fsm.startLoop(fs->current_time);
  while(!done && fsm.error==0){
    //pprintf(TextOutputStream,"state is %d\n",fsm.getState());
    switch(fsm.getState()){
      case INIT:
        {
          TRANS_CONT(fsm,ORDER_POINTS,5,"Start");
        }
        break;
      case ORDER_POINTS:
        {
          if(!inited) {
            outsideTime = fs->current_time;
            
            if(!fs->robot_is_paused) {
              inited = true;
              if(num_waypoints == 5){
                int i;
                for(i = 0; i < 5; i++){
                  if(fixed_angles[i])
                    break;
                }
                if(i == 5){ /* No fixed angle and 5 points, so find optimal order */
                  int ordering[5];
                  vector2d points[5];
                  for(i = 0; i < 5; i++){
                    points[i].x = waypoints[i].x;
                    points[i].y = waypoints[i].y;
                  }
                  optimalOrdering(points, global_localize_loc, global_localize_angle, 
                                  ordering);
                  for(i = 0; i < 5; i++){
                    waypoints[i].x = points[ordering[i]].x;
                    waypoints[i].y = points[ordering[i]].y;
                    // Angle doesn't matter since we aren't using it; they're all set to 0.0
                  }
                }
              }

              TRANS_CONT(fsm,START_GLOBAL_LOCALIZE,5,"Initialized");
            }
          }

          done = true;
        }
        break;
      case START_GLOBAL_LOCALIZE:
        {
          if(fs->vision_info->cyan_goal.confidence > .4 &&
             fs->vision_info->cyan_goal.distance < 1000.0){
            TRANS_CONT(fsm,GLOBAL_LOCALIZE,5,"NearGoal");
          }

          if(fsm.timeInState() > SecToTime(18.0)){
            TRANS_CONT(fsm,GOTO_WAYPOINT,6,"TimeOut");
          }

          command->motion_cmd = MOTION_WALK_TROT;
          if(fs->vision_info->cyan_goal.confidence > .4){
            command->vx = MaxDX;
            command->vy = 0.0;
            goal_loc.set(V2COMP(fs->vision_info->cyan_goal.loc));
            command->va = goal_loc.angle();
            command->led.cmd |= LED_UPPER_LEFT | LED_UPPER_RIGHT;
          }else if(goal_loc.x!=0.0 || goal_loc.y!=0.0){
            command->vx = MaxDX;
            command->vy = 0.0;
            command->va = goal_loc.angle();
          }else{
            command->vx = 0.0;
            command->vy = 0.0;
            command->va = 1.0;
          }

          command->head_cmd = HEAD_LOOKAT;
          command->head_lookat.set(1000.0,0.0,100.0);

          done = true;
        }
        break;
      case GLOBAL_LOCALIZE:
        {
          if(fsm.timeInState() > SecToTime(20.0)){
            TRANS_CONT(fsm,GOTO_WAYPOINT,5,"Timeout");
          }

          if(fsm.timeInState() < SecToTime(10.0)){
            command->motion_cmd = MOTION_STAND_NEUTRAL;
            command->vx = 0.0;
            command->vy = 0.0;
            command->va = 0.0;
            done = true;
            continue;
          }

          bool done_here=true;
          
          vector2d bodyCenter = fs->my_position.position.mean;
          bodyCenter -= fs->neck_offset.rotate(fs->my_position.heading.mean.angle());
          
          double dx = bodyCenter.x - waypoints[cur_waypoint_idx].x;
          double dy = bodyCenter.y - waypoints[cur_waypoint_idx].y;
          double da = norm_angle(fs->my_position.heading.mean.angle() - waypoints[cur_waypoint_idx].z);
          
          if(fs->my_position.position.sx > xDevCutOff){
            done_here = false;
          }
          
          if(fs->my_position.position.sy > yDevCutOff){
            done_here = false;
          }
          
          if(fs->my_position.heading.sMaj > aDevCutOff){
            done_here = false;
          }
          
          if(dx > 2*dxCutOff){
            done_here = false;
          }

          if(dy > 2*dyCutOff){
            done_here = false;
          }

          if(da > 2*daCutOff){
            done_here = false;
          }

          if(done_here && fsm.timeInState() > SecToTime(5.0)){
            TRANS_CONT(fsm,GOTO_WAYPOINT,6,"Localized");
          }

          goto_point->setTarget(V2COMP(global_localize_loc),global_localize_angle);

          done = true;
        }
        break;
      case INPLACE_GLOBAL_LOCALIZE:
        {
          if(fsm.timeInState() > SecToTime(6.0)){
            TRANS_CONT(fsm,GOTO_WAYPOINT,6,"TimeOut");
          }

          command->motion_cmd = MOTION_WALK_TROT;
          command->vx = 0.0;
          command->vy = 0.0;
          command->va = 1.0;

          command->head_cmd = HEAD_LOOKAT;
          command->head_lookat.set(1000.0,0.0,100.0);

          done = true;
        }
        break;
      case GOTO_WAYPOINT:
        {
          if(fs->my_position.position.sx > 1000.0 ||
             fs->my_position.position.sy > 1000.0 ||
             fs->my_position.heading.sMaj > 2.0){
            TRANS_CONT(fsm,INPLACE_GLOBAL_LOCALIZE,7,"NeedToLocalize");
          }

          if(cur_waypoint_idx >= num_waypoints){
            // we're done folks!
            pprintf(TextOutputStream,"localization challenge complete\n");

            if(looping){
              cur_waypoint_idx = 0;
            }else{            
              command->motion_cmd = MOTION_STAND_NEUTRAL;
              command->vx = 0.0;
              command->vy = 0.0;
              command->va = 0.0;
              
              return 1.0;
            }
          }
          if(fixed_angles[cur_waypoint_idx]){
            goto_point->setTarget(waypoints[cur_waypoint_idx].x, waypoints[cur_waypoint_idx].y, waypoints[cur_waypoint_idx].z);
          }else{
            goto_point->setTarget(waypoints[cur_waypoint_idx].x, waypoints[cur_waypoint_idx].y);
          }
          (*goto_point)(fs,command);
          
          vector2d bodyCenter = fs->my_position.position.mean;
          bodyCenter -= fs->neck_offset.rotate(fs->my_position.heading.mean.angle());
          
          double dx = bodyCenter.x - waypoints[cur_waypoint_idx].x;
          double dy = bodyCenter.y - waypoints[cur_waypoint_idx].y;
          double da = norm_angle(fs->my_position.heading.mean.angle() - waypoints[cur_waypoint_idx].z);
          
          bool done_here = true;
          
          if(fabs(dx) > dxCutOff){
            done_here = false;
            outsideTime = fs->current_time;
          }
          
          if((!useForDebug) && done_here && (fs->my_position.position.sx > xDevCutOff)){
            done_here = false;
          }
          
          if(done_here && (fabs(dy) > dyCutOff)){
            done_here = false;
            outsideTime = fs->current_time;
          }
          
          if((!useForDebug) && done_here && (fs->my_position.position.sy > yDevCutOff)){
            done_here = false;
          }
          
          if(done_here && fixed_angles[cur_waypoint_idx]){
            if(fabs(da) > daCutOff){
              done_here = false;
            }
            
            if((!useForDebug) && fs->my_position.heading.sMaj > aDevCutOff){
              done_here = false;
            }
            
          }
          
          if(!fixed_angles[cur_waypoint_idx]){ // turn to outside if not localizing
            
            if(fs->current_time - outsideTime > blc_get_more_info_time){
              double angle = atan2a(waypoints[cur_waypoint_idx].y, waypoints[cur_waypoint_idx].x);
              angle = norm_angle(angle);
              
              waypoints[cur_waypoint_idx].z = angle;
              fixed_angles[cur_waypoint_idx] = true;
            }
          }
          
          if(fs->current_time - startTime < 5*one_second){
            done_here = false;
          }
  
          if(done_here){
            cur_waypoint_idx++;
            TRANS_CONT(fsm,WAG_TAIL,5,"AtTarget");
          }
        }
        done = true;
        break;
      case WAG_TAIL:
        {
          if(fsm.timeInState() > pauseTime){
            TRANS_CONT(fsm,GOTO_WAYPOINT,5,"DoneWagging");
          }

          if(useForDebug){
            command->motion_cmd = MOTION_WALK_TROT;
            command->vx = 0.0;
            command->vy = 0.0;
            command->va = MaxDA;
          }else{
            command->motion_cmd = MOTION_STAND_NEUTRAL;
            command->vx = 0.0;
            command->vy = 0.0;
            command->va = 0.0;
          }
          
          static bool wag_left=true;
          wag_left = !wag_left;
          command->tail_cmd = TAIL_AIM;
          command->tail_pan = (wag_left ? -100 : 100);
        }
        done = true;
        break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();

  command->led.cmd |= fsm.getState();

  return 1.0;
}

bool LocalizationTest::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *fs_ep;

  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 LocalizationTest::setupEventMgr(){
  return true;
};

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

const MotionCommand *LocalizationTest::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(LocalizationTest,LocalizationTest::beh_name,LocalizationTest::create);

void BehaviorSystem::localization_test(FeatureSet *FS)
{
  static LocalizationTest *be_loc = NULL;
  if(be_loc==NULL)
    be_loc = new LocalizationTest();

  (*be_loc)(FS,command);
}