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

enum Dist_t {
    pink_close,
    blue_close,
    equal_dist
};

DATAEVENT_IMPLEMENTATION(Dist_t, unsigned int)

class DecideNode: public VisualRoutinesStateNode{
  public:
    DecideNode(): VisualRoutinesStateNode("DecideNode") {}
    virtual void DoStart() {
      NEW_SKETCH(camFrame, uchar, sketchFromSeg());
      NEW_SKETCH(blue_stuff, bool,visops::colormask(camFrame,"blue"));
      NEW_SKETCH(blue_area, uint, visops::areacc(blue_stuff));
      NEW_SKETCH(pink_stuff, bool,visops::colormask(camFrame,"pink"));
      NEW_SKETCH(pink_area, uint,visops::areacc(pink_stuff));
      int blueSize = blue_area->max();
      int pinkSize = pink_area->max();
      int diff = blueSize - pinkSize;
			printf("BlueSize %d PinkSize %d diff: %d\n", blueSize, pinkSize, diff);
      if(diff > 25)
        postStateSignal<Dist_t>(blue_close);
      else if (diff < -25)
        postStateSignal<Dist_t>(pink_close);
      else
        postStateSignal<Dist_t>(equal_dist);
    }
 };

class DistanceBehavior : public VisualRoutinesStateNode {
 public:
  DistanceBehavior() : VisualRoutinesStateNode("DistanceBehavior") {}
  
	virtual void setup() {
		 printf("DistanceBehavior setup\n");
     #statemachine
     startnode: DecideNode() 
		 startnode =S<Dist_t>(blue_close)=> speakBlue
     startnode =S<Dist_t>(pink_close)=> speakPink
     startnode =S<Dist_t>(equal_dist)=> speakEqual
     speakBlue: SoundNode($,"howl.wav")
     speakPink: SoundNode($,"barklow.wav")
     speakEqual: SoundNode($,"ping.wav")
     #endstatemachine
	}
};