#ifndef _GRASPER_H
#define _GRASPER_H

#include "Shared/RobotInfo.h"
#if (defined(TGT_HAS_ARMS) || !defined(STRICT_TGT_MODEL)) && !defined(TGT_IS_AIBO)

#include "Behaviors/StateNode.h"
#include "Behaviors/Nodes/ArmNode.h"
#include "Behaviors/Nodes/DynamicMotionSequenceNode.h"
#include "Behaviors/Nodes/PostMachine.h"
#include "Behaviors/Transitions/CompareTrans.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Crew/GrasperRequest.h"
#include "Crew/MotionNodes.h"
#include "Events/GrasperEvent.h"

/*
  TODO
  - Add code to validate Grasper requests and complain, e.g., if reach is missing an object
  - fix up the Fail/Success/Complete thing; do we need all those different classes?
  - plan failure is currently not being caught; the Grasper hangs
  - take out all the DEBUG printouts
 
  - Sweep
  - Test-suite
  - Calculate the offset of the gripper frame from wristYaw when projected in XY
*/

class Grasper: public StateNode {
public:
#if defined(TGT_IS_CALLIOPE5)
	typedef ShapeSpacePlanner3DR<NumArmJoints-2> Planner;
	typedef PlannerObstacle3D PlannerObstacleG;
	static const unsigned int numPlannerJoints = NumArmJoints-2;
#elif defined(TGT_IS_CALLIOPE2)
	typedef ShapeSpacePlanner3DR<NumArmJoints-1> Planner;
	typedef PlannerObstacle3D PlannerObstacleG;
	static const unsigned int numPlannerJoints = NumArmJoints-1;
#elif defined(TGT_HAS_FINGERS)
	typedef ShapeSpacePlanner2DR<NumArmJoints-2> Planner;
	typedef PlannerObstacle2D PlannerObstacleG;
	static const unsigned int numPlannerJoints = NumArmJoints-2;
#elif defined(TGT_HAS_GRIPPER)
	typedef ShapeSpacePlanner2DR<NumArmJoints-1> Planner;
	typedef PlannerObstacle2D PlannerObstacleG;
	static const unsigned int numPlannerJoints = NumArmJoints-1;
#else
	typedef ShapeSpacePlanner2DR<NumArmJoints> Planner;
	typedef PlannerObstacle2D PlannerObstacleG;
	static const unsigned int numPlannerJoints = NumArmJoints;
#endif
	typedef Planner::NodeType_t NodeType_t;
	typedef NodeType_t::NodeValue_t NodeValue_t;
	
	typedef unsigned int GrasperVerbosity_t;
  static const GrasperVerbosity_t GVstart = 1<<0;
  static const GrasperVerbosity_t GVexecuteRequest   = 1<<1;
  static const GrasperVerbosity_t GVnumObstacles     = 1<<2;
  static const GrasperVerbosity_t GVexecutePath      = 1<<3;
  static const GrasperVerbosity_t GVcomputeGoals     = 1<<4;
  static const GrasperVerbosity_t GVsetJoint         = 1<<5;

private:
  //**************** Nodes Used in Grasper Actions ****************
  //
  // A collection of nodes from which complex Grasper actions are built

  //! Forward the request type through a SignalTrans
  $nodeclass ForwardRequestType : StateNode {
    virtual void doStart();
  }

  //! Forward the target's centroid through a SignalTrans
  $nodeclass ForwardTargetLocation : StateNode : doStart {
    postStateSignal<Point>(curReq->targetLocation->getCentroid());
  }

  // ==== PLANNING NODES ====

  //! Plan an unconstrained path
  $nodeclass PathPlanUnconstrained : StateNode {
    virtual void doStart();
  }

  //! Plan a constrained path
  $nodeclass PathPlanConstrained : StateNode {
    virtual void doStart();
  }

  //! Plan a path to the rest state
  $nodeclass PathPlanToRest : StateNode {
    virtual void doStart();
  }

  //! Runs all appropriate planners based on the requested action: move OBJECT to TARGET and then REST
  $nodeclass Plan : StateNode : setup {
    $statemachine{
      startnode: ForwardRequestType
	// Should probably replace this with a switch statement to
	// avoid activating and deactivating so many transitions:
      startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::grasp)=> objectUncon
      startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::checkGraspable)=> objectUncon
      startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::reach)=> objectUncon
      startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::computeReach)=> objectUncon
      startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::touch)=> objectUncon
      startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::release)=> targetCon
      startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::moveTo)=> objectUnconCont
      startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::checkMovable)=> objectUnconCont
      startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::rest)=> restPlan
      startnode =S<GrasperRequest::GrasperRequestType_t>(GrasperRequest::checkRestable)=> restPlan

      objectUncon: PathPlanUnconstrained =C=> restPlan

      objectUnconCont: PathPlanUnconstrained =C=> targetCon

      targetCon: PathPlanConstrained =C=> restPlan

      restPlan: PathPlanToRest  =C=> PostMachineCompletion

      {objectUncon,objectUnconCont,targetCon,restPlan} =S<GraspError>=> GrasperFailed
    }
  }

  //==== MOTION ====
  
  //! Executes an arm movement
  $nodeclass MoveArm(GrasperRequest::GrasperPathType_t pa) : DynamicMotionSequenceNode {
    virtual void doStart();
    void executeMove(const std::vector<NodeValue_t>& path);
    unsigned int advTime(const NodeValue_t& confDif) {
      float maxDiff = abs(confDif[0]);
      for (unsigned int i = 1; i < NumArmJoints; i++) {
        if (confDif[i] > maxDiff)
          maxDiff = abs(confDif[i]);
      }
      unsigned int time = (unsigned int)( maxDiff/(M_PI/6)*1500 );
      return max((unsigned)11, (unsigned int)(time * curReq->armTimeFactor));
    }
  }

  //! Grasps or releases an object from the gripper if the gripper is manipulable
  $nodeclass Grasp(bool closeGripper = true): StateNode : setup {
    $statemachine {
      startnode: StateNode =N=> split

      split: StateNode
      split >==CompareTrans<bool>($,$$,&closeGripper,CompareTrans<bool>::EQ,true)==> openTheGripper
      split >==CompareTrans<bool>($,$$,&closeGripper,CompareTrans<bool>::EQ,false)==> closeTheGripper

      openTheGripper: OpenTheGripper =C=> PostMachineCompletion
      closeTheGripper: CloseTheGripper =C=> PostMachineCompletion

      failed: SignalGraspError($, GrasperRequest::badMove)
    }
  }

  //! Moves the arm to the rest state
  $nodeclass Rest: StateNode {
    $nodeclass GetRestType: StateNode : doStart {
      postStateSignal<GrasperRequest::GrasperRestType_t>(curReq->restType);
    }

    $nodeclass GetOpenGripperFlag : StateNode : doStart {
      postStateSignal<bool>(curReq->openGripperOnRest);
    }

    virtual void setup() {
      $statemachine{
        startnode: GetRestType
        startnode =S<GrasperRequest::GrasperRestType_t>(GrasperRequest::stationary)=> succeeded
        startnode =S<GrasperRequest::GrasperRestType_t>(GrasperRequest::settleArm)=> restArm
        startnode =S<GrasperRequest::GrasperRestType_t>(GrasperRequest::settleBodyAndArm)=> restToOpen
        
        restArm: MoveArm($, GrasperRequest::disengage) =C=> succeeded
        restArm =F=> failed
        
        restToOpen: MoveArm($, GrasperRequest::disengage) =C=> open
        restToOpen =F=> failed
        
        open: GetOpenGripperFlag
        open =S<bool>(false)=> succeeded
        open =S<bool>(true)=> OpenTheGripper =C=> succeeded
        
        succeeded: PostMachineCompletion
        
        failed: SignalGraspError($, GrasperRequest::badMove)
      }
    }
  }

  //! Set joint to specified angle
	$nodeclass SetJoint(string jointName, float storedVal = 0, float speed = 0.4f): ArmNode {
		void moveJoint(float value);	
		virtual void doStart() {
			if (const DataEvent<float> *dev = dynamic_cast<const DataEvent<float>*>(event)) {
				float receivedVal = dev->getData();
				moveJoint(receivedVal);
			} else {
				moveJoint(storedVal);
			}
		}
	}

  //! Open the gripper
  $nodeclass OpenTheGripper : StateNode : setup {
#if defined(TGT_HAS_GRIPPER)
    $statemachine{
      startnode: StateNode =N=> {openLeft, openRight}
      openLeft: SetJoint($, "ARM:gripperLeft", outputRanges[capabilities.findFrameOffset("ARM:gripperLeft")][0])
      openRight: SetJoint($, "ARM:gripperRight", outputRanges[capabilities.findFrameOffset("ARM:gripperRight")][1])
      {openLeft, openRight} =C=> PostMachineCompletion
    }
#endif
  }

  //! Close the gripper
  $nodeclass CloseTheGripper : StateNode : setup {
#if defined(TGT_HAS_GRIPPER)
    $statemachine{
      startnode: StateNode =N=> {closeLeft, closeRight}

      closeLeft: SetJoint($, "ARM:gripperLeft", outputRanges[capabilities.findFrameOffset("ARM:gripperLeft")][1])
      closeRight: SetJoint($, "ARM:gripperRight", outputRanges[capabilities.findFrameOffset("ARM:gripperRight")][0])

      {closeLeft, closeRight} =C=> PostMachineCompletion
    }
#endif
  }

  //==== POSTING OF EVENTS ====

  //! Signal the failure of a grasp sub-statemachine via a signalTrans
  $nodeclass SignalGraspError(GraspError e = GrasperRequest::noError) : StateNode : doStart {
    if (const DataEvent<GraspError> *datev = dynamic_cast<const DataEvent<GraspError>*>(event))
      postParentSignal<GraspError>(datev->getData());
    else
      postParentSignal<GraspError>(e);
  }

  //! Report successful completion of the grasper request via a GrasperEvent
  $nodeclass GrasperSucceeded : StateNode : doStart {
    GrasperEvent ev(true, curReq->requestType, GrasperRequest::noError, (size_t)curReq->requestingBehavior);
    switch (curReq->populateEventPathWith) {
    case GrasperRequest::moveFree:
      ev.path = curReq->moveFreePath;
      break;
    case GrasperRequest::moveConstrained:
      ev.path = curReq->moveConstrainedPath;
      break;
    case GrasperRequest::disengage:
      ev.path = curReq->disengagePath;
      break;
    case GrasperRequest::noPath:
    default:
      break;
    }
    erouter->postEvent(ev);
  }

  //! Report failure of the grasper request via a GrasperEvent
  $nodeclass GrasperFailed(GraspError err = GrasperRequest::badMove) : StateNode : doStart {
    GraspError e(err);
    if (const DataEvent<GraspError> *datev = dynamic_cast<const DataEvent<GraspError>*>(event))
      e = datev->getData();
    GrasperEvent myEvent(false, curReq->requestType, e, (size_t)curReq->requestingBehavior);
    if (e == GrasperRequest::pickUpUnreachable || e == GrasperRequest::dropOffUnreachable) {
      // myEvent.suggestedRobotLocation = VRmixin::grasper->getDesiredRobotLocation();
      // myEvent.suggestedLookAtPoint = VRmixin::grasper->getDesiredLookAtPoint();
    }
    erouter->postEvent(myEvent);
  }

  //! Move on to the next grasper request
  $nodeclass NextRequest : StateNode : doStart {
    delete curReq;
    curReq = NULL;
    VRmixin::grasper->executeRequest();
    stop();
  }

  //**************** End of Grasper Actions ****************
	

  StateNode *checkRoot;		//!< The state machine entrance for any check* request
  StateNode *graspRoot;		//!< The state machine for only acquiring and grasping
  StateNode *releaseRoot;		//!< The state machine for only releasing
  StateNode *moveRoot;		//!< The state machine to move an object to the targetLocation
  StateNode *reachRoot;		//!< The state machine for acquiring
  StateNode *touchRoot;		//!< The state machine for performing the touch action
  StateNode *restRoot;		//!< The state machine to execute resting
  StateNode *computeReachRoot;

public:
			
  Grasper(): StateNode(),
	     checkRoot(), graspRoot(), releaseRoot(), moveRoot(), reachRoot(), 
	     touchRoot(), restRoot(), computeReachRoot(),
             // desiredRobotLocation(), desiredLookAtPoint(),
	     requests(), idCounter(0)
  {
    curReq = NULL;
  }
  
  virtual void setup() {
    $statemachine{
      startnode: StateNode
	check_Plan  : Plan =C=> succeeded
	check_Plan   =S<GraspError>=> failed
			
	reach_Plan  : Plan =C=> reach_Move =C=> reach_Rest =C=> succeeded
	reach_Plan   =S<GraspError>=> failed
	reach_Move  : MoveArm($, GrasperRequest::moveFree) =S<GraspError>=> failed
	reach_Rest  : Rest
			
	grasp_Plan  : Plan =C=> grasp_Move =C=> grasp_Grasp =C=>
                        grasp_Rest =C=> succeeded
	grasp_Plan   =S<GraspError>=> failed
	grasp_Move  : MoveArm($, GrasperRequest::moveFree) =S<GraspError>=> failed
	grasp_Grasp : Grasp($, false) =S<GraspError>=> failed
	grasp_Rest  : Rest
			
	release_Plan  : Plan =C=> release_Move =C=> release_Grasp =C=>
                          release_Rest =C=> succeeded
	release_Plan   =S<GraspError>=> failed
	release_Move  : MoveArm($, GrasperRequest::moveConstrained) =S<GraspError>=> failed
	release_Grasp : Grasp($, true) =S<GraspError>=> failed
	release_Rest  : Rest
			
	moveTo_Plan   : Plan =C=> moveTo_MoveA =C=> moveTo_GraspO =C=>
                          moveTo_MoveB =C=> moveTo_GraspC =C=> moveTo_Rest =C=> succeeded
	moveTo_Plan    =S<GraspError>=> failed
	moveTo_MoveA  : MoveArm($, GrasperRequest::moveFree) =S<GraspError>=> failed
	moveTo_GraspO : Grasp($, false) =S<GraspError>=> failed
	moveTo_MoveB  : MoveArm($, GrasperRequest::moveConstrained) =S<GraspError>=> failed
	moveTo_GraspC : Grasp($, true) =S<GraspError>=> failed
	moveTo_Rest   : Rest
			
	touch_Plan  : Plan =C=> touch_Move =C=> touch_Grasp =C=> succeeded
	touch_Plan   =S<GraspError>=> failed
	touch_Move  : MoveArm($, GrasperRequest::moveFree) =S<GraspError>=> failed
	touch_Grasp : Grasp($, false) =S<GraspError>=> failed
			
	rest_Plan   : Plan =C=> rest_Rest =C=> succeeded
	rest_Plan    =S<GraspError>=> failed
	rest_Rest   : Rest =S<GraspError>=> failed
        
	computeReach_Plan: Plan =C=> succeeded
	computeReach_Plan =S<GraspError>=> failed

	succeeded : GrasperSucceeded =N=> next
	failed    : GrasperFailed =N=> next
        next      : NextRequest
    }

    checkRoot = check_Plan;
    reachRoot = reach_Plan;
    graspRoot = grasp_Plan;
    releaseRoot = release_Plan;
    moveRoot = moveTo_Plan;
    touchRoot = touch_Plan;
    restRoot = rest_Plan;
    computeReachRoot = computeReach_Plan;
  }

  virtual void preStart() {
    StateNode::preStart();
    if ( verbosity & GVstart ) {
      cout << "Grasper starting up" << endl;
    }
    while ( !requests.empty() ) {
      delete requests.front();
      requests.pop();
    }
  }
	
  //! User-level interface for queueing a request to the Grasper
  unsigned int executeRequest(const GrasperRequest& req, BehaviorBase* requestingBehavior);

  //! Executes the next pending request, i.e., the one at the front of the queue
  void executeRequest();

  //! Dispatches on the request type, running the appropriate state machine; called by executeRequest()
  void dispatch();
	
  //! Converts the manipulated object to a planner obstacle and adds it to the planner obstacles list
  void appendObjectToObstacles() {
    if (!curReq->moveConstrainedPath.empty()) {
#if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE5)
      fmat::Column<3> newPos = fmat::pack(curReq->targetLocation->getCentroid().coordX(),
                                          curReq->targetLocation->getCentroid().coordY(),
                                          curReq->targetLocation->getCentroid().coordZ());
#else
      fmat::Column<2> newPos = fmat::pack(curReq->targetLocation->getCentroid().coordX(),
                                          curReq->targetLocation->getCentroid().coordY());
#endif
      curReq->objectPlannerObstacle->updatePosition(newPos);
    }
  }
	
  //! Removes the manipulated object's planner obstacle from the planner obstacles list
  void removeTargetFromObstacles() {
    if (!curReq->object.isValid()) return;

    delete curReq->objectPlannerObstacle;
    curReq->objectPlannerObstacle = NULL;
  }
  
  //! Gets current state of robot's arm from the end effector given
  /* If no joint is given, pulls values from WorldState */
  static void getCurrentState(NodeValue_t &current, KinematicJoint* endEffector=NULL);
	
  //! Computes all goal states around a point and rotating the approach orientation
  void computeGoalStates(Point& toPt, std::vector<std::pair<float, float> >& rangesX,
                         std::vector<std::pair<float, float> >& rangesY, std::vector<std::pair<float, float> >& rangesZ,
                         float res, std::vector<NodeValue_t>& goals, fmat::Column<3> offset);

protected:
  //	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
	
  std::queue<GrasperRequest*> requests; //!< Queue of pending Grasper requests
  unsigned int idCounter;               //!< Counter for assigning a unique id to each Grasper request

public:
  static GrasperRequest* curReq;        //!< The request itself
  static GrasperVerbosity_t verbosity;

private:
  Grasper(const Grasper& o);  //!< Copy constructor; do not use
  void operator=(const Grasper& o); //!< Assignment operator; do not use
};

#else   // not TGT_HAS_ARMS
class Grasper: public StateNode {
public:
  unsigned int executeRequest(const GrasperRequest& req, BehaviorBase* requestingBehavior) {
    std::cout << "Warning:  cannot execute Grasper request: robot has no arms!" << std::endl;
    return 0;
  }
};
#endif

#endif