#include "Behaviors/StateMachine.h"

$nodeclass ArtsiCanister1 : VisualRoutinesStateNode {

  $nodeclass ReadyArm : ArmNode : doStart {
    getMC()->setJointValue(ArmBaseOffset-ArmOffset, 0.5);
    getMC()->setJointValue(ArmShoulderOffset-ArmOffset, -0.75);
  }

  $nodeclass RaiseArm : ArmNode : doStart {
    getMC()->setMaxSpeed(0.2);
    getMC()->setJointValue(ArmShoulderOffset-ArmOffset, 0.1);
  }

  $nodeclass FindCanisters : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
    mapreq.addObjectColor(cylinderDataType,"red");
  }

  $nodeclass GoToCanister : PilotNode(PilotTypes::walk) : doStart {
    NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));
    if ( cyls.empty() ) {
      cancelThisRequest();
      return;
    }
    // Calculate a point from which to grasp the cylinder.
    // This is currently just a crude estimate; a correct
    // calculation would have to take into account the amount
    // by which the arm is offset from the robot's center.
    Point vecToCyl = cyls[0]->getCentroid() - theAgent->getCentroid();
    AngSignPi turn = atan2(vecToCyl.getCoords()[1],vecToCyl.getCoords()[0]);
    float dist = vecToCyl.xyNorm();
    pilotreq.da = turn - AngSignPi(deg2rad(5.0));
    pilotreq.dx = dist - 200;
  }

  $setupmachine{
    startnode: ReadyArm =C=> find

    find: FindCanisters =MAP=> OpenGripper(0.65) =C=> go

    go: GoToCanister
    go =F=> SpeechNode("No canisters seen") =C=> find
    go =C=> Turn(deg2rad(30.0)) =C=> WalkForward(75.0) =C=>
         OpenGripper(0.2) =T(1000)=> RaiseArm =C=> SpeechNode("Done!") =C=> ResetGripper
  }

}

REGISTER_BEHAVIOR(ArtsiCanister1);