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

using namespace DualCoding;

#define BLUE_ID 0
#define PINK_ID 1
#define NO_BALL_ID 2

Point ellipsePos;
Point bluePoint;
Point pinkPoint;
int ballColor;


class BuildMap : public VisualRoutinesStateNode {
public:
    BuildMap() : VisualRoutinesStateNode("BuildMap") {}
  
    virtual 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.objectColors[lineDataType].insert(pink_index);
        req.objectColors[lineDataType].insert(blue_index);
	req.objectColors[ellipseDataType].insert(pink_index);
	req.objectColors[ellipseDataType].insert(blue_index);

        // execute request
        unsigned int mapreq_id = mapBuilder.executeRequest(req);
        erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::statusETID);
    }

    virtual void processEvent(const EventBase &) {
        postStateCompletion();
    }

};

class PositionArm1 : public ArmNode {
public:
  PositionArm1() : ArmNode("PositionArm1") {}

  virtual void DoStart() {
        getMC()->setJoints(1.571,-1.309,-1.141);		
  }
};

class PositionHead1 : public HeadPointerNode {
public:
  PositionHead1() : HeadPointerNode("PositionHead1") {}

  virtual void DoStart() {
        getMC()->setJoints(0, 0.0896, -1.205);
  }
};

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

    virtual void DoStart() {

        // find all lines and balls      
        NEW_SHAPEVEC(lines,LineData,select_type<LineData>(localShS));
        NEW_SHAPEVEC(ellipses,EllipseData,select_type<EllipseData>(localShS));

        if(ellipses.empty()) {
            // if there isn't a ball, don't do anything in the next step
            ballColor = NO_BALL_ID;
        }
        else {
            // get the ball position and color and the line positions
            NEW_SHAPE(ellipse,EllipseData,max_element(ellipses,EllipseData::AreaLessThan()));
            ballColor = IsColor("blue")(ellipse) ? BLUE_ID : PINK_ID;

            NEW_SHAPEVEC(allBlueLines,LineData,subset(lines, IsColor("blue")));
            NEW_SHAPEVEC(allPinkLines,LineData,subset(lines, IsColor("pink")));
            NEW_SHAPE(blueLine,LineData,max_element(allBlueLines,LineData::LengthLessThan()));
            NEW_SHAPE(pinkLine,LineData,max_element(allPinkLines,LineData::LengthLessThan()));

            ellipsePos = (EllipseData(ellipses.front())).centerPt();
            bluePoint = blueLine->getCentroid();
            pinkPoint = pinkLine->getCentroid();

	    Shape<LineData> ballToBlueLine(localShS, ellipsePos, bluePoint);
	    Shape<LineData> ballToPinkLine(localShS, ellipsePos, pinkPoint);

	    int offset = 50;

	    double theta = orientation_t(ballToBlueLine->getOrientation());
	    Point p = ballToBlueLine->rightPt();
	    bluePoint = Point(p.coordX() - offset*cos(theta), p.coordY() - offset*sin(theta), p.coordZ());

	    theta = orientation_t(ballToPinkLine->getOrientation());
	    p = ballToPinkLine->rightPt();
	    pinkPoint = Point(p.coordX() - offset*cos(theta), p.coordY() - offset*sin(theta), p.coordZ());
        }
        postStateCompletion();
    }
};

class MoveBall : public ArmNode {
public:
  MoveBall() : ArmNode("MoveBall") {}

  virtual void DoStart() {

	getMC()->setMaxSpeed(0, 0.35);
	getMC()->setMaxSpeed(1, 0.35);
	getMC()->setMaxSpeed(2, 0.35);
        if(ballColor == BLUE_ID) {
            getMC()->moveToPoint(bluePoint.coordX(),bluePoint.coordY(),bluePoint.coordZ());
        }
        else if(ballColor == PINK_ID) {
            getMC()->moveToPoint(pinkPoint.coordX(),pinkPoint.coordY(),pinkPoint.coordZ());
        }
        else {
            // no ball, do nothing
            sndman->speak("I can has ball please?");
            postStateCompletion();
        }
  }
};

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

    virtual void setup() {
        #statemachine

		startnode: StateNode=N=>PositionArm1()=C=>PositionHead1()=E(buttonEGID, ChiaraInfo::GreenButOffset, activateETID)=>BuildMap()=C=>FindObjects()=C=>MoveBall()=C=>startnode

        #endstatemachine
    }
};