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

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

using namespace DualCoding;

class BuildMap : public VisualRoutinesStateNode {
public:
  BuildMap() : VisualRoutinesStateNode("BuildMap") {}
  
  virtual void DoStart() {
    
    //**************** Set up MapBuilder request ****************
		const int pink_index = ProjectInterface::getColorIndex("pink");
    const int blue_index = ProjectInterface::getColorIndex("blue");

    NEW_SHAPE(gazePt, PointData, new PointData(localShS,Point(200,200,41,egocentric)));

    MapBuilderRequest req(MapBuilderRequest::localMap);
    req.searchArea = gazePt;
    req.maxDist = 500;
    req.pursueShapes = true;
		req.numSamples = 5;
		req.motionSettleTime = 2000;

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

    req.occluderColors[lineDataType].insert(blue_index);
    req.occluderColors[lineDataType].insert(pink_index);

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

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

class ExamineMap : public VisualRoutinesStateNode {
public:
    ExamineMap() : VisualRoutinesStateNode("ExamineMap") {}

    virtual void DoStart() {
        fmat::Transform gripper = kine->linkToBase(GripperFrameOffset);
        fmat::Column<3> gripperPos = gripper.translation();
        Point gripperPt(gripperPos);

        NEW_SHAPEVEC(ells, EllipseData, select_type<EllipseData>(localShS));

        SHAPEVEC_ITERATE(ells, EllipseData, ell)
            printf("Exam1: dist: %.3f\n", ell->getCentroid().xyDistanceFrom(gripperPt));
            if(ell->getCentroid().distanceFrom(gripperPt) < 150)
            {
                postStateSignal<bool>(true);
                return;
            }
        END_ITERATE
        postStateSignal<bool>(false);
    }
};


class ExamineMap2 : public ArmNode, VRmixin {
public:
	ExamineMap2() : ArmNode("ExamineMap2") {}

  virtual void DoStart() {
    
		const int pink_index = ProjectInterface::getColorIndex("pink");
    const int blue_index = ProjectInterface::getColorIndex("blue");
    printf("In exam2\n");

    fmat::Transform gripper = kine->linkToBase(GripperFrameOffset);
    fmat::Column<3> gripperPos = gripper.translation();
    Point gripperPt(gripperPos);

    NEW_SHAPEVEC(ells, EllipseData, select_type<EllipseData>(localShS));

    NEW_SHAPEVEC(lines, LineData, select_type<LineData>(localShS));
    NEW_SHAPE(pinkLine, LineData, max_element(subset(lines, IsColor("pink")), LineData::LengthLessThan()))
    NEW_SHAPE(blueLine, LineData, max_element(subset(lines, IsColor("blue")), LineData::LengthLessThan()))
    float minDist = 10000; 
    NEW_SHAPE(closest, EllipseData, ells[1]);
    SHAPEVEC_ITERATE(ells, EllipseData, ell)
        float dist = ell->getCentroid().xyDistanceFrom(gripperPt);
        if(dist < minDist)
        {
            closest = ell;
            minDist = dist;
        }
    END_ITERATE
    
    int closestColor = ProjectInterface::getColorIndex(closest->getColor());

    NEW_SHAPE(targetLine, LineData, pinkLine);    

    if(closestColor == blue_index)
    {
        printf("Closest is blue\n");
        targetLine = blueLine;
    }
    else
    {
        printf("Closest is pink\n");
    }

    NEW_SHAPE(lineCenter, PointData, PointData(localShS, targetLine->getCentroid()))
    
    NEW_SHAPE(origin, PointData, PointData(localShS, Point(0,0,0)));
    float distToO = lineCenter->xyDistanceFrom(origin);    

    float newX = lineCenter->coordX() + 50*(lineCenter->coordX()/distToO);
    float newY = lineCenter->coordY() + 50*(lineCenter->coordY()/distToO);

    
    printf("Old: (%.3f, %.3f), new: (%.3f, %.3f)\n", lineCenter->coordX(), lineCenter->coordY(), newX, newY);
    
    NEW_SHAPE(newPt, PointData, PointData(localShS, Point(newX, newY, 0)));

    getMC()->setMaxSpeed(0,.5);
    getMC()->setMaxSpeed(1,.5);
    
    if(!getMC()->moveToPoint(newX, newY, 0)
    {  
        sndman->speak("No kinematic solution");
        printf("No kinematic solution\n");
    } 
    
  }
};

class Sorter : public VisualRoutinesStateNode {
public:
  Sorter(): VisualRoutinesStateNode("Sorter") {}
  
	virtual void setup() {
#statemachine
    startnode: StateNode =N=> startpos
    startpos: ArmNode("startpos")[getMC()->moveToPoint(100,150,41); getMC()->setJointValue(2,-1.5)] =C=> startcam
    startcam: HeadPointerNode("starthead")[getMC()->lookAtPoint(200,200,41)]=E(buttonEGID, ChiaraInfo::GreenButOffset, activateETID)=>buildmap
	buildmap: BuildMap() =MAP=> exam1
    exam1: ExamineMap()=S<bool>(true)=> exam2
    exam1 =S<bool>(false)=> startpos
    exam2: ExamineMap2()=C=> startpos
#endstatemachine
	    }
};

#endif