#ifndef __Landmark_H
#define __Landmark_H

#include "Behaviors/BehaviorBase.h"
#include "Events/EventRouter.h"
#include "Motion/Kinematics.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/PIDMC.h"
#include "Motion/MotionPtr.h"
#include "Shared/fmat.h"
#include "Shared/RobotInfo.h"
#include "Sound/SoundManager.h"

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


using namespace DualCoding;

//class GoThroughLandmarks : public VisualRoutinesStateNode {
class GoThroughLandmarks : public XWalkNode, public VRmixin {
    public:
    //GoThroughLandmarks(): VisualRoutinesStateNode("asdf") {}
    GoThroughLandmarks(): XWalkNode("Go Through Landmarks", 0, 0, 0), VRmixin() {}

    virtual void DoStart() {
//        PilotRequest preq(PilotRequest::gotoShape);

        NEW_SHAPEVEC(blobs, EllipseData, select_type<EllipseData>(worldShS));
        NEW_SHAPEVEC(pinks, EllipseData, subset(blobs, IsColor("pink")));
        NEW_SHAPEVEC(blues, EllipseData, subset(blobs, IsColor("blue")));
        
        NEW_SHAPE(biggestblue, EllipseData, 
            max_element(blues, EllipseData::AreaLessThan()));
        NEW_SHAPE(biggestpink, EllipseData, 
            max_element(pinks, EllipseData::AreaLessThan()));

        NEW_SHAPE(midPoint, PointData, 
            new PointData(worldShS, (biggestblue->getCentroid()+
            biggestpink->getCentroid())/2));
        
//        preq.targetShape = midPoint;
//        pilot.executeRequest(this, preq);
//        fprintf(stderr,"*** Req complete\n");
        //x is forward
        getMC()->setTargetVelocity(10, 0, 0);
    }
};

class TakePic : public StateNode, public LGmixin {
private:
    char name[256];
public:    
    TakePic(char *n) : StateNode("aa"), LGmixin() {
        strcpy(name, n);
    }

    virtual void DoStart() {
        fprintf(stderr," Taking a picture... \n");
        uploadCameraImage(name);
        displayImageFile(name);
        postStateCompletion();
    }
};

class Landmark : public MapBuilderNode, public LGmixin {
    class BuildWMap : public MapBuilderNode {
        public:
        BuildWMap() : MapBuilderNode("BuildLMap") {}

        void DoStart() {
            //**************** Set up map builder request ****************
            const int pink_index = ProjectInterface::getColorIndex("pink");
            const int blue_index = ProjectInterface::getColorIndex("blue");
                
            MapBuilderRequest req(MapBuilderRequest::worldMap);

            req.maxDist = 1500;
            req.pursueShapes = true;

            req.objectColors[ellipseDataType].insert(blue_index);
            req.objectColors[ellipseDataType].insert(pink_index);

            mapBuilder.executeRequest(req);
        }
    };

public:
    Landmark() : MapBuilderNode("SortObjects"), LGmixin() {}

    virtual void setup() {
#statemachine
        startnode: StateNode
        
        pic1: TakePic("1.jpg")
        pic2: TakePic("2.jpg")
        turn: XWalkNode() [getMC()->setTargetVelocity(0, 0, 0.157);]
        stop1: XWalkNode() [getMC()->setTargetVelocity(0, 0, 0);]
        stop2: XWalkNode() [getMC()->setTargetVelocity(0, 0, 0);]
        go: GoThroughLandmarks()
        wmapbuilder: BuildWMap()

        startnode =T(5000)=> pic1 
        pic1 =C=> wmapbuilder
        wmapbuilder =MAP=> go 
        go =T(20000)=> stop1
        stop1 =T(1000)=> turn
        turn =T(20000)=> stop2
        stop2 =T(1000)=> pic2
        
        #endstatemachine
        /*
        wmapbuilder: BuildWMap()
        go: GoThroughLandmarks()

        startnode =N=> wmapbuilder
        wmapbuilder =MAP=> go
          */
    }
};
#endif