/* Solution to Part II: Freestyle
    Finds all pink objects not contained within a region. 
*/

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

enum ExtDist_t {
    eblueClose,
    epinkClose,
    esameDist
};

DATAEVENT_IMPLEMENTATION(ExtDist_t, unsigned int)

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

    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(blue_stuff, bool, visops::colormask(camFrame,"blue"));
        NEW_SKETCH(blue_blobs, bool, visops::minArea(blue_stuff, 10));

        NEW_SKETCH(green_line, bool, visops::colormask(camFrame,"green"));
        NEW_SKETCH(green_fillin, bool, visops::fillin(green_line, 4, 4, 8, 0));
        NEW_SKETCH(green_exterior, bool, visops::fillExterior(green_fillin));

        NEW_SKETCH(pink_result, bool, pink_blobs & green_exterior);
        NEW_SKETCH(blue_result, bool, blue_blobs & green_exterior);

        NEW_SKETCH(pink_area, uint, visops::areacc(pink_result));
        NEW_SKETCH(blue_area, uint, visops::areacc(blue_result));

        int diff = pink_area->max() - blue_area->max();
        if(diff > 50) {
            postStateSignal<ExtDist_t>(epinkClose);
        }
        else if(diff < -50) {
            postStateSignal<ExtDist_t>(eblueClose);
        }
        else {
            postStateSignal<ExtDist_t>(esameDist);
        }

    }
};

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

    virtual void setup() {
        #statemachine

        startnode: ExtVisionNode() =S<ExtDist_t>(epinkClose)=> pinkSound
        startnode =S<ExtDist_t>(eblueClose)=> blueSound
        startnode =S<ExtDist_t>(esameDist)=> sameSound

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

        #endstatemachine
    }
};