#include "Behaviors/StateMachine.h"
#include "Events/EventRouter.h"
#include "DualCoding/DualCoding.h"

using namespace DualCoding;
enum vision {
	lifted,
	unlifted
};
enum result {
	win,
	loss
};

DATAEVENT_IMPLEMENTATION(result, unsigned int);
DATAEVENT_IMPLEMENTATION(vision, unsigned int);

class ShellGameBehavior : public VisualRoutinesStateNode {
public:

	ShellGameBehavior(): VisualRoutinesStateNode("ShellGameBehavior") {}
	

	class VisionNode : public VisualRoutinesStateNode {
	public:
		VisionNode(): VisualRoutinesStateNode("VisionNode") {}
		
		virtual void DoStart() {
			camSkS.clear();
			NEW_SKETCH(camFrame, uchar, sketchFromSeg());
			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_blob, bool, b_area > 50);
			NEW_SKETCH(b_result, uint, visops::labelcc(b_blob));
			
			int const num_b_blobs = b_result->max();

			if (num_b_blobs < 3) { /* Someone lifted up one of the blobs*/
				postStateSignal<vision>(lifted);
				return;
			}
			else {
				postStateSignal<vision>(unlifted);
				return;
			}
		}
	};
	
	class ShellGameNode : public VisualRoutinesStateNode {
	public:
		ShellGameNode(): VisualRoutinesStateNode("ShellGameNode") {}

		virtual void DoStart() {
			camSkS.clear();
			NEW_SKETCH(camFrame, uchar, sketchFromSeg());
			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_blob, bool, p_area > 15);
			NEW_SKETCH(p_result, uint, p_cc*p_blob);
			int const temp = p_result->max();
		
			if (temp > 0) {/* there is a pink stuff(tape) whose area is greater than 15 */
				postStateSignal<result>(win);
				return;
			}
			else {
				postStateSignal<result>(loss);
				return;
			}
		}
										
	};
	
	virtual void setup() {
#statemachine
	
			result_node: ShellGameNode()
			vision_node: VisionNode()

			left: ArmNode [getMC()->setJoints(0,0.5,0.4);]
			middle: ArmNode [getMC()->setJoints(0,0,0);] 
			right: ArmNode [getMC()->setJoints(0,-0.5,-0.4);]
			
			win_sound: SoundNode($, "barkhigh.wav")
			loss_sound: SoundNode($, "fart.wav")
			
			result_node =S<result>(win)=> win_sound
			result_node =S<result>(loss)=> loss_sound

			pick_random:StateNode
			{left, middle, right} =C(1)=> vision_node
			vision_node =S<vision>(lifted)=>result_node
 		  vision_node =S<vision>(unlifted)=>wait: StateNode
      wait =T(1000)=> vision_node											 

#endstatemachine
		RandomTrans *rtrans = new RandomTrans(left, 1);
		rtrans->addDestination(middle); rtrans->addDestination(right);
		pick_random->addTransition(rtrans);
		startnode = pick_random;
											 
	}
};