#include "Behaviors/StateMachine.h"

#define THRESHOLD 700

$nodeclass Lab6Q5Inspect : VisualRoutinesStateNode {

    $nodeclass IsTwoLines : VisualRoutinesStateNode : doStart {
         NEW_SKETCH(camFrame, uchar, sketchFromSeg());
            NEW_SKETCH(bluemask, bool, visops::colormask(camFrame,"blue"));
         NEW_SKETCH(labelmask, uint, visops::labelcc(bluemask));
         NEW_SKETCH(areamask, uint, visops::areacc(labelmask));
         NEW_SKETCH(canmask, bool, areamask >= THRESHOLD);

         if (canmask->max() == 1) {
            cout << "enough, success" << endl;
            postStateCompletion();
            return;
         }
         cout << "too few, failure" << endl;
         postStateFailure();
     }


  $setupmachine {
    launch: StateNode =N=> DynamicMotionSequenceNode("grabinspectcan.mot") =C=> istwo

    istwo: IsTwoLines
    istwo =C=> SpeechNode("Got the can") =C=> rp =C=> launch
    istwo =F=> SpeechNode("Missed Can, please place back in spot") =T(4000)=> launch


    rp: DynamicMotionSequenceNode("rotateplacecan.mot")

  }
}

REGISTER_BEHAVIOR(Lab6Q5Inspect);