#include "Shared/RobotInfo.h"

#if defined(TGT_HAS_ARMS) && defined(TGT_HAS_HEAD)

#include "Behaviors/StateMachine.h"
#include "Behaviors/Nodes/PostureNode.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Motion/MotionPtr.h"

#define SLOW_SPEED M_PI*1/8
#define FAST_SPEED M_PI*3/4
#define NUM_STEPS 20

$nodeclass GrabSnack : StateNode {

    

  $nodeclass ArmMoveCameraOffset : PostureNode : doStart {
    fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
    fmat::Column<3> Pgripper = Tgripper.translation();

    /* what will be provided */
    float depthOffset = 200;

    fmat::Column<3> targetCam = fmat::pack(0, 0, depthOffset);
    fmat::Column<3> targetBase = kine->linkToBase(CameraFrameOffset) * targetCam;
    
    float dist = (Pgripper - targetBase).norm();
    std::cout << "dist:" << dist << std::endl;
    
    for(unsigned int i=ArmOffset; i<ArmOffset+NumArmJoints; ++i)
        getMC()->setMaxSpeed(i,(float)SLOW_SPEED);

    fmat::Column<3> noOffset = fmat::pack(0, 0, 0);



#ifdef TGT_IS_CALLIOPE5
    unsigned int link = LeftFingerFrameOffset;
#else
	unsigned int link = GripperFrameOffset;
#endif

    getMC()->solveLinkPosition(targetBase, link, noOffset);
  }



  $nodeclass ArmMoveClose : PostureNode : doStart {

    /* what will be provided */
    fmat::Column<3> target = fmat::pack(500-100,25,150);
    
    fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
//    fmat::Column<3> targetBase = kine->linkToBase(BaseFrameOffset) * noOffset;

    for(unsigned int i=ArmOffset; i<ArmOffset+NumArmJoints; ++i)
        getMC()->setMaxSpeed(i,(float)SLOW_SPEED);
      
      
    fmat::Quaternion oriTgt = fmat::Quaternion::identity();
    fmat::Quaternion oriOffset = fmat::Quaternion::identity();

	unsigned int link = GripperFrameOffset;

    getMC()->solveLink(target, oriTgt, link, noOffset, oriOffset);
  }


  $nodeclass ArmMoveThere : DynamicMotionSequenceNode : doStart {

    PostureEngine p;


    /* what will be provided */
    fmat::Column<3> startPos = fmat::pack(500-100, 25, 150);
    fmat::Column<3> endPos = fmat::pack(500,25,150);
    
    fmat::Column<3> noOffset = fmat::pack(0, 0, 0);
    
    unsigned int link = GripperFrameOffset;
    
    fmat::Column<3> deltaPos = (endPos - startPos);
    cout << "DeltaPos: " << deltaPos << endl;

    fmat::Column<3> deltaStep = deltaPos / NUM_STEPS;
    cout << "DeltaStep: " << deltaStep << endl;
    fmat::Column<3> pos = startPos;

    for(int i = 0; i < NUM_STEPS; i++) {
        cout << "Position: " << pos << endl;

        pos += deltaStep;
        p.solveLinkPosition(pos, link, noOffset);
        getMC()->setPose(p);
        getMC()->advanceTime(200);
    }
  }



  $nodeclass WaitNode : StateNode : doStart {
      std::cout << "waiting" << std::endl;
  }

  virtual void setup() {
		// We make the posture motion command here and supply it to the
		// PostureNode via setMC() so that it only has to be registered
		// with the MotionManager once. Registering and deregistering a
		// PostureMC inside a tight sensor loop is inefficient and leads
		// to crashes.
		MotionManager::MC_ID posturemc = addMotion(MotionPtr<PostureMC>());

    $statemachine{
        wait : WaitNode
        wait =TM("local")=> ArmMoveClose[setMC(posturemc)] =C=> open =C=> {there, close} =C(2)=> wait
        wait =TM("camera")=> ArmMoveCameraOffset[setMC(posturemc)] =C=> wait

        there : ArmMoveThere
        open : DynamicMotionSequenceNode("opengripper.mot")
        close : DynamicMotionSequenceNode("closegripper.mot")
    }
  }

}

REGISTER_BEHAVIOR(GrabSnack);

#endif