/* 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 "DiveDog.h"

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

char const * const DiveDog::state_names[DiveDog::NumStates] = {
  "WAITING",
  "BLOCKING" 
};


static const double dive_thresh     = 1000.0;//750
static const double dive_vel        = 350.0;

DiveDog::DiveDog()
{
  // Initialize the finite state machine. We give it
  // our state names/etc, tell it which state to start out
  // in. The numbers reserve storage for transitions between
  // states for tracking infinite loops. We'll talk more about
  // how it's setup later.
  fsm.init(state_names,NumStates,WAITING,16,16);

  fs_id = ~0;
  fs = NULL;

  track_objects = new BeTrackObjects();
}

DiveDog::~DiveDog()
{
}

// Behaviors can be reset by higher level behaviors between uses.
// For example, a ChaseBall beh might reset a find-ball behavior once
// it had found the ball and transition to a different behavior
// to approach and track it.
void
DiveDog::reset(ulong time) {

  // We don't actually need to reset any of our state variables
  // here, so just tell the state machine where it should start
  // us out the next time we're called.
  fsm.setState(WAITING,0,"Reset",time);
  track_objects->reset(time);
}

// This is the actual guts of the behavior. It's where we decide what
// to do and fill in the MotionCommand buffer.
double
DiveDog::operator()(FeatureSet *FS, MotionCommand *command) {

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

  // We'll set a flag when we don't transition to another state
  bool done=false;

  // tell the finite state machine that we're about to start our loop
  fsm.startLoop(FS->current_time);

  while(!done && fsm.error==0){

    switch(fsm.getState()){
      
    case WAITING:
      
      if(FS->see_ball_med && FS->ball_trj.approach_vel > dive_vel &&
	 FS->ball_vector.length() < dive_thresh &&
	 fabs(FS->ball_trj.perp_offset) < 150){
	
	pprintf(TextOutputStream,"I should block\n");
	
	TRANS_CONT(fsm,BLOCKING,1,"BlockShot");
      }
      
      
      command->motion_cmd = MOTION_STAND_NEUTRAL;
      command->led.cmd = LED_MIDDLE_LEFT | LED_MIDDLE_RIGHT; // pretty lights
      
      // We did not transition to a new state, so set the done flag.
      done = true;
      break;
      
    case BLOCKING:
      
      if(fsm.isNewState()){
	double offset = FS->ball_trj.perp_offset;
	
	if(fabs(offset) < 50.0){
	  command->motion_cmd = MOTION_BLOCK_GOALIE_C;
	}else if(offset > 0.0 && offset < 150.0){
	  command->motion_cmd = MOTION_BLOCK_GOALIE_L;
	}else if(offset < 0.0 && offset > -150.0){
	  command->motion_cmd = MOTION_BLOCK_GOALIE_R;
	}
      } else {
	TRANS_CONT(fsm,WAITING,1,"wait for another shot");
      }
      
      done = true;
      break;
    }
  }
  
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  
  fsm.endLoop();
  
  return 1.0;
}

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

// This is a call-back that even processors can call to tell
// our behavior that they have updated information available.
// We don't do any processing here since we always "get" the
// most recent info when our own get method is called so that
// we can return an appropriate motion command.
bool DiveDog::update(ulong time,const EventList *events)
{
  return true;
}

// The behavior system will call this function to find out
// what motions the robot should execute this frame.
const MotionCommand *DiveDog::get(ulong time)
{
  FeatureSet *lfs=NULL; // local feature set

  // mzero is a macro that zeros out all of the fields in a structure
  // or class. Never use it if your class has virtual methods.
  // (Motion commands do not, so it's safe in this case)
  // We're just setting some sane defaults.
  mzero(out_command);

  // In initConnections, we asked the event system for an event processor
  // called a FeatureSet. It contains information from localization, vision,
  // and the network all in one package. We won't this information at all,
  // but it's here as an example of how to subscribe to events.

  // If the event system actually returned an event processor, get it's
  // most recent data. This is like the behavior system calling our
  // get method in order to retrieve a motion command.
  if(fs!=NULL){
    lfs = fs->get(time);
    
    (*this)(lfs,&out_command);
  }
  
  return &out_command;
}

// This bit is VERY important. You pass it your class name, a
// human-readable version of that name (to be placed in run.cfg
// to actually run your behavior), and a pointer to a function
// that creates an instance of your behavior class. It's not
// terribly important to worry about what this macro does.
// In short, it registers behaviors with the behavior system so
// that it knows they exist and can instantiate them if they
// are to be run.
REGISTER_EVENT_PROCESSOR(DiveDog,DiveDog::beh_name,DiveDog::create);

#if 0
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;
}
#endif