//-*-c++-*-
#ifndef INCLUDED_ShellSorter_h_
#define INCLUDED_ShellSorter_h_

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

using namespace DualCoding;


enum Shell_t {
	blue,
	pink,
	empty
};

DATAEVENT_IMPLEMENTATION(Shell_t, unsigned int)

class ShellSorter : public VisualRoutinesStateNode {
	public:

  ShellSorter(): VisualRoutinesStateNode("ShellSorter") {}
  
	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 = 1500;
    	req.pursueShapes = false;
			req.numSamples = 5;
			req.motionSettleTime = 2000;
			req.maxDist = 2000;
			req.minBlobAreas[blue_index] = 50;
			req.minBlobAreas[pink_index] = 50;
	
    	req.objectColors[lineDataType].insert(blue_index);
    	req.objectColors[lineDataType].insert(pink_index);
    	req.objectColors[ellipseDataType].insert(pink_index);
    	req.objectColors[ellipseDataType].insert(blue_index);
	
    	unsigned int mapreq_id = mapBuilder.executeRequest(req);
    	erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::statusETID);
  	}
	
  	virtual void processEvent(const EventBase &) {
			postStateCompletion();
		}
	
	};
	
	class ExamineMap : public VisualRoutinesStateNode {
	public:
  
		ExamineMap() : VisualRoutinesStateNode("ExamineMap") {}
	
		virtual void DoStart() {
		coordinate_t blue_x, blue_y, pink_x, pink_y, shell_x, shell_y;
			Point pinkLnCtr;
			Point blueLnCtr;
			Point shellCtr;
			Point gripperPoint;
			SharedObject<ArmMC> arm_mc;
			arm_mc->setMaxSpeed(0, 0.5);
			arm_mc->setMaxSpeed(1, 0.5);
			arm_mc->setMaxSpeed(2, 0.5);
    	sndman->playFile("barklow.wav");
    	cout << "MapBuilder found " << localShS.allShapes().size() << " shapes." << endl;
			NEW_SHAPEVEC(lines, LineData, select_type<LineData>(localShS));
			NEW_SHAPEVEC(bluelines, LineData, subset(lines, IsColor("blue")));
			NEW_SHAPEVEC(pinklines, LineData, subset(lines, IsColor("pink")));
			NEW_SHAPE(blueLine, LineData,
					max_element(bluelines, LineData::LengthLessThan()));
			NEW_SHAPE(pinkLine, LineData,
					max_element(pinklines, LineData::LengthLessThan()));
			pinkLnCtr = Point(pinkLine->getCentroid());
			pink_x = pinkLnCtr.coordX();
			pink_y = pinkLnCtr.coordY();
			cout << "pink line center: " << pink_x << " "
						<<  pink_y << endl;

			blueLnCtr = Point(blueLine->getCentroid());
			blue_x = blueLnCtr.coordX();
			blue_y = blueLnCtr.coordY();
			cout << "blue line center: " << blue_x << " "
						<<  blue_y << endl;
			NEW_SHAPE(shell, EllipseData, find_if<EllipseData>(localShS));
			shellCtr = Point(shell->getCentroid());
			shell_x = shellCtr.coordX();
			shell_y = shellCtr.coordY();
			cout << "shell center: " << shell_x << " " << shell_y << endl;
			fmat::Transform gripper = kine->jointToBase(GripperFrameOffset);
			fmat::Column<3> gripperPos = fmat::SubVector<3>(&gripper(0,3));
			gripperPoint = Point(gripperPos);
			if (shellCtr.distanceFrom(gripperPoint) > 700.0) {
				cout << "shell too far away!" << endl;
				postStateCompletion();
			}
			else if (IsColor("blue")(shell)) {
				cout << "shell blue!" << endl;
				arm_mc->moveToPoint(blue_x, blue_y, -20.0);
			}
			else {
				cout << "shell pink!" << endl;
				arm_mc->moveToPoint(pink_x, pink_y, -20.0);
			}
			motman->addPrunableMotion(arm_mc);
  	}
		
	};
	
	virtual void setup() {
#statemachine
	armleft: ArmNode [getMC()->setJoints(1.0, -1.0 , -1.5);]
	waiter: StateNode
	builder: BuildMap()
	examiner: ExamineMap()
	sleeper: StateNode
	complain: SoundNode($, "howl.wav")

	startnode: StateNode =N=> armleft
	armleft =C=> waiter 
	waiter =E(buttonEGID, ChiaraInfo::GreenButOffset, activateETID)=> builder
	builder =C=> examiner
	examiner =T(5000)=> sleeper
	sleeper =T(2000)=> armleft
	examiner =C=> complain
	complain =C=> waiter

#endstatemachine
	}

	protected:

	
};

#endif