#ifndef _GRASPER_H
#define _GRASPER_H

#include <Crew/GrasperRequest.h>
#include <Events/GrasperEvent.h>
#include <Planners/RRT/RRTPlanner.h>
#include <Crew/MotionNodes.h>

/*
TODO
	- Sweep
	- Test-suite
	
	- Calculate the offset of the gripper frame from wristYaw when projected in XY
*/


class Grasper: public StateNode {
#ifdef TGT_HAS_ARMS
	BehaviorBase* requestingBehavior;		//!The requesting behavior for this grasper request
	GrasperRequest* curReq;					//!The request itself
	
	//! A collection of actions that higher level Grasper actions can consume
	struct GripperNode: public StateNode {
		GripperNode(const string &str): StateNode(str)	{}		
		
		//! Forwards the given point through a signalTrans
		#shortnodeclass ForwardPoint(Point p) : StateNode : doStart
		{
			postStateSignal<Point>(p);
		}

		//! Forwards the error through a signalTrans
		#shortnodeclass ForwardError(GrasperRequest::GrasperErrorType_t t) : StateNode : doStart
		{
			cout<<"Forwarding Error"<<t<<endl;
			postStateSignal<GraspError>(t);
		}

		//! Posts the parent's completion
		#shortnodeclass PostNodeSuccess(GripperNode& a) : StateNode : doStart
		{
			a.postStateCompletion();
		}

		//! Posts the parents error via a signalTrans
		#nodeclass PostNodeFailure(GripperNode& a, GrasperRequest::GrasperErrorType_t e = GrasperRequest::noError) : StateNode 
			void postSignal(GraspError t) {
				a.postStateSignal<GraspError>(t);
			}
		
			#nodemethod doStart
			{
				if(const DataEvent<GraspError> *datev = dynamic_cast<const DataEvent<GraspError>*>(event)) {
					GraspError err = datev->getData();
					postSignal(err);
				} else {
					postSignal(e);
				}
			}
			#endnodemethod
			
		#endnodeclass
		
		//! Calculates the ground plane value required for manipulation through a signalTrans
		#nodeclass ForwardGroundValue : StateNode
			virtual void doStart();
		#endnodeclass
		
		//! Forwards the target's centroid through a signalTrans
		#nodeclass ForwardTargetLocation : StateNode
			virtual void doStart();
		#endnodeclass
		
		//! Forwards the object's centroid through a signalTrnas
		#nodeclass ForwardObjectCentroid : StateNode
			virtual void doStart();
		#endnodeclass
			
		//! Forwards the request type through a signalTrans
		#nodeclass ForwardRequestType : StateNode
			virtual void doStart();
		#endnodeclass
		
		//! Plans the constrained path
		#nodeclass PathPlanConstrained : StateNode
			virtual void doStart();
		#endnodeclass
		
		//! Plans the unconstrained path
		#nodeclass PathPlanUnconstrained : StateNode
			virtual void doStart();
		#endnodeclass
		
		//! Path plans to the rest state
		#nodeclass PathPlanToRest : StateNode
			virtual void doStart();
		#endnodeclass
	};
	
	//! Executes an arm movement
	#nodeclass MoveArm(GrasperRequest::GrasperPathType_t pa) : DynamicMotionSequenceNode
		void executeMove(const RRTPath& path);
		
		unsigned int advTime(const ArmConfiguration& confDif) {
			float shDiff = abs(confDif[0]);
			float elDiff = abs(confDif[1]);
			float wrDiff = abs(confDif[2]);
			// Kind of voodoo. Could be tidied up
			unsigned int time = (unsigned int)( (max(max(shDiff,elDiff),wrDiff)/(M_PI/6))*1500 );
			return max((unsigned)11, (unsigned int)(time * VRmixin::grasper->getCurReq()->armTimeFactor));
		}
		
		virtual void doStart();
	#endnodeclass
	
	//! Spawns all appropriate planners based on the requested action
	#shortnodeclass Plan : GripperNode : setup
	{
		#statemachine
			restPlan: PathPlanToRest =S<GraspError>=> PostNodeFailure($, *this)
			objectUncon: PathPlanUnconstrained =S<GraspError>=> PostNodeFailure($, *this)
			objectUnconCont: PathPlanUnconstrained =S<GraspError>=> PostNodeFailure($, *this)
			targetCon: PathPlanConstrained =S<GraspError>=> PostNodeFailure($, *this)
			targetConCont: PathPlanConstrained =S<GraspError>=> PostNodeFailure($, *this)
			objectUnconCont =C=> targetConCont
			objectUncon =C=> restPlan
			targetCon =C=> restPlan
			targetConCont =C=> restPlan
			
			startnode: ForwardRequestType =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::checkGraspable)=> objectUncon =C=> PostNodeSuccess($, *this)
			startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::reach)=> objectUncon =C=> PostNodeSuccess($, *this)
			startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::grasp)=> objectUncon =C=> PostNodeSuccess($, *this)
			startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::release)=> targetCon =C=> PostNodeSuccess($, *this)
			startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::moveTo)=> objectUnconCont
			startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::touch)=> objectUncon =C=> PostNodeSuccess($, *this)
			startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::rest)=> restPlan =C=> PostNodeSuccess($, *this)
		#endstatemachine
	}
	
	//! Chooses and changes the working plane of the robot
	struct ChoosePlane: public GripperNode {
		ChoosePlane(const string& str): GripperNode(str)	{}
		
		#nodeclass Startup : StateNode
			virtual void doStart();
		#endnodeclass
		
		void setup() {
		#statemachine
			stand: SetGround($, -130)
			startnode: Startup =C=> stand =F=> PostNodeFailure($, *this, GrasperRequest::badMove)
			stand =T(3000)=> PostNodeSuccess($, *this)
			startnode =F=> PostNodeSuccess($, *this)
		#endstatemachine
		}
	};
			
	#nodeclass OpenTheGripper : StateNode
		void setup() {
		#statemachine
			openLeft: SetJoint($, "ARM:gripperLeft", openLeftGripperVal)
			openRight: SetJoint($, "ARM:gripperRight", openRightGripperVal)
			startnode: StateNode =N=> {openLeft, openRight}
				{openLeft, openRight} =C(1)=> PostNodeSuccess($, *this)
		#endstatemachine
		}
		#nodeclass PostNodeSuccess(OpenTheGripper& a) : StateNode : doStart
			a.postStateCompletion();
		#endnodeclass
	#endnodeclass
			
	#nodeclass CloseTheGripper : StateNode
		void setup() {
		#statemachine
			closeLeft: SetJoint($, "ARM:gripperLeft", closedLeftGripperVal)
			closeRight: SetJoint($, "ARM:gripperRight", closedRightGripperVal)
			startnode: StateNode =N=> {closeLeft, closeRight}
				{closeLeft, closeRight} =C(1)=> PostNodeSuccess($, *this)
		#endstatemachine
			}
		#nodeclass PostNodeSuccess(CloseTheGripper& a) : StateNode : doStart
			a.postStateCompletion();
		#endnodeclass
	#endnodeclass
	
	//! Grasps or releases an object from the gripper if the gripper is manipulable
	struct Grasp: public GripperNode {
		Grasp(const string& str, bool _closeGripper = true): GripperNode(str), closeGripper(_closeGripper)	{}
		
		bool closeGripper;
		
		void setup() {
		#statemachine
			failed: PostNodeFailure($, *this, GrasperRequest::badMove)
			pitchDown: SetJoint($, "ARM:wristPitch", wristPitchDownVal,0.75) =F=> failed
			pitchUp: SetJoint($, "ARM:wristPitch", wristPitchUpVal,0.75) =F=> failed
			lowerToGrasp: SetGround =F=> failed
			openTheGripper: OpenTheGripper
			closeTheGripper: CloseTheGripper
			setPlane: ChoosePlane =F=> failed
			
			split: StateNode >==CompareTrans<bool>(openTheGripper, &this->closeGripper, CompareTrans<bool>::EQ, true)==> openTheGripper =C=> setPlane
			split >==CompareTrans<bool>(closeTheGripper, &this->closeGripper, CompareTrans<bool>::EQ, false)==> closeTheGripper =C=> setPlane
		
			startnode: StateNode =N=> pitchDown =C=> ForwardGroundValue =S<float>=> lowerToGrasp =T(3000)=> split
			setPlane =C=> pitchUp =C=> PostNodeSuccess($, *this)
		#endstatemachine
		}
	};
	
	//! Executes moving to the rest state
	struct Rest: public GripperNode {
		Rest(const string& str): GripperNode(str)	{}
		
		#nodeclass Split: StateNode
			virtual void doStart();
		#endnodeclass
		
		#nodeclass SplitOnGripper : StateNode
			virtual void doStart();
		#endnodeclass
		
		void setup() {
		#statemachine
			succeeded: PostNodeSuccess($, *this)
			failed: PostNodeFailure($, *this, GrasperRequest::badMove)
			restArm: MoveArm($, GrasperRequest::disengage) =F=> failed
			restArm =C=> succeeded
			openTheGripper: OpenTheGripper =C=> succeeded
			fork: SplitOnGripper
			
			pitchUp: SetJoint($, "ARM:wristPitch", wristPitchUpVal) =F=> failed
			armLeft: MoveArm($, GrasperRequest::disengage) =F=> failed
			sitDown: SetGround($, -10) =F=> failed
			pitchUp =C=> armLeft =C=> sitDown =T(3000)=> fork
			fork =S<bool>(false)=> succeeded
			fork =S<bool>(true)=> openTheGripper
			
			startnode: Split =S<GrasperRequest::GrasperRestType_t>(GrasperRequest::stationary)=> succeeded
			startnode =S<GrasperRequest::GrasperRestType_t>(GrasperRequest::settleArm)=> restArm
			startnode =S<GrasperRequest::GrasperRestType_t>(GrasperRequest::settleBodyAndArm)=> pitchUp
		#endstatemachine
		}
	};
	
	//! Posts a failure based grasper event
	#nodeclass PostFailure(GrasperRequest::GrasperErrorType_t err = GrasperRequest::badMove) : StateNode
		void doPostEvent(GraspError e);
	
		#shortnodemethod doStart
		{
			if(const DataEvent<GraspError> *datev = dynamic_cast<const DataEvent<GraspError>*>(event)) {
				GraspError e = datev->getData();
				doPostEvent(e);
			} else {
				doPostEvent(err);
			}
		}
	
	#endnodeclass
	
	//! Posts a successful grasper event
	#nodeclass PostCompletion : StateNode
		virtual void doStart();
	#endnodeclass
			
	#nodeclass PopulateEventWithFreePath : StateNode
		virtual void doStart();
	#endnodeclass
	
			
	Plan *checkRoot;		//! The state machine entrance for any check* request
	Plan *graspRoot;		//! The state machine for only acquiring and grasping
	Plan *releaseRoot;		//! The state machine for only releasing
	Plan *moveRoot;			//! The state machine to move an object to the targetLocation
	Plan *reachRoot;		//! The state machine for acquiring
	Plan *touchRoot;		//! The state machine for performing the touch action
	Plan *restRoot;			//! The state machine to execute resting
	Plan *computeReachRoot;
	
//	Point desiredRobotLocation;		//! When an object or target is out of range, this will hold the desired robot's location in order to manipulate
//	Point desiredLookAtPoint;		//! When an object or target is out of range, this will hold the desired robot's orientation in order to manipulate
	
	RRTStateVector rrtStates;	//!The pool of rrtStates to use
	RRTPlanner rrtp;			//! The RRT Planner itself
	
	//! Converts all environmental obstacles to planner obstacles
	void convertObstacles();
	
	Grasper(const Grasper& o);
	void operator=(const Grasper& o);
public:
	GrasperRequest::GrasperPathType_t populateEventPathWith;
			
	Grasper():
		StateNode(),
		requestingBehavior(NULL),
		curReq(),
		checkRoot(),
		graspRoot(),
		releaseRoot(),
		moveRoot(),
		reachRoot(),
		touchRoot(),
		restRoot(),
		computeReachRoot(),
//		desiredRobotLocation(),
//		desiredLookAtPoint(),
		rrtStates(0, 0),
		rrtp(rrtStates, GripperFrameOffset, 0),
		populateEventPathWith(GrasperRequest::noPath)
	{
		this->setAutoDelete(false);
		
		#statemachine
			startnode: StateNode
			failed : PostFailure
		
			ch_Plan : Plan =S<GraspError>=> failed
			ch_Plan =C=> PostCompletion
			
			re_Plan : Plan =S<GraspError>=> failed
			re_Stand : ChoosePlane =S<GraspError>=> failed
			re_Move : MoveArm($, GrasperRequest::moveFree) =S<GraspError>=> failed
			re_Rest : Rest
			re_Plan =C=> re_Stand =C=> re_Move =C=> re_Rest =C=> PostCompletion
			
			gr_Plan : Plan =S<GraspError>=> failed
			gr_Stand : ChoosePlane =S<GraspError>=> failed
			gr_Move : MoveArm($, GrasperRequest::moveFree) =S<GraspError>=> failed
			gr_Grasp: Grasp($, false) =S<GraspError>=> failed
			gr_Rest : Rest
			gr_Plan =C=> gr_Stand =C=> gr_Move =C=> gr_Grasp =C=> gr_Rest =C=> PostCompletion
			
			rl_Plan : Plan =S<GraspError>=> failed
			rl_Stand : ChoosePlane =S<GraspError>=> failed
			rl_Move : MoveArm($, GrasperRequest::moveConstrained) =S<GraspError>=> failed
			rl_Grasp: Grasp($, true) =S<GraspError>=> failed
			rl_Rest : Rest
			rl_Plan =C=> rl_Stand =C=> rl_Move =C=> rl_Grasp =C=> rl_Rest =C=> PostCompletion
			
			mv_Plan : Plan =S<GraspError>=> failed
			mv_Stand : ChoosePlane =S<GraspError>=> failed
			mv_MoveA : MoveArm($, GrasperRequest::moveFree) =S<GraspError>=> failed
			mv_GraspO: Grasp($, false) =S<GraspError>=> failed
			mv_MoveB: MoveArm($, GrasperRequest::moveConstrained) =S<GraspError>=> failed
			mv_GraspC: Grasp($, true) =S<GraspError>=> failed
			mv_Rest : Rest
			mv_Plan =C=> mv_Stand =C=> mv_MoveA =C=> mv_GraspO =C=> mv_MoveB =C=> mv_GraspC =C=> mv_Rest =C=> PostCompletion
			
			tc_Plan : Plan =S<GraspError>=> failed
			tc_Stand : ChoosePlane =S<GraspError>=> failed
			tc_Move : MoveArm($, GrasperRequest::moveFree) =S<GraspError>=> failed
			tc_Grasp: Grasp($, false) =S<GraspError>=> failed
			tc_Plan =C=> tc_Stand =C=> tc_Move =C=> tc_Grasp =C=> PostCompletion
			
			rs_Plan : Plan =S<GraspError>=> failed
			rs_Rest : Rest =S<GraspError>=> failed
			rs_Plan =C=> rs_Rest =C=> PostCompletion
		
			cr_Plan: Plan =S<GraspError>=> failed
			cr_Plan =C=> PopulateEventWithFreePath =C=> PostCompletion
		#endstatemachine
		
		checkRoot = ch_Plan;
		reachRoot = re_Plan;
		graspRoot = gr_Plan;
		releaseRoot = rl_Plan;
		moveRoot = mv_Plan;
		touchRoot = tc_Plan;
		restRoot = rs_Plan;
		computeReachRoot = cr_Plan;
	}
	
	void executeRequest(GrasperRequest& req, BehaviorBase* RequestingBehavior) {
		this->requestingBehavior = RequestingBehavior;
		Grasper::curReq = &req;
		if(req.verbosity)
			cout<<"executeRequest"<<endl;
		
		if(req.object.isValid() && req.object->getSpace().getRefFrameType() == VRmixin::worldShS.getRefFrameType()) {
			req.object = VRmixin::mapBuilder->importWorldToLocal(req.object);
		}
		
		if(req.targetLocation.isValid() && req.targetLocation->getSpace().getRefFrameType() == VRmixin::worldShS.getRefFrameType()) {
			req.targetLocation = VRmixin::mapBuilder->importWorldToLocal(req.targetLocation);
		}
		
		if(req.armTimeFactor < 0) {
			if(req.verbosity)
				cout<<"GRASPER - armTimeFactor was less than 0, changing to 1"<<endl;
			req.armTimeFactor = 1;
		}
			
		rrtStates.initialize(req.numberOfStatesForRRT, req.numJointsToUse);
		rrtp.initialize(req.manipulatorOffset, req.numJointsToUse, req.maxRRTIterations, req.RRTTolerance, req.RRTstepsize, req.RRTItrStepsize);
		convertObstacles();
		
		int reqType = req.requestType;
		switch( reqType ) {
			case GrasperRequest::reach:
				reachRoot->start();
				break;
			case GrasperRequest::grasp:
				graspRoot->start();
				break;
			case GrasperRequest::release:
				releaseRoot->start();
				break;
			case GrasperRequest::moveTo:
				moveRoot->start();
				break;
			case GrasperRequest::touch:
				touchRoot->start();
				break;
			case GrasperRequest::rest:
				restRoot->start();
				break;
			case GrasperRequest::checkGraspable:
				req.requestType = GrasperRequest::grasp;
				checkRoot->start();
				break;
			case GrasperRequest::checkMovable:
				req.requestType = GrasperRequest::moveTo;
				checkRoot->start();
				break;
			case GrasperRequest::checkRestable:
				req.requestType = GrasperRequest::rest;
				checkRoot->start();
				break;
			case GrasperRequest::computeReach:
				req.requestType = GrasperRequest::reach;
				computeReachRoot->start();
				break;
			case GrasperRequest::sweep:
				cout<<"Grasper functionality "<<reqType<<" not implemented"<<endl;
				break;
			default:
				cout << "Unidentified requestType" << endl;
				break;
		}
	}
	
	RRTPlanner& getRRTP() { return rrtp; }
	BehaviorBase* getRequestingBehavior() const { return requestingBehavior; }
	GrasperRequest::GrasperRequestType_t getRequestType() const { return curReq->requestType; }
	GrasperRequest* getCurReq() const { return curReq; }
	
//	Point& getDesiredRobotLocation() { return desiredRobotLocation; }
//	Point& getDesiredLookAtPoint() { return desiredLookAtPoint; }
	
	//! Converts the manipulatee object to a planner obstacle and adds it to the planner obstacles
	void appendTargetToObstacles() {
		if(!curReq->object.isValid()) {
			return;
		}
		
		if(!curReq->doNotStand) {
			//Assume that the arm will clear the obstacle since we're picking it up
			return;
		}
		
		if(curReq->objectPlannerObs == NULL) {
			convertShapeToPlannerObstacle(curReq->object, curReq->objectPlannerObs);
		}
		
		if(!curReq->moveConstrainedPath.empty()) {
			fmat::Column<2> newPos = fmat::pack(curReq->targetLocation->getCentroid().coordX(), curReq->targetLocation->getCentroid().coordY());
			curReq->objectPlannerObs->updatePosition(newPos);
		}
		
		curReq->plannerObstacles.push_back(curReq->objectPlannerObs);
	}
	
	//! Removes the manipulatee object's planner obstacle from the planner obstacles
	void removeTargetFromObstacles() {
		if(!curReq->object.isValid()) {
			return;
		}
		
		if(!curReq->doNotStand) {
			//Assume that the arm will clear the obstacle since we're picking it up
			return;
		}
	
		vector<PlannerObstacle*>::iterator obj = std::find(curReq->plannerObstacles.begin(), curReq->plannerObstacles.end(), curReq->objectPlannerObs);
		if(obj != curReq->plannerObstacles.end()) {
			curReq->plannerObstacles.erase(obj);
		}
		
		delete curReq->objectPlannerObs;
		curReq->objectPlannerObs = NULL;
	}
	
	//! Computes all goal states around a point and rotating the approach orientation
	void computeGoalStates(Point& toPt, std::vector<std::pair<float, float> >& ranges, float res, std::vector<RRTStateDef>& goals, fmat::Column<3> offset);
	
#else
public:
	void executeRequest(const GrasperRequest& req, BehaviorBase* RequestingBehavior) {}
#endif
};

#endif