#ifndef INCLUDED_JudgingDistanceBehavior_h_
#define INCLUDED_JudgingDistanceBehavior_h_

#define AREA_DIFF_THRESHOLD 50

#include "DualCoding/DualCoding.h"

/**
 * JudgingDistance: decides which is closer: a green blob or a blue blob
 * (based on size). Plays ping if green, howl if blue, bark if neither.
 **/
using namespace DualCoding;

class JudgingDistanceBehavior : public StateNode {
public:
	
	JudgingDistanceBehavior() : StateNode("JudgingDistanceBehavior") {}

	enum closerIndication {
		pinkCloser,
		blueCloser,
		neitherCloser
	};

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

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

		camSkS.clear();

		NEW_SKETCH(camFrame, uchar, sketchFromSeg());
		NEW_SKETCH(pink_stuff, bool,
           		visops::colormask(camFrame,"green"));
		NEW_SKETCH(p_cc, uint, visops::labelcc(pink_stuff));
		NEW_SKETCH(p_area, uint, visops::areacc(p_cc));

		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));

		int area_diff = b_area->max() - p_area->max();

		if (area_diff > AREA_DIFF_THRESHOLD) {
			cout << "blueCloser!" << endl;
			postStateSignal<unsigned int>(blueCloser);
		}
		else if (area_diff < -AREA_DIFF_THRESHOLD) {
			cout << "greenCloser!" << endl;
			postStateSignal<unsigned int>(pinkCloser);
		}
		else {
			cout << "neitherCloser!" << endl;
			postStateSignal<unsigned int>(neitherCloser);
		}

	}

	};

	virtual void setup() {
	
	#statemachine
	starthere: StateNode
	 
	starthere =N=> decide
	decide =S<unsigned int>(pinkCloser)=> pinkAlertNode 
	decide =S<unsigned int>(blueCloser)=> blueAlertNode 
	decide =S<unsigned int>(neitherCloser)=> neitherAlertNode
	pinkAlertNode =C=> decide
	blueAlertNode =C=> decide
	neitherAlertNode =C=> decide

	decide: DecideNode($)
	pinkAlertNode: SoundNode($, "ping.wav")
	blueAlertNode: SoundNode($, "howl.wav")
	neitherAlertNode: SoundNode($, "barklow.wav")
	


	#endstatemachine
	
	startnode = starthere;
	}

};

#endif