//-*-c++-*-
//ssh student@chiara1e 'killall -9 tekkotsu-CHIARA'
#ifndef INCLUDED_Sorting_h_
#define INCLUDED_MapBuild
#include "Behaviors/Leapers.h"
#include <sstream>
#include <string>
#include <iostream>
using namespace std;



Shape<BlobData> blobMarker;

#nodeclass Sorting: LeapMachine

	#shortnodeclass FindBlobs : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));
		mapreq.searchArea = gazePoly;
		mapreq.motionSettleTime = 2000;
		mapreq.maxDist = 1e6;
		mapreq.clearShapes = true;
		mapreq.pursueShapes = true;
		mapreq.rawY = true;
		mapreq.addObjectColor(blobDataType, "yellow");
		mapreq.addObjectColor(blobDataType, "green");
		mapreq.minBlobAreas[ProjectInterface::getColorIndex("yellow")] = 150;
		mapreq.minBlobAreas[ProjectInterface::getColorIndex("green")] = 150; 

	#shortnodeclass GetLargestBlob : VisualRoutinesStateNode : DoStart
		NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
		if(blobs.empty())
			postStateFailure();
		blobMarker = max_element(blobs, BlobData::AreaLessThan());
		postStateCompletion(); 

	#shortnodeclass OrientToTarget : StateNode : DoStart
		Point cent = blobMarker->getCentroid();
		float ang = atan2(cent.coordY(), cent.coordX());
		cout<<" I WANT TO TURN "<<ang<<endl<<endl<<endl;
		if(abs(ang) < M_PI/18)
			postStateCompletion();
		else
			postStateSignal<float>(ang);

	#shortnodeclass MoveToTarget : StateNode : DoStart
		Point cent = blobMarker->getCentroid();
		cout<<"I WANT TO MOVE FORWARD "<<cent.coordX() - 275<<endl<<endl<<endl;
		if(cent.coordX() < 275)
			postStateCompletion();
		else
			postStateSignal<float>(cent.coordX()-275);


	#shortnodeclass SwingLeftRight : StateNode : DoStart
			if(IsColor("yellow")(blobMarker)) 
				postStateCompletion();
			else
				postStateFailure();

	#shortnodemethod setup
		#statemachine
			startnode : StateNode
			find : FindBlobs
			findlargest : GetLargestBlob 
			identify : SwingLeftRight
			wrong: Play($, "whimper.wav")
			orient : OrientToTarget
			walk : MoveToTarget
			swingright : SwingArmRight($, 500) 
			swingleft : SwingArmLeft($, 500)
			swing : SwingArm($, 3000, 0, 0, 0) 
			swingright2 : SwingArmRight($, 350)
			swingleft2 : SwingArmLeft($, 350)
timewaster: Play($, "whimper.wav")
			

	startnode =T(3000)=> find
	find =MAP=> findlargest 
	findlargest =F=> wrong
	findlargest =C=> identify

	identify =C=> swingright
	identify =F=> swingleft
	swingright =C=> orient
	swingleft =C=> orient

	orient =S<float>=> Turn =C=> find
	orient =C=> walk
	walk =S<float>=> Forward =C=> find
	walk =C=> swingright2 =C=> swingleft2 =C=> swing
	swing =C=> startnode

			
		#endstatemachine

#endnodeclass
#endif