/* 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 "FootDog.h"
#include "../headers/Sensors.h"

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

char const * const FootDog::state_names[FootDog::NumStates] = {
  "WALKING",
  "PICKED_UP"
};


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

  fs_id = ~0;
  fs = NULL;  

  sensor_id = 0;
  sensor_data = NULL;
}

FootDog::~FootDog()
{
}

// 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
FootDog::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(WALKING,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
FootDog::operator()(FeatureSet *FS, SensorData *sd, MotionCommand *command) {
  static int numFrames = 0;
  const int lookBack = 30;
  const SensorDataFrame *frame = sd->getFrame(0);    
  bool flPad = frame->paw[0];
  bool frPad = frame->paw[1];
  bool blPad = frame->paw[2];
  bool brPad = frame->paw[3];
  double zSum = 0, ySum = 0;
  int maxFrames = (numFrames > lookBack ? -lookBack : -numFrames);
    for(int i = 0; i > maxFrames; i--) {
      zSum += sd->getFrame(i)->accel.z;
      ySum += sd->getFrame(i)->accel.y;
    }
    double zAVG = zSum / (-maxFrames);
    double yAVG = ySum / (-maxFrames);
    bool done = false;
    
    fsm.startLoop(FS->current_time);
    if (numFrames % 25 == 0) {
      cout << "zAVG every 25: " << zAVG << endl;
      cout << "yAVG every 25: " << yAVG << endl;
    }
    while(!done && fsm.error == 0)
      {
	switch(fsm.getState()){

	case WALKING:
	  if(zAVG > -0.85)
	    {
	      cout << "Dog was picked up" << endl;
	      cout << zAVG << endl;
	      TRANS_CONT(fsm,PICKED_UP,1,"Dog was picked up");
	    }

	  command->motion_cmd = MOTION_WALK_TROT;
	  command->vx = MaxDX/2.0; // x velocity (forward-backwards) of 0
	  command->vy = 0.0; // y velocity (left-right) of 0
	  command->va = 0.0; // angular velocity of half of our max turn speed
	  
	  command->head_cmd = HEAD_SCAN_BALL; // do something with the head
	    
	  done = true;
	  break;
	  
	case PICKED_UP:
	  if(zAVG < -1.1)
	    {
	      cout << "Dog was put down" << endl;
	      cout << zAVG << endl;
	      TRANS_CONT(fsm,WALKING,1,"Dog was put down");

	    }
 
	  command->motion_cmd = MOTION_STAND_NEUTRAL;
	  command->vx = 0.0; // x velocity (forward-backwards) of 0
	  command->vy = 0.0; // y velocity (left-right) of 0
	  command->va = 0.0; // angular velocity of half of our max turn speed

	done = true;
	}
      }

	if(yAVG < -.2)
	    command->setFaceLEDs(LED_E, LED_BRIGHT, MODE_A); // pretty lights
	if(yAVG > .3)
	    command->setFaceLEDs(LED_F, LED_BRIGHT, MODE_A); // pretty lights

		 if(flPad)
	    command->setFaceLEDs(LED_A, LED_BRIGHT, MODE_A); // pretty lights
	    if(frPad)
	    command->setFaceLEDs(LED_B, LED_BRIGHT, MODE_A); // pretty lights
	    if(blPad)
	    command->setFaceLEDs(LED_C, LED_BRIGHT, MODE_A); // pretty lights
	  if(brPad)
	    command->setFaceLEDs(LED_D, LED_BRIGHT, MODE_A); // pretty lights
    
    if(fsm.error != 0)
      {
	fsm.handleErr(command);
      }
    
    fsm.endLoop();
    ++numFrames;
    return 1.0;
}

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

  mzero(out_command);
  mzero(out_command.led);

  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(FootDog,FootDog::beh_name,FootDog::create);