#ifndef _Neutral_
#define _Neutral_

#include "Events/LocomotionEvent.h"
#include "Behaviors/Nodes/MotionSequenceNode.h"
#include "Behaviors/Nodes/WalkNode.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/WalkMC.h"
#include "Motion/MMAccessor.h"
#include "Shared/WorldState.h"
#include "CardRecognition.h"
#include "Motion/WalkMC.h"

#include "DualCoding/DualCoding.h"
using namespace DualCoding;

#define pi 3.1415926535


class Neutral : public VisualRoutinesStateNode {
 public:
	Neutral() : VisualRoutinesStateNode("Neutral"),
		walk_mc(), walk_id(MotionManager::invalid_MC_ID), 
		head_mc(), head_id(MotionManager::invalid_MC_ID){}

		void DoStart(){
			VisualRoutinesStateNode::DoStart();
			cout<<"Neutral starting up.\n";

			 const int yellow_index = ProjectInterface::getColorIndex("yellow");

			 localShS.clear(); // clear manually so we don't lose gazePoly

			 vector<Point> gazePts;
			 gazePts.push_back(Point(220,80,-100));
			 gazePts.push_back(Point(220, 80, 100));
			 gazePts.push_back(Point(350,300,-100));
			 gazePts.push_back(Point(35, 300, 100));
			 gazePts.push_back(Point(1000,0,-100));
			 gazePts.push_back(Point(1000, 0, 100));
			 gazePts.push_back(Point(350,-300,-100));
			 gazePts.push_back(Point(350, -300, 100));
			 gazePts.push_back(Point(220,-80,-100));
			 gazePts.push_back(Point(220, -80, 100));
			 gazePts.push_back(Point(300,0,-100));
			 NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, gazePts, false));

			 MapBuilderRequest req(MapBuilderRequest::localMap);
			 req.searchArea = gazePoly;
			 req.maxDist = 8000;
			 req.rawY = true;
			 req.clearShapes = false;
			 req.objectColors[blobDataType].insert(yellow_index);

			 unsigned int mapreq_id = mapBuilder.executeRequest(req);
			 erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::deactivateETID);
			 walk_id = motman->addPersistentMotion(walk_mc);
		}


		void processEvent(const EventBase & event) {
			if(event.getSourceID() == 1000){
				camShS.clear();
				int const yellow_index = ProjectInterface::getColorIndex("yellow");
				NEW_SHAPEVEC(yblobs,BlobData,getBlobsFromRegionGenerator(yellow_index,200,BlobData::groundplane,1));
				if ( yblobs.size() == 0 ){
					MMAccessor<WalkMC>(walk_id)->setTargetVelocity(0,0,0,0);
					cout<<"I don't see a yellow blob\n";
					return;
				}
				
				cout << "Yellow Blob size: " << yblobs[0]->getArea()  << "\n";
				
				if ( yblobs[0]->getArea() > 4500){
					MMAccessor<WalkMC>(walk_id)->setTargetVelocity(0,0,0,0);
					erouter->removeTimer(this);
					erouter->addTimer(this, 1001, 1500, false);
				}
				else{
					Point ypoint = yblobs[0]->getCentroid();
					double const head_pan_angle = state->outputs[HeadOffset+PanOffset];
					double const head_move = (camSkS.getWidth()/2 - ypoint.coordX()) / camSkS.getWidth() * CameraFOV;
					double const head_nod_move = (camSkS.getHeight()/2 - ypoint.coordY()) / camSkS.getHeight() * CameraFOV;
					double new_pan = head_pan_angle + head_move;
					double tilt = state->outputs[HeadOffset+TiltOffset];
					double nod = state->outputs[HeadOffset+NodOffset];
					double new_nod = nod + head_nod_move;
					MMAccessor<HeadPointerMC>(head_id)->setJoints(tilt, new_pan, new_nod);
					MMAccessor<WalkMC>(walk_id)->setTargetVelocity(40,new_pan/3, 4/3*new_pan);
					erouter->addTimer(this, 1000, 500, false);
				}	  
			}

			else if(event.getSourceID() == 1001){
				motman->removeMotion(head_id);
				motman->removeMotion(walk_id);
				return;
			}
			
 
			else if(event.getGeneratorID() == EventBase::mapbuilderEGID){
				NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
				if(blobs.size()==0){
					cout<<"There are no yellow blobs.\n";
					return;
				}
				NEW_SHAPE(biggestblob, BlobData, max_element(blobs,BlobData::AreaLessThan()));
				Point target=biggestblob->getCentroid();
				head_id = motman->addPersistentMotion(head_mc);
				MMAccessor<HeadPointerMC>(head_id)->lookAtPoint(target.coordX(),target.coordY(),target.coordZ());
				erouter->addTimer(this, 1000, 1000, false);
			}
		}


		void DoStop(){
			motman->removeMotion(walk_id);
			motman->removeMotion(head_id);
			cout << "Neutral Stopped." << endl;
			VisualRoutinesStateNode::DoStop();
		}

		
 private:
		SharedObject<WalkMC> walk_mc;
		MotionManager::MC_ID walk_id;
		SharedObject<HeadPointerMC> head_mc;
		MotionManager::MC_ID head_id;
		Neutral(const Neutral&);
		Neutral& operator=(const Neutral&);

};






#endif