//-*-c++-*-
#ifndef INCLUDED_EggHunt_h_
#define INCLUDED_EggHunt_h_

#include <iostream>
using namespace std;

#include "Behaviors/Leapers.h"

Shape<BlobData> eggMarker;

#nodeclass EggHunt: LeapMachine

    #shortnodeclass Map : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
        NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), 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")] = 100;
	
    #shortnodeclass FindEgg : VisualRoutinesStateNode : DoStart
	NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
	if(blobs.empty())
		postStateFailure();
	eggMarker = max_element(blobs, BlobData::AreaLessThan());
	postStateCompletion();

    #shortnodeclass OrientToEgg : StateNode : DoStart
       Point cent = eggMarker->getCentroid();
       float ang = atan2(cent.coordY(), cent.coordX());
       if(abs(ang) < M_PI/18)
               postStateCompletion();
       else
               postStateSignal<float>(ang);

    #shortnodeclass WalkToEgg : StateNode : DoStart
	Point cent = eggMarker->getCentroid();
	float dist = (cent.coordX());
	if(260 < abs(dist) && abs(dist) < 290) {
		cout<<"I think I'm the right distance away at :"<<dist<<endl<<endl;
		postStateCompletion();
	}
	else {
		cout<<"I'm not the right distance at "<<dist<<" so I'm moving "<<dist - 275<<endl<<endl;
		postStateSignal<float>(dist - 275);
	}
	
    #shortnodeclass GetInfo : StateNode : DoStart
    	Point ctr = eggMarker->getCentroid();
    	fmat::Column<3> target = kine->linkToBase(GripperFrameOffset).translation();
	Point gripperCenter(target[0], target[1], target[2]);
	Point difference = ctr - gripperCenter;
	float angle = atan2(difference.coordY(), difference.coordX());
	float dist = sqrt(difference.coordX()*difference.coordX() +
	difference.coordY()*difference.coordY());
	difference *= 30 / dist;
	Point approachPoint = ctr - difference;
	approachPoint.setCoords(approachPoint.coordX(), approachPoint.coordY(), angle);
	postStateSignal<Point>(ctr);

    #shortnodeclass ReachToEgg(int time = 2000) : DynamicMotionSequenceNode($) : DoStartEvent
	const DataEvent<Point> *datev = dynamic_cast<const DataEvent<Point>*>(&event);
	if ( datev != NULL ) {
		Point ac = datev->getData();
		PlanarThreeLinkArm solver;
		cout<<"I want to move the arm to "<<ac.coordX()<<", "<<ac.coordY()<<" at orientation "<<ac.coordZ()<<endl;
		PlanarThreeLinkArm::Solutions sols = solver.invKin3Link(ac.coordX(),ac.coordY(), ac.coordZ());
		if(sols.valid == false || sols.noSols < 1)
			postStateFailure();
		getMC()->advanceTime(time);
		cout<<"Moving Arm to position :"<<sols.angles(0,0)<<", "<<sols.angles(0,1)<<", "<<sols.angles(0,2)<<endl;
		getMC()->setOutputCmd(RobotInfo::ArmShoulderOffset, sols.angles(0,0));
		getMC()->setOutputCmd(RobotInfo::ArmElbowOffset, sols.angles(0,1));
		getMC()->setOutputCmd(RobotInfo::WristOffset, sols.angles(0,2));
	}
	
    #shortnodeclass MiniMap : MapBuilderNode($,MapBuilderRequest::localMap) : constructor
	std::vector<Point> pts;
	pts.push_back(Point(100,0,0));
	pts.push_back(Point(200,-50,0));
	pts.push_back(Point(200,0,-200));
	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")] = 100;

    #shortnodemethod setup
	#statemachine
		
		startnode : Map
		find : FindEgg
		orientegg : OrientToEgg
		walkegg : WalkToEgg
		searchturn : Turn($, M_PI/2)
		armprep : SwingArmRight($, 500)
		minimap : MiniMap
		reach : ReachToEgg
		info : GetInfo
		
		startnode =MAP=> armprep
		armprep =C=> find
		find =C=> orientegg
		find =F=> searchturn
		searchturn =C=> startnode
		orientegg =S<float>=> Turn =C=> startnode
		orientegg =C=> walkegg
		walkegg =S<float>=> Forward =C=> startnode
		walkegg =C=> minimap
		minimap =MAP=> info
		info =S<Point>=> reach

        #endstatemachine

#endnodeclass

#endif