#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/PilotTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Crew/GrasperRequest.h"
#include "Crew/MapBuilderNode.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

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

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

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

  //! Plan body motion to approach the object
  $nodeclass PlanBodyMotion : StateNode {
    virtual void doStart();
  }

  $nodeclass FindObj : MapBuilderNode {
    virtual void doStart();
  }

  $nodeclass PlanReachArm : StateNode {
    virtual void doStart();
  }

  $nodeclass GraspBody : PilotNode(PilotTypes::waypointWalk) {
    virtual void doStart();
  }

  $nodeclass GraspArm : ArmNode {
    virtual void doStart();
  }

  //! Runs all appropriate planners based on the requested action: move OBJECT to TARGET and then REST
  $nodeclass Plan : StateNode : objectUncon_(NULL), targetCon_(NULL), objectUnconCon_(NULL), restPlan_(NULL) {

    StateNode *objectUncon_, *targetCon_, *objectUnconCon_, *restPlan_;

    virtual void setup() {
      $statemachine{
	objectUncon: StateNode =C=> restPlan

	objectUnconCon: StateNode =C=> targetCon

	targetCon: PathPlanConstrained =C=> restPlan

	restPlan: PathPlanToRest  =C=> PostMachineCompletion

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

      objectUncon_ = objectUncon;
      targetCon_ = targetCon;
      objectUnconCon_ = objectUnconCon;
      restPlan_ = restPlan;
    }

    virtual void doStart();

    Plan(const Plan&);
    Plan& operator=(const Plan&);
  }

  //==== MOTION ====
  
  //! Moves the body
  $nodeclass ReachBody : PilotNode : doStart {
    if ( ! curReq->reachPose.isValid() ) {
      cancelThisRequest();
      return;
    }
    if ( curReq->pilotreq != NULL )
      pilotreq = *curReq->pilotreq;
    else
      pilotreq = PilotRequest(PilotTypes::goToShape);
    pilotreq.targetShape = curReq->reachPose;
  }

  //! Executes an arm movement with time delays estimated for hand/eye system
  $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::doRelease) =C=> succeeded
        restArm =F=> failed
        
        restToOpen: MoveArm(GrasperRequest::doRelease) =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 ====

  $nodeclass IfRequestIs(GrasperRequest::GrasperRequestType_t rt) : StateNode : doStart {
    if ( curReq->requestType == rt )
      postStateSuccess();
    else
      postStateCompletion();
  }

  //! 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::doReach:
      ev.path = curReq->reachPath;
      break;
    case GrasperRequest::doMove:
      ev.path = curReq->movePath;
      break;
    case GrasperRequest::doRelease:
      ev.path = curReq->releasePath;
      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 *startmain_;	//!< The main reach-grasp-move-release-rest sequence

public:
			
  Grasper(): StateNode(), startmain_(), requests(), idCounter(0)
  {
    curReq = NULL;
  }
  
  virtual void setup() {
    $statemachine{
      startnode: StateNode
	startmain  : PlanBodyMotion =C=> reachbody
	startmain  =S<GraspError>=> failed

	// need to park the arm if we're going to travel

	reachbody  : ReachBody =PILOT(noError)=> findobj
	reachbody  =F=> planreacharm   // pilot request was cancelled
	reachbody  =PILOT=> pilotFailed

	findobj    : FindObj =C=> planreacharm   // find the object in local space
	findobj    =F=> planreacharm

	planreacharm  : PlanReachArm =C=> reacharm
	planreacharm  =S<GraspError>=> failed

	reacharm   : MoveArm(GrasperRequest::doReach) =C=> reacharm2
	reacharm    =S<GraspError>=> failed
	reacharm2  : IfRequestIs(GrasperRequest::reach) =S=> succeeded
	reacharm2    =C=> graspbody

	graspbody  : GraspBody =C=> grasparm

	grasparm   : GraspArm =C=> succeeded

	reach_Plan  : Plan =C=> reach_ReachBody =C=> reach_ReachArm =C=> reach_Rest =C=> succeeded
	reach_Plan   =S<GraspError>=> failed
	reach_ReachBody : ReachBody =F=> reach_ReachArm
	reach_ReachArm  : MoveArm(GrasperRequest::doMove) =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::doMove) =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::doMove) =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::doMove) =S<GraspError>=> failed
	moveTo_GraspO : Grasp(false) =S<GraspError>=> failed
	moveTo_MoveB  : MoveArm(GrasperRequest::doMove) =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::doMove) =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

	pilotFailed : StateNode

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

  startmain_ = startmain;
  }

  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->movePath.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