//-*-c++-*-
#ifndef INCLUDED_Pilot_h_
#define INCLUDED_Pilot_h_

#include <queue>

#include "Behaviors/Nodes/WaypointWalkNode.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Crew/PilotRequest.h"
#include "Crew/MapBuilderNode.h"
#include "Motion/MMAccessor.h"
#include "Motion/WaypointWalkMC.h"
#include "Motion/WaypointEngine.h"
#include "Motion/WaypointList.h"
#include "Planners/Navigation/ShapeSpacePlanner.h"


#include "DualCoding/DualCoding.h"

using namespace std;

namespace DualCoding {

	class Pilot : public StateNode  {
  public:
    typedef unsigned int PilotVerbosity_t;
    static const PilotVerbosity_t PVstart = 1<<0;
    static const PilotVerbosity_t PVevents = 1<<1;
    static const PilotVerbosity_t PVexecute = 1<<2;
    static const PilotVerbosity_t PVsuccess = 1<<3;
    static const PilotVerbosity_t PVfailure = 1<<4;
    static const PilotVerbosity_t PVcomplete = 1<<5;
    static const PilotVerbosity_t PVabort = 1<<6;
    static const PilotVerbosity_t PVqueued = 1<<7;       

    static PilotVerbosity_t getVerbosity() { return verbosity; }
    static void setVerbosity(PilotVerbosity_t v) { verbosity = v; }
	  
	  struct Path {
		  Path() : path(){};
	  	  std::vector<DualCoding::Point> path;
      }pathToTake;
  
    Pilot() : StateNode("Pilot"), pathToTake(),
	      requests(), curReq(NULL), idCounter(0), 
	      waypointwalk_id(MotionManager::invalid_MC_ID), dispatch_(NULL)
    {}

    virtual void doStart() {
      if ( verbosity & PVstart)
	cout << "Pilot starting up: waypointwalk_id=" << waypointwalk_id << endl;
      addNode(new RunMotionModel)->start();
    }

    virtual void doStop() {
      if ( verbosity & PVstart )
	cout << "Pilot is shutting down." << endl;
      abort();
      motman->removeMotion(waypointwalk_id);
      waypointwalk_id = MotionManager::invalid_MC_ID;
      // The following teardown() call is necessary because setup
      // hard-wires the MC_ID for all the walk nodes.  If we leave the
      // waypoint walk motion command active when Tekkotsu shuts down,
      // it crashes and generates a nasty backtrace.  But if we remove
      // the motion command, there's no easy way to put it back because
      // setup won't be called again unless we do this teardown() call.
      teardown();
    }

    virtual void teardown() {
      StateNode::teardown();
    }

    virtual void doEvent() {
      if ( event->getGeneratorID() == EventBase::timerEGID && event->getSourceID() == 9999 )
	executeRequest();
      else cout << "Pilot got unexpected event: " << event->getDescription() << endl;
    }

    void unwindForNextRequest() {
      erouter->addTimer(this,9999,1,false); // allow time to unwind the event stack before executing next request
    }
	
    //! Pilot's setAgent is called by its RunMotionModel behavior.  Or user can call directly.
    void setAgent(const Point &loc, AngTwoPi heading, bool quiet=false, bool updateWaypoint=true) {
      if ( updateWaypoint ) {
	MMAccessor<WaypointWalkMC> wp_acc(waypointwalk_id);
	wp_acc->setCurPos(loc.coordX(), loc.coordY(), heading);
      }
      VRmixin::mapBuilder->setAgent(loc, heading, quiet);
      // Setting the best particle (below) is a no-op if setAgent was
      // invoked by RunMotionModel, but not if the user called
      // setAgent directly.
      VRmixin::particleFilter->setBestParticle();
    }

    unsigned int executeRequest(BehaviorBase* requestingBehavior, const PilotRequest& req) {
      requests.push(new PilotRequest(req));
      const unsigned int reqID = ++idCounter;
      requests.back()->requestID = reqID;
      requests.back()->requestingBehavior = requestingBehavior;
      if ( curReq == NULL )
	unwindForNextRequest();
      else if ( verbosity & PVqueued )
	cout << "Pilot request " << requests.back()->requestID << " queued." << endl;
      return reqID;
    }

    void executeRequest() {
      if ( curReq == NULL && !requests.empty() ) {
	curReq = requests.front();
	if ( verbosity & PVexecute )
	  cout << "Pilot executing request " << curReq->requestID << endl;
	VRmixin::particleFilter->setBestParticle();
	dispatch_->start();
      }
    }

    void requestComplete(const bool result) {
      if ( curReq == NULL ) {
	cout << "Pilot::requestComplete had NULL curReq !!!!!!!!!!!!!!!!" << endl;
	return;
      }
      //if ( curReq->trackRequest != NULL )
      //  VRmixin::lookout.stopTrack();
      dispatch_->finish();
      const size_t reqID = curReq->requestID;
      const BehaviorBase* requester = curReq->requestingBehavior;
      delete curReq->mapBuilderRequest;
      delete curReq;
      curReq = NULL;
      requests.pop();
      // cout << "*** Pilot dropping waypointwalk priority" << endl;
      motman->setPriority(waypointwalk_id,MotionManager::kIgnoredPriority);
      // motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
      if ( verbosity & PVcomplete )
	cout << "Pilot request " << reqID << " complete." << endl;
      erouter->postEvent(EventBase(EventBase::pilotEGID, reqID,
				   EventBase::statusETID, 0, "Pilot Completion", float(result)));
      erouter->postEvent(EventBase(EventBase::pilotEGID,
				   (requester==NULL) ? reqID : (size_t)requester, 
				   EventBase::deactivateETID, 0, "Pilot Completion", float(result)));
      unwindForNextRequest();
    }

    void abort() {
      if ( verbosity & PVabort )
	cout << "Pilot aborting." << endl;
      dispatch_->stop();
      curReq = NULL;
      while (!requests.empty()) {
	delete requests.front();
	requests.pop();
      }
      motman->setPriority(waypointwalk_id,MotionManager::kIgnoredPriority);
    }

    MotionManager::MC_ID getWaypointwalk_id() const { return waypointwalk_id; }

    class RunMotionModel : public StateNode {
      virtual void doStart(){
	//cout<<"RunMotionModel doStart Begun\n";
	erouter->addTimer(this, 1, 250, true);
      }
  
      virtual void doEvent(){
	if ( event->getGeneratorID() == EventBase::timerEGID) {
	  //cout<<"RunMotionModel doEvent begun \n"; 
	  VRmixin::particleFilter->updateMotion();
	  const PFShapeLocalization::particle_type &best = VRmixin::particleFilter->getBestParticle();
		//std::cout<<"best particle x: "<<best.x<<" y: "<<best.y<<" theta: "<<best.theta<<std::endl;
	  VRmixin::pilot->setAgent(Point(best.x,best.y,0,allocentric), best.theta, true, false);
	}
      } 
    };

#shortnodeclass ReportSuccess : StateNode : doStart
    if ( Pilot::getVerbosity() & Pilot::PVsuccess )
      cout << "Pilot result: Success" << endl;
    VRmixin::pilot->requestComplete(true);

#shortnodeclass ReportFailure : StateNode : doStart
    if ( Pilot::getVerbosity() & Pilot::PVfailure )
      cout << "Pilot result: Failure" << endl;
    VRmixin::pilot->requestComplete(false);

#shortnodeclass WalkMachine : WaypointWalkNode : doStart
    PilotRequest &req = *VRmixin::pilot->curReq;
    MMAccessor<WaypointWalkMC> wp_acc(getMC());
    wp_acc->getWaypointList().clear();
#if defined(TGT_CREATE) || defined(TGT_CALLIOPE)
    float vx = (req.fwdSpeed > 0) ? req.fwdSpeed : 50.f;
    float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
    if ( req.da != 0 ) {
      wp_acc->addEgocentricWaypoint(0, 0, req.da, true, vx, va);
    }
    if ( req.dx != 0 ) {
      wp_acc->addEgocentricWaypoint(req.dx, 0, (req.dx >= 0) ? 0 : -M_PI , true, vx, va);
    }
#else
    float vx = (req.fwdSpeed > 0) ? req.fwdSpeed : 15.f;
    float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
    wp_acc->addEgocentricWaypoint(req.dx, req.dy, req.da, true, vx, va);
#endif
    // cout << "*** Pilot raising waypointwalk priority" << endl;
    motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
    wp_acc->go();
    

#nodeclass WaypointWalkMachine : WaypointWalkNode : doStart
    PilotRequest &req = *VRmixin::pilot->curReq;
    MMAccessor<WaypointWalkMC> wp_acc(getMC());
    if(req.clearWaypoints)
      wp_acc->clearWaypointList();
    wp_acc->appendWaypoints(*req.waypointList);
    wp_acc->go();
#endnodeclass
		

#nodeclass GoToShapeMachine : VisualRoutinesStateNode : nextLocation()
	// distance in mm that we can get within a given point and be "there"
	static const float ALLOWABLE_DISTANCE_ERROR = 50;
	static const float ALLOWABLE_ANGULAR_ERROR = 0.15;
		
	struct Location {
		Location() : nextPoint(), angleToTurn() {};
		fmat::Column<2> nextPoint;
		AngSignPi angleToTurn;
	} nextLocation;
	
	#shortnodeclass Punt : StateNode : doStart
		postParentCompletion();		
		
    #nodeclass PlanPath : VisualRoutinesStateNode : doStart
	  //std::cout<<"started GOTOSHAPEMACHINE \n";

		PilotRequest &req = *VRmixin::pilot->curReq;
		const ShapeRoot &target = req.targetShape;
		if ( ! target.isValid() ) {
		//	std::cout<<"PostingStateFailure\n";
			postStateFailure();
			return;
		}
		
	    const Point targetLoc = target->getCentroid();
		
		std::cout << "Planning path to point: " << targetLoc << std::endl;
		GET_SHAPE(worldBounds, PolygonData, worldShS);
		ShapeSpacePlanner planner = ShapeSpacePlanner(worldShS, worldBounds, 255);
		// get current location
		DualCoding::Point position = theAgent->getCentroid();
		//std::cout<<"position : "<<position<<std::endl;
	    Path &pathToTake = getParent()->getParent()->parentAs<Pilot>()->pathToTake;
		pathToTake.path = planner.planPath(position, targetLoc);

		if ( pathToTake.path.size() > 0 ) {
			NEW_SHAPE(plannedPath, PolygonData,
					  new PolygonData(worldShS, pathToTake.path, false));
			plannedPath->setColor(rgb(0,0,255));
			//std::cout<<"req.executepath : "<<req.executePath<<"\n";
			if (req.executePath) {
				postStateCompletion();
				return;
			} else{
				//std::cout<<"execute Path is set to false\n";
				postStateFailure();
				return;
			}
		} else {
			std::cout<<"Collision Detected\n";
			postStateFailure();
		}

	  
    #endnodeclass
		
	#nodeclass CheckLocation : VisualRoutinesStateNode : doStart
		// check to see if we're close to the next waypoint
		DualCoding::Point position = theAgent->getCentroid();
		Path &pathToTake = getParent()->getParent()->parentAs<Pilot>()->pathToTake;

		if (pathToTake.path.size() == 0) {
			//std::cout << "Warning: empty path in FollowPathNode::CheckLocation::doStart" << std::endl;
			postStateCompletion();
			return;
		}
		
		//std::cout << "Checking current location: " << position << std::endl;
		//std::cout << "against next point: " << pathToTake.path[0] << std::endl;
		
		while (position.xyDistanceFrom(pathToTake.path[0]) < ALLOWABLE_DISTANCE_ERROR) {
			std::cout << "Point " << pathToTake.path[0] << " reached." << std::endl;
			pathToTake.path.erase(pathToTake.path.begin(),pathToTake.path.begin()+1);
			
			if (pathToTake.path.size() == 0) {
				postStateCompletion();
				return;
			}
		}
		
		fmat::Column<3> pos = position.getCoords();
		fmat::Column<3> next = pathToTake.path[0].getCoords();
		
		Location &nextLocation = parentAs<GoToShapeMachine>()->nextLocation;
		nextLocation.nextPoint = fmat::SubVector<2>(next) - fmat::SubVector<2>(pos);
		nextLocation.angleToTurn = (atan2(nextLocation.nextPoint[1], nextLocation.nextPoint[0]) - theAgent->getOrientation());
		
		//cout << "Heading to next point: " << nextLocation.nextPoint << endl;
		// are we facing the next point?
		if (fabs(nextLocation.angleToTurn) > ALLOWABLE_ANGULAR_ERROR) {
		//	cout << "Turning to face next point!" << endl;
		//	cout << "Angle is: " << nextLocation.angleToTurn << endl;
			postStateSignal(true);
			return;
		} else {
		//	cout << "Advancing to next point!" << endl;
			nextLocation.nextPoint *= fmin( 100.0/nextLocation.nextPoint.norm(), 1.0 );
			postStateSignal(false);
		}
	#endnodeclass
		
 	#nodeclass Walk : WaypointWalkNode : doStart
		PilotRequest &req = *VRmixin::pilot->curReq;
		Location &nextLocation = parentAs<GoToShapeMachine>()->nextLocation;
		MMAccessor<WaypointWalkMC> wp_acc(getMC());
		wp_acc->getWaypointList().clear();
		bool turnOnly = extractSignal<bool>(event);

		#if defined(TGT_CREATE) || defined(TGT_CALLIOPE)
				float vx = (req.fwdSpeed > 0) ? req.fwdSpeed : 50.f;
				float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
				//std::cout<<"vx : "<<vx<<" va : "<<va<<" turnOnly : "<<turnOnly<<std::endl;
				if ( turnOnly ) {
					wp_acc->addEgocentricWaypoint(0, 0, fmin(nextLocation.angleToTurn, 0.5), true, vx, va);
				}
				else {
					//std::cout<<"nexPoint.norm() : "<<nextLocation.nextPoint.norm()<<std::endl;
					wp_acc->addEgocentricWaypoint(nextLocation.nextPoint.norm(), 0,  0, true, vx, va);
				}
		#else
				float vx = (req.fwdSpeed > 0) ? req.fwdSpeed : 15.f;
				float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
		        if ( turnOnly ) {
					wp_acc->addEgocentricWaypoint(0, 0, fmin(nextLocation.angleToTurn, 0.5), true, vx, va);
				}
				else {
					wp_acc->addEgocentricWaypoint(nextLocation.nextPoint.norm(), 0,  0, true, vx, va);
				}
		#endif
		// cout << "*** Pilot raising waypointwalk priority" << endl;
		motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
		wp_acc->go();
	#endnodeclass
		
	#nodemethod setup
		#statemachine
  			plan: PlanPath =C=> check: CheckLocation
	  		plan =F=> Punt
	  
	  		check =S<bool>=> Walk[setMC(VRmixin::pilot->waypointwalk_id)] =C=> check
	  		check =C=> Punt
		#endstatemachine
	#endnodemethod
#endnodeclass //GoToShapeMachine

#nodeclass VisualSearchMachine : StateNode
#nodeclass Search : MapBuilderNode : doStart
    if ( pilot->curReq->mapBuilderRequest == NULL ) {
      cout << "Pilot received visualSearch request with invalid mapBuilderRequest field" << endl;
      pilot->requestComplete(false);
    }
    else if ( pilot->curReq->exitTest == NULL ) {
      cout << "Pilot received visualSearch request with no exitTest" << endl;
      pilot->requestComplete(false);
    }
    mapreq = *pilot->curReq->mapBuilderRequest;
#endnodeclass

#shortnodeclass Check : StateNode : doStart
    if ( VRmixin::pilot->curReq->exitTest() )
      VRmixin::pilot->requestComplete(true);
    else if ( VRmixin::pilot->curReq->searchRotationAngle == 0 )
      VRmixin::pilot->requestComplete(false);
    else
      postStateCompletion();
		 
#shortnodeclass Rotate : WaypointWalkNode : doStart
    MMAccessor<WaypointWalkMC> wp_acc(getMC());
    wp_acc->getWaypointList().clear();
    wp_acc->addEgocentricWaypoint(0,0,VRmixin::pilot->curReq->searchRotationAngle,true,0.01f);
    wp_acc->go();

#shortnodemethod setup   // set up the VisualSearchMachine
#statemachine
    startnode: Search() =MAP=> Check =C=>
    Rotate[setMC(VRmixin::pilot->waypointwalk_id)] =C=> startnode
#endstatemachine

#endnodeclass

	       // All the Pilot's little state machines are children of the
	       // Dispatch node, so when a call to Dispatch::finish() causes it to
	       // complete and exit, everything gets shut down.

#nodeclass* Dispatch : StateNode : walkMachine_(NULL), goToShapeMachine_(NULL), visualSearchMachine_(NULL), waypointWalkMachine_(NULL)
    StateNode *walkMachine_, *goToShapeMachine_, *visualSearchMachine_, *waypointWalkMachine_;

    void finish() {
      postStateCompletion();
    }

#nodemethod doStart
    PilotRequest::PilotRequestType_t reqType = VRmixin::pilot->curReq->getRequestType();
    switch ( reqType) {
    case PilotRequest::walk:
      walkMachine_->start();
      break;
    case PilotRequest::goToShape:
      goToShapeMachine_->start();
      break;
    case PilotRequest::visualSearch:
      visualSearchMachine_->start();
      break;
    case PilotRequest::waypointWalk:
      waypointWalkMachine_->start();
      break;
    default:
      cout << "Illegal Pilot request type: " << reqType << endl;
      VRmixin::pilot->requestComplete(false);
    }
				
#nodemethod setup
#statemachine
    startnode: StateNode("Dispatch Dummy Start")
      reportSuccess: ReportSuccess
      reportFailure: ReportFailure

      walkmachine: WalkMachine[setMC(VRmixin::pilot->waypointwalk_id)] =pilot_walkmachine_succeed:C=> reportSuccess
      walkmachine =pilot_walkmachine_fail:F=> reportFailure

      gotoshapemachine: GoToShapeMachine =C=> reportSuccess
      gotoshapemachine =F=> reportFailure

      visualsearchmachine: VisualSearchMachine =C=> reportSuccess
      visualsearchmachine =F=> reportFailure
		
      waypointwalkmachine: WaypointWalkMachine[setMC(VRmixin::pilot->waypointwalk_id)] =C=> reportSuccess
      waypointwalkmachine =F=>  reportFailure

#endstatemachine

			   // Save these statenode pointers for use by dispatch,
			   // and set up the walks to use a common motion command
      walkMachine_ = walkmachine;
    goToShapeMachine_ = gotoshapemachine;
    visualSearchMachine_ = visualsearchmachine;
    waypointWalkMachine_ = waypointwalkmachine;
#endnodemethod
#endnodeclass

    virtual void setup() {   // Pilot's setup
      SharedObject<WaypointWalkMC> waypointwalk_mc;
      waypointwalk_id = motman->addPersistentMotion(waypointwalk_mc, MotionManager::kIgnoredPriority);
#statemachine
    startnode: StateNode("Pilot Dummy Start")
	dispatch: Dispatch =pilot_dispatch:C=> startnode
#endstatemachine
	VRmixin::pilot->dispatch_ = dispatch;
    }

    static const unsigned int invalid_Pilot_ID = -1U;

  private:
    Pilot(const Pilot&); //!< copy constructor; do not call
    Pilot& operator=(const Pilot&); //!< assignment operator; do not call

    static PilotVerbosity_t verbosity;

    std::queue<PilotRequest*> requests;   //!< Queue of Pilot requests
    PilotRequest *curReq;
    unsigned int idCounter;   //!< Pilot request counter for assigning a unique id
    MotionManager::MC_ID waypointwalk_id;
    Dispatch *dispatch_;
	  


  };  // Pilot class

}; // namespace

#endif