//-*-c++-*-
/**
 * @file LandmarkWalk.h.fsm
 * @brief Takes a photo of two landmarks, then walks between them and
 * takes another photo from the other side. Then displays both photos
 * side-by-side in the Looking Glass.
 * @author Charles Ruhland <cruhland@andrew>
 */

#ifndef INCLUDED_LandmarkWalk_h_
#define INCLUDED_LandmarkWalk_h_

#include <string>

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

using namespace DualCoding;

static int color_one, color_two;

class LandmarkWalk : public VisualRoutinesStateNode {
public:
    LandmarkWalk() : VisualRoutinesStateNode("LandmarkWalk") {
        color_one = ProjectInterface::getColorIndex("blue");
        color_two = ProjectInterface::getColorIndex("pink");
    }
  
    virtual void setup() {
        #statemachine
        startnode: BuildWorldMap() =MAP=> photo_one
        photo_one: PhotographLandmarks() =C=> display_image
        display_image: DisplayImage()
        #endstatemachine
    }

    class BuildWorldMap : public VisualRoutinesStateNode {
    public:
        BuildWorldMap() : VisualRoutinesStateNode("BuildWorldMap") {}
  
        virtual void DoStart() {
            erouter->addListener(this, EventBase::buttonEGID,
                                 RobotInfo::GreenButOffset,
                                 EventBase::activateETID);
        }

        void processEvent(const EventBase &) {
            //**************** Set up MapBuilder request ****************
            NEW_SHAPE(gazePt, PointData,
                      new PointData(localShS, Point(600, -200, 40, egocentric)));

            MapBuilderRequest req(MapBuilderRequest::worldMap);
            req.searchArea = gazePt;
            req.pursueShapes = true;

            req.objectColors[blobDataType].insert(color_one);
            req.objectColors[blobDataType].insert(color_two);

            mapBuilder.executeRequest(this, req);
        }
    };

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

        virtual void DoStart() {
            // Get largest area green and orange blobs in world space
            NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(worldShS));
            BlobData::AreaLessThan comp;
            NEW_SHAPE(landmark_one, BlobData,
                      max_element(subset(blobs, IsColor(color_one)), comp));
            NEW_SHAPE(landmark_two, BlobData,
                      max_element(subset(blobs, IsColor(color_two)), comp));

            // Find midpoint of blobs and use that as photo target
            Point centroid_one = landmark_one->getCentroid();
            Point centroid_two = landmark_two->getCentroid();
            NEW_SHAPE(join_line, LineData, *new LineData(worldShS, centroid_one,
                                                         centroid_two));
            NEW_SHAPE(midpoint, PointData,
                      *new PointData(worldShS, join_line->getCentroid()));
            Point p = midpoint->getCentroid();
            getMC()->lookAtPoint(p.coordX(), p.coordY(), p.coordZ());
        }
    };
    
    class DisplayImage : public StateNode, LGmixin {
    public:
        DisplayImage() : StateNode("DisplayImage"), LGmixin() {}

        virtual void DoStart() {
            std::string image_name("first_image.jpg");
            uploadCameraImage(image_name);
            displayImageFile(image_name);
        }
    };

};

#endif // INCLUDED_LandmarkWalk_h_