#ifndef __SORTING_OBJ_H
#define __SORTING_OBJ_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 "DualCoding/DualCoding.h"
#include "Behaviors/Nodes/ArmNode.h"
#include "Behaviors/StateMachine.h"

using namespace DualCoding;

#define LINE_THRESH 50
#define DIST_THRESH 100

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

    int findClosestOne(vector<Shape<EllipseData> > blobs) {
        int i;

        /* Get the coordinates of the gripper */
        fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
        cout << "Transform:\n" << Tgripper.fmt("%7.3f") << endl;
        fmat::Column<3> target = fmat::SubVector<3>(&Tgripper(0,3));
        Point *gripper = new Point(target);

        float min = 10000;
        int minIndex = -1;
        
        /* Debug purpose:: how many blobs are there? */
        cout << "b size: " << blobs.size() << endl;


        /* Get the closest blob by comparing the distances */
        for (i = 0; i < blobs.size(); i++) {
            float dist = blobs[i]->getCentroid().xyDistanceFrom(*gripper);

            cout << "Blob " << i << ", " << dist << "away" << endl;
            if (dist < min) {
                min = dist;
                minIndex = i;
            }
        }

        /* the closest should be reasonably close enough -- within DIST_THRESH */            
        if (min < DIST_THRESH)
            return minIndex;
        else
            return -1;
    }

    void moveGripperToThePoint(Point pt) {
        fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
        
        if (!getMC()->moveToPoint(pt.coordX(), pt.coordY(), Tgripper(2,3))) { // if IK fails
            cout << "*** ERROR ***: Can't reach" << endl;
            sndman->speak("Inverse kinematics failed!");
            // NOTE:: we couldn't test soundman function because it wasn't on our chiara
            postStateCompletion();
        }
        return;
    }

    float getLineLength(LineData &l) {
        float dist;
        dist = l.end1Pt().xyDistanceFrom(l.end2Pt());
        return dist;
    }

    /* Calculates the target point that the robot should move the shell to */
    Point getP2(Point lin_cen, Point &diff) {
        float distFactor = 50;
        return lin_cen + diff * distFactor;
    }

    void DoStart() {
        NEW_SHAPEVEC(blobs, EllipseData, select_type<EllipseData>(localShS));
        NEW_SHAPEVEC(lines, LineData, select_type<LineData>(localShS));

        int minIndex = findClosestOne(blobs);
        cout << minIndex << endl;
        if (minIndex == -1) { // checks if there's a viable ellipse nearby 
            cout << "*** ERROR *** : No shells nearby" << endl;
            postStateCompletion();
            return;
        }

        // filtering out lines that's about the same size as any of the blobs in the view
        // (based on the comment that the lines will be reasonably longer than the ellipses)
        for (int i = 0; i < lines.size(); i++) { 
            for (int j = 0; j < blobs.size(); j++) {
                if (abs(getLineLength(lines[i]) - blobs[j]->getSemimajor()) < blobs[j]->getSemimajor()*0.1 || 
                    abs(getLineLength(lines[i]) - blobs[j]->getSemiminor()) < blobs[j]->getSemiminor()*0.1) {
                    lines.erase(lines.begin() + i);
                }
            }
        }

        // extracts pink lines and blue lines.
        NEW_SHAPEVEC(pinkLines, LineData, subset(lines, IsColor("pink")));
        NEW_SHAPEVEC(blueLines, LineData, subset(lines, IsColor("blue")));
        
        pinkLines = stable_sort(pinkLines, not2(LineData::LengthLessThan()));
        blueLines = stable_sort(blueLines, not2(LineData::LengthLessThan()));
        
        if (pinkLines.size() == 0 || blueLines.size() == 0) {
            cout << "*** Error *** : No viable lines - lines should be reasonably longer than the ellipses" << endl;        
            postStateCompletion();
            return;              
        }

        // get our target line, which is the longest line
        NEW_SHAPE(pinkLine, LineData, pinkLines[0]->copy());
        NEW_SHAPE(blueLine, LineData, blueLines[0]->copy());

        // the line should be at least longer than some threshold
        if (getLineLength(pinkLine) < LINE_THRESH
            || getLineLength(blueLine) < LINE_THRESH) {
            cout << "*** Error *** : No viable lines - lines should be reasonably longer than the ellipses" << endl;        
            postStateCompletion();
            return;
        }

        // pick the right color line
        NEW_SHAPE(targetLine, LineData, blueLine->copy());
        if (IsColor("pink")(blobs[minIndex]))
            targetLine = pinkLine->copy();

        // calc an unit vector from the centroid of the shell towards the mid point of the line.
        Point diff = targetLine->getCentroid() - blobs[minIndex]->getCentroid();
        diff /=  diff.xyNorm();

        cout << "Our vector: " << diff.coordX() << " " << diff.coordY() << " " << endl;
        
        //set speed caps
        getMC()->setMaxSpeed(0,0.5);
        getMC()->setMaxSpeed(1,0.5);
        getMC()->setMaxSpeed(2,0.5);

        //calc the target position
        Point tmp = getP2(targetLine->getCentroid(),diff);
        cout << "Target Point: " << tmp.coordX() << " " << tmp.coordY() << " " << endl;
        NEW_SHAPE(P2, PointData, new PointData(localShS, tmp));

        //move to target
        moveGripperToThePoint(tmp);
    }
};

class SortObjects : public MapBuilderNode {
    class BuildLMap : public MapBuilderNode {
        public:
        BuildLMap() : 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::localMap);

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

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

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

            mapBuilder.executeRequest(req);
        }
    };

public:
    SortObjects() : MapBuilderNode("SortObjects") {}

    virtual void setup() {
#statemachine
        startnode: StateNode
        getready: PostureNode("Ready", "SortObjsReady.pos")
        startnode =N=> getready

        lmapbuilder: BuildLMap()
        pushnode: PushObjects()
        getready =E(buttonEGID, ChiaraInfo::GreenButOffset, activateETID)=> lmapbuilder

        lmapbuilder =MAP=> pushnode
        pushnode =C=> getready
#endstatemachine
    }
};
#endif