#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 {

    /**
    A node used to poll camera image ever time it gets to run, and check 
    if any shell is removed, then check if it can see green blob. Based 
    on the result, it post success, failure events or just return. 
    */
    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));
            //count the number of shells
            int count = b_cc->max();
            printf("egg count: %d\n", count);
            //if a shell is removed
            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);
                //can't see a tape
                if (tape_count < 1)
                    postStateSignal<Result>(failure);
                else
                    postStateSignal<Result>(success);
            }
        }
    };

public:
    ShellGame() : VisualRoutinesStateNode("ShellGame") {}
    /**
    First startnode pick a move node with random transition, then every move 
    node has a completion transition to judge node. The judge node will either
    post a event if some shell is removed, or have a time out transition back 
    to it self. Based on the event judge post, either success sound or failure
    sound will be played. We made an assumption that user will remove the 
    shell that is pointed by the chiara.

    happy sound: howl.wav
    sad sound: barkhigh.wav
    */
    virtual void setup() {
#statemachine
        startnode: StateNode
        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 =T(30)=> judge
        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);
    }
};