#include "DualCoding/DualCoding.h"

#include "Behaviors/StateMachine.h"

#include "Events/EventRouter.h"

using namespace DualCoding;

class ShellGame : public VisualRoutinesStateNode {
 public:
 ShellGame() : VisualRoutinesStateNode("ShellGame") {}
  enum shellState { win, lose, noChange };
  class ShellGameNode : public VisualRoutinesStateNode {
  public:
    int screenPart;
  ShellGameNode(int sP) : VisualRoutinesStateNode("ShellGameNode"), screenPart(sP){ }
    virtual void DoStart(){
      //get the third of the screen
      NEW_SKETCH(camFrame, uchar, sketchFromSeg());
      NEW_SKETCH(screenMask, bool, visops::zeros(camSkS));
      for ( int x=106*screenPart; x<106*(screenPart+1); x++ )
	for ( int y = 0; y<=240; y++ )
	  screenMask(x,y) = 1;

      //get pink blobs
      NEW_SKETCH(pink_all, bool, visops::colormask(camFrame,"pink"));
      NEW_SKETCH(pink_stuff, bool, pink_all & screenMask);
      NEW_SKETCH(p_blobs, bool, visops::minArea(pink_stuff, 10));
      NEW_SKETCH(p_cc, uint, visops::labelcc(p_blobs));
      int pcc_max = p_cc->max();
      
      //get green tape if any
      NEW_SKETCH(green_all, bool, visops::colormask(camFrame,"green"));
      NEW_SKETCH(green_stuff, bool, green_all & screenMask);
      NEW_SKETCH(g_tape, bool, visops::minArea(green_stuff, 10));
      NEW_SKETCH(g_cc, uint, visops::labelcc(g_tape));
      int gcc_max = g_cc->max();

      if(pcc_max > 0){
     	postStateSignal<unsigned int>(noChange);
      }else {
	if(gcc_max>0) {
	  postStateSignal<unsigned int>(win);
	} else {
	  postStateSignal<unsigned int>(lose);
	}
      }
    }
  };
  
  virtual void setup() {
    
#statemachine
//states

startGame: StateNode

move0: ArmNode [getMC()->setMaxSpeed(0,0.2);
getMC()->setMaxSpeed(1,0.2);
getMC()->setMaxSpeed(2,0.2);
getMC()->setJoints(0.7,0,0);]//left

move1: ArmNode [getMC()->setMaxSpeed(0,0.2);
getMC()->setMaxSpeed(1,0.2);
getMC()->setMaxSpeed(2,0.2);
getMC()->setJoints(0,0,0);]//center

move2: ArmNode [getMC()->setMaxSpeed(0,0.2);
getMC()->setMaxSpeed(1,0.2);
getMC()->setMaxSpeed(2,0.2);
getMC()->setJoints(-0.7,0,0);]//right

test0: ShellGameNode(0)

test1: ShellGameNode(1)

test2: ShellGameNode(2)

wait0: StateNode

wait1: StateNode

wait2: StateNode

happy_sound: SoundNode($, "barkmed.wav")
      
sad_sound: SoundNode($, "howl.wav")

//transistions

startGame >==RandomTrans==> {move0,move1,move2}

move0 =C=> test0

move1 =C=> test1

move2 =C=> test2

test0 =S<unsigned int>(noChange)=> wait0

wait0 =T(750)=> test0

test0 =S<unsigned int>(win)=>happy_sound

test0 =S<unsigned int>(lose)=>sad_sound

test1 =S<unsigned int>(noChange)=>wait1

wait1 =T(750)=> test1

test1 =S<unsigned int>(win)=>happy_sound

test1 =S<unsigned int>(lose)=>sad_sound

test2 =S<unsigned int>(noChange)=>wait2

wait2 =T(750)=> test2

test2 =S<unsigned int>(win)=>happy_sound

test2 =S<unsigned int>(lose)=>sad_sound
    
#endstatemachine

      startnode = startGame;
  }

};