/* 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 "../headers/Geometry.h"
#include "../headers/field.h"
#include "state_machine.h"

#include "BehaviorPacketEncoder.h"
#include "HeadLowLevel.h"


/* =============================================================== */
/* ================================================> BeLookAtPoint */
/* =============================================================== */

BeLookAtPoint::BeLookAtPoint()
{
}

BeLookAtPoint::~BeLookAtPoint()
{
}

void BeLookAtPoint::reset()
{
}

double BeLookAtPoint::operator()(FeatureSet *features,
				 MotionCommand *command,
				 vector3d rel_point)
{
  command->head_cmd = HEAD_LOOKAT;
  command->head_lookat = rel_point;
  command->head_tilt_offset = 0;
  
  return 1.0;
}



/* ======================================================================= */
/* ================================================> BeLookAtVisibleObject */
/* ======================================================================= */

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

char const * const BeLookAtVisibleObject::state_names[BeLookAtVisibleObject::NumStates] = {
  "LOOK_AT_VIS_OBJECT",
  "ERROR"};

BeLookAtVisibleObject::BeLookAtVisibleObject()
{
  fsm.init(state_names,NumStates,LOOK_AT_VIS_OBJECT,16,16);

  look_at_point = new BeLookAtPoint;

  setTargetNoReset(VisionInterface::BALL);
}

BeLookAtVisibleObject::~BeLookAtVisibleObject()
{
  if(look_at_point!=NULL)
    delete look_at_point;
}

void BeLookAtVisibleObject::reset(ulong timestamp)
{
  fsm.setState(LOOK_AT_VIS_OBJECT,0,"Reset",timestamp);
}

void BeLookAtVisibleObject::sleep()
{
  fsm.sleep();
}

void BeLookAtVisibleObject::setTargetNoReset(int _object_idx)
{
  object_idx = _object_idx;
}

void BeLookAtVisibleObject::setTarget(ulong timestamp,int _object_idx)
{
  if(object_idx != _object_idx){
    reset(timestamp);
    setTargetNoReset(_object_idx);
  }
}

double BeLookAtVisibleObject::operator()(FeatureSet *features,
                                         MotionCommand *command)
{
  vector3d target_pt(500.0,0.0,0.0);
  bool visible=true;

  VObject *vobj = ((VObject *)features->vision_info)+object_idx;
  if(vobj->confidence > see_threshold_med){
    target_pt = vobj->loc;
  }else{
    visible = false;
  }

  double ret_act = 1.0;
  bool done=false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case LOOK_AT_VIS_OBJECT:
      if(!visible){
	TRANS_CONT(fsm,ERROR,5,"NotVisible");
      }
      (*look_at_point)(features,command,target_pt);
      done = true;
      break;
    case ERROR:
      if(visible){
	TRANS_CONT(fsm,LOOK_AT_VIS_OBJECT,5,"Visible");
      }
      ret_act = 0.0;
      done = true;
      break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();

  return ret_act;
}

uchar *BeLookAtVisibleObject::encodeAllNames(ulong time,uchar *buf,int buf_size)
{
  uchar *bufp;

  bufp = BehaviorEncodeNames::encodeAllNames<FSM,State>(buf,beh_id,beh_name,NumStates,state_names,&fsm);

  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,0);

  return bufp;
}

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

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

  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,0);

  return bufp;
}


/* ================================================================ */
/* ================================================> BeLookAtObject */
/* ================================================================ */

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

char const * const BeLookAtObject::state_names[BeLookAtObject::NumStates] = {
  "LOOK_AT_VIS_OBJECT",
  "LOOK_AT_MODEL_OBJECT",
  "LOOK_AT_TEAMMATE_MODEL_OBJECT",
  "FIND_OBJECT" };

// max time we are willing to spend trying to fixate on something
static const ulong lao_look_time   = SecToTime(1.5);
// cosine of angle needed to be considered looking in the desired direction
static const double lao_cos_fixate_angle = cos(10*M_PI/180.0);
// time to fixate on point after hit desired angle
static const ulong lao_fixate_time = SecToTime(0.2);
// time to see object to consider fixated on object
static const ulong lao_fixated_time= SecToTime(0.2);
// amount of time it takes do do one cycle of HEAD_SCAN_BALL
static const ulong ball_scan_cycle_time = SecToTime(2.0);

BeLookAtObject::BeLookAtObject()
{
  fsm.init(state_names,NumStates,LOOK_AT_MODEL_OBJECT,64,64);

  look_at_point          = new BeLookAtPoint;

  model_fixate_time = lao_fixate_time;

  resetVars();
}

BeLookAtObject::~BeLookAtObject()
{
  if(look_at_point!=NULL)
    delete look_at_point;
}

void BeLookAtObject::resetVars()
{
  object_idx = VisionInterface::BALL;
  fixate_timeout = 0;
  time_seen_balance = 0;
  last_time = 0;
  last_seen_time = 0;
}

void BeLookAtObject::reset(ulong timestamp)
{
  fsm.setState(LOOK_AT_MODEL_OBJECT,0,"Reset",timestamp);
  
  resetVars();
}

void BeLookAtObject::sleep()
{
  fsm.sleep();
}

void BeLookAtObject::setTargetNoReset(int _object_idx)
{
  object_idx = _object_idx;
}

void BeLookAtObject::setTarget(ulong timestamp,int _object_idx)
{
  reset(timestamp);
  setTargetNoReset(_object_idx);
}

void BeLookAtObject::setFixateTimeNoReset(ulong fixate_time)
{
  model_fixate_time = fixate_time;
}

void BeLookAtObject::setFixateTime(ulong timestamp,ulong fixate_time)
{
  reset(timestamp);
  setFixateTimeNoReset(fixate_time);
}

double BeLookAtObject::operator()(FeatureSet *features,
                                  BeLookAtObjectInfo *look_at_object_info,
                                  MotionCommand *command)
{
  bool visible=true;

  if(last_time == 0)
    last_time = features->current_time;

  VObject *vobj = ((VObject *)features->vision_info)+object_idx;
  visible = (vobj->confidence > see_threshold_med);

  ulong time_diff;
  time_diff = features->current_time - last_time;
  if(visible)
    time_seen_balance += time_diff;
  else if(time_seen_balance < time_diff)
    time_seen_balance = 0;
  //else
  //time_seen_balance -= time_diff;

  if(visible)
    last_seen_time = features->current_time;

  double act=0.0;
  bool done=false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case LOOK_AT_VIS_OBJECT:
      {
	if(fsm.isNewState()){
	  look_at_object_info->scanning = false;
	}

	vector3d target_pt;
	vector2d target_pt_2d;

        target_pt = vobj->loc;

        // compensate for robot velocity
        static const double latency=0.10; // in seconds
        target_pt_2d.set(V2COMP(target_pt));
        target_pt_2d += features->neck_offset;
        target_pt_2d -= vector2d(V2COMP(features->velocity))*latency;
        target_pt_2d = target_pt_2d.rotate(-features->velocity.z * latency);
        target_pt_2d -= features->neck_offset;
        target_pt.x = target_pt_2d.x;
        target_pt.y = target_pt_2d.y;
        //pprintf(TextOutputStream,"velocity=(%g,%g,%g) target_pt=(%g,%g,%g) angle=%g\n",
        //        V3COMP(features->velocity),V3COMP(target_pt),target_pt_2d.angle());

	act = (*look_at_point)(features,command,target_pt);
	if(!visible){
	  TRANS_CONT(fsm,LOOK_AT_MODEL_OBJECT,5,"NotVisible");
	}

	//look at point always returns 1.0
	 	if(act==0.0){
		  TRANS_CONT(fsm,LOOK_AT_MODEL_OBJECT,6,"LookAtVisObject failed");
	 	}
	//pprintf(TextOutputStream,"LookVisible %f %f\n",vobj->loc.x, vobj->loc.y);

	done = true;
      }
      break;
    case LOOK_AT_MODEL_OBJECT:
      {
	look_at_object_info->scanning = false;

	if(fsm.isNewState()){
	  fixate_timeout = 0;
	}

	if(visible){
	  TRANS_CONT(fsm,LOOK_AT_VIS_OBJECT,5,"SeeObject");
	}

	if(fsm.timeInState() > lao_look_time){
          if (object_idx == BALL) {
            TRANS_CONT(fsm,LOOK_AT_TEAMMATE_MODEL_OBJECT,11,"LookTimeout");
          }
          else {
            TRANS_CONT(fsm,FIND_OBJECT,6,"LookTimeout");
          }
	}
	  
	if(fixate_timeout!=0 &&
	   features->current_time > fixate_timeout){
          if (object_idx == BALL) {
            TRANS_CONT(fsm,LOOK_AT_TEAMMATE_MODEL_OBJECT,12,"LookTimeout");
          }
          else {
            TRANS_CONT(fsm,FIND_OBJECT,7,"FixatedTimeout");
          }
	}
	  
	vector3d target_pt;
	vector2d target_pt_2d;

	if(object_idx == BALL){
	  if(features->valid_ball_hyp){
	    target_pt.set(features->world_ball_vector.x,
			  features->world_ball_vector.y, 
			  0.0);	  
	  }else{
	    TRANS_CONT(fsm,LOOK_AT_TEAMMATE_MODEL_OBJECT,8,"NoBallHypothesis");
	  }
	}else if(MARKER <= object_idx && object_idx < MARKER+NUM_MARKERS){
	  target_pt.set(MarkerLocations[object_idx-MARKER].x,
			MarkerLocations[object_idx-MARKER].y,
			markerHeight);
	}else if(NUM_MARKERS <=object_idx && 
		 object_idx < BALL){
	  target_pt.set(GoalLocations[object_idx-NUM_MARKERS].x,
			GoalLocations[object_idx-NUM_MARKERS].y,
			GoalHeight/2.0);
	}else{
          if (object_idx == BALL) {
            TRANS_CONT(fsm,LOOK_AT_TEAMMATE_MODEL_OBJECT,13,"NoModelForObject");
          }
          else {
            TRANS_CONT(fsm,FIND_OBJECT,9,"NoModelForObject");
          }
	}

	target_pt = features->getRelativePosn(target_pt);

        // compensate for robot velocity
        static const double latency=0.10; // in seconds
        target_pt_2d.set(V2COMP(target_pt));
        target_pt_2d += features->neck_offset;
        target_pt_2d -= vector2d(V2COMP(features->velocity))*latency;
        target_pt_2d = target_pt_2d.rotate(-features->velocity.z * latency);
        target_pt_2d -= features->neck_offset;
        target_pt.x = target_pt_2d.x;
        target_pt.y = target_pt_2d.y;
        //pprintf(TextOutputStream,"velocity=(%g,%g,%g) target_pt=(%g,%g,%g) angle=%g\n",
        //        V3COMP(features->velocity),V3COMP(target_pt),target_pt_2d.angle());

	if(!features->isPointInFrontLocal(vector2d(target_pt.x, target_pt.y))){
	  TRANS_CONT(fsm,FIND_OBJECT,10,"ModelLocationNotVisible");
	}

	if(fixate_timeout == 0){
	  double cos_angle_to_target;
	  cos_angle_to_target = features->cosAngleCameraToPoint(target_pt);
	  if(cos_angle_to_target >= lao_cos_fixate_angle){
	    fixate_timeout = features->current_time + model_fixate_time;
	  }
	}

	//pprintf(TextOutputStream,"LookModel %f %f\n",target_pt.x, target_pt.y);

	act = (*look_at_point)(features,command,target_pt);
	done = (act!=0.0);
      }
      break;

    case LOOK_AT_TEAMMATE_MODEL_OBJECT: 
      {
        static vector3d target_pt_static;

        look_at_object_info->scanning = false;

        if (object_idx != BALL) {
          pprintf(TextOutputStream,
                  "BUG: LOOK_AT_TEAMMATE_MODEL_OBJECT state entered" \
                  "when looking for an object other than BALL\n");
          TRANS_CONT(fsm,FIND_OBJECT,11,"NotLookingForBall");
        }
        
        
        if (fsm.isNewState()) {
	  fixate_timeout = 0;
          
          Gaussian2 teammate_best_obs;
          if (features->world_model->teammate_ball_tracker_observation_global(teammate_best_obs)) {
	    target_pt_static.set(teammate_best_obs.mean.x,
                                 teammate_best_obs.mean.y,
                                 0.0);
          }
          else {
	    TRANS_CONT(fsm,FIND_OBJECT,8,"NoBallHypothesis");
          }
        }
        
	if(visible){
	  TRANS_CONT(fsm,LOOK_AT_VIS_OBJECT,5,"SeeObject");
	}

	if(fsm.timeInState() > lao_look_time){
            TRANS_CONT(fsm,FIND_OBJECT,6,"LookTimeout");
	}
	  
	if(fixate_timeout!=0 &&
	   features->current_time > fixate_timeout){
          TRANS_CONT(fsm,FIND_OBJECT,7,"FixatedTimeout");
	}
	
        vector3d target_pt = features->getRelativePosn(target_pt_static);
	vector2d target_pt_2d;
             
        // compensate for robot velocity
        static const double latency=0.10; // in seconds
        target_pt_2d.set(V2COMP(target_pt));
        target_pt_2d += features->neck_offset;
        target_pt_2d -= vector2d(V2COMP(features->velocity))*latency;
        target_pt_2d = target_pt_2d.rotate(-features->velocity.z * latency);
        target_pt_2d -= features->neck_offset;
        target_pt.x = target_pt_2d.x;
        target_pt.y = target_pt_2d.y;
        //pprintf(TextOutputStream,"velocity=(%g,%g,%g) target_pt=(%g,%g,%g) angle=%g\n",
        //        V3COMP(features->velocity),V3COMP(target_pt),target_pt_2d.angle());

	if(!features->isPointInFrontLocal(vector2d(target_pt.x, target_pt.y))){
	  TRANS_CONT(fsm,FIND_OBJECT,10,"ModelLocationNotVisible");
	}

	if(fixate_timeout == 0){
	  double cos_angle_to_target;
	  cos_angle_to_target = features->cosAngleCameraToPoint(target_pt);
	  if(cos_angle_to_target >= lao_cos_fixate_angle){
	    fixate_timeout = features->current_time + model_fixate_time;
	  }
	}

	//pprintf(TextOutputStream,"LookModel %f %f\n",target_pt.x, target_pt.y);

	act = (*look_at_point)(features,command,target_pt);
	done = (act!=0.0);
      }
      break;

    case FIND_OBJECT:
      {
	time_seen_balance = 0;
	look_at_object_info->scanning = true;

	if(visible){
	  look_at_object_info->scanning = false;
	  TRANS_CONT(fsm,LOOK_AT_VIS_OBJECT,5,"SeeObject");
	}
	  
	if(object_idx == VisionInterface::BALL){
          // HEAD_SCAN_BALL takes 2 seconds to do one cycle - if we've
          // not seen the ball in that time, try looking at a
          // teammate's ball again.
          if(fsm.timeInState() > ball_scan_cycle_time){
            TRANS_CONT(fsm,LOOK_AT_TEAMMATE_MODEL_OBJECT,6,"FindObjectTimeoutOnBall");
          }
	  command->head_cmd = HEAD_SCAN_BALL;
	}else{
	  command->head_cmd = HEAD_SCAN_MARKERS;
	}

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

  look_at_object_info->fixated = (time_seen_balance > lao_fixated_time);

  last_time = features->current_time;
  
  return 1.0;
}

uchar *BeLookAtObject::encodeAllNames(ulong time,uchar *buf,int buf_size)
{
  uchar *bufp;

  bufp = BehaviorEncodeNames::encodeAllNames<FSM,State>(buf,beh_id,beh_name,NumStates,state_names,&fsm);

  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,0);

  return bufp;
}

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

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

  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,0);

  return bufp;
}


/* ================================================================== */
/* ================================================> BeActiveLocalize */
/* ================================================================== */

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

char const * const BeActiveLocalize::state_names[BeActiveLocalize::NumStates] = {
  "PICK_POINT",
  "FIXATE_ON_POINT"};


const ulong al_fixate_time =  500000L;
const ulong al_wait_time =   2000000L;

const double al_confidence_thresh = .4;
const double al_pick_ball_prob = .70;

const bool al_use_goals = true;

BeActiveLocalize::BeActiveLocalize()
{
  fsm.init(state_names,NumStates,PICK_POINT,16,16);

  look_at = new BeLookAtPoint();

  resetVars();
}

BeActiveLocalize::~BeActiveLocalize()
{
  if(look_at!=NULL){
    delete look_at;
    look_at = NULL;
  }
}

void BeActiveLocalize::resetVars()
{
  point.set(0, 0, 0);
  marker_id = -1;
  looking_at_ball = false;
  looking_at_goal = false;
}

void BeActiveLocalize::reset(ulong timestamp)
{
  fsm.setState(PICK_POINT,0,"Reset",timestamp);

  resetVars();

  if(look_at!=NULL)
    look_at->reset();
}

void BeActiveLocalize::sleep()
{
  fsm.sleep();
}

double BeActiveLocalize::operator()(FeatureSet *features,
                                    BeActiveLocalizeInfo *info,
				    MotionCommand *command,
				    bool also_use_ball)
{
  info->fixated = false;

  double ret_act=1.0;
  bool done=false;

  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case PICK_POINT:
      if(also_use_ball){
	double p = ((double)(rand() % 1000))/1000.0;
	looking_at_ball = (p < al_pick_ball_prob);
      }else{
	looking_at_ball = false;
      }

      if(looking_at_ball) {
	point.x = features->ball_vector.x;
	point.y = features->ball_vector.y;
	point.z = BallRadius;
	looking_at_goal = false;
      }else{
	//pick an object to look at - marker or goal
	marker_id = pickRandomMarker(features);
          
	if(marker_id==-1){
	  done = true;
	  ret_act = 0.0;
	  continue;
	}
	
	//get position of selected marker or goal
	if(marker_id < NUM_MARKERS){
	  point.x = MarkerLocations[marker_id].x;	
	  point.y = MarkerLocations[marker_id].y;	
	  point.z = markerHeight;
	  looking_at_goal = false;
	}else{
	  point.x = GoalLocations[marker_id - NUM_MARKERS].x;
	  point.y = GoalLocations[marker_id - NUM_MARKERS].y;
	  point.z = markerHeight/2.0;
	  looking_at_goal = true;
	}
	
	// Convert to local coordinates.
	point = features->getRelativePosn(point);
      }
      
      TRANS_CONT(fsm,FIXATE_ON_POINT,5,"Done");
      break;
	
    case FIXATE_ON_POINT:
      if(fsm.isNewState()){
	look_at->reset();
      }
	
      /* Do we see the object we're looking for with reasonable
	 confidence? If so, replace our estimate with where vision
	 reports the object.
      */
      if(looking_at_ball){
	if(features->vision_info->ball.confidence > al_confidence_thresh){
	  point = features->vision_info->ball.loc;
	}
      }else if(looking_at_goal){

	if(marker_id - NUM_MARKERS == 0 &&
	   features->vision_info->cyan_goal.confidence > al_confidence_thresh){
	  point = features->vision_info->cyan_goal.loc;
	  point.z = 0;
	}
	else if(marker_id - NUM_MARKERS == 1 &&
		features->vision_info->yellow_goal.confidence > al_confidence_thresh){
	  point = features->vision_info->yellow_goal.loc;
	  point.z = 0;
	}
	
      }else{
	if(features->vision_info->marker[marker_id].confidence > al_confidence_thresh){
	  point = features->vision_info->marker[marker_id].loc;
	    
	  // We know the true height of the markers. Let's fill this
	  // in from our world model instead of making the poor robot
	  // guess. (This assumes the robot is always on the ground)
	  point.z = markerHeight;
	}
      }
	
      (*look_at)(features, command, point);
	
      if(fsm.timeInState() > al_fixate_time){
	
	if(looking_at_ball){
	  info->fixated = true;
	}
	else if(looking_at_goal){
	  if(marker_id - NUM_MARKERS == 0 &&
	     features->vision_info->cyan_goal.confidence > al_confidence_thresh){
	    info->fixated = true;
	  }
	  else if(marker_id - NUM_MARKERS == 1 &&
	     features->vision_info->yellow_goal.confidence > al_confidence_thresh){
	    info->fixated = true;
	  }
	  else{
	    //if we can't fixate on the goal then just scan
	    command->head_cmd = HEAD_SCAN_MARKERS;
	    if(fsm.timeInState() > al_fixate_time + 500000){
	      info->fixated = true;
	    }
	  }
	}
	else{
	  if(features->vision_info->marker[marker_id].confidence > al_confidence_thresh){
	    info->fixated = true;
	  }else{
	    command->head_cmd = HEAD_SCAN_MARKERS;
	    if(fsm.timeInState() > al_fixate_time + 500000){
	      info->fixated = true;
	    }
	  }
	}
      }

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

  return ret_act;
}

/* Picks a random marker that's in front of us and returns
   the index in the MarkerLocations array.

   Returns -1 if no marker is in sight. Recall that the index
   is in global coordinates and don't do stupid things converting
   to local coordinates like I always do.
*/

int
BeActiveLocalize::pickRandomMarker(FeatureSet *features)
{
  int visible, index;
  int retval = -1;
  vector2d temp;
  int marker_num;

  marker_num = NUM_MARKERS;
  if(al_use_goals)
    marker_num += 2;

  /* Count the number of markers we can see. */
  visible = 0;
  for(int i=0; i<marker_num; i++){

    if(!USE_MIDDLE_MARKERS &&  (i == MARKER_GOP || i == MARKER_POG))
      continue;

    if(i < NUM_MARKERS){
      temp = MarkerLocations[i];
    }else{
      temp = GoalLocations[i-NUM_MARKERS];
    }
    
    if(features->isPointInFront(temp)){
      visible++;
    }
  }

  /* Pick the index-th visible marker to look at where index takes
     on a random value from 0 -> visible - 1.
  */
  if(visible>0){
    index = rand() % visible;
    
    for(int i=0; i<marker_num; i++){
      if(!USE_MIDDLE_MARKERS && (i == MARKER_GOP || i == MARKER_POG))
	continue;

      if(i < NUM_MARKERS){
	temp = MarkerLocations[i];
      }else{
	temp = GoalLocations[i-NUM_MARKERS];
      }
     
      if(features->isPointInFront(temp)){
	if(index==0){
	  retval = i;
	  break;
	}else{
	  index--;
	}
      }
    }
  }

  return retval;
}

uchar *BeActiveLocalize::encodeAllNames(ulong time,uchar *buf,int buf_size)
{
  uchar *bufp;

  bufp = BehaviorEncodeNames::encodeAllNames<FSM,State>(buf,beh_id,beh_name,NumStates,state_names,&fsm);

  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,0);

  return bufp;
}

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

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

  // sub state machines
  SPOutEncoder::encodeAs<uchar>(&bufp,0);

  return bufp;
}

/* ================================================================ */
/* ================================================> BeTrackObjects */
/* ================================================================ */

/* Track the locations of multiple objects by switching among them.
 */

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

char const * const BeTrackObjects::state_names[BeTrackObjects::NumStates] = {
  "PICKING_OBJECT",
  "LOCATING_OBJECT"};

static const ulong to_stare_ball_time   =  SecToTime(1.0);
static const ulong to_stare_marker_time =  SecToTime(0.1);

static const ulong to_must_find_ball_time   =  SecToTime(1.5);
static const ulong to_must_find_marker_time =  SecToTime(1.0);

static const ulong to_max_find_ball_time   = SecToTime(3.0);
static const ulong to_max_find_marker_time = SecToTime(0.9);

static const double to_loc_lost_thresh = 60.0;

//angle needed to see both goal posts at the same time
static const double to_goal_post_angle = 40.0*M_PI/180.0;

static const bool to_use_goals = true;

BeTrackObjects::BeTrackObjects(void)
{
  fsm.init(state_names,NumStates,PICKING_OBJECT,16,16);

  //how many landmarks we want to look at
  if(to_use_goals)
    num_landmarks = NUM_MARKERS+6; //4 posts, 2 goal centers
  else
    num_landmarks = NUM_MARKERS;

  resetVars();

  look_at_object = new BeLookAtObject();
  look_at_object->setTargetNoReset(object_to_find==BALL ? BALL : object_to_find);
}

BeTrackObjects::~BeTrackObjects()
{
  if(look_at_object!=NULL){
    delete look_at_object;
    look_at_object = NULL;
  }

  last_tried_ball = false;
  find_ball_failed = false;
}

void BeTrackObjects::resetVars()
{
  object_to_find = BALL;
  for(int i=0; i<num_landmarks+1; i++)
    last_seen[i] = 0UL;
  saw_marker_while_scanning = false;
}

void BeTrackObjects::reset(ulong timestamp)
{
  fsm.setState(PICKING_OBJECT,0,"Reset",timestamp);

  resetVars();

  look_at_object->reset(timestamp);
  look_at_object->setTarget(timestamp,object_to_find==BALL ? BALL : object_to_find);
}

void BeTrackObjects::sleep()
{
  fsm.sleep();
  look_at_object->sleep();
}

double BeTrackObjects::operator()(FeatureSet *features,
                                  MotionCommand *command,
                                  bool UseMarkers,
                                  bool UseBall)
{
  vector2d temp;

  // Update the times we last saw the objects we're tracking based on
  // our current features
  if(features->see_ball_med)
    last_seen[BALL] = features->current_time;
  
  for(int i=0; i<num_landmarks; i++){
    if(!USE_MIDDLE_MARKERS &&  (i == MARKER_GOP || i == MARKER_POG))
      continue;

    VObject *vobj = ((VObject *)features->vision_info)+i;
    if(vobj->confidence > see_threshold_med)
      last_seen[i] = features->current_time;
  }


  bool done=false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){   
    case PICKING_OBJECT:
      {
	// pick new object to look at
	bool pick_ball  =false;
	bool pick_landmark=false;
	int last_seen_marker_idx = -1;
	object_to_find = -1;

	//reset look_at_object
	look_at_object->reset(features->current_time);
	look_at_object_info.scanning = false;
	look_at_object_info.fixated = false;

	// We track the second newest marker. This ensures that the robot
	// doesn't just keep fixating on the same marker and that we
	// use a second marker when a marker is always in our frame.
	ulong last_saw_marker = 0;
	ulong last_saw_second_newest_marker = 0;
	for(int marker_idx=0; marker_idx<num_landmarks; marker_idx++){
	  if(last_seen[marker_idx] > last_saw_marker) {
	    last_saw_second_newest_marker = last_saw_marker;
	    last_saw_marker = last_seen[marker_idx];
	    last_seen_marker_idx = marker_idx;
	  }
	}
	
	//decide what we need to look at - ball or a landmark
	if(!UseBall && UseMarkers){
	  pick_landmark = true;
	}else if(!UseMarkers && UseBall){
	  pick_ball = true;
	}else{
	  if(last_seen[BALL] - features->current_time > to_must_find_ball_time &&
	     !last_tried_ball && !find_ball_failed){
	    pick_ball = true;
	  } else {
   	    //look for marker if we havent seen one in a while or think we're lost
	    if((features->current_time - last_saw_second_newest_marker > to_must_find_marker_time) ||
	       (features->my_position.position.sMaj > to_loc_lost_thresh)){
	      pprintf(TextOutputStream,"picking landmark, lost = %d\n",(features->my_position.position.sMaj > to_loc_lost_thresh));
	      pick_landmark = true;
	    }else{
	      pick_ball = true;
	    }
	  }
	}
	  
	if(pick_ball) {
	  //look at the ball
	  object_to_find = BALL;
	  last_tried_ball = true;	
	  look_at_object->setTarget(features->current_time,object_to_find);
	  TRANS_CONT(fsm,LOCATING_OBJECT,5,"PickedBall");
	} else {
	  last_tried_ball = false;
	  find_ball_failed = false; // we can't fail if we don't try
	}
	  
	//count up how many landmarks are visible
	int visible=0;
	for(int idx=0; idx<num_landmarks; idx++){
	    
	  if(!USE_MIDDLE_MARKERS &&  (idx == MARKER_GOP || idx == MARKER_POG))
	    continue;

	  if(idx < NUM_MARKERS){
	    temp = MarkerLocations[idx];
	  }else{
	    temp = GoalLocations[idx-NUM_MARKERS];
	  }

	  bool in_front = features->isPointInFront(temp);
	  if(in_front && 
	     (temp - features->my_position.position.mean).sqlength() < sq(3500.0) &&
	     (temp - features->my_position.position.mean).sqlength() > sq(100.0)){
	    visible++;
	  }
	}
	
	if(visible > 0){
	  int index;
	    
	  /* Pick the index-th visible marker to look at where index takes
	     on a random value from 0 -> visible - 1.
	  */
	  index = rand() % visible;
	    
	  for(int i=0; i<num_landmarks; i++){
	      
	    if(!USE_MIDDLE_MARKERS &&  (i == MARKER_GOP || i == MARKER_POG))
	      continue;
	      
	    if(i < NUM_MARKERS){
	      temp = MarkerLocations[i];
	    }else{
	      temp = GoalLocations[i-NUM_MARKERS];
	    }

	    if(features->isPointInFront(temp) &&
	       (temp - features->my_position.position.mean).sqlength() < sq(3500.0) &&
	       (temp - features->my_position.position.mean).sqlength() > sq(100.0) &&
	       (i != last_seen_marker_idx || visible == 1)){
	      if(index<=1){
		object_to_find = i;
		break;
	      }else{
		index--;
	      }
	    }
	  }
	  
	  //above we considered all the goal landmarks separately (because it was easier)
	  //but since they are all really close, we can be smarter about it.  Even though
	  //we dont use the center of the goal for localization, looking at it from far 
	  //away will allow us to see both edges of the goal.  So if the angle between the 
	  //posts is small - look at the center.  Otherwise either go with the post selected
	  //previously, or pick the closest one.  
	  if(object_to_find >= YELLOW_GOAL && object_to_find < CYAN_GOAL){
	    double angle_left_post = GoalLocations[1].angle();
	    double angle_right_post = GoalLocations[2].angle();
	  
	    if(fabs(angle_left_post - angle_right_post) < to_goal_post_angle){
	      object_to_find = YELLOW_GOAL;
	    }else if(object_to_find == YELLOW_GOAL){
	      if(fabs(angle_left_post) < fabs(angle_right_post)){
		object_to_find = YELLOW_GOAL + LEFT_SIDE;
	      }else{
		object_to_find = YELLOW_GOAL + RIGHT_SIDE;
	      }
	    }
	  }
	  else if(object_to_find >= CYAN_GOAL && object_to_find < BALL){
	    double angle_left_post = GoalLocations[4].angle();
	    double angle_right_post = GoalLocations[5].angle();  

	    if(fabs(angle_left_post - angle_right_post) < to_goal_post_angle){
	      object_to_find = CYAN_GOAL;
	    }else if(object_to_find == CYAN_GOAL){
	      if(fabs(angle_left_post) < fabs(angle_right_post)){
		object_to_find = CYAN_GOAL + LEFT_SIDE;
	      }else{
		object_to_find = CYAN_GOAL + RIGHT_SIDE;
	      }
	    }
	  }


   
	}else{
	  // Pick a purely random marker to search for when none 
	  // are visible according to our world model.
	  if(pick_landmark) {
	    if(USE_MIDDLE_MARKERS)
	      object_to_find = rand() % NUM_MARKERS;
	    else 
	      object_to_find = rand() % (NUM_MARKERS-2);
	  }

	}

	look_at_object->setTarget(features->current_time, object_to_find);
	TRANS_CONT(fsm,LOCATING_OBJECT,6,"PickedLandmark");
	
      }
      break;
      
    case LOCATING_OBJECT:
      if(fsm.isNewState()){
        timer = 0UL;
      }
      
      ulong timeout;
      timeout = (object_to_find==BALL) ? to_max_find_ball_time : to_max_find_marker_time;
      if(timer==0UL && fsm.timeInState() > timeout){
	// Did we look for the ball and fail to find it?
	if(object_to_find==BALL)
	  find_ball_failed = true;
	
        TRANS_CONT(fsm,PICKING_OBJECT,7,"Timeout");
      }

      if(timer==0UL && look_at_object_info.fixated){
	if(object_to_find==BALL) {
	  find_ball_failed = false;
          timer = fsm.timeInState() + to_stare_ball_time;
	} else {	  
	  timer = fsm.timeInState() + to_stare_marker_time;
	}
      }
      
      //if we are looking for a marker,stop as soon as one marker was seen, doesnt
      //matter which one
      if(object_to_find != BALL && look_at_object_info.scanning ){
	  
	for(int i=0; i<num_landmarks; i++){
	  if(!USE_MIDDLE_MARKERS &&  (i == MARKER_GOP || i == MARKER_POG))
	    continue;
	  
	  VObject *vobj = ((VObject *)features->vision_info)+i;
	  if(vobj->confidence > see_threshold_med &&
	     vobj->edge == 0){

	    saw_marker_while_scanning = true;
	    break;
	  }

	}
	
	//can't put TRANS_CONT in a for loop since it will just 
	//do a 'continue'.  Set variable and call it here.
	if(saw_marker_while_scanning){
	  saw_marker_while_scanning = false;
	  TRANS_CONT(fsm,PICKING_OBJECT,8,"SawOneMarker");
	}
	
      }
     
      if(timer!=0UL && fsm.timeInState() > timer){
	TRANS_CONT(fsm,PICKING_OBJECT,9,"Done fixating");
      }

      (*look_at_object)(features,&look_at_object_info,command);
            
      done = true;
      break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();
  
  return (done ? 1.0 : 0.0);
}

uchar *BeTrackObjects::encodeAllNames(ulong time,uchar *buf,int buf_size)
{
  uchar *bufp;

  bufp = BehaviorEncodeNames::encodeAllNames<FSM,State>(buf,beh_id,beh_name,NumStates,state_names,&fsm);

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

  return bufp;
}

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

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

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

  return bufp;
}