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

#include <iostream>
using namespace std;

#include "Behaviors/Leapers.h"

Shape<BlobData> blobMarker;

#nodeclass Swat: LeapMachine

    #shortnodeclass FindBlobs : 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, "blue");
        mapreq.addObjectColor(blobDataType, "yellow");
	mapreq.minBlobAreas[ProjectInterface::getColorIndex("blue")] = 100;
	mapreq.minBlobAreas[ProjectInterface::getColorIndex("yellow")] = 100;

    #shortnodeclass GetLargestBlob : VisualRoutinesStateNode : DoStart
	NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
	if(blobs.empty())
		postStateFailure();
	blobMarker = max_element(blobs, BlobData::AreaLessThan());
	postStateCompletion();

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

    #shortnodeclass IsBlobBlue : StateNode : DoStart
       if(IsColor("blue")(blobMarker))
               postStateCompletion();
       else
               postStateFailure();

    #shortnodeclass BlobWhack : StateNode : DoStart
       if(IsColor("blue")(blobMarker))
               postStateCompletion();
       else
               postStateFailure();

    #shortnodeclass WalkToBlob : StateNode : DoStart
	Point cent = blobMarker->getCentroid();
	float dist = (cent.coordX());
	cout<<"I WANT TO MOVE "<<dist<<endl<<endl<<endl;
	if(abs(dist) < 350)
		postStateCompletion();
	else
		postStateSignal<float>(dist -340);

    #shortnodemethod setup
	#statemachine

		startnode : FindBlobs
		largeblob : GetLargestBlob
		orientblob : OrientToBlob
		walkblob : WalkToBlob
		blueblob : IsBlobBlue
		whackblob : BlobWhack
		swingr : SwingArmRight($, 500)
		swingl : SwingArmLeft($, 500)
		whackr : SwingArmRight($, 500)
		whackl : SwingArmLeft($, 500)

		startnode =MAP=> largeblob
		largeblob =F=> startnode
		largeblob =C=> blueblob
		blueblob =F=> swingr
		blueblob =C=> swingl
		swingr =C=> orientblob
		swingl =C=> orientblob
		orientblob =S<float>=> Turn =C=> startnode
		orientblob =C=> walkblob
		walkblob =S<float>=> Forward =C=> startnode
		walkblob =C=> whackblob
		whackblob =C=> whackr
		whackblob =F=> whackl
		whackl =C=> startnode
		whackr =C=> startnode

        #endstatemachine

#endnodeclass

#endif