#include <stdio.h>
#include "Behaviors/StateMachine.h"
#include "Shared/RobotInfo.h"

//#if defined(TGT_HAS_ARMS) && defined(TGT_HAS_HEAD)

//#include "Behaviors/Nodes/PostureNode.h"
//#include "Behaviors/Transitions/EventTrans.h"
//#include "Motion/MotionPtr.h"

$nodeclass ArrivalToToss : VisualRoutinesStateNode {

	$provide unsigned int visualTestCount;
	$provide fmat::Column<3> globalArmShouldPos;
	//$provide Point globalArmShouldPos;

	$nodeclass GetArmShoulderOffset : PostureNode : doStart 
	{
		$reference ArrivalToToss::globalArmShouldPos;
		fmat::Transform currArmShouldPos = kine->linkToBase(ArmShoulderOffset);
        	//globalArmShouldPos.setCoords(currArmShouldPos.translation()); //need not just translation but orientation
		globalArmShouldPos = currArmShouldPos.translation();
		NEW_SHAPE(shoulderPos, PointData, new PointData(localShS, Point(globalArmShouldPos[0], globalArmShouldPos[1],globalArmShouldPos[2]))); 
		postStateCompletion();
	}

	$nodeclass FindBlobs : MapBuilderNode(MapBuilderRequest::localMap) : doStart
	{	
		mapreq.addObjectColor(blobDataType, "red");
		mapreq.addObjectColor(blobDataType, "blue");
		postStateCompletion();
	}
	// do I use "(MapBuilderRequest::localMap)" to project onto local space?

	$nodeclass GetTolerance : VisualRoutinesStateNode : doStart
	{
		vector<Point> define_points;	
		NEW_SHAPEVEC(blob_shapes, BlobData, select_type<BlobData>(localShS));
		NEW_SHAPEVEC(blobs, BlobData, stable_sort(blob_shapes,not2(BlobData::AreaLessThan())));
		//blobs = stable_sort(blob_shapes,not2(BlobData::AreaLessThan()));
		Point centerOne = blobs[0]->getCentroid();
		centerOne.setCoords(centerOne.coordX() - 100,centerOne.coordY(),centerOne.coordZ());
		define_points.push_back(centerOne);
		Point centerTwo = blobs[1]->getCentroid();
		centerTwo.setCoords(centerTwo.coordX() - 100,centerTwo.coordY(),centerTwo.coordZ());
		define_points.push_back(centerTwo);
		Point topPoint;
		float topPointY = ((centerOne.coordY() + centerTwo.coordY()) / 2.0);
		topPoint.setCoords(centerTwo.coordX() - 75,topPointY,centerTwo.coordZ());
		define_points.push_back(topPoint);
		Point bottomPoint;
		bottomPoint.setCoords(centerTwo.coordX() - 125,topPointY,centerTwo.coordZ());
		define_points.push_back(bottomPoint);
		//float yCenter = (centerOne.coordY() + centerTwo.coordY()) / 2.0;
		//float zCenter = (centerOne.coordZ() + centerTwo.coordZ()) / 2.0;
		//Point finalCenter(xCenter, yCenter, zCenter, egocentric);
		// so is this final point, and the other points for that matter, in local space?
		//NEW_SHAPE(tolerance, EllipseData, new EllipseData(localShS, finalCenter, 500, 500, 0));
		NEW_SHAPE(tolerance_polygon, PolygonData, new PolygonData(localShS, define_points, true, true, true));	
		postStateCompletion();
	}

	//$nodeclass ProjectTolerance : MapBuilderNode(MapBuilderRequest::localMap) : doStart 
	//{
	//	mapreq.clearCamera = false;
	//	postStateCompletion();
	//}

	$nodeclass VisualTest : VisualRoutinesStateNode : doStart
	{ 	
		$reference ArrivalToToss::globalArmShouldPos;
		$reference ArrivalToToss::visualTestCount;
		GET_SHAPE(tolerance_polygon,PolygonData,localShS);
		GET_SHAPE(shoulderPos,PointData,localShS);
		
		//if (tolerance_polygon->isInside(globalArmShouldPos)) {
		  if (tolerance_polygon->isInside(shoulderPos)) {
		   ++visualTestCount;
		   postStateCompletion(); 
		   }
		else {
		   postStateCompletion(); 		   
       		   }
        }
	 
	$nodeclass Check : StateNode : doStart 
	{
		$reference ArrivalToToss::visualTestCount;
		if (visualTestCount == 2) {
		   postStateSuccess(); 
		   }
		else {
		   postStateFailure(); 		   
       		   }
	}
	
	$nodeclass Toss : StateNode 
	{
		$setupmachine {
		startnode: DynamicMotionSequenceNode("relax.mot") =C=> DynamicMotionSequenceNode("open.mot") =C=> StateNode =T(1000)=> PostureNode("e.pos") =C=> 
StateNode =T(1000)=> DynamicMotionSequenceNode("relax.mot")
		}
	}
	$setupmachine {
	//needs camera to be facing straight and for the flaps to be flat
	//still need to erase local shapes to do the test over again?
	launch: GetArmShoulderOffset =C=> FindBlobs =C=> GetTolerance =C=> VisualTest =C=> GetTolerance =C=> VisualTest =C=> check
	check: Check
	check =S=> Toss
	check =F=> SoundNode("howl.wav") //go and orient to starting point and orientation
	}
}

REGISTER_BEHAVIOR(ArrivalToToss);

//#endif