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

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

char const * const JointDog::state_names[JointDog::NumStates] = {
  "WAIT_FOR_PRESS",
  "WAIT_FOR_RELEASE" 
};

// Wait REACTION_TIME microseconds while a button is being held
// down before transitioning to avoid spurious transition. Also
// wait until it has been released this long before transitioning
// back into the button pressed state.
static const ulong REACTION_TIME = 500000;

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

  fs_id = ~0;
  fs = NULL;

  sensor_id = 0;
  sensor_data = NULL;
}

JointDog::~JointDog()
{
}

// 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
JointDog::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(WAIT_FOR_PRESS,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
JointDog::operator()(FeatureSet *FS, SensorData *s_data,
		     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;

  const SensorDataFrame *frame = s_data->getFrame(0);
  // 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 WAIT_FOR_PRESS:
      
      if(fsm.timeInState() > REACTION_TIME &&
	 (frame->button & ChinButton) != 0){

	// print out joint data
	pprintf(TextOutputStream,"LegAng(b, 0, %f, %f, %f);\n",
		frame->legAngles[0],frame->legAngles[1],frame->legAngles[2]);
	pprintf(TextOutputStream,"LegAng(b, 1, %f, %f, %f);\n",
		frame->legAngles[3],frame->legAngles[4],frame->legAngles[5]);
	pprintf(TextOutputStream,"LegAng(b, 2, %f, %f, %f);\n",
		frame->legAngles[6],frame->legAngles[7],frame->legAngles[8]);
	pprintf(TextOutputStream,"LegAng(b, 3, %f, %f, %f);\n",
		frame->legAngles[9],frame->legAngles[10],frame->legAngles[11]);
	pprintf(TextOutputStream,"HeadAng(b, %f, %f, %f);\n\n",
		frame->headAngles[0],frame->headAngles[1],frame->headAngles[2]);

	// "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,WAIT_FOR_RELEASE,1,"button down");

	// TRANSCONT breaks out of the switch statement so code down
	// here will not be executed.
      }

      // We did not transition to a new state, so set the done flag.
      done = true;
      break;
      
    case WAIT_FOR_RELEASE:

      if(fsm.timeInState() > REACTION_TIME &&
	 (frame->button & ChinButton) == 0){
	// "transition and continue"
	// tell the macro where you want to end up 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,WAIT_FOR_PRESS,1,"button release");

	// TRANSCONT breaks out of the switch statement so code down
	// here will not be executed.
      }

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

bool JointDog::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *ep_ptr;

  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  ep_ptr = event_mgr->listenEventProcessor(beh_name,fs_id);
  fs = (FeatureSet *)(ep_ptr);

  sensor_id = event_mgr->getEventProcessorId("SensorData");
  ep_ptr = event_mgr->listenEventProcessor(beh_name, sensor_id);
  sensor_data = (SensorData *)(ep_ptr);

  return true;
}

bool JointDog::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 JointDog::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 *JointDog::get(ulong time)
{
  FeatureSet *lfs=NULL; // local feature set
  SensorData *lsd = NULL;

  // 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 && sensor_data!=NULL){
    lfs = fs->get(time);
    
    lsd = sensor_data->get(time);

    (*this)(lfs,lsd, &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(JointDog,JointDog::beh_name,JointDog::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