#include "Behaviors/StateMachine.h"

$nodeclass CanFlip : VisualRoutinesStateNode {

  $nodeclass SetupCamera : MapBuilderNode : doStart {
    mapreq.addObjectColor(blobDataType,"red");
  }

  // looks for a large red object (hopefully the can)
  $nodeclass VisualCanCheck : VisualRoutinesStateNode : doStart {

    NEW_SHAPEVEC(cans,BlobData,select_type<BlobData>(camShS));

    int count = 0;

    SHAPEVEC_ITERATE(cans,BlobData,myblob)

      if (myblob->getArea() > 50)
      {
        count = count + 1;
      }

    END_ITERATE;

    if (count == 0) {
      
      cout << "No Cans Found!" << endl;
      postStateFailure();

    } else {

      cout << "Can-Like Objects Found: " << count << endl;
      postStateSuccess();
      
    }

  }

  $setupmachine {
  
    // launch node sets up the arm and head
    launch: StateNode =N=> bringToCamera

    // bring the can to the camera
    bringToCamera: DynamicMotionSequenceNode("can_to_camera.mot")

    // bring the can from the camera back to its original position (but upside down)
    bringFromCamera: DynamicMotionSequenceNode("camera_to_can.mot")

    // check for the camera
    checkForCan: VisualCanCheck

    // pick up the can and check for the can
    bringToCamera =C=> SetupCamera =C=> checkForCan

    // yell and wait if we don't have the can, else continue
    checkForCan =S=> bringFromCamera
    checkForCan =F=> SpeechNode("Please put the can back") =T(5000)=> bringFromCamera

    // the cycle starts anew
    bringFromCamera =C=> bringToCamera
  }

}

REGISTER_BEHAVIOR(CanFlip);