#include "Behaviors/StateMachine.h"

$nodeclass FlipCan : StateNode {

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

	$nodeclass ReportCan : VisualRoutinesStateNode : doStart {

		NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
		NEW_SHAPE(longest, LineData, max_element(lines, LineData::LengthLessThan()));

		if (longest->getLength() < 100) {
			cout << "Cannot find can. Please place a can to grab." << endl;
			postStateFailure();
		} else {
			postStateCompletion();
		}

	}

	$setupmachine {

		grab: DynamicMotionSequenceNode("grabCanAndLook.mot")
		
		grab =C=> DynamicMotionSequenceNode("rotateAndPlace.mot")
		grab =F=> StateNode =T(5000)=> grab

	}

}

REGISTER_BEHAVIOR(FlipCan);