//-*-c++-*-
//ssh student@chiara1e '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 ind = 0;
MorseList lst = "";

Point ctr; 

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

#nodeclass MorseCodeReceiver: 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(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


	  class ReceiveDOT : public StateNode {
		   public:
		    ReceiveDOT(const std::string &nodename="ReceiveDOT") : StateNode(nodename) {}
		    virtual void DoStart() {
				lst += DOT;
				postStateCompletion();
	    }
	  };
		
		
	  class ReceiveDASH : public StateNode {
		   public:
		    ReceiveDASH(const std::string &nodename="ReceiveDASH") : StateNode(nodename) {}
		    virtual void DoStart() {
				lst += DASH;
				postStateCompletion();
	    }
	  };
		
		
	  class ReceiveSTOP : public StateNode {
		   public:
		    ReceiveSTOP(const std::string &nodename="ReceiveSTOP") : StateNode(nodename) {}
		    virtual void DoStart() {
				lst += STOP;
				postStateCompletion();
	    }
	  };
	


	#shortnodemethod setup
		#statemachine
			startnode : LegsUp
			lookstraight : LookStraight
			raiseblueleg2 : RaiseBlueLeg
			lookleg2 : LookLeg
			blueblob2 : BlueBlobsRedux
			checkleg2 : CheckBlueHorizontal
			raiseblueleg : RaiseBlueLeg
			lowerblueleg : LowerBlueLeg
			lookleg : LookLeg
			blueblob : BlueBlobsRedux
			checkleg : CheckBlueHorizontal
			lookchiara :LookChiara
			findcenter : FindCenterOfBlobs
			checkyellow : CheckYellow
			dot : ReceiveDot
			dash : ReceiveDash
			stop : ReceiveStop
			
			

			startnode =T(500)=> lookstraight
			lookstraight =T(500)=> raiseblueleg2
			raiseblueleg2 =C=> lookleg2
			lookleg2 =MAP=> blueblob2
			blueblob2 =C=> checkleg2
			checkleg2 =F=> lookleg2
			checkleg2 =C=> lowerblueleg
			raiseblueleg =C=> lookleg
			lookleg =MAP=> blueblob
			blueblob =C=> checkleg
			checkleg =F=> lowerblueleg
			checkleg =C=> lookchiara
			lookchiara =MAP=> findcenter
			findcenter =C=> checkyellow
			checkyellow =C=> dash
			checkyellow =F=> dot
			checkyellow =T(500)=> stop
			dash =C=> raiseblueleg
			dot =C=> raiseblueleg
			stop =C=> raiseblueleg
			raiseblueleg =C=> lookleg
			lowerblueleg =C=> lookleg
			
		
						
		#endstatemachine

	#endnodeclass
	#endif