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

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

#define PI 3.14159

using namespace DualCoding;

class LandmarkBehavior : public VisualRoutinesStateNode, public LGmixin {
public:
	
	class LandmarkFinder : public VisualRoutinesStateNode {
	public:
		double angle;
		LandmarkFinder(double ang) : VisualRoutinesStateNode("LandmarkFinder"), mapreq(MapBuilderRequest::worldMap) {angle = ang;}

		virtual void DoStart() {
			const color_index green_index = ProjectInterface::getColorIndex("green");
			const color_index blue_index = ProjectInterface::getColorIndex("blue");
			worldShS.clear();

			// prepare a MapBuilder request that will be passed to the Pilot
			NEW_SHAPE(gazePoly, PolygonData, new PolygonData(worldShS, Lookout::groundSearchPoints(), false));
			mapreq.searchArea = gazePoly;
			mapreq.maxDist = 2000;
			mapreq.clearShapes = false;
			mapreq.rawY = true;
			mapreq.objectColors[blobDataType].insert(green_index);
            mapreq.objectColors[blobDataType].insert(blue_index);
			mapreq.minBlobAreas[green_index] = 100;
            mapreq.minBlobAreas[blue_index] = 100;

			// these blobs stand on the floor; they do not lie in the ground plane
			mapreq.blobOrientations[green_index] = BlobData::pillar;              
			mapreq.blobOrientations[blue_index] = BlobData::pillar;

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

			pilot.executeRequest(this, preq);
		}

		virtual void DoStop() {
			// note: this deletes the world map
			pilot.abort();
		}

	private:
		MapBuilderRequest mapreq;
	};

    class MoveBetween : public HeadPointerNode {
    public:
        MoveBetween() : HeadPointerNode("MoveBetween") {}

        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());

			Point midp(avg_x,avg_y,avg_z);
			Shape<LineData> robotToMidpoint(worldShS, Point(0,0,0), midp); 		
			double offset = 600;
	    	double theta = atan2(midp.coordY(), midp.coordX()); 
			Shape<PointData> walkTarget(worldShS, Point(midp.coordX()+offset*cos(theta),
										midp.coordY()+offset*sin(theta),0)); 
			cout << "walk target: " << walkTarget << ", midp: " << midp << "\n";
			PilotRequest preq(PilotRequest::gotoShape);
      		preq.targetShape = walkTarget;
      		pilot.executeRequest(this,preq);
        }

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

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


 	class LookAtBlue : public HeadPointerNode {
    	public:
        	LookAtBlue() : HeadPointerNode("LookAtBlue") {}

        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(blue_cent, PointData, PointData(worldShS, blue_obj->getCentroid()))

            getMC()->lookAtPoint(blue_cent->coordX(), blue_cent->coordY(), blue_cent->coordZ());
		}
	};

 	class LookAtGreen : public HeadPointerNode {
    	public:
        	LookAtGreen() : HeadPointerNode("LookAtGreen") {}

        virtual void DoStart() {
            NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(worldShS))
            NEW_SHAPE(green_obj, BlobData, max_element(subset(blobs, IsColor("green")), BlobData::AreaLessThan()))
            NEW_SHAPE(green_cent, PointData, PointData(worldShS, green_obj->getCentroid()))

            getMC()->lookAtPoint(green_cent->coordX(), green_cent->coordY(), green_cent->coordZ());
		}
	};


	class TakePicture : public HeadPointerNode {
		public:
			TakePicture() : HeadPointerNode("TakePicture") {}

		virtual void DoStart() {
			LGmixin::uploadCameraImage("image1.jpeg");
			LGmixin::displayImageFile("image1.jpeg");
		}
	};

	class RotateBody : public XWalkNode {
		public:
			RotateBody() : XWalkNode("RotateBody") {}
		
		virtual void DoStart() {
			getMC()->setTargetVelocity(0,0,PI/15.0);
		}

	};


  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: StateNode =E(buttonEGID, ChiaraInfo::GreenButOffset, activateETID)=> 
	    LandmarkFinder(1) =PILOT=> LookAtBlue() =T(3000)=> TakePicture() =C=> LookAtGreen() =T(3000)=> 
	    TakePicture() =C=> MoveBetween() =PILOT=> XWalkNode(0,0,0,0,PI/50.0,PI) =C=>
        LandmarkFinder(1) =PILOT=> LookAtBlue() =T(3000)=> TakePicture() =C=> LookAtGreen() =T(3000)=>
        TakePicture()

        #endstatemachine
	}
};

#endif