#ifndef INCLUDED_SortingObjBehavior_h_
#define INCLUDED_SortingObjBehavior_h_


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


class BuildMap : public VisualRoutinesStateNode {
public:	       	 
  BuildMap() : VisualRoutinesStateNode("BuildMap") {}
  
  virtual void DoStart() {
    	  
    //**************** Set up map builder request ****************
			const int pink_index = ProjectInterface::getColorIndex("pink");
    const int blue_index = ProjectInterface::getColorIndex("blue");

    NEW_SHAPE(gazePt, PointData, new PointData(localShS,Point(400,0,-100,egocentric)));
    MapBuilderRequest req(MapBuilderRequest::localMap);
    req.searchArea = gazePt;
    req.maxDist = 500;
    req.pursueShapes = true;
    req.numSamples = 3;
    req.motionSettleTime = 2000;

    req.objectColors[lineDataType].insert(pink_index);
    req.objectColors[lineDataType].insert(blue_index);
    req.occluderColors[lineDataType].insert(pink_index);
    req.occluderColors[lineDataType].insert(blue_index);
    
    req.objectColors[ellipseDataType].insert(blue_index);
    req.objectColors[ellipseDataType].insert(pink_index);

    mapBuilder.executeRequest(this, req);
  }
};

class ExamineMap : public VRmixin, public ArmNode {
public:
	ExamineMap() : ArmNode("ExamineMap") {}

	virtual void DoStart() {
		NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(localShS));
		//if there is no ellipse found
		if (ellipses.size() < 1) {
			sndman->speak("No ellipse found");
			postStateCompletion();
			return;
		}

		fmat::SubVector<3> grip = kine->getJointPosition(GripperFrameOffset); 
		Point grip_pos(grip); 
         
		// figure out the minimum distance from the blob to the gripper 
		float the_dist = 50.0; 
		float min_dist = std::numeric_limits<float>::infinity(); 
		Shape<EllipseData> closest; 

		SHAPEVEC_ITERATE(ellipses, EllipseData, iter) 
			Point centroid = iter->getCentroid(); 
		float distance = grip_pos.distanceFrom(centroid); 
		if (distance < min_dist && distance < the_dist) { 
			min_dist = distance; 
			closest = iter; 
		} 
		END_ITERATE 

			// the assumed blob is not valid which means there is no blob in hand
			if (!closest.isValid()) {
				sndman->speak("No blob found");
				postStateCompletion();
				return;
			}

		// find the longest line of the same color as blob
		rgb ellipseColor = closest->getColor(); 
		IsColor hasEllipseColor(ellipseColor); 
		NEW_SHAPEVEC(lines, LineData, select_type<LineData>(localShS)); 
		//if there is no line found
		if (lines.size() < 1) {
			sndman->speak("No line found");
			postStateCompletion();
			return;
		}
	 
		float max_len = 0.0; 
		Shape<LineData> max_line; 
		SHAPEVEC_ITERATE(lines, LineData, iter) 
			if (hasEllipseColor(iter) && iter->getLength() > max_len) { 
				max_len = iter->getLength(); 
				max_line = iter; 
			} 
		END_ITERATE 

	  Point line_pos = max_line->getCentroid(); 

		// calculate and move Arm using ArmMC
		line_pos.setCoords(line_pos.coordX(), line_pos.coordY(), grip_pos.coordZ()); 
		float norm = line_pos.xyNorm(); 
		Point normal(line_pos.coordX() / norm, line_pos.coordY() / norm, 0.0); 
		line_pos += normal * 50; 

		getMC()->moveToPoint(line_pos.coordX(), line_pos.coordY(), line_pos.coordZ()); 
	}

};


class SortingObjBehavior : public VisualRoutinesStateNode {
public:
	SortingObjBehavior(): VisualRoutinesStateNode("SortingObjBehavior") {}

	virtual void setup() {
#statemachine 
	startnode: StateNode =N=> move_arm
			move_arm: ArmNode [getMC()->setJoints(0.75, 0.0, -1.5);] =C=> move_head 
			move_head: HeadPointerNode [getMC()->setJoints(0.0, 1.0, -0.8);] =C=> wait 
			wait: StateNode =E(buttonEGID, ChiaraInfo::GreenButOffset, activateETID)=> build_map 
			build_map: BuildMap() =MAP=> sort 
			sort: ExamineMap() =C=> move_arm 
#endstatemachine 
			}
};


#endif