#include "DualCoding/DualCoding.h"
#include <stdlib.h>

#include "Behaviors/Nodes/ArmNode.h"
#include "Behaviors/StateMachine.h"
#include "Events/EventRouter.h"

using namespace DualCoding;

enum Result {
    success,
    failure
};

DATAEVENT_IMPLEMENTATION (Result, unsigned int);

class ShellGame : public VisualRoutinesStateNode {

    class JudgingNode : public VisualRoutinesStateNode {
        public:
        JudgingNode() : VisualRoutinesStateNode("Judging") {}

        void DoStart() {
            camSkS.clear();
            NEW_SKETCH(camFrame, uchar, sketchFromSeg());
            NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame,"blue"));
            NEW_SKETCH(b_cc, uint, visops::labelcc(blue_stuff, 30));
            int count = b_cc->max();
            printf("egg count: %d\n", count);

            if (count < 3) {
                NEW_SKETCH(green_stuff, bool, visops::colormask(camFrame,"green"));
                NEW_SKETCH(g_cc, uint, visops::labelcc(green_stuff, 30));
                int tape_count = g_cc->max();
                printf("tape count: %d\n", tape_count);
                if (tape_count < 1)
                    postStateSignal<Result>(failure);
                else
                    postStateSignal<Result>(success);
            }
        }
    };

public:
    ShellGame() : VisualRoutinesStateNode("ShellGame") {}
    
    virtual void setup() {
#statemachine

        moveLeft: ArmNode("left") [getMC()->setJoints(0.9,-0.4,0);]
        moveMiddle: ArmNode("middle") [getMC()->setJoints(0, 0, 0);]
        moveRight: ArmNode("right") [getMC()->setJoints(-0.9, 0.4, 0);]

        sucSoundNode: SoundNode($, "howl.wav") 
        failSoundNode: SoundNode($, "barkhigh.wav")

        judge: JudgingNode()

        judge =S<Result>(success)=> sucSoundNode
        
        judge =S<Result>(failure)=> failSoundNode

        moveLeft =C=> judge

        moveMiddle =C=> judge
        
        moveRight =C=> judge

#endstatemachine
        RandomTrans *move = new RandomTrans(moveLeft, 0.333);
        move->addDestination(moveMiddle, 0.333);
        move->addDestination(moveRight, 0.333);
        
        startnode->addTransition(move);


    }
};