#include "Behaviors/StateMachine.h"

$nodeclass FlipCanBehavior : VisualRoutinesStateNode {
  static const int CAN_THRESHOLD = 10000;

  $nodeclass BuildMap : MapBuilderNode : doStart {
    mapreq.addObjectColor(ellipseDataType, "red");
    mapreq.addObjectColor(ellipseDataType, "blue");
  }

  $nodeclass GetCan : VisualRoutinesStateNode : doStart {
    // Get ellipses.
    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));

	cout << camSkS.getWidth() << endl;
	cout << camSkS.getHeight() << endl;

    SHAPEVEC_ITERATE(ellipses, EllipseData, e) {
		if (e->getArea() > CAN_THRESHOLD) {
			postStateSuccess();
			return;
		}
	} END_ITERATE;
	postStateFailure();
  }

  $setupmachine {
	// Declare long state names here.
	part1: DynamicMotionSequenceNode("grabcanpart1.mot", false)
    part2: DynamicMotionSequenceNode("grabcanpart2.mot", false)
	lookAtCan: GetCan
    reset: DynamicMotionSequenceNode("grabcanreset.mot", false)
    failure: SpeechNode("Failed to flip can. Resetting...")

    part1 =C=> BuildMap =MAP=> lookAtCan

    lookAtCan =F=> {reset, failure}
	lookAtCan =S=> part2 =C=> StateNode =T(5000)=> part1

    reset =C=> StateNode =T(5000)=> part1
  }
}

REGISTER_BEHAVIOR(FlipCanBehavior);