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

using namespace DualCoding;
  
enum signal_t {
			blue,
			pink,
			both
		};
DATAEVENT_IMPLEMENTATION(signal_t, unsigned int);

class DistanceBehavior : public VisualRoutinesStateNode {

  public:
	
	DistanceBehavior(): VisualRoutinesStateNode("DistanceBehavior") {}
 
  class DistanceNode : public VisualRoutinesStateNode {
	  public: 
		DistanceNode(): VisualRoutinesStateNode("DistanceNode") {}
		
		virtual void DoStart() {
			camSkS.clear();
			NEW_SKETCH(camFrame, uchar, sketchFromSeg());
			NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame, "blue"));
			NEW_SKETCH(pink_stuff, bool, visops::colormask(camFrame, "pink"));

			NEW_SKETCH(b_cc, uint, visops::labelcc(blue_stuff));
			NEW_SKETCH(p_cc, uint, visops::labelcc(pink_stuff));

			NEW_SKETCH(b_area, uint, visops::areacc(b_cc));
			NEW_SKETCH(p_area, uint, visops::areacc(p_cc));
		
			
			/* Original approach I used to solve this JudgingDistance problem.*/ 
			/*	 Measures edist from the centerbottom of the camera frame to the blobs */
			/* ******************************************************************
			Sketch<bool> center(camSkS, "center");
			center = false;
			for (int i=155; i < 166; i++)
				center(i,235) = true;
			for (int i = 230; i < 240; i++)
				center(160,i) = true;
			center->V();

			NEW_SKETCH(dist, uint, visops::edist(center));

			NEW_SKETCH(b_dist, uint, dist*b_blobs);
			NEW_SKETCH(p_dist, uint, dist*p_blobs);
		
			int const blue_distance = b_dist->findMinPlus();
			int const pink_distance = p_dist->findMinPlus();
			 *******************************************************************   */


			int const blue_area = b_area->max(); /* greater area means shorter distance */
			int const pink_area = p_area->max();
			
			if (blue_area < pink_area) { /* pink blob is closer */
				postStateSignal<signal_t>(pink);
				std::cout << "======= pink blob is closer =====" << std::endl;
				return;
			}
			if (blue_area >  pink_area) { /* blue blob is closer */
				postStateSignal<signal_t>(blue);
				std::cout << "======= blue blob is closer =====" << std::endl;
				return;
			}
			else { /* two blobs are about the same close */
				postStateSignal<signal_t>(both);
				std::cout << "======= both closer =====" << std::endl;
				return;
			}
		}
	};

	virtual void setup() {
#statemachine
 
	decide: DistanceNode()

			blue_closer: SoundNode($, "barklow.wav")
			pink_closer: SoundNode($, "howl.wav")
			both_closer: SoundNode($, "ping.wav")

			decide =S<signal_t>(blue)=> blue_closer

			decide =S<signal_t>(pink)=> pink_closer

			decide =S<signal_t>(both)=> both_closer

#endstatemachine
	startnode = decide;
			
	}

};