/* 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 "BePenaltyShooter.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 BePenaltyShooter::beh_name = "BePenaltyShooter";

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

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

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

BePenaltyShooter::~BePenaltyShooter(){
}

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

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

char *BePenaltyShooter::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;

  // Some behaviors to play with
  if(features->current_time - features->ball_last_seen_med < SecToTime(1.0)
     && features->valid_ball_hyp
     && features->near_ball) {

    if ((features->ball_near_wall || features->near_front_wall) &&
	features->obst_left_of_ball && features->obst_right_of_ball &&
	features->current_time - wall_turn_end_time > SecToTime(3.0)){
      beh_seq = "WallTurnSoft";
      pprintf(TextOutputStream,"PenaltyShooter: WallTurnSoft\n");

    } 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)){
      beh_seq = "KickNearObstacle";
      pprintf(TextOutputStream,"PenaltyShooter: KickNearObstacle\n");

    } else {
      if ((2 > turn_with_ball_ctr) && (!features->ball_near_wall)) {
	beh_seq="TurnWithBall";
	pprintf(TextOutputStream,"PenaltyShooter: TurnWithBall %lu\n",turn_with_ball_ctr);

      } else {
	beh_seq="ShootOnGoal";
	pprintf(TextOutputStream,"PenaltyShooter: ShootOnGoal %lu\n",turn_with_ball_ctr);
	turn_with_ball_ctr=0;
      }
    }

  } 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 BePenaltyShooter::operator()(FeatureSet *features,
				    MotionCommand *command, 
				    bool goalie)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("BePenaltyShooter::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) {
	    //=============================================
	    if (strcmp(playseq,"TurnWithBall")==0) {
	      turn_with_ball_ctr++;
	      pprintf(TextOutputStream,"BePenaltyShooter: TurnWithBall failed %lu times\n",turn_with_ball_ctr);
	    } 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();

  return calcActivation(features); 
}

double BePenaltyShooter::calcActivation(FeatureSet *features) {
  
  double retval = 1.0;

  return retval;
}

void BePenaltyShooter::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 *BePenaltyShooter::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 *BePenaltyShooter::encodeTrace(uchar *buf,int buf_size)
{
  uchar *bufp;

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


bool BePenaltyShooter::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 BePenaltyShooter::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 *BePenaltyShooter::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 BePenaltyShooter::update(ulong time,const EventList *events){
  return true;
}


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