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


class JudgingDistance : public VisualRoutinesStateNode {
  public: 
  
    enum Dist_t{
        blue,
        pink,
        thesame
    };

    JudgingDistance() : VisualRoutinesStateNode("JudgingDistance") {}
   
      
    class JudgingDistanceNode : public VisualRoutinesStateNode {
      public: JudgingDistanceNode() : VisualRoutinesStateNode("JudgingDistanceNode") {}
       
      virtual void DoStart() {       
           
            camSkS.clear();
            NEW_SKETCH(camFrame, uchar, sketchFromSeg());
            
            // Get colors
            NEW_SKETCH(pink_stuff, bool, visops::colormask(camFrame,"pink"));
            NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame, "blue"));
            //Elimiate blocks that's less than 5 pixels  
            NEW_SKETCH(pink_filt, bool, visops::minArea(pink_stuff, 5));
            NEW_SKETCH(blue_filt, bool, visops::minArea(blue_stuff, 5));
            // Create sketches of individual colors with labels
            NEW_SKETCH(b_cc, uint, visops::labelcc(blue_stuff));
             NEW_SKETCH(p_cc, uint, visops::labelcc(pink_stuff));
            //Get the area of the blobs
            NEW_SKETCH(b_area, uint, visops::areacc(b_cc));
            NEW_SKETCH(p_area, uint, visops::areacc(p_cc));
            //determine which one is bigger
            int b_pixel = b_area->max();
            int p_pixel = p_area->max();
            //blue is bigger: bark
            if (b_pixel > p_pixel)
            {
               postStateSignal<Dist_t>(blue); 
               NEW_SKETCH(result, uchar, blue_stuff);
            }
            else if(p_pixel>b_pixel) //pink is bigger: pingg
            {
                postStateSignal<Dist_t>(pink);
                NEW_SKETCH(result, uchar, pink_stuff);
            }
            else //they are equal : howl
            {
                postStateSignal<Dist_t>(thesame); 
            }
            
            return; 
       }
   };

   virtual void setup() {       
 
       #statemachine
        startnode: JudgingDistanceNode()
        
        startnode =S<Dist_t>(blue)=> bark
        startnode =S<Dist_t>(pink)=> ping
        startnode =S<Dist_t>(thesame)=> howl

        bark: SoundNode($, "barkmed.wav")
        bark =C=> startnode
        
        ping: SoundNode($, "ping.wav")
        ping =C=> startnode

        howl: SoundNode($, "howl.wav")
        howl =C=> startnode

        #endstatemachine                            
    }
};

DATAEVENT_IMPLEMENTATION(JudgingDistance::Dist_t, unsigned int);