#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() {
		// **** with moveToPoint here, the state machine's MoveBall node works, but with setJoints here, MoveBall doesn't work
		//getMC()->moveToPoint(250, 150, 41);
		//getMC()->setJoints(1.79907,-1.66088,-1.174);		
		getMC()->setJoints(1.5, -1.5, -1);		
  }
};

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() {

	//erouter->addListener(this, EventBase::motmanEGID,getMC()->getID(),EventBase::statusETID);
	printf("ellipse position: (%f,%f,%f)\n", ellipsePos.coordX(),ellipsePos.coordY(),ellipsePos.coordZ());

        if(ballColor == BLUE_ID) {
					//	    printf("moving to blue, (%f,%f,%f)\n", bluePoint.coordX(),bluePoint.coordY(),bluePoint.coordZ());
					cout << "moving to blue point " << bluePoint << endl;
            getMC()->moveToPoint(bluePoint.coordX(),bluePoint.coordY(),bluePoint.coordZ());
        }
        else if(ballColor == PINK_ID) {
					//	    printf("moving to pink, (%f,%f,%f)\n", pinkPoint.coordX(),pinkPoint.coordY(),pinkPoint.coordZ());
					cout << "moving to pink point " << pinkPoint << endl;
            //getMC()->moveToPoint(pinkPoint.coordX(),pinkPoint.coordY(),pinkPoint.coordZ());
	    bool result = getMC()->moveToPoint(400,-150, 41);
	    cout << "result = " << result << "\n";
        }
        else {
            // no ball, do nothing
            sndman->speak("Please place a ball in the gripper.");
            postStateCompletion();
        }
  }
};

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

		virtual void DoStart() {
			getMC()->moveToPoint(400, -150, 41);
		}
	};

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

    virtual void setup() {
        #statemachine

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

        #endstatemachine
    }
};