#include "Behaviors/StateMachine.h"

$nodeclass Lab6Q5 : VisualRoutinesStateNode {
    enum outcome {gotcan, droppedcan};
    
    $nodeclass CheckForCan : MapBuilderNode : doStart{
        int num_reds, num_greens;
        mapreq.addObjectColor(ellipseDataType, "green");
        mapreq.addObjectColor(ellipseDataType, "red");
        NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
        SHAPEVEC_ITERATE(ellipses, EllipseData, e){
            if (e->getColor().green == 200)
                num_greens++;
            else if (e->getColor().red == 200)
                num_reds++;
        }END_ITERATE;
    
        if (num_reds > 0 && num_greens > 0)
            postStateSignal<outcome>(gotcan);
        else
            postStateSignal<outcome>(droppedcan);
    } 
    
    $setupmachine {
        startnode : StateNode
        lookatcan : DynamicMotionSequenceNode("spin1.mot")
        spincan : DynamicMotionSequenceNode("spin2.mot")
        gethelp : SpeechNode("Have pity on me. Please put my can where I can reach it.")
        foundcan : SpeechNode("Flip!")
        checkcan : CheckForCan
        
        startnode =N=> lookatcan =C=> checkcan
        checkcan =S<outcome>(gotcan)=> foundcan =C=> spincan =C=> startnode
        checkcan =S<outcome>(droppedcan)=> gethelp =T(5000)=> spincan =C=> startnode
    }
}


REGISTER_BEHAVIOR(Lab6Q5);