#include "Behaviors/StateMachine.h"

$nodeclass FlipCanBehavior : StateNode {
  enum TopColor { startColor, endColor };

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

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

    if (ellipses.size() < 2) {
        cout << "Could not find the two halves of the can. Resetting..." << endl;
        postStateFailure();
        return;
    }
    Shape<EllipseData> e1 = ellipses[0];
    Shape<EllipseData> e2 = ellipses[1];
    Shape<EllipseData> topEllipse = e1->getCentroid().isAbove(e2->getCentroid()) ? e1 : e2;

    // We always set up the can so that red is on top.
    if (topEllipse->getColor().red > topEllipse->getColor().blue)
        postStateSignal<TopColor>(startColor);
    else
        postStateSignal<TopColor>(endColor);
  }

  $setupmachine {
    getStartColor: StateNode =N=> BuildMap =MAP=> GetTopColor
    getEndColor: StateNode =N=> BuildMap =MAP=> GetTopColor

    getStartColor =S<TopColor>(startColor)=> part1 =C=> getEndColor
    getStartColor =S<TopColor>(endColor)=> {reset, startFailure}

    getEndColor =S<TopColor>(endColor)=> part2 =C=> getStartColor
    getEndColor =S<TopColor>(startColor)=> {reset, endFailure}

    GetTopColor =F=> {reset, findCanFailure}

    reset =T(5000)=> getStartColor

    part1: DynamicMotionSequenceNode("grabcanpart1.mot", false)
    part2: DynamicMotionSequenceNode("grabcanpart2.mot", false)
    reset: DynamicMotionSequenceNode("grabcanreset.mot", false)
    startFailure: SpeechNode("Can must start with red on top. Resetting...")
    endFailure: SpeechNode("Failed to flip can. Resetting...")
    findCanFailure: SpeechNode("Couldn't find can. Resetting...")
    
  }
}

REGISTER_BEHAVIOR(FlipCanBehavior);