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

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

#include <string>
using namespace std;

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;

#nodeclass MorseCodeChiara: LeapMachine

	#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 = "leap";
		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();
			return;
		}
		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();
			return;
		}
		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;
		postStateSignal<string>(str);


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


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

	#shortnodemethod setup
		#statemachine
			startnode : 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)
			threestops : ThreeStopsInARow
			clearlist : ClearList
			statenode : StateNode
			statenode2 : StateNode
			statenode3 : StateNode
			statenode4 : StateNode
			numups : NumUp
			
			startnode =T(1000)=> clearlist
			clearlist =C=> lookatpoint
			lookatpoint =T(100)=> 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

		#endstatemachine

	#endnodeclass
	#endif