#include "Shared/RobotInfo.h"

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

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

$nodeclass HoldingHands : StateNode {

  $nodeclass MoveAbout : WalkNode {
    
    fmat::Column<3> basePos;

    virtual void doStart() {
      basePos = kine->linkToBase(GripperFrameOffset) * fmat::pack(0,0,0);

      erouter->addListener(this,EventBase::sensorEGID);
    }

    virtual void doEvent() {
      fmat::Column<3> currPos = kine->linkToBase(GripperFrameOffset) * fmat::pack(0,0,0);

      float diffx = currPos[0] - basePos[0];
      float diffy = currPos[1] - basePos[1];

      cout << "[" << diffx << "," << diffy << "]" << endl;

      float movethreshold = 2;
      float turnthreshold = 15;
      float movefactor = 0.5;
      float turnfactor = 0.05;

      // default case
      getMC()->setTargetVelocity(0,0,0);

      // move forward/backward
      if (abs(diffx) > abs(movethreshold)) {
	getMC()->setTargetVelocity(diffx * movefactor,0,0);
      }
      // turn left
      if (abs(diffy) > abs(turnthreshold)) {
	getMC()->setTargetVelocity(0,0,diffy * turnfactor);
      }
      
    }
    
  }
  
  $setupmachine {
    
    // this uses a posture file from the previous lab
    launch: PostureNode("01_ready.pos") =T(1000)=> {pos, move}

    pos: PostureNode("01_ready.pos")
    
    move: MoveAbout

    pos =C=> pos
  }

}

REGISTER_BEHAVIOR(HoldingHands);

#endif