//-*-c++-*-
#ifndef INCLUDED_PointingGameBehavior_h_
#define INCLUDED_PointingGameBehavior_h_
 
#include "Behaviors/StateMachine.h"
#include "Behaviors/Nodes/ArmNode.h"
#include "Events/EventRouter.h"
#include "DualCoding/DualCoding.h"

/**
 * PointingGame: picks left, right, or middle, and plays
 * ping.wav if it wins (sees green shell). Plays howl.wav
 * if it loses (no green).
 **/

class PointingGameBehavior : public StateNode {

public:
  PointingGameBehavior() : StateNode("PointingGameBehavior") {}

  enum third {
	leftThird,
	rightThird,
	centerThird
  };

  enum watchSignal {
	winSignal,
	loseSignal
  };

 class WatchNode : public VisualRoutinesStateNode {
  public: 
	WatchNode(char *name) : VisualRoutinesStateNode(name) {}

	void setThird(int toWatch) {
		myThird = toWatch;
	}

	virtual void DoStart() {
		VisualRoutinesStateNode::DoStart();

		camSkS.clear();

		NEW_SKETCH(camFrame, uchar, sketchFromSeg());
		NEW_SKETCH(green_stuff, bool,
			visops::colormask(camFrame,"green"));

		int lowbound = 0; int highbound = green_stuff->getWidth();
		Sketch<bool> mask(camSkS, "mask");
		mask = false;
		switch (myThird) {
			case 0: highbound = highbound / 3;
				cout << "watchLeft!" <<endl;
				break;
			case 1: lowbound = highbound / 3;
					highbound = 2 * (highbound / 3);
				cout<<"watchCenter!" << endl;
				break;
			case 2: lowbound = 2* (highbound / 3);
				cout<<"watchRight!" << endl;
				break;
		}
		for (int i = lowbound; i < highbound; i++) {
			for (int j = 0; j < green_stuff->getHeight(); j++) {
				mask(i,j) = true;
			}
		}
		mask->V();

		NEW_SKETCH(result, bool, green_stuff & mask);

		NEW_SKETCH(g_cc, uint, visops::labelcc(result));
		NEW_SKETCH(g_area, uint, visops::areacc(g_cc));
		NEW_SKETCH(g_ball, bool, g_area > 10);

		if (g_area->max() > 50) {
			postStateSignal<unsigned int>(winSignal);
		}
	}

  private:
	int myThird;

 }; 

  virtual void setup() {
#statemachine

startnode: StateNode 

armNeutral: ArmNode [getMC()->setJoints(1.0, -2.0 ,1.0);]

pick: StateNode

armLeft: ArmNode [getMC()->setJoints(0.5, 0.0, 0.0);]

armCenter: ArmNode [getMC()->setJoints(0.0, 0.0 ,0.0);]

armRight:  ArmNode [getMC()->setJoints(-0.5, 0.0 ,0.0);]

pauseLeft: StateNode
pauseCenter: StateNode
pauseRight: StateNode

watchLeft: WatchNode($) [setThird(0);] 

watchCenter: WatchNode($) [setThird(1);] 

watchRight: WatchNode($) [setThird(2);] 

respondWin: SoundNode($, "ping.wav")

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

startnode =N=> armNeutral

armNeutral =C=> pick

armLeft =C=> pauseLeft
armCenter =C=> pauseCenter
armRight =C=> pauseRight
pauseLeft =T(500)=> watchLeft
pauseCenter =T(500)=> watchCenter
pauseRight =T(500)=> watchRight
watchLeft =T(1500)=> respondLose
watchCenter =T(1500)=> respondLose
watchRight =T(1500)=> respondLose
watchLeft =S<unsigned int>(winSignal)=> respondWin
watchCenter =S<unsigned int>(winSignal)=> respondWin
watchRight =S<unsigned int>(winSignal)=> respondWin

respondLose =C=> armNeutral
respondWin =C=> armNeutral



#endstatemachine

RandomTrans *rtrans = new RandomTrans(armLeft, 0.33);
rtrans->addDestination(armCenter, 0.33);
rtrans->addDestination(armRight, 0.34);
pick->addTransition(rtrans);

  startnode = armNeutral;
  }       // end of setup()

};

#endif