//-*-c++-*-
//ssh student@chiara2e 'killall -9 tekkotsu-CHIARA'

#ifndef INCLUDED_communicatefinal_h_
#define INCLUDED_MapBuild
#include "Behaviors/Leapers.h"
#include "MorseConverter.h"

#include <sstream>
#include <string>
using namespace std;

template <class T>
inline std::string to_string (const T& t)
{
   std::stringstream ss;
   ss << t;
   return ss.str();
}

unsigned int numStops = 0;
unsigned int numUps = 0;
unsigned int ind = 0;
MorseList inlst = "";
MorseList outlst = "";


Point ctr;

Shape<BlobData> blobMarker;
Shape<BlobData> blue_leg;
int sawBlue = 0;
int sawPink = 0;
int sawPurple = 0;
int sawRed = 0;


#nodeclass AMorseCodeFinal: LeapMachine


	#shortnodeclass FindBlobs : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		std::vector<Point> pts;
		pts.push_back(Point(1400,0,0));
		pts.push_back(Point(1400,600,0));
		pts.push_back(Point(1400,-600,0));
		pts.push_back(Point(1400,-300,0));
		pts.push_back(Point(1400,300,0));
		pts.push_back(Point(800,300,0));
		pts.push_back(Point(800,-300,0));
		pts.push_back(Point(800,0,0));
		pts.push_back(Point(800,-600,0));
		pts.push_back(Point(800,600,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, "blue");
		mapreq.addObjectColor(blobDataType, "pink");
		mapreq.addObjectColor(blobDataType, "purple");
		mapreq.addObjectColor(blobDataType, "red");


	#shortnodeclass CountBlobs : VisualRoutinesStateNode : DoStart
		NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
		NEW_SHAPEVEC(blue_blobs, BlobData, subset(blobs, IsColor("blue"))); 
		NEW_SHAPEVEC(pink_blobs, BlobData, subset(blobs, IsColor("pink"))); 
		NEW_SHAPEVEC(purple_blobs, BlobData, subset(blobs, IsColor("purple"))); 
		NEW_SHAPEVEC(red_blobs, BlobData, subset(blobs, IsColor("red"))); 
		sawBlue = blue_blobs.size();	
		sawPink = pink_blobs.size();
		sawPurple = purple_blobs.size();
		sawRed = red_blobs.size();
		postStateCompletion();
		cout<<"I saw blue"<<sawBlue<<endl<<endl<<endl;
		cout<<"I saw pink"<<sawPink<<endl<<endl<<endl;
		cout<<"I saw purple"<<sawPurple<<endl<<endl<<endl;
		cout<<"I saw red"<<sawRed<<endl<<endl<<endl;

	#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/36)
			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 = 250) : 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 


	#nodeclass LowerBlueLeg(int time = 250) : DynamicMotionSequenceNode($) : constructor
		getMC()->advanceTime(time);
		getMC()->setOutputCmd(RobotInfo::RFrLegOffset + RobotInfo::SweepOffset, 0);
		getMC()->setOutputCmd(RobotInfo::RFrLegOffset + RobotInfo::ElevatorOffset, 1.2);
		getMC()->setOutputCmd(RobotInfo::RFrLegOffset + RobotInfo::KneeOffset, 2);
		#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(3*xdif > 2*ydif){
			numUps++;
			postStateCompletion();
		}	
		else{
			numUps = 0;
			postStateFailure(); 
		}


	#shortnodeclass StuckInLegUp : StateNode: DoStart
		if(numUps >= 3)
			postStateCompletion();
		else
			postStateFailure();


	#shortnodeclass MorseTester : StateNode : DoStart
		string a = " " + to_string(sawBlue) + " " + to_string(sawPink) + " " + to_string(sawPurple) + " " + to_string(sawRed);
		MorseList b = StringToList(a);
		string c = ListToString(b);
		cout<<"a = \""<<a<<"\""<<endl<<"b = \""<<b<<"\""<<endl<<"c = \""<<c<<"\""<<endl;
		if(a == c) {
			cout<<"Conversion successful"<<endl;
		} 
		else {
			cout<<"Conversion unsuccessful"<<endl;
		}
		inlst = b;
		postStateCompletion();


	#shortnodeclass HasMoreCharacters : StateNode : DoStart
		if(ind >= inlst.size()) {
			cout<<"No more characters"<<endl;
			postStateFailure();
		}
		 else {
			cout<<"More characters"<<endl;
			postStateCompletion();
		}
			
	#shortnodeclass GetNextCharInList : StateNode : DoStart
		char ch = inlst[ind];
		ind++;
		if(ch == DOT) {
			cout<<"next is dot"<<endl;
			postStateCompletion();
		}
		else if(ch == DASH) {
			cout<<"next is dash"<<endl;
			postStateFailure();
		}
			
	#shortnodeclass LookChiara : MapBuilderNode($,MapBuilderRequest::cameraMap) : DoStart
		mapreq.clearShapes = true;
		mapreq.rawY = true;
		mapreq.motionSettleTime = 2000;
		mapreq.maxDist = 1e6;
		mapreq.addObjectColor(blobDataType, "green");
		mapreq.addObjectColor(blobDataType, "red");
		mapreq.minBlobAreas[ProjectInterface::getColorIndex("green")] = 50; 


	
	#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 Checkgreen: StateNode : DoStart
		NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(camShS));		//all blobs in cam space
		NEW_SHAPEVEC(colored, BlobData, subset(blobs, IsColor("green")));	//all green blobs
		if(colored.empty()) {
			cout<<"I DIDN'T SEE ANYTHING GREEN!"<<endl;
			postStateFailure();
		}
		Shape<BlobData> gre = max_element(colored, BlobData::AreaLessThan());	//biggest green blob
		Point greenCtr = gre->getCentroid();
		float xDif = ctr.coordX() - greenCtr.coordX();
		float yDif = ctr.coordY() - greenCtr.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 green 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) > .4) {
			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


	  #shortnodeclass ReceiveDOT : StateNode : DoStart
		numStops = 0;
		outlst += DOT;
		cout<<"received dot"<<endl;
		postStateCompletion();
		
		
	  #shortnodeclass ReceiveDASH : StateNode : DoStart
		numStops = 0;
		outlst += DASH;
		cout<<"received dash"<<endl;
		postStateCompletion();
		
		
	  #shortnodeclass ReceiveSTOP :StateNode : DoStart
		numStops++;
		outlst += STOP;
		cout<<"received stop"<<endl;
		postStateCompletion();


	#shortnodeclass ThreeStopsInARow : StateNode: DoStart
		if(numStops >= 3)
			postStateCompletion();
		else
			postStateFailure();

  	#shortnodeclass TurnListToString :StateNode : DoStart
		string str = ListToString(outlst);
		cout<<"The other Chiara said: "<<str<<endl;
		sprintf(const_cast<char*>(str.c_str()), "%d %d %d %d %d", sawBlue, sawPink, sawPurple, sawRed);
		postStateSignal<string>(str);


	#shortnodeclass ClearList : StateNode : DoStart
		inlst.clear();
		outlst. clear();
		ind = 0;
		postStateCompletion();


	#shortnodeclass NumUp : StateNode : DoStart
		numUps = 0;
		postStateCompletion();


	#shortnodeclass KillBlue : StateNode : DoStart
		if (sawBlue <= 0)
			postStateFailure();
		else {
			postStateCompletion();
			sawBlue = 0;
		}


	#shortnodeclass KillPink : StateNode : DoStart
		if (sawPink <= 0)
			postStateFailure();
		else {
			postStateCompletion();
			sawPink = 0;
		}



	#shortnodeclass KillPurple : StateNode : DoStart
		if (sawPurple <= 0)
			postStateFailure();
		else {
			postStateCompletion();
			sawPurple = 0;
		}


	#shortnodeclass KillRed : StateNode : DoStart
		if (sawRed <= 0)
			postStateFailure();
		else {
			postStateCompletion();
			sawRed = 0;
		}



	#shortnodeclass FindBlue : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		std::vector<Point> pts;
		pts.push_back(Point(800,300,0));
		pts.push_back(Point(800,-300,0));
		pts.push_back(Point(800,0,0));
		pts.push_back(Point(800,-600,0));
		pts.push_back(Point(800,600,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, "blue");
		mapreq.minBlobAreas[ProjectInterface::getColorIndex("blue")] = 50; 


	#shortnodeclass FindPink : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		std::vector<Point> pts;
		pts.push_back(Point(800,300,0));
		pts.push_back(Point(800,-300,0));
		pts.push_back(Point(800,-600,0));
		pts.push_back(Point(800,600,0));
		pts.push_back(Point(800,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.minBlobAreas[ProjectInterface::getColorIndex("pink")] = 50; 


	#shortnodeclass FindPurple : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		std::vector<Point> pts;
		pts.push_back(Point(800,300,0));
		pts.push_back(Point(800,-300,0));
		pts.push_back(Point(800,0,0));
		pts.push_back(Point(800,-600,0));
		pts.push_back(Point(800,600,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, "purple");
		mapreq.minBlobAreas[ProjectInterface::getColorIndex("purple")] = 50; 


	#shortnodeclass FindRed : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
		std::vector<Point> pts;
		pts.push_back(Point(800,300,0));
		pts.push_back(Point(800,-300,0));
		pts.push_back(Point(800,0,0));
		pts.push_back(Point(800,-600,0));
		pts.push_back(Point(800,600,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, "red");
		mapreq.minBlobAreas[ProjectInterface::getColorIndex("red")] = 50; 


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


	#shortnodemethod setup
		#statemachine
			startnode : LegsUp
			findblobs : FindBlobs
			swoosh : Pose($, "swoosh.pos")
			swoosh1 : Pose($, "swoosh.pos")
			swoosh2 : Pose($, "swoosh.pos")
			swoosh3 : Pose($, "swoosh.pos")
			swoosh4 : Pose($, "swoosh.pos")
			swoosh5 : Pose($, "swoosh.pos")
			countblobs : CountBlobs
			fail :  Speak($, "fail")
			walkout : Forward($, -800) 
			backup : Forward($, -300)
			forward : Forward($, 100)
			findchiaraleft : FindChiaraLeft
			findchiararight : FindChiaraRight
			orient : OrientToTarget
			orient1 : OrientToTarget
			orient2 : OrientToTarget
			orient3 : OrientToTarget
			orient4 : OrientToTarget
			orient5 : OrientToTarget
			checkorient : CheckOrient
			checkorient1 : CheckOrient
			checkorient2 : CheckOrient
			checkorient3 : CheckOrient
			checkorient4 : CheckOrient
			checkorient5 : CheckOrient
			redchiara : RedChiara
			redchiara2 : RedChiara
			legsup : LegsUp
			lookatpoint : LookAtPoint($, 1200, 0, 0)
			stuckinup : StuckInLegUp
			raiseblueleg : RaiseBlueLeg
			raiseblueleg2 : RaiseBlueLeg
			raiseblueleg3 : RaiseBlueLeg
			raisebluelegfinal : RaiseBlueLeg
			lowerblueleg : LowerBlueLeg
			lowerblueleg2 : LowerBlueLeg
			lookleg : LookLeg
			lookleg2 : LookLeg
			lookleg3 : LookLeg
			looklegfinal : LookLeg
			blueblob1 : BlueBlobsRedux		
			blueblob2 : BlueBlobsRedux
			blueblob3 : BlueBlobsRedux
			blueblobfinal : BlueBlobsRedux
			checkleg : CheckBlueHorizontal
			checkleg2 : CheckBlueHorizontal
			checkleg3 : CheckBlueHorizontal
			checklegfinal : CheckBlueHorizontal
			senddash : SwingArm($, 100, -1.4, 0, 0) 
			senddot : SwingArm($, 100, 1.4, 0, 0)
			sendstop : SwingArm($, 100, 0, 0, 0)
			morsetester : MorseTester
			hasMore : HasMoreCharacters
			getCodeChar : GetNextCharInList
			checkgreen : Checkgreen
			checkgreenfinal : Checkgreen
			findcenter : FindCenterOfBlobs
			findcenterfinal : FindCenterOfBlobs
			lookchiara : LookChiara
			lookchiarafinal : LookChiara
			getstop : ReceiveSTOP
			getstopfinal : ReceiveSTOP
			getdash : ReceiveDASH
			getdashfinal : ReceiveDASH
			getdot : ReceiveDOT
			getdotfinal : ReceiveDOT
			listtostring : TurnListToString
			done : SwingArm($, 200, 0, 0, 0)
			walkarm : SwingArm($, 200, 0, 0, 0)
			threestops : ThreeStopsInARow
			clearlist : ClearList
			statenode : StateNode
			statenode2 : StateNode
			statenode3 : StateNode
			statenode4 : StateNode
			legsdown5 : LegsDown
			legsup7 : LegsUp
			legsup8 : LegsUp
			legsup9 : LegsUp
			legsup10 : LegsUp
			legsdown1 : LegsDown
			legsdown12 : LegsDown
			legsdown13 : LegsDown
			legsdown14 : LegsDown
			legsdown15 : LegsDown
			numups : NumUp
			poke1 : SwingArm($, 100, -1.4, 2.2, -0.8)
			poke12 : SwingArm($, 100, -1.4, 2.2, -0.8)
			poke13 : SwingArm($, 100, -1.4, 2.2, -0.8)
			poke14 : SwingArm($, 100, -1.4, 2.2, -0.8)
			poke15 : SwingArm($, 100, -1.4, 2.2, -0.8)
			poke2 : SwingArm($, 25, -1.1, 1.85, -0.65)
			poke3 : SwingArm($, 25, -0.8, 1.5, -0.5)
			poke4 : SwingArm($, 25, -0.5, 1.2, -0.4)
			poke5 : SwingArm($, 25, -0.3, 0.8, -0.3)
			poke6 : SwingArm($, 25, -0.15, 0.4, -0.15)
			poke7 : SwingArm($, 25, -0.08, 0.2, -0.8)
			poke8 : SwingArm($, 25, 0, 0, 0)
			turnaround : Turn($, M_PI)
			killblue : KillBlue
			killpink : KillPink
			killpurple : KillPurple
			killred : KillRed
			findblue : FindBlue
			findpink : FindPink
			findpurple : FindPurple
			findred : FindRed
			walk2 : MoveToTarget
			walk3 : MoveToTarget
			walk4 : MoveToTarget
			walk5 : MoveToTarget


			startnode =T(1000)=> swoosh
			swoosh =T(2000)=> findblobs
			findblobs =MAP=> countblobs
			countblobs =C=> legsdown5
			legsdown5 =T(2000)=> walkarm
			walkarm =T(500)=> walkout
			walkout =C=> findchiararight
			findchiararight =MAP=> redchiara
			redchiara =C=> orient
			orient =S<float>=> Turn =C=> LookStraight =T(200)=> checkorient
			checkorient =MAP=> redchiara
			orient =C=> legsup
			redchiara =F=> findchiaraleft
			findchiaraleft =MAP=> redchiara2
			redchiara2 =C=> orient
			redchiara2 =F=> findchiararight
			legsup =T(700)=> clearlist
			clearlist =C=> lookatpoint
			lookatpoint =T(200)=> raiseblueleg
			raiseblueleg =C=> lookleg
			lookleg =MAP=> blueblob1
			blueblob1 =C=> checkleg
			checkleg =F=> lookleg
			checkleg =C=> statenode
			statenode =T(1000)=> lowerblueleg
			lowerblueleg =C=> statenode2
			statenode2 =T(500)=> morsetester
			morsetester =C=>  hasMore
			hasMore =C=> getCodeChar
			hasMore =F=> done
			getCodeChar =C=> senddot
			getCodeChar =F=> senddash
			getCodeChar =T(20)=> sendstop
			senddot =T(200)=> raiseblueleg2
			senddash =T(200)=> raiseblueleg2
			sendstop =T(200)=> raiseblueleg2
			raiseblueleg2 =C=> lookleg2
			lookleg2 =MAP=> blueblob2
			blueblob2 =C=> checkleg2
			checkleg2 =F=> lookleg2
			checkleg2 =C=> lookchiara
			lookchiara =MAP=> findcenter
			findcenter =C=> checkgreen
			checkgreen =C=> getdash
			checkgreen =F=> getdot
			checkgreen =T(400)=> getstop
			getdash =C=> lowerblueleg
			getdot =C=> lowerblueleg
			getstop =C=> lowerblueleg
			done =T(150)=> raisebluelegfinal
			raisebluelegfinal =C=> looklegfinal
			looklegfinal =MAP=> blueblobfinal
			blueblobfinal =C=> checklegfinal
			checklegfinal =F=> looklegfinal
			checklegfinal =C=> lookchiarafinal
			lookchiarafinal =MAP=> findcenterfinal
			findcenterfinal =C=> checkgreenfinal
			checkgreenfinal =C=> getdashfinal
			checkgreenfinal =F=> getdotfinal
			checkgreenfinal =T(400)=> getstopfinal
			getdashfinal =C=> threestops
			getdotfinal =C=> threestops
			getstopfinal =C=> threestops
			threestops =F=> statenode4
			statenode4 =T(1500)=> looklegfinal
			threestops =C=> statenode3
			statenode3 =T(1000)=> numups
			numups =C=> lowerblueleg2
			lowerblueleg2 =C=> lookleg3
			lookleg3 =MAP=> blueblob3
			blueblob3 =C=> checkleg3
			checkleg3 =C=> stuckinup
			checkleg3 =F=> listtostring
			stuckinup =F=> lookleg3
			stuckinup =C=> raiseblueleg3
			raiseblueleg3 =C=> statenode3
			listtostring =S<string>=> Speak =T(1000)=> legsdown1
			legsdown1 =T(100)=> forward
			forward =C=> turnaround
			turnaround =C=> killblue
			killblue =F=> killpink
			killpink =F=> killpurple
			killpurple =F=> killred
			killred =F=> fail
			killblue =C=> legsup7
			killpink =C=> legsup8
			killpurple =C=> legsup9
			killred =C=> legsup10
			legsup7 =T(1000)=> swoosh2
			legsup8 =T(1000)=> swoosh3
			legsup9 =T(1000)=> swoosh4
			legsup10 =T(1000)=> swoosh5
			swoosh2 =T(2000)=> findblue
			swoosh3 =T(2000)=> findpink
			swoosh4 =T(2000)=> findpurple
			swoosh5 =T(2000)=> findred
			findblue =MAP=> legsdown12
			findpink =MAP=> legsdown13
			findpurple =MAP=> legsdown14
			findred =MAP=> legsdown15
			legsdown12 =T(1000)=> poke12
			poke12 =T(500)=> orient2
			legsdown13 =T(1000)=> poke13
			poke13 =T(500)=> orient3
			legsdown14 =T(1000)=> poke14
			poke14 =T(500)=> orient4
			legsdown15 =T(1000)=> poke15
			poke15 =T(500)=> orient5
			orient2 =S<float>=> Turn =C=> legsup7
			orient3 =S<float>=> Turn =C=> legsup8
			orient4 =S<float>=> Turn =C=> legsup9
			orient5 =S<float>=> Turn =C=> legsup10
			orient2 =C=> walk2
			orient3 =C=> walk3
			orient4 =C=> walk4
			orient5 =C=> walk5
			walk2 =S<float>=> Forward =C=> legsup7
			walk3 =S<float>=> Forward =C=> legsup8
			walk4 =S<float>=> Forward =C=> legsup9
			walk5 =S<float>=> Forward =C=> legsup10
			walk2 =C=> poke1
			walk3 =C=> poke1
			walk4 =C=> poke1
			walk5 =C=> poke1
			poke1 =T(1000)=> poke2
			poke2 =T(25)=> poke3
			poke3 =T(25)=> poke4
			poke4 =T(25)=> poke5
			poke5 =T(25)=> poke6
			poke6 =T(25)=> poke7
			poke7 =T(25)=> poke8
			poke8 =T(500)=> backup
			backup =C=> killblue
			

		#endstatemachine

	#endnodeclass
	#endif