#include "Behaviors/Demos/Navigation/PilotDemo.h"

#define PI 3.141592654

$nodeclass PushCanisters : PilotDemo {
    enum outcome {done, notDone};

	virtual void buildMap() {
        std::vector<Point> boxpts;
        float boxSide = 750;  // millimeters
        float offset = 500 + boxSide/2;
        boxpts.push_back(Point(offset-boxSide/2, boxSide/2,0,allocentric));
        boxpts.push_back(Point(offset+boxSide/2, boxSide/2,0,allocentric));
        boxpts.push_back(Point(offset+boxSide/2,-boxSide/2,0,allocentric));
        boxpts.push_back(Point(offset-boxSide/2,-boxSide/2,0,allocentric));
        NEW_SHAPE(box, PolygonData, new PolygonData(worldShS, boxpts, true));
        box->setColor("blue");
        box->setObstacle(false);
	}
	
    $nodeclass LookAround : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
        mapreq.addObjectColor(blobDataType, "red");
        mapreq.addBlobOrientation("red",BlobData::pillar);
        mapreq.addObjectColor(blobDataType, "green");
        mapreq.addBlobOrientation("green",BlobData::pillar);
    }

	$nodeclass Push : PilotNode(PilotTypes::pushObject) : doStart {
   	    cout << "======> Pushing!\n";
		GET_SHAPE(box, PolygonData, worldShS);
		pilotreq.targetShape = box;
		bool isDone = true;

	    NEW_SHAPEVEC(cans, BlobData, select_type<BlobData>(worldShS));
	    IsColor redtest("red");
	    float mindist = 99999999;
        SHAPEVEC_ITERATE(cans, BlobData, obj){
            float dist = obj->getCentroid().distanceFrom(theAgent->getCentroid());
            if (redtest(obj)) {
                if(obj->isObstacle() && !box->isInside(obj->getCentroid()) && dist < mindist) {
                    cout << "======> Found one red thing not inside the box\n";
        		    pilotreq.objectShape = obj;
        		    isDone = false;
        		} else {
        		    obj->setObstacle(false);
        		}
        	} else {
        	    cout << "======> Nope!\n";
        	}
    	}END_ITERATE;
    	
    	if (isDone)
        	postStateSignal<outcome>(done);
	}
	
    $setupmachine {
        push : Push
  	    startdemo: LookAround =C=> Turn(PI/4) =C=> LookAround =C=> Turn(-PI/2) =C=> LookAround =C=> push
  	    push =S<outcome>(done)=> SpeechNode("Done.")
  	    push =PILOT=> LookAround =C=> push
    }



}

REGISTER_BEHAVIOR(PushCanisters);