#include "DualCoding/DualCoding.h"

#include "Behaviors/StateMachine.h"

#include "Events/EventRouter.h"

using namespace DualCoding;

enum distanceResult { blue, pink, same };

class VisualJudge : public VisualRoutinesStateNode {
 public:
 VisualJudge() : VisualRoutinesStateNode("VisualJudge") {}
  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 numBlueRegion = 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 numPinkRegion = p_area->max();

    //based on which one is closer make different sounds
    NEW_SKETCH(result, bool, numBlueRegion > numPinkRegion ? b_max : p_max);

    int eps;

    //calculate epsilon for range of values for which the two nodes are different
    if(numBlueRegion >= numPinkRegion){
      eps = numPinkRegion/numBlueRegion;
    }else {
      eps = numBlueRegion/numPinkRegion;
      }

    //first we check if the regions are roughly equal
    if(eps < 1.1 || eps > 0.9)
      postStateSignal<distanceResult>(same);
    else if ( numBlueRegion > numPinkRegion)  // blue region greater
      postStateSignal<distanceResult>(blue);
    else // pink region greater
      postStateSignal<distanceResult>(pink); 
  }
};

class SoundBehavior : public VisualRoutinesStateNode {
 public:
SoundBehavior() : VisualRoutinesStateNode("SoundBehavior") {}
  virtual void setup() {
    std::cout << getName() << " is setting up the state machine." << std::endl;

#statemachine

visualJudge: VisualJudge

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

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

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

visualJudge =S<distanceResult>(same)=>same_node

visualJudge =S<distanceResult>(pink)=>pink_node

visualJudge =S<distanceResult>(blue)=>blue_node
    
#endstatemachine

startnode = visualJudge;
}

  virtual void DoStart() {
    std::cout << getName() << " is starting up." << std::endl;
  }
  
  virtual void DoStop() {
    std::cout << getName() << " is shutting down." << std::endl;
  }
  
 private:  // Dummy methods to satisfy the compiler
  SoundBehavior(const SoundBehavior&);
  SoundBehavior& operator=(const SoundBehavior&);
  
};