/*  Robotics 445/600 Spring 2009 - Behavior Example
 *  Michael Ferguson
 *  
 *  Behavior Description: 
 *    Implements a right wall following behavior,
 *       interrupted by a pan and scan. 
 *    When we find a forward wall, we turn away. 
 *
 *
 */
 
#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"
//#include "Motion/MMAccessor.h"
//#include "Motion/MotionManager.h"
#include "Motion/WalkMC.h"
#include "Shared/ProjectInterface.h"
//#include "Motion/MotionSequenceMC.h"
#include "Motion/PostureMC.h"

#define REG_SPEED	70          // forward speed, 70mm per second
#define STOPTIMER	4242		// timer ID to use for end of motion cycle...
#define STARTPAN	4248        
#define PANTIMER	4243		// timer that starts panscan
#define PAN1TIMER	4244
#define PAN2TIMER	4245
#define TURNOPENING	4345
#define VISTIMEOUT	4444
#define DONETIMER	4448

/* Definitions for flags */
#define USE_PANSCAN			(flags&0x04)
#define USE_CRUISE			(flags&0x08)
#define FOLLOW_LEFT			(flags&0x10)
#define FOLLOW_RIGHT		(flags&0x20)
//#define FOUND				(flags&0x80)
#define USE_ANY				(flags&0x40)

#define DISABLE_PANSCAN		flags&= 0xFB
#define DISABLE_CRUISE		flags&= 0xF7
#define KILL_PANSCAN		flags&= 0xBB
#define KILL_CRUISE			flags&= 0xB7

#define ENABLE_ANY			flags|= 0x40
#define ENABLE_PANSCAN		flags|= 0x04
#define ENABLE_CRUISE		flags|= 0x08
#define ENABLE_LEFT			flags= (flags&0xDF)|0X10
#define ENABLE_RIGHT		flags= (flags&0xEF)|0x20
//#define FOUND_GOAL			flags|= 0x80

#define PANHOLDTIME			1250		// was 2500, didnt see some openings
#define PAN_FREQ			750			// worked well at 1000
#define TURN_TIME			1000		// was 2750 in testing (turn alone)
#define CRITICAL_DIST		200
#define SAFE_DIST			235
#define FOLLOW_DIST			275
#define NO_WALL				650
#define WALK1BOX			250
#define PANTIME				500

#define LEFT_ANGLE			RAD(30)
#define LOOK_LEFT			RAD(90)
#define FULL_LEFT			RAD(90)
#define RIGHT_ANGLE			RAD(-30)
#define FULL_RIGHT			RAD(-90)
#define FORWARD				RAD(0)

#define GOAL_SID            ProjectIntefrace::visPinkBallSID

/* Conditional walk behavior */
class bMaze : public BehaviorBase {
protected: 
  int flags;	
  int foundFlags;
  int done;
  MotionManager::MC_ID walk_id;	
  MotionManager::MC_ID head_id; 
	
  /* Helper function that starts walking */
  void startWalk(double x, double angle){
    // need to call this every time, right before we edit the motion command.
	MMAccessor<WalkMC> walk(walk_id);
	if(done == 0){
		walk.mc()->setTargetVelocity(x,0,angle);
	}
  }
  
  /* Helper function that stops walking */
  void stopWalk(){
	MMAccessor<WalkMC> walk(walk_id);
	walk.mc()->setTargetVelocity(0,0,0);
  }
  
  /* Helper functions that turns robot 90degrees */
  void turnLeft(){
	std::cout << "I Turnz Left." << std::endl;
    // we add a timer, which will kill our turning after some pre-determined time
	erouter->addTimer(this,STOPTIMER,TURN_TIME+300,false);
	startWalk(0,RAD(90));
  }
  void turnRight(){
	std::cout << "I Turnz Right." << std::endl;
	erouter->addTimer(this,STOPTIMER,TURN_TIME,false);
	startWalk(0,RAD(-90));
  }
  
  /* Helper function to position head */
  void posHead(double pan){
	MMAccessor<HeadPointerMC> head(head_id);
	head.mc()->setJoints(0,pan,RAD(0));
  }

public:
  bMaze() : BehaviorBase("Maze"), flags(0), foundFlags(0),done(0),
    walk_id(motman->addPersistentMotion(SharedObject<WalkMC>())),
    head_id(motman->addPersistentMotion(SharedObject<HeadPointerMC>())){} 

  /* Start up listeners for timers, sensors, etc */
  virtual void DoStart() {
    BehaviorBase::DoStart();

	// move head to position
	posHead(LEFT_ANGLE);
	
	std::cout << "Maze Behavior Started." << std::endl;
	
	//ENABLE_RIGHT;
	ENABLE_LEFT;		// start wall following on the left
	ENABLE_ANY;
	
	ENABLE_CRUISE;
	//ENABLE_PANSCAN;
    // this timer will have us check for a forward wall
	erouter->addTimer(this,STARTPAN,PANHOLDTIME,false);
	
	// sensor, visual listeners
	erouter->addListener(this,EventBase::visObjEGID,GOAL_SID);
	erouter->addListener(this,EventBase::sensorEGID);
  }
 
  virtual void DoStop() {
    std::cout << "I'm through with this crap..." << std::endl;
    motman->removeMotion(walk_id);
	motman->removeMotion(head_id);
    BehaviorBase::DoStop();
  }  
  
  // where stuff really happens...
  virtual void processEvent(const EventBase &event){
	switch( event.getGeneratorID() ){
		
		// sensor values updated every 32 milliseconds - causes Cruise()
		case EventBase::sensorEGID:	
			if((USE_CRUISE > 0) && (USE_ANY > 0)){
				if(state->sensors[RobotInfo::NearIRDistOffset] < SAFE_DIST){
					// bigger correction needed
					if(FOLLOW_RIGHT){
						startWalk(REG_SPEED,RAD(7));
					}else{
						startWalk(REG_SPEED,RAD(-7));
					}
				}else{				
					if(state->sensors[RobotInfo::NearIRDistOffset] < FOLLOW_DIST){
						// too close, arc away
						if(FOLLOW_RIGHT){
							startWalk(REG_SPEED,RAD(5));
						}else{
							startWalk(REG_SPEED,RAD(-5));
						}
					}else{
						// too far, arc towards
						if(FOLLOW_RIGHT){
							startWalk(REG_SPEED,RAD(-5));
						}else{
							startWalk(REG_SPEED,RAD(5));
						}
					}

				}
			}
			break;
		
		// handle timer events
		case EventBase::timerEGID:
			if(event.getSourceID() == STOPTIMER){
				// turn is done, restart cruise, wait to restart pan/scan
				ENABLE_CRUISE;	
				erouter->addTimer(this,STARTPAN,PANHOLDTIME,false);
				std::cout << "Turn Complete" << std::endl;
			}
			if(event.getSourceID() == STARTPAN){
				// this is our hystersis to avoid turning 180 degrees
				// it also acts like a just turned setting!
				ENABLE_PANSCAN;
				std::cout << "Pan/scan enabled" << std::endl;
				erouter->addTimer(this,PANTIMER,PAN_FREQ,false);
			}
			// do pan/scan behavior... PART1 - move head to left/right	
			if((event.getSourceID() == PANTIMER) && (USE_PANSCAN > 0) && (USE_ANY > 0)){			
				DISABLE_CRUISE;
				if(FOLLOW_RIGHT){
					// head all the way to right
					posHead(RIGHT);
				}else{
					// head all the way to left
					posHead(LOOK_LEFT);
				}
				// wait to get there
				erouter->addTimer(this,PAN1TIMER,PANTIME,false);	
			}
			// pan/scan behavior ... PART2 - read left/right, head to center
			if((event.getSourceID() == PAN1TIMER) && (USE_ANY > 0)){
				// get followed wall reading
				if(state->sensors[NearIRDistOffset] > 400){
					// walk forward a little
					erouter->addTimer(this,TURNOPENING,WALK1BOX,false);
					std::cout << "No Near Wall" << std::endl;
					// move head home
					if(FOLLOW_RIGHT){			
						posHead(RIGHT_ANGLE);
					}else{
						posHead(LEFT_ANGLE);
					}
				}else{
					posHead(FORWARD);
					// this timer will trigger a read on the front
					erouter->addTimer(this,PAN2TIMER,PANTIME,false);	
				}
			}
			// continue pan/scan behavior...
			if((event.getSourceID() == PAN2TIMER) && (USE_ANY > 0)){
				// move head home, we can issue this at the beginning, since state won't change but ENABLE_CRUISE happens below
				if(FOLLOW_RIGHT){			
					posHead(RIGHT_ANGLE);
				}else{
					posHead(LEFT_ANGLE);
				}
				// read IR
				if(state->sensors[NearIRDistOffset] < CRITICAL_DIST){
					std::cout << "Forward Wall Detected" << std::endl;
					// turn away from forward wall
					if(FOLLOW_RIGHT){
						// we are following a right wall, hit a end wall, turn left
						turnLeft();
					}else{
						turnRight();
					}
				}else{
					std::cout << "Forward wall reading: " << state->sensors[NearIRDistOffset] << std::endl;
					ENABLE_CRUISE;
					ENABLE_PANSCAN;
					erouter->addTimer(this,PANTIMER,PAN_FREQ,false);
				}
			}
			if(event.getSourceID() == TURNOPENING){
				if(FOLLOW_RIGHT){
					turnRight();
				}else{
					turnLeft();
				}
			}
			if(event.getSourceID() == VISTIMEOUT){
				// we have lost the object
				// this should really be called lost flags!
				foundFlags++;
				if(done == 0){
					MMAccessor<WalkMC> walk(walk_id);
					switch(foundFlags%3){
						case 1:
							posHead(RAD(-30));
							startWalk(0,RAD(-30));
							erouter->addTimer(this,VISTIMEOUT,10000,false);
							break;
						case 2:
							// strafe left?
							walk.mc()->setTargetVelocity(0,70,0);
							erouter->addTimer(this,VISTIMEOUT,7000,false);
							break;
						case 0:
							// strafe right?
							walk.mc()->setTargetVelocity(0,-70,0);
							erouter->addTimer(this,VISTIMEOUT,7000,false);
							break;
					}
					
				}
			}
			if(event.getSourceID() == DONETIMER){
				stopWalk();
				// sit!
				motman->addPrunableMotion(SharedObject<PostureMC>("situp.pos"));
				
				// done. 
				std::cout << "we done" << std::endl;
				erouter->removeListener(this, EventBase::visObjEGID);
				erouter->removeListener(this, EventBase::timerEGID);
			}	
			break;
		
		// goal seen
		case EventBase::visObjEGID:{
			posHead(0);
			if( event.getSourceID() == GOAL_SID ){	
				float area = static_cast<const VisionObjectEvent*>(&event)->getBoundaryArea();
				std::cout << "obj area:" << area << std::endl;
				KILL_CRUISE;
				KILL_PANSCAN;
				//if(FOUND == 0){
					// move head home
					posHead(0);
					//startWalk(100,0);
					//FOUND_GOAL;
				//}else{
					if(area > 2.0){
						//close enough
						// walk forward a few
						if(done == 0){
							done = 1;
							erouter->addTimer(this,DONETIMER,5000,false);
						}
					}
					// get location
					float horiz=static_cast<const VisionObjectEvent*>(&event)->getCenterX();
					float vert=static_cast<const VisionObjectEvent*>(&event)->getCenterY();
					std::cout << "Ball at " << horiz << " " << std::endl;
					
					MMAccessor<HeadPointerMC> head(head_id);
					head.mc()->setJoints(0,0,state->outputs[RobotInfo::HeadOffset + RobotInfo::TiltOffset] - vert);
					
					// load our last pan direction	
					double angle = -horiz*2;
					
					//lastdir= horiz;
					startWalk(100, angle);	
				}
				erouter->addTimer(this,VISTIMEOUT,700,false);
			}
			break;
		
		
		// all other events
		default:
		    break;
	}
  }
  
private:
    bMaze(const bMaze&);
    bMaze operator=(const bMaze&);  
};