/* Solution to Part II: Judging Distance 
*/

#include "DualCoding/DualCoding.h"
#include "Behaviors/StateMachine.h"
using namespace DualCoding;

enum Dist_t {
    blueClose,
    pinkClose,
    sameDist
};

DATAEVENT_IMPLEMENTATION(Dist_t, unsigned int)

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

    void DoStart() {
        camSkS.clear();
        NEW_SKETCH(camFrame, uchar, sketchFromSeg());
        
        NEW_SKETCH(pink_stuff, bool, visops::colormask(camFrame,"pink"));
        NEW_SKETCH(pink_blobs, bool, visops::minArea(pink_stuff, 10));
        NEW_SKETCH(pink_area, uint, visops::areacc(pink_blobs));

        NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame,"blue"));
        NEW_SKETCH(blue_blobs, bool, visops::minArea(blue_stuff, 10));
        NEW_SKETCH(blue_area, uint, visops::areacc(blue_blobs));

        int diff = pink_area->max() - blue_area->max();
        if(diff > 50) {
            postStateSignal<Dist_t>(pinkClose);
        }
        else if(diff < -50) {
            postStateSignal<Dist_t>(blueClose);
        }
        else {
            postStateSignal<Dist_t>(sameDist);
        }

    }
};


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

    virtual void setup() {
        #statemachine

        startnode: VisionNode() =S<Dist_t>(pinkClose)=> pinkSound
        startnode =S<Dist_t>(blueClose)=> blueSound
        startnode =S<Dist_t>(sameDist)=> sameSound

        blueSound: SoundNode($, "howl.wav")
        pinkSound: SoundNode($, "barklow.wav")
        sameSound: SoundNode($, "ping.wav")

        #endstatemachine
    }
};