//-*-c++-*-
//ssh student@chiara1e 'killall -9 tekkotsu-CHIARA'
#ifndef INCLUDED_communicatefinal_h_
#define INCLUDED_MapBuild
#include "Behaviors/Leapers.h"


Point ctr;

Shape<BlobData> blobMarker;
Shape<BlobData> blue_leg;
bool sawPink = false;


#nodeclass communicatefinal: LeapMachine


	#shortnodeclass FindColor : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		std::vector<Point> pts;
		pts.push_back(Point(900,0,0));
		pts.push_back(Point(900,600,0));
		pts.push_back(Point(900,-600,0));
		pts.push_back(Point(1200,0,0));
		NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, pts, false)); 
		mapreq.searchArea = gazePoly;
		mapreq.motionSettleTime = 2000;
		mapreq.maxDist = 1e6;
		mapreq.clearShapes = true;
		mapreq.pursueShapes = true;
		mapreq.rawY = true;
		mapreq.addObjectColor(blobDataType, "pink");
		mapreq.addObjectColor(blobDataType, "blue");


	#shortnodeclass PinkBlobs : VisualRoutinesStateNode : DoStart
			NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
			NEW_SHAPEVEC(pink_blobs, BlobData, subset(blobs, IsColor("pink"))); 
			if(pink_blobs.empty()) {
				sawPink = false;
				postStateFailure();
			}
			else {
				sawPink = true;
				postStateCompletion();
			}
			

	#shortnodeclass BlueBlobs : VisualRoutinesStateNode : DoStart
			NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
			NEW_SHAPEVEC(blue_blobs, BlobData, subset(blobs, IsColor("blue"))); 
			if(blue_blobs.empty())
				postStateFailure();
			else
				postStateCompletion();

	#shortnodeclass DidISeePink : StateNode : DoStart
		if(sawPink)
			postStateCompletion();
		else
			postStateFailure();

	#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 FindChiaraLeft : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		std::vector<Point> pts;
		pts.push_back(Point(1400,7001,0));
		pts.push_back(Point(1400,7000,0));
		NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, pts, false)); 
		mapreq.searchArea = gazePoly;
		mapreq.motionSettleTime = 2000;
		mapreq.maxDist = 1e6;
		mapreq.clearShapes = true;
		mapreq.pursueShapes = false;
		mapreq.rawY = true;
		mapreq.addObjectColor(blobDataType, "red");


	#shortnodeclass FindChiaraRight : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		std::vector<Point> pts;
		pts.push_back(Point(1400,-7001,0));
		pts.push_back(Point(1400,-7000,0));
		NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, pts, false)); 
		mapreq.searchArea = gazePoly;
		mapreq.motionSettleTime = 2000;
		mapreq.maxDist = 1e6;
		mapreq.clearShapes = true;
		mapreq.pursueShapes = false;
		mapreq.rawY = true;
		mapreq.addObjectColor(blobDataType, "red");


	#shortnodeclass RedChiara : VisualRoutinesStateNode : DoStart
		NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
		NEW_SHAPEVEC(red_blobs, BlobData, subset(blobs, IsColor("red"))); 
		if(red_blobs.empty())
			postStateFailure();
		else
			blobMarker = max_element(blobs, BlobData::AreaLessThan());
			postStateCompletion();


	#shortnodeclass CheckOrient : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		std::vector<Point> pts;
		pts.push_back(Point(1200,0,0));
		pts.push_back(Point(1200,1,0));
		NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, pts, false)); 
		mapreq.searchArea = gazePoly;
		mapreq.motionSettleTime = 2000;
		mapreq.maxDist = 1e6;
		mapreq.clearShapes = true;
		mapreq.pursueShapes = false;
		mapreq.rawY = true;
		mapreq.addObjectColor(blobDataType, "red");

	
	#nodeclass RaiseBlueLeg(int time = 2000) : DynamicMotionSequenceNode($) : constructor
		getMC()->advanceTime(time);
		getMC()->setOutputCmd(RobotInfo::RFrLegOffset + RobotInfo::SweepOffset, 0);
		getMC()->setOutputCmd(RobotInfo::RFrLegOffset + RobotInfo::ElevatorOffset, 1.59);
		getMC()->setOutputCmd(RobotInfo::RFrLegOffset + RobotInfo::KneeOffset, 1.59);
		#endnodeclass 


	#shortnodeclass LookLeg : MapBuilderNode($,MapBuilderRequest::cameraMap) : constructor
		mapreq.clearShapes = true;
		mapreq.rawY = true;
		mapreq.addObjectColor(blobDataType, "blue");
		

	#shortnodeclass BlueBlobsRedux : StateNode : DoStart
		NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(camShS));
		NEW_SHAPEVEC(colored, BlobData, subset(blobs, IsColor("blue")));
		blue_leg = max_element(colored, BlobData::AreaLessThan());
		cout<<"Trying to complete out of Redux..."<<endl;
		postStateCompletion();


	#shortnodeclass CheckBlueHorizontal : StateNode : DoStart
		BoundingBox bb = blue_leg->getBoundingBox();
		float xdif = bb.xmax - bb.xmin;
		float ydif = bb.ymax - bb.ymin;
		if(xdif > ydif)
			postStateCompletion();
		else
			postStateFailure(); 


	#shortnodeclass LookChiara : MapBuilderNode($,MapBuilderRequest::cameraMap) : DoStart
		mapreq.clearShapes = true;
		mapreq.rawY = true;
		mapreq.motionSettleTime = 2000;
		mapreq.maxDist = 1e6;
		mapreq.addObjectColor(blobDataType, "yellow");
		mapreq.addObjectColor(blobDataType, "red");


	
	#shortnodeclass FindCenterOfBlobs(const std::string& color = "red") : VisualRoutinesStateNode($) : DoStart
		ctr.setCoords(0,0,0);
		NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(camShS));
		NEW_SHAPEVEC(colored, BlobData, subset(blobs, IsColor("red")));
		if(colored.empty())
			postStateFailure();
		SHAPEVEC_ITERATE(colored, BlobData, blob)
			ctr += blob->getCentroid();
		END_ITERATE;
		ctr.setCoords(ctr.coordX() / colored.size(), ctr.coordY() / colored.size(), ctr.coordZ() / colored.size());
		postStateCompletion();


	#shortnodeclass CheckYellow: StateNode : DoStart
		NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(camShS));		//all blobs in cam space
		NEW_SHAPEVEC(colored, BlobData, subset(blobs, IsColor("yellow")));	//all yellow blobs
		Shape<BlobData> yel = max_element(colored, BlobData::AreaLessThan());	//biggest yellow blob
		Point yellowCtr = yel->getCentroid();
		float xDif = ctr.coordX() - yellowCtr.coordX();
		float yDif = ctr.coordY() - yellowCtr.coordY();
		cout<<"xDif = "<<xDif<<" yDif = "<<yDif<<endl;
		if(yDif == 0) {
			cout<<"ERROR: This should never happen, avoiding divide by zero"<<endl;
			return;	//This shouldn't happen. This means that the red blob and yellow blob are EXACTLY the same level...?
		}
		float ratio = xDif / abs(yDif);	//What's the ratio of how far sideways it is to vertical?
		if(abs(ratio) > 1) {
			if(xDif > 0)
				postStateCompletion();	//Completion trans if on the left
			else
				postStateFailure();		//Failure trans if on the right
		}
		//Timeout if not to one side or the other


	#shortnodemethod setup
		#statemachine
			startnode : FindColor
			pinkblob : PinkBlobs
			blueblob : BlueBlobs
			saypink : Speak($, "I found a pink egg")
			sayblue : Speak($, "I found a blue egg")
			fail :  Speak($, "fail")
			walkout : Forward($, -500) 
			findchiaraleft : FindChiaraLeft
			findchiararight : FindChiaraRight
			orient : OrientToTarget
			checkorient : CheckOrient
			redchiara : RedChiara
			redchiara2 : RedChiara
			raiseblue : RaiseBlueLeg
			lookleg : LookLeg
			blueblob2 : BlueBlobsRedux
			checkleg : CheckBlueHorizontal
			pinkblob2 : DidISeePink
			swingright : SwingArmRight($, 500) 
			swingleft : SwingArmLeft($, 500)
			lookchiara : LookChiara
			findcenter : FindCenterOfBlobs
			checkyellow : CheckYellow
			saypink2 : Speak($, "The other chiara found a pink egg")
			sayblue2 : Speak($, "The other chiara found a blue egg")
			greenlight : GreenLEDOn
			bluelight : BlueLEDOn

			startnode =MAP=> pinkblob
			pinkblob =C=> saypink
			saypink =T(3000)=> walkout
			pinkblob =F=> blueblob
			blueblob =C=> sayblue
			sayblue =T(2000)=> walkout
			blueblob =F=> fail
			walkout =C=> findchiararight
			findchiararight =MAP=> redchiara
			redchiara =C=> orient
			orient =S<float>=> Turn =C=> LookStraight =T(1000)=> checkorient
			checkorient =MAP=> redchiara
			orient =C=> LegsUp =T(1500)=> raiseblue
			redchiara =F=> findchiaraleft
			findchiaraleft =MAP=> redchiara2
			redchiara2 =C=> orient
			redchiara2 =F=> findchiararight
			raiseblue =C=> lookleg
			lookleg =MAP=> blueblob2
			blueblob2 =C=> checkleg
			checkleg =C=> pinkblob2
			checkleg =F=> lookleg
			pinkblob2 =C=> swingright
			pinkblob2 =F=> swingleft
			swingright =C=> lookchiara
			swingleft =C=> lookchiara
			lookchiara =MAP=> findcenter
			findcenter =C=> checkyellow
			checkyellow =C=> saypink2
			checkyellow =F=> sayblue2
			checkyellow =T(5000)=> lookchiara
			saypink2 =T(2000)=> greenlight
			sayblue2 =T(2000)=>  bluelight
		
						
		#endstatemachine

	#endnodeclass
	#endif