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

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

using namespace DualCoding;

enum CloserBall {
    blue,
    pink,
    not_sure
};

DATAEVENT_IMPLEMENTATION (CloserBall, unsigned int);

class JudgingDis : public VisualRoutinesStateNode {

    class DNode : public VisualRoutinesStateNode {
        public:
        DNode() : VisualRoutinesStateNode("DNode") {}

        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));
            NEW_SKETCH(b_area, uint, visops::areacc(b_cc));

            NEW_SKETCH(pink_stuff, bool, visops::colormask(camFrame,"pink"));
            NEW_SKETCH(p_cc, uint, visops::labelcc(pink_stuff));
            NEW_SKETCH(p_area, uint, visops::areacc(p_cc));

            int blue_size = b_area->max();
            int pink_size = p_area->max();
            
            int eps = (int)((blue_size + pink_size) * 0.03);       
            printf("%d %d %d\n",blue_size,pink_size, eps);

            if (abs(blue_size-pink_size) < eps) {
                postStateSignal<CloserBall>(not_sure);
                printf("not sure!\n");
            }
            else if (blue_size < pink_size) {
                postStateSignal<CloserBall>(pink);
                printf("blue is smaller!\n");        
            }
            else {
                postStateSignal<CloserBall>(blue);
                printf("pink is smaller!\n");
            }
        }
    };

public:
    JudgingDis() : VisualRoutinesStateNode("JudginDis") {}
    
    virtual void setup() {
#statemachine
        startnode:StateNode

        judging: DNode()
        unsureSound: SoundNode($,"howl.wav")
        pinkSound: SoundNode($,"ping.wav")
        blueSound: SoundNode($,"toc.wav")

        startnode =N=> judging
        judging =S<CloserBall>(not_sure)=>unsureSound
        judging =S<CloserBall>(pink)=>pinkSound
        judging =S<CloserBall>(blue)=>blueSound

#endstatemachine
    }
};