#include "Behaviors/StateMachine.h"
#include "Events/EventRouter.h"

$nodeclass HoldingHandsBehavior : StateNode {
  static const int DISTANCE_THRESHOLD = 5;
  static const double DISTANCE_FACTOR = 1.0 / 5;
  static const double ROTATION_FACTOR = 1.0 / 100;
  static const double MAGNIFY_FACTOR = 5.0;
  
  static std::string getClassDescription() {
    return "Guide the robot along by moving its hand!";
  }

  $nodeclass ArmOut : PostureNode : doStart {
    // constants defined in CalliopeComponents.h
    for (unsigned int i = ArmOffset; i < ArmOffset + NumArmJoints; i++) {
      getMC()->setOutputCmd(i, 0.0);
      getMC()->setMaxSpeed(i, 0.2);
    }
    // set shoulder higher off the ground than usual
    getMC()->setOutputCmd(ArmOffset + 1, 1.75);

    // bend the arm into an S shape
    getMC()->setOutputCmd(ArmOffset + 2, -1.5);
  }

  $nodeclass FollowArm : WalkNode {
    fmat::Column<3> lastGripperPos;

    virtual void doStart() {
      cout << "Move my arm up to go backwards, down to go forwards, left to turn left, and right to turn right." << endl;

      lastGripperPos = kine->getInterestPoint(BaseFrameOffset, "GripperFrame");

      // subscribe to all sensor events
      erouter->addListener(this, EventBase::sensorEGID);
    }

    virtual void doEvent() {
      fmat::Column<3> gripperPos = kine->getInterestPoint(BaseFrameOffset, "GripperFrame");

      fmat::Column<3> difference = gripperPos - lastGripperPos;
      // multiplication faster than sqrt
      if (difference.sumSq() > DISTANCE_THRESHOLD * DISTANCE_THRESHOLD) {
        // not much play downwards, so magnify the difference
        if (difference[2] < 0)
          difference[2] *= MAGNIFY_FACTOR;
        // arm left = +y, right = -y, up = +z, down = -z
        getMC()->setTargetVelocity(-difference[2] * DISTANCE_FACTOR, 0, difference[1] * ROTATION_FACTOR);
      } else {
        // stop it if difference is not enough
        getMC()->setTargetVelocity(0, 0, 0);
      }
    }
  }

  $setupmachine{
    startnode: ArmOut =C=> { followArm, announceReady }
    announceReady : SpeechNode("Ready")
    followArm : FollowArm
  }

}

REGISTER_BEHAVIOR(HoldingHandsBehavior);