#include "DualCoding/DualCoding.h"

#include "Behaviors/StateMachine.h"

#include "Events/EventRouter.h"

using namespace DualCoding;

class VisualJudge : public VisualRoutinesStateNode { 
 public:
  enum distanceResult { blue, pink, same };
 VisualJudge() : VisualRoutinesStateNode("VisualJudge") {}
  class VisualJudgeNode : public VisualRoutinesStateNode {
  public:
  VisualJudgeNode() : VisualRoutinesStateNode("VisualJudgeNode") {}
    virtual void DoStart(){
      NEW_SKETCH(camFrame, uchar, sketchFromSeg());
      
      // find the largest blue blob in the image
      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(b_max, bool, b_area == b_area->max());
      int const blueRegionSizeMax = b_area->max();
      
      //find the largest pink blob in the image
      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));
      NEW_SKETCH(p_max, bool, p_area == p_area->max());
      int const pinkRegionSizeMax = p_area->max();
      
      //based on which one is closer make different sounds
      NEW_SKETCH(result, bool, blueRegionSizeMax > pinkRegionSizeMax ? b_max : p_max);
      
      double eta = 0.0001;
      
      //calculate epsilon for range of values for which the two nodes are different
      double eps = ((double) pinkRegionSizeMax +eta)/((double)blueRegionSizeMax +eta);
      
      //first we check if the regions are roughly equal
      if((eps < 1.1) && (eps > 0.9))
	postStateSignal<unsigned int>(same);
      else if ( blueRegionSizeMax >  pinkRegionSizeMax )  // blue region greater
	postStateSignal<unsigned int>(blue);
      else // pink region greater
	postStateSignal<unsigned int>(pink); 
    }
  };
  
  virtual void setup() {
    
#statemachine
    
startJudge:  VisualJudgeNode()

same_node: SoundNode($, "barkmed.wav")

blue_node: SoundNode($, "howl.wav")

pink_node: SoundNode($, "toc.wav")

startJudge =S<unsigned int>(same)=>same_node

startJudge =S<unsigned int>(pink)=>pink_node 

startJudge =S<unsigned int>(blue)=>blue_node 
    
#endstatemachine

startnode = startJudge;
  }
};