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

#include <iostream>
using namespace std;

#include "Behaviors/Leapers.h"

Shape<BlobData> eggMarker;
Point myTgt;
float oriAngle;

#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());
	cout<<"I see the blob as "<<dist<<" away"<<endl;
	if(255 < abs(dist) && abs(dist) < 265) { 
		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 - 260<<endl<<endl;
		postStateSignal<float>(dist - 260);
	}
	
    #shortnodeclass GetInfoApproach : 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;
	oriAngle = M_PI/2;//atan2(difference.coordY(), difference.coordX());
	float dist = sqrt(difference.coordX()*difference.coordX() +
	difference.coordY()*difference.coordY());
	difference *= 30 / dist;
	myTgt = ctr - difference;
	postStateCompletion();	
    
    #shortnodeclass GetInfoTop : 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;
	oriAngle = M_PI/2;//-atan2(difference.coordY(), difference.coordX());
	myTgt = ctr;
	postStateCompletion();

    #shortnodeclass ReachToEgg(int time = 2000) : DynamicMotionSequenceNode($) : DoStart
	PlanarThreeLinkArm solver;
	fmat::Column<3> shoulder = kine->linkToBase(ArmShoulderOffset).translation();
	Point shoulderOffset(shoulder[0], shoulder[1], 0);
	myTgt -= shoulderOffset;
	cout<<endl<<endl<<"----------------Attempting to move arm to "<<myTgt.coordX()<<", "<<myTgt.coordY()<<", "<<oriAngle<<endl;
	PlanarThreeLinkArm::Solutions sols = solver.invKin3Link(myTgt.coordX(),myTgt.coordY(), oriAngle);
	if(sols.valid == false || sols.noSols < 1) {
		cout<<"Fail :-("<<endl;
		postStateFailure();
		return;
	}
	cout<<"Moving arm..."<<endl;
	getMC()->advanceTime(time);
	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(1000,0,0));
	pts.push_back(Point(2000,-500,0));
	pts.push_back(Point(2000,0,-2000));
	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
		reachfinal : ReachToEgg
		info : GetInfoApproach
		top : GetInfoTop
		reposition : Sideways ($, -50)
		minimap2 : MiniMap
		reposition2 : Sideways ($, 30)
		minimap3 : MiniMap
		
		startnode =MAP=> armprep
		armprep =C=> minimap
		minimap =MAP=> find
		find =F=> searchturn
		searchturn =C=> startnode
		find =C=> orientegg
		orientegg =S<float>=> Turn =C=> minimap
		orientegg =C=> walkegg
		walkegg =S<float>=> Forward =C=> minimap
		walkegg =C=> info
		info =C=> reach
		reach =F=> reposition
		reposition =C=> minimap2
		minimap2 =MAP=> reach
		reach =C=> top
		top =C=> reachfinal
		reachfinal =F=> reposition2
		reposition2 =C=> minimap3
		minimap3 =MAP=> reachfinal

        #endstatemachine

#endnodeclass

#endif