#include "Shared/RobotInfo.h"

#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 DISPLACEMENT_THRESHOLD 10
#define VELOCITY_SCALAR 20.0f
#define ROTATION_SCALAR 0.2f


$nodeclass Lab7p3 : StateNode {

  $nodeclass PositionArm : PostureNode : doStart {
//    cout << "position arm " << std::endl;
    for(unsigned int i=ArmOffset; i<ArmOffset+NumArmJoints; ++i)
       getMC()->setMaxSpeed(i,(float)SLOW_SPEED);

    getMC()->setOutputCmd(ArmBaseOffset, 0.0);
    getMC()->setOutputCmd(ArmShoulderOffset, 0.45);
    getMC()->setOutputCmd(ArmElbowOffset, 1.26);
    getMC()->setOutputCmd(ArmWristOffset, -1.8);
    getMC()->setOutputCmd(WristRotateOffset, -0.6);
//    cout << "done position arm " << std::endl;
  }

  $nodeclass CheckWalk : WalkNode : doStart {

    fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
    fmat::Column<3> Pgripper = Tgripper.translation();

    fmat::Column<3> Pideal = fmat::pack(475, -67, 250);
    fmat::Column<3> delta = (Pgripper - Pideal);
    float dmag = delta.norm();

    cout << "Gripper at " << Pgripper << " displaced " << delta << endl;

    if(dmag > DISPLACEMENT_THRESHOLD) {
        float velocity = delta[0] * VELOCITY_SCALAR;
        float rotation = delta[1] * ROTATION_SCALAR;
        getMC()->setTargetVelocity(velocity, 0, rotation);
    }

  }

  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{
        position: PositionArm[setMC(posturemc)] =E(sensorEGID)=> CheckWalk =E(sensorEGID)=> position

    }
  }

}

REGISTER_BEHAVIOR(Lab7p3);