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

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Events/EventBase.h" 

using namespace DualCoding;

class LandmarkBehavior : public VisualRoutinesStateNode {
public:
	
	class LandmarkFinder : public VisualRoutinesStateNode {
	public:

		LandmarkFinder() : VisualRoutinesStateNode("LandmarkFinder"), mapreq(MapBuilderRequest::worldMap) {}

		virtual void DoStart() {
			const color_index green_index = ProjectInterface::getColorIndex("green");
			const color_index blu_index = ProjectInterface::getColorIndex("blue");
			worldShS.clear();
			NEW_SHAPE(gazePoly, PolygonData, new PolygonData(worldShS, Lookout::groundSearchPoints(), false));

			// Prepare a MapBuilder request that will be passed to the Pilot
			mapreq.searchArea = gazePoly;
			mapreq.maxDist = 2000;
			mapreq.clearShapes = false;
			mapreq.rawY = true;
			mapreq.objectColors[blobDataType].insert(green_index);
            mapreq.objectColors[blobDataType].insert(blu_index);
			mapreq.minBlobAreas[green_index] = 100;
            mapreq.minBlobAreas[blu_index] = 100;
			mapreq.blobOrientations[green_index] = BlobData::pillar;  // this blob stands on the floor; it does not lie in the ground plane
            
			mapreq.blobOrientations[blu_index] = BlobData::pillar;  // this blob stands on the floor; it does not lie in the ground plane

			PilotRequest preq(PilotRequest::visualSearch);
			preq.mapBuilderRequest = &mapreq;
			preq.exitTest = &checkForBlob;
			preq.searchRotationAngle = 1;  // radians

			pilot.executeRequest(this, preq);
		}

		virtual void DoStop() {
			pilot.abort();
		}

	private:
		MapBuilderRequest mapreq;
	};

    class LookAtLandmarks : public HeadPointerNode, VRmixin {
    public:
        LookAtLandmarks() : HeadPointerNode("LookAtLandmarks") {}

        virtual void DoStart() {
            NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(worldShS))
            NEW_SHAPE(blue_obj, BlobData, max_element(subset(blobs, IsColor("blue")), BlobData::AreaLessThan()));
            NEW_SHAPE(green_obj, BlobData, max_element(subset(blobs, IsColor("green")), BlobData::AreaLessThan()))
            NEW_SHAPE(blue_cent, PointData, PointData(worldShS, blue_obj->getCentroid()))
            NEW_SHAPE(green_cent, PointData, PointData(worldShS, green_obj->getCentroid()))
            float avg_x = avg(blue_cent->coordX(), green_cent->coordX());
            float avg_y = avg(blue_cent->coordY(), green_cent->coordY());
            float avg_z = avg(blue_cent->coordZ(), green_cent->coordZ());

            printf("Looking now\n");
            getMC()->lookAtPoint(avg_x, avg_y, avg_z);
            }

        float avg(coordinate_t x, coordinate_t y)
        {
            return (x + y)/2.0;
        }
    };

	class HeadForGreenPillar : public VisualRoutinesStateNode {
	public:

		HeadForGreenPillar() : VisualRoutinesStateNode("HeadForOranveCan") {}

		virtual void DoStart() {
      PilotRequest preq(PilotRequest::gotoShape);
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(worldShS));
      NEW_SHAPE(biggestblob, BlobData, max_element(blobs,BlobData::AreaLessThan()));
      preq.targetShape = biggestblob;
      pilot.executeRequest(this,preq);
    }
  };

  static bool checkForBlob() {
    if(!find_if<BlobData>(subset(worldShS, IsColor("blue"))).isValid())
        return false;
    return find_if<BlobData>(subset(worldShS, IsColor("green"))).isValid();
  }

	//******** Parent state machine ********

  LandmarkBehavior(): VisualRoutinesStateNode("LandmarkBehavior") {}

	virtual void setup() {
#statemachine
	startnode: LandmarkFinder() =PILOT=> look
    look: LookAtLandmarks() =E(buttonEGID, ChiaraInfo::GreenButOffset, activateETID)=>
#endstatemachine
	}
};

#endif