#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
    $nodeclass ForwardPoint(Point p) : StateNode : doStart {
      postStateSignal<Point>(p);
    }

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

    //! Posts the parent's completion
    $nodeclass 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);
      }
		
      virtual void doStart() {
	if(const DataEvent<GraspError> *datev = dynamic_cast<const DataEvent<GraspError>*>(event)) {
	  GraspError err = datev->getData();
	  postSignal(err);
	} else {
	  postSignal(e);
	}
      }
			
			
    }
		
    //! Calculates the ground plane value required for manipulation through a signalTrans
    $nodeclass ForwardGroundValue : StateNode {
      virtual void doStart();
    }
		
    //! Forwards the target's centroid through a signalTrans
    $nodeclass ForwardTargetLocation : StateNode {
      virtual void doStart();
    }
		
    //! Forwards the object's centroid through a signalTrnas
    $nodeclass ForwardObjectCentroid : StateNode {
      virtual void doStart();
    }
			
    //! Forwards the request type through a signalTrans
    $nodeclass ForwardRequestType : StateNode {
      virtual void doStart();
    }
		
    //! Plans the constrained path
    $nodeclass PathPlanConstrained : StateNode {
      virtual void doStart();
    }
		
    //! Plans the unconstrained path
    $nodeclass PathPlanUnconstrained : StateNode {
      virtual void doStart();
    }
		
    //! Path plans to the rest state
    $nodeclass PathPlanToRest : StateNode {
      virtual void doStart();
    }
  };
	
  //! 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();
  }
	
  //! Spawns all appropriate planners based on the requested action
  $nodeclass 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)
    }
  }
	
  //! 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();
    }
		
    virtual void setup() {
      $statemachine{
      stand: SetGround($, -130)
	  startnode: Startup =C=> stand =F=> PostNodeFailure($, *this, GrasperRequest::badMove)
	  stand =T(3000)=> PostNodeSuccess($, *this)
	  startnode =F=> PostNodeSuccess($, *this)
	  }
    }
  };
			
  $nodeclass OpenTheGripper : StateNode {
    virtual void setup() {
      $statemachine{
      openLeft: SetJoint($, "ARM:gripperLeft", openLeftGripperVal)
	  openRight: SetJoint($, "ARM:gripperRight", openRightGripperVal)
	  startnode: StateNode =N=> {openLeft, openRight}
	{openLeft, openRight} =C(1)=> PostNodeSuccess($, *this)
				 }
    }
    $nodeclass PostNodeSuccess(OpenTheGripper& a) : StateNode : doStart {
      a.postStateCompletion();
    }
  }
			
  $nodeclass CloseTheGripper : StateNode {
    virtual void setup() {
      $statemachine{
      closeLeft: SetJoint($, "ARM:gripperLeft", closedLeftGripperVal)
	  closeRight: SetJoint($, "ARM:gripperRight", closedRightGripperVal)
	  startnode: StateNode =N=> {closeLeft, closeRight}
	{closeLeft, closeRight} =C(1)=> PostNodeSuccess($, *this)
      }
    }

    $nodeclass PostNodeSuccess(CloseTheGripper& a) : StateNode : doStart {
      a.postStateCompletion();
    }
  }
	
  //! 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;
		
    virtual 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)
	  }
    }
  };
	
  //! Executes moving to the rest state
  struct Rest: public GripperNode {
    Rest(const string& str): GripperNode(str)	{}
		
    $nodeclass Split: StateNode {
      virtual void doStart();
    }
		
    $nodeclass SplitOnGripper : StateNode {
      virtual void doStart();
    }
		
    virtual 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
	  }
    }
  };
	
  //! Posts a failure based grasper event
  $nodeclass PostFailure(GrasperRequest::GrasperErrorType_t err = GrasperRequest::badMove) : StateNode {
    void doPostEvent(GraspError e);
	
    virtual void doStart() {
      if(const DataEvent<GraspError> *datev = dynamic_cast<const DataEvent<GraspError>*>(event)) {
	GraspError e = datev->getData();
	doPostEvent(e);
      } else {
	doPostEvent(err);
      }
    }
  }
	
  //! Posts a successful grasper event
  $nodeclass PostCompletion : StateNode {
    virtual void doStart();
  }
			
  $nodeclass PopulateEventWithFreePath : StateNode {
    virtual void doStart();
  }
	
			
  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
	}
		
    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