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


class ShellGame : public VisualRoutinesStateNode {
  public: 
  
    enum Signal_t{
        three,
        sad, 
        happy
    };

    ShellGame() : VisualRoutinesStateNode("ShellGame") {}
   
      
    class ShellGameNode : public VisualRoutinesStateNode {
      public: ShellGameNode() : VisualRoutinesStateNode("ShellGameNode") {}
       
      virtual void DoStart(int target) {       
            
                camSkS.clear();
                NEW_SKETCH(camFrame, uchar, sketchFromSeg());
               
                int totalW = camFrame.width; 
                int onethird = totalW/3 ; 
                int twothird = onethird + onethird;
                int totalH = camFrame.height;

                NEW_SKETCH(maskl, bool, camFrame);
                NEW_SKETCH(maskc, bool, camFrame);
                NEW_SKETCH(maskr, bool, camFrame);
                maskl = false;
                maskc = false;
                maskr = false;
                
                for (int i = 0; i < onethird; i++) {
                    for (int j = 0; j < totalH; j++) {
                        maskl(i,j) = true;
                    }
                }

                for (int i = onethird; i < twothird; i++) {
                    for (int j = 0; j < totalH; j++) {
                        maskc(i,j) = true;
                    }
                }
                for (int i = twothird; i < totalW; i++) {
                    for (int j = 0; j < totalH; j++) {
                        maskr(i,j) = true;
                    }
                }

                // Get colors
                NEW_SKETCH(pink_stuff, bool, visops::colormask(camFrame,"pink"));
                NEW_SKETCH(orange_stuff, bool, visops::colormask(camFrame, "orange"));
               // orange_stuff = visops::colormask(camFrame, "orange");
               
               //Elimiate blocks that's less than 5 pixels  
                NEW_SKETCH(pink_filt, bool, visops::minArea(pink_stuff, 5));
                NEW_SKETCH(orange_filt, bool, visops::minArea(orange_stuff, 5));

                // Create sketches of individual colors with labels
                NEW_SKETCH(p_cc, uint, visops::labelcc(pink_filt));
                NEW_SKETCH(o_cc, uint, visops::labelcc(orange_filt));
                
                int pinkblob = p_cc->max();
                int orangeblob = o_cc->max(); 

               if (pinkblob < 3 )
               {
                   if(orangeblob>0)
                   {
                       postStateSignal<Signal_t>(happy);
                    }
                    else
                    {
                        postStateSignal<Signal_t>(sad);
                    }
               }
               else 
              { 
                  postStateSignal<Signal_t>(three); 
              }
            return; 
    }    
   };

   virtual void setup() {       

        //use an ArmNode to set the shoulder, elbow and wrist joint angles directly
        int target;

       #statemachine
        startnode: StateNode=RND=>{centernode, leftnode, rightnode}
       
        centernode: ArmNode [getMC()->setJoints(0,0,0);]=C=>decidenode
        leftnode: ArmNode [getMC()->setJoints(0, 0.8, 0);]=C=>decidenode
        rightnode: ArmNode [getMC()->setJoints(0,-0.8,0);]=C=>decidenode
        
        decidenode: ShellGameNode(target) 
        dumbnode: StateNode =N=>decidenode

        decidenode =S<Signal_t>(three)=>dumbnode
        decidenode =S<Signal_t>(sad)=>ping
        decidenode =S<Signal_t>(happy)=>bark

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

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

        #endstatemachine                            
    }
};

DATAEVENT_IMPLEMENTATION(ShellGame::Signal_t, unsigned int);