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

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

char const * const SpinDog::state_names[SpinDog::NumStates] = {
  "SPIN_CCW",
  "SPIN_CW" 
};


// Times on the robot are measured in microseconds. Let's
// spin each way for 4 seconds at a time.
static const ulong SPIN_TIME = 4000000;

SpinDog::SpinDog()
{
  // 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,SPIN_CCW,16,16);

  fs_id = ~0;
  fs = NULL;
}

SpinDog::~SpinDog()
{
}

// 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
SpinDog::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(SPIN_CCW,0,"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
SpinDog::operator()(FeatureSet *FS, MotionCommand *command) {

  // It's possible to start in one state and transition to another.
  // If that's the case, we want to execute the code for our new
  // state so that a valid motion command is returned. This is why
  // everythinhg is in a loop. The finite state machine class detects
  // infinite loops and reports an error when that happens.

  // 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 SPIN_CCW:
      
      // If we've been here for SPIN_TIME, switch directions.
      if(fsm.timeInState() > SPIN_TIME){
	// "transition and continue"
	// tell the macro where you want to end up (SPIN_CW) and
	// give it a transition number (this is the first transition
	// out of this state, so we label it 1), and name the transition
	// (usually with the reason why you switched states)
	TRANS_CONT(fsm,SPIN_CW,1,"Timeout");

	// TRANSCONT breaks out of the switch statement so code down
	// here will not be executed.
      }
      
      // Create a motion command
      command->motion_cmd = MOTION_WALK_TROT;
      command->vx = 0.0; // x velocity (forward-backwards) of 0
      command->vy = 0.0; // y velocity (left-right) of 0

      command->va = MaxDA/2.0; // angular velocity of half of our max turn speed

      command->head_cmd = HEAD_SCAN_BALL; // do something with the head
      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 SPIN_CW:

      // ****** This is identical to the above case, except we transition
      // back to the original state when we timeout and the sign of our
      // angular velocity is reversed.
      
      // If we've been here for SPIN_TIME, switch directions.
      if(fsm.timeInState() > SPIN_TIME){
	// "transition and continue"
	// tell the macro where you want to end up (SPIN_CW) and
	// give it a transition number (this is the first transition
	// out of this state, so we label it 1), and name the transition
	// (usually with the reason why you switched states)
	TRANS_CONT(fsm,SPIN_CCW,1,"Timeout");

	// TRANSCONT breaks out of the switch statement so code down
	// here will not be executed.
      }
      
      // Create a motion command
      command->motion_cmd = MOTION_WALK_TROT;
      command->vx = 0.0; // x velocity (forward-backwards) of 0
      command->vy = 0.0; // y velocity (left-right) of 0
      command->va = -MaxDA/2.0; // angular velocity of half of our max turn speed

      command->head_cmd = HEAD_SCAN_BALL; // do something with the head
      command->led.cmd = LED_MIDDLE_LEFT | LED_MIDDLE_RIGHT; // pretty lights

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

bool SpinDog::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 SpinDog::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 SpinDog::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 *SpinDog::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(SpinDog,SpinDog::beh_name,SpinDog::create);