#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"

static float for_Speed = 10;

using namespace DualCoding;

/**
    Process the world map, compute the mid point of the 2 landmarks, since the
    robot initially is at (0, 0) in the world map, thus we assmue that 
    2 * mid point is the target point. We also assume that the target point is
    along the x axis of world frame, since turning the robot is pretty hard.
*/
class ParseWorldMap : public VisualRoutinesStateNode {
public:
    ParseWorldMap() : VisualRoutinesStateNode("Go Through Landmarks") {}

    virtual void DoStart() {
        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));

        NEW_SHAPE(targetPoint, PointData, 
            new PointData(worldShS, midPoint->getCentroid() * 2));
        float dist = targetPoint->getCentroid().xyNorm();
        for_Speed = dist / 60;

        // look at the middle point of the 2 land marks
        LookoutPointRequest lpr;
        lpr.setTarget(midPoint->getCentroid());
        lpr.motionSettleTime = 1000;

        lookout.executeRequest(lpr);
    }
};

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

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

/**
    display the images side by side
*/
class DisplayPic : public StateNode, public LGmixin {
public:    
    DisplayPic() : StateNode("DisplayPic"), LGmixin() {}
    virtual void DoStart() {
        fprintf(stderr," Displaying a picture... \n");
        uploadFile("ms/lg/display.html");
        displayHtmlFile("display.html");
        postStateCompletion();
    }
};

/**
    Build the world map, will pursue shape. 
*/
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")
        display: DisplayPic()

        turn: XWalkNode() [getMC()->setTargetVelocity(0, 0, 0.0785);]
        stop1: XWalkNode() [getMC()->setTargetVelocity(0, 0, 0);]
        stop2: XWalkNode() [getMC()->setTargetVelocity(0, 0, 0);]
        forward: XWalkNode() [getMC()->setTargetVelocity(for_Speed, 0, 0);]
        parse: ParseWorldMap()
        wmapbuilder: BuildWMap()
        
        //We basically use time out for all motion stuff, and adjust the speed
        //based on the distance.
        startnode =T(5000)=> pic1 
        pic1 =C=> wmapbuilder
        wmapbuilder =MAP=> parse
        parse =T(3000)=> forward
        forward =T(63000)=> stop1
        stop1 =T(1000)=> turn
        turn =T(43000)=> stop2
        stop2 =T(1000)=> pic2
        pic2 =C=> display
        
        #endstatemachine
    }
};
#endif