/* 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 "BehaviorPacketEncoder.h"
#include "BePrimaryAttacker.h"

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

#include "../Vision/VisionInterface.h"

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

char const * const BePrimaryAttacker::state_names[BePrimaryAttacker::NumStates]
 = {"SELECT_BEH_SEQ","RUN_BEH_SEQ"};

BePrimaryAttacker::BePrimaryAttacker(){
  fsm.init(state_names,NumStates,SELECT_BEH_SEQ,20,20);

  fs_id = ~0;
  fs = NULL;
  circle_end_time = 0;
  wall_turn_end_time = 0;
  stand_scan_time = 0;
}

BePrimaryAttacker::~BePrimaryAttacker(){
}

void
BePrimaryAttacker::reset(ulong timestamp){
  fsm.setState(SELECT_BEH_SEQ,0,"Reset",timestamp);
  circle_end_time = 0;
  wall_turn_end_time = 0;
}

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

char *BePrimaryAttacker::chooseBehSeq(FeatureSet *features)
{
  //checkBallObst() checks for obstacles to the left and right 
  //of the ball using the local model.  Since this is expensive, we
  //only run it when we have to. It sets the obs_left_of_ball, 
  //obst_right_of_ball, ball_near_obst in feature set, which we will
  //use in the tree.
  features->checkBallObst(160.0);
  
  char *beh_seq=NULL;

  //valid ball hyp for all BehSeq other than GetNearBall
  if(features->current_time - features->ball_last_seen_med < SecToTime(1.0)
     && features->valid_ball_hyp){

    if(features->get_kick_time(2) > features->last_saw_marker &&
       features->get_kick_time(2) > features->last_saw_goal &&
       features->current_time - stand_scan_time > SecToTime(5.0)){
      beh_seq = "StandScanMarkers";
      stand_scan_time = features->current_time;
      return beh_seq;
    }
    
    // Is this a kickoff? While we are paused waiting for a kickoff,
    // we force this state.
    //if(RobotMain::GetObject().robot_state==ROBOCUP_STATE_SET) {
      // beh_seq = "ShootOnGoal";
      //} else 
      
    if (features->teammate_passed_recently && // Should we intercept a pass?
	       features->canInterceptBall(MOTION_BLOCK_SUPP_L)) {
      beh_seq = "InterceptBall";
    } else if (features->time_in_role <= SecToTime(1.0) &&
             features->canInterceptBall(MOTION_BLOCK_SUPP_L) &&
             features->shouldInterceptIfPossible()) {
      // We just became primary and the ball seems to be rolling by
      // us.
      beh_seq = "InterceptBall";
    } else if(features->near_ball){
      if((features->ball_near_wall || features->high_loc_uncertainty) &&
	 features->obst_left_of_ball && features->obst_right_of_ball &&
	 features->current_time - wall_turn_end_time > SecToTime(3.0)){
	beh_seq = "WallTurn";
      } else if(features->near_wall && 
		((features->obst_left_of_ball  && !features->obst_right_of_ball) ||
		 (features->obst_right_of_ball && !features->obst_left_of_ball)) &&
		features->current_time - circle_end_time > SecToTime(3.0)){
	if (features->in_front_corner && 
            features->pass_receiver > 0) {
	  beh_seq = "PassToTeammate";
	} else {
	  beh_seq = "KickNearObstacle";
	}
      }else if(features->see_ball_low && 
	       features->ball_vector.length() > 150 && features->shouldDribble() &&
	       features->world_ball_vector.length() > 200.0){

	// We need to see the ball, we need to be close to it, some flag [no clue],
	// and we do not dribble within 20 cm of the center circle to prevent a
	// dribble on kickoff.

	beh_seq = "WalkDribble";
      }else{ // !ball_near_obst
	switch(features->field_region_x){
	case DEF: beh_seq="KickClear"; break;
	case MID: 
	    beh_seq="ShootOnGoal"; 
	  break;
	case FWD:
	  if(features->in_front_corner && 
	     features->pass_receiver > 0) {
	    beh_seq = "PassToTeammate";
	  }else{ // !in_front_corner
	    if(features->high_loc_uncertainty &&
	       !features->ball_near_obst &&
	       !features->ball_near_robot)
	      beh_seq="ShootOnGoal";  
	    else
	      beh_seq="ShootOnGoal";
	  }
	  break;
	}
      }
    }else{ // !near_ball
      beh_seq = "GetNearBall";
    }
  }else{
    // we do not have a valid ball hypothesis or we have not seen the
    // ball within the last 1 second
    beh_seq = "GetNearBall";
  }
  
  pprintf(TextOutputStream,"beh seq = %s\n",beh_seq);
  return beh_seq;
}

double
BePrimaryAttacker::operator()(FeatureSet *features,
			      MotionCommand *command, 
			      bool goalie)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("BePrimaryAttacker::operator()",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  bool done=false;
  fsm.startLoop(features->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
    case SELECT_BEH_SEQ:
      {
        playseq = chooseBehSeq(features);

	TRANS_CONT(fsm,RUN_BEH_SEQ,6,"Execute behavior");
	
      }
      break;
      
    case RUN_BEH_SEQ:
      {
	//get the ID of the behavior sequence
	playseq_id = event_mgr->getEventProcessorId(playseq);
	if(playseq_id==~0UL){
	  pprintf(TextOutputStream,"PrimAttacker: unable to find behavior '%s'",playseq);
	  playseq = "Default";
	  playseq_id = event_mgr->getEventProcessorId(playseq);
	}
	
	playseq_ep = dynamic_cast<BehaviorSequence *>
	  (event_mgr->getEventProcessor(playseq_id));
	if(playseq_ep==NULL){
	  pprintf(TextOutputStream,"PrimAttacker:  failed to getEventProcessor()\n");
	  command->motion_cmd = MOTION_STAND_CROUCH;
	  return 0.0;
	}
	
	if(fsm.isNewState()){
	  //reset the BehaviorSequence 
	  playseq_ep->reset(features->current_time);
	}
	
	//check the termination conditions to make sure this
	//behavior is valid (if its not, the state machine will
	//infinite loop, making the robot beep)
	double runStat = playseq_ep->status(features);

	//if the BehaviorSequence says its done...
	if(runStat != 0.0){
	  if(runStat == 1.0) //successful termination
            playseq_ep->numSuccesses++;
	  else if(runStat == -1.0) //failure termination
            playseq_ep->numFailures++;
	  else if(runStat == -2.0 && strcmp(playseq,"KickNearObstacle") == 0){
	    circle_end_time = features->current_time;
	  }
	  if(strcmp(playseq,"WallTurn") == 0){
	    wall_turn_end_time = features->current_time;
	  }
	  
	  //select a new behavior
	  TRANS_CONT(fsm,SELECT_BEH_SEQ,6,"Select new behavior");
	}

	//run the BehaviorSequence and get the motion command
	playseq_cmd = playseq_ep->get(features->current_time);
	copyMotionCmd(command,playseq_cmd);


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


  if(features->near_front_wall){
    //command->setBinLEDs(LED_EARS_RED);
  }

  return calcActivation(features); 
}

double BePrimaryAttacker::calcActivation(FeatureSet *features) {
  
  double retval = 0;

  /* Is the ball in the defense box? 
     We yield at some highly inappropriate times, so only yield
     to the goalie if we're close to the defense box. If we're on
     our own side of the field, just chase the ball because
     vision is probably wrong about the ball's location.
  */
  if(features->isBallInDefenseBox() &&
     (fabs(features->defend_goal_const.x - 
  	   features->my_position.position.mean.x) < 700))
    retval = 0;
  else if(features->team_msg_mgr->isPrimaryAttacker())
    retval = 1.0;
  else
    retval = 0.0;
  
  return retval;
}

void BePrimaryAttacker::copyMotionCmd(MotionCommand *dest, const MotionCommand *source){
  
  dest->motion_cmd = source->motion_cmd;
  dest->head_cmd = source->head_cmd;
  dest->tail_cmd = source->tail_cmd;
  dest->bound_mode = source->bound_mode;
  dest->vx = source->vx;
  dest->vy = source->vy;
  dest->va = source->va;
  dest->head_lookat = source->head_lookat;
  dest->head_tilt_offset = source->head_tilt_offset;
  dest->head_tilt = source->head_tilt;
  dest->head_pan = source->head_pan;
  dest->head_roll = source->head_roll;
  dest->head_tilt2 = source->head_tilt2;
  dest->tail_pan = source->tail_pan;
  dest->tail_tilt = source->tail_tilt;
  dest->mouth = source->mouth;
  dest->sound_cmd = source->sound_cmd;
  dest->sound_frequency = source->sound_frequency;
  dest->sound_duration = source->sound_duration;
  dest->sound_file = source->sound_file;
  dest->led = source->led;

}

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

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

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

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


bool BePrimaryAttacker::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 BePrimaryAttacker::setupEventMgr(){
  event_mgr = EventManager::getManager();
 
  return true;
}


// The behavior system will call this function to find out
// what motions the robot should execute this frame.
const MotionCommand *BePrimaryAttacker::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;
}


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


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