#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/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 "Crew/PilotNode.h"
#include "Events/GrasperEvent.h"
#include "Motion/IKSolver.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();
  }

  //! Execute body motion to approach the object
  $nodeclass ReachBody : PilotNode(PilotTypes::goToShape) {
    virtual void doStart();
  }

  //! Re-acquire object on local map after moving the body
  $nodeclass FindObj : MapBuilderNode {
    virtual void doStart();
  }

  //! Plan arm motion to get fingers around the object
  $nodeclass PlanReachArm : StateNode {
    virtual void doStart();
  }

  //! Execute arm motion to get fingers around the object
  $nodeclass ReachArm : StateNode {
#ifdef TGT_IS_CALLIOPE2
    // For Calliope2SP: deploy the arm and set finger height first, then open fingers
    $setupmachine{
      MoveArm(GrasperRequest::doReach) =C=> FingersReach =C=> PostMachineCompletion
    }
#else
    // Open fingers and then move the arm to acquire the object
    $setupmachine{
      FingersReach =C=> MoveArm(GrasperRequest::doReach) =C=> PostMachineCompletion
    }
#endif
  }

  //! Open fingers far enough to accomodate object
  $nodeclass FingersReach : ArmNode {
    virtual void doStart();
  }

  //! For Calliope2SP: move body instead of arm to acquire the object
  $nodeclass ReachBody2 : PilotNode(PilotTypes::waypointWalk) {
    virtual void doStart();
  }

  //! Verify that object is grasped; currently looks for an AprilTag
  $nodeclass Verify {
    $nodeclass GetAprilTag : MapBuilderNode(MapBuilderRequest::cameraMap) {
      virtual void doStart();
    }

    $nodeclass CheckAprilTag {
      virtual void doStart();
    }

    $setupmachine{
      LookAtGripper =C=> GetAprilTag =C=> CheckAprilTag
    }
  }

  //! Close fingers to grasp the object
  $nodeclass GraspArm : ArmNode {
    virtual void doStart();
  }

  //! Move the object to test that it was grasped successfully
  $nodeclass NudgeArm : DynamicMotionSequenceNode {
    virtual void doStart();
  }

  //! Plan body move to dropoff location
  $nodeclass PlanMoveBody : StateNode {
    virtual void doStart();
  }

  //! Execute body move for dropoff
  $nodeclass MoveBody : PilotNode(PilotTypes::goToShape) {
    virtual void doStart();
  }



  //==== MOTION ====
  
  //! Moves the body

  //! 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=> parkarm
	startmain  =F=> planreacharm // body motion not needed
	startmain  =S<GraspError>=> failed

	parkarm    : ParkArm =C=> reachbody

	reachbody  : ReachBody =PILOT(noError)=> findobj
	reachbody  =F=> planreacharm   // invalid reachPose found; shouldn't happen
	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   : ReachArm =C=> reachbody2

	reachbody2 : ReachBody2 =C=> verify1
        reachbody2 =F=> verify1

	verify1    : Verify =C=> verify1a
	verify1    =S<GraspError>=> failed

	verify1a  : IfRequestIs(GrasperRequest::reach) =S=> succeeded
	verify1a    =C=> grasparm

	grasparm   : GraspArm =C=> nudgearm

	nudgearm   : NudgeArm =C=> verify2

	verify2    : Verify =C=> verify2a
	verify2     =S<GraspError>=> failed

	verify2a   : IfRequestIs(GrasperRequest::grasp) =S=> succeeded
	verify2a    =C=> planmovebody

	planmovebody : PlanMoveBody
	planmovebody =F=> planmovearm
	planmovebody =C=> movebody

	movebody    : MoveBody

	planmovearm : StateNode // to be written

	pilotFailed : StateNode // to be written

	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();
  void doStop();
	
  //! 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(IKSolver::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, const IKSolver::Point &offset);

  //! Helper function for computeGoalStates
  void checkGoalCandidate(const IKSolver::Point &offset, const IKSolver::Rotation &ori, 
			  KinematicJoint* effector, const IKSolver::Point &pt, std::vector<NodeValue_t>& goals);

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