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

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

#include <string>
using namespace std;

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


Point ctr;

Shape<BlobData> blobMarker;
Shape<BlobData> blue_leg;

#nodeclass MorseTest: LeapMachine


#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");


	
	#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;
			postStateSignal<float>(0);
		}
		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) > .3) {
			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();


	#shortnodemethod setup
		#statemachine
			startnode : LegsUp
			lookchiara : LookChiara
			findcenter : FindCenterOfBlobs
			checkgreen : Checkgreen
			getdot : ReceiveDOT
			getdash : ReceiveDASH
			getstop : ReceiveSTOP


			startnode =T(500)=> lookchiara
			lookchiara =MAP=> findcenter
			findcenter =C=> checkgreen
			checkgreen =C=> getdash
			checkgreen =F=> getdot
			checkgreen =T(700)=> getstop

#endstatemachine

	#endnodeclass
	#endif