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

#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.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"

using namespace DualCoding;

//! Moves the arm and the camera to the left side
class MoveArm : public StateNode {
public:
  MoveArm() : StateNode("MoveArm"),
	      armMover(SharedObject<ArmMC>()),
	      headMover() {}
  
  virtual void DoStart() {
    addMotion(armMover);
    addMotion(headMover);
    armMover->setJoints(1.1695f, -0.0230f, -1.4459f);
    erouter->addListener(this, EventBase::sensorEGID);
  }
  
  virtual void processEvent(const EventBase&) {
    fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
    fmat::Column<3> target = fmat::SubVector<3>(&Tgripper(0,3));
    headMover->lookAtPoint(target[0],target[1],target[2]);
    if(!armMover->isDirty())
      postStateCompletion();
  }
  
protected:
  MotionPtr<ArmMC> armMover;
  MotionPtr<HeadPointerMC> headMover;
};

//! Creates the map 
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 orange_index = ProjectInterface::getColorIndex("orange");
    const int green_index = ProjectInterface::getColorIndex("green");
    const int yellow_index = ProjectInterface::getColorIndex("yellow");
    const int blue_index = ProjectInterface::getColorIndex("blue");
    
    //NEW_SHAPE(gazePt, PointData, new PointData(localShS,Point(800,390,-900,camcentric)));
    // NEW_SHAPE(gazeLine, LineData, new LineData(localShS, EndPoint(Point(800,390,-900,egocentric)), EndPoint(Point(400,390,-900,egocentric))));
    
    vector<Point> gazePts;
    gazePts.push_back(Point(220,80,-100));
    gazePts.push_back(Point(350,300,-100));
    gazePts.push_back(Point(1000,0,-100));
    gazePts.push_back(Point(350,-300,-100));
    gazePts.push_back(Point(220,-80,-100));
    gazePts.push_back(Point(300,0,-100));
    localShS.clear();
    NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, gazePts, false));

    MapBuilderRequest req(MapBuilderRequest::localMap);
    req.searchArea = gazePoly;
    req.maxDist = 1500;
    req.pursueShapes = false;
    req.motionSettleTime = 2000;
    
    req.objectColors[lineDataType].insert(pink_index);
    req.objectColors[lineDataType].insert(blue_index);
    req.occluderColors[lineDataType].insert(yellow_index);
    req.occluderColors[lineDataType].insert(green_index);
    req.occluderColors[lineDataType].insert(orange_index);
    
    req.objectColors[ellipseDataType].insert(orange_index);
    req.objectColors[ellipseDataType].insert(blue_index);
    req.objectColors[ellipseDataType].insert(pink_index);
    req.objectColors[ellipseDataType].insert(green_index);
    
    unsigned int mapreq_id = mapBuilder.executeRequest(req);
    erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::statusETID);
  }
  virtual void processEvent(const EventBase &) {
    postStateCompletion();
  }
};

class MoveEgg : public VisualRoutinesStateNode {
public: 
  MoveEgg() : VisualRoutinesStateNode("MoveEgg"),
	      armMover(SharedObject<ArmMC>()) {}
  virtual void DoStart() {
    //find all blobs and lines
    std::vector<ShapeRoot> allLines = localShS.allShapes(lineDataType);
    std::vector<ShapeRoot> allBlobs = localShS.allShapes(ellipseDataType);
    
    //calculate the gripper's coordinates in the base frame
    fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
    fmat::Column<3> target = fmat::SubVector<3>(&Tgripper(0,3));
    //point representing the center of the blob closest to the gripper:
    Point minPoint;
    //set min distance to infinity
    float minDist = INFINITY;
    bool foundBlob = false;
    bool isPinkBlob = false;
    IsColor blueTest("blue");
    IsColor pinkTest("pink");
    //find the centroid of the blob which is the minimum distance away from the arm
    for(int i = 0; i < (int)allBlobs.size(); i++){
      if(blueTest(allBlobs[i])){
	Point centroid = allBlobs[i].getData().getCentroid();
	float d_blob2arm = centroid.distanceFrom(Point(target));
	if(minDist > d_blob2arm){
	  foundBlob = true;
	  minDist = d_blob2arm;
	  minPoint = centroid;
	  isPinkBlob = false;
	}
      } else if(pinkTest(allBlobs[i])){
	Point centroid = allBlobs[i].getData().getCentroid();
	float d_blob2arm = centroid.distanceFrom(Point(target));
	if(minDist > d_blob2arm){
	  foundBlob = true;
	  minDist = d_blob2arm;
	  minPoint = centroid;
	  isPinkBlob = true;
	}
      }
    }
    //find the pink and blue line closest to the arm
    float minDistBlue = INFINITY;
    float minDistPink = INFINITY;
    Point midPointBlue;
    Point midPointPink;
    bool foundBlue = false;
    bool foundPink = false;
    for(int i = 0; i < (int)allLines.size(); i++){
      //check color first before computing distance to save time
      if(blueTest(allLines[i])){
	//note that the centroid should be the midpoint of the line
	Point midPoint = allLines[i].getData().getCentroid();
	float d_line2arm = midPoint.distanceFrom(Point(target));
	if(minDistBlue > d_line2arm){
	  foundBlue = true;
	  minDistBlue = d_line2arm;
	  midPointBlue = midPoint;
	}
      } else if (pinkTest(allLines[i])) {
	Point midPoint = allLines[i].getData().getCentroid();
	float d_line2arm = midPoint.distanceFrom(Point(target));
	if(minDistPink > d_line2arm){
	  foundPink = true;
	  minDistPink = d_line2arm;
			midPointPink = midPoint;
	}
      }
    }
    
    //check if the Pink and Blue lines exist; if not print out an error message
    if(!foundBlue){
      cout << "You tricked me!  There is no blue line  :(." << endl;
    } else if (!foundPink){
      cout << "You tricked me! There is no pink line  :(." << endl;
    } //check if the egg exists, if not print out an error message
    else if (!foundBlob){
      cout << "You tricked me!  There are no eggs  :(." << endl;
    } //if all of the lines and the egg are present then move the egg beyond the line
    else {
      bool moved = false;
      //move the arm toward the egg
      cout << "Printing out the coordinates: " << endl;
      cout << "The x coordinate of the ellipse is: " << minPoint.coordX() << endl;
      cout << "The y coordinate of the ellipse is: " << minPoint.coordY() << endl;
      cout << "The z coordinate of the ellipse is: " << minPoint.coordZ() << endl;
      //if(!armMover->isDirty())
      //postStateCompletion();
      armMover->setJoints(0.4f, -0.2f, 1.4459f);
      /*moved = armMover->moveToPoint(343.7f, 10.4f, 24.6f);
      cout << "Now moving beyond line" << endl;
      if(!moved)
	cout << "Arm did not move to egg" << endl;
      //move the egg to the centroid of the line and beyond
      if(isPinkBlob)
	moved = armMover->moveToPoint(midPointPink.coordX(), midPointPink.coordY(), midPointPink.coordZ());
      else
	moved = armMover->moveToPoint(midPointBlue.coordX(), midPointBlue.coordY(), midPointBlue.coordZ());
      if(!moved)
      cout << "Arm did not move past line" << endl;*/
      erouter->addListener(this, EventBase::sensorEGID);
    }
  }
  virtual void processEvent(const EventBase&) {
    
    if(!armMover->isDirty())
      postStateCompletion();
  }
protected:
  MotionPtr<ArmMC> armMover;
};

class SortingObjects : public VisualRoutinesStateNode {
public:
  SortingObjects(): VisualRoutinesStateNode("SortingObjects") {}
  virtual void setup() {
#statemachine
    
  startSort: StateNode
      
//set the joints of the arm
      
      //movehead: HeadPointerNode[getMC()->setMaxSpeed(0,0.2);
      //getMC()->lookInDirection(800.00f, 390.00f, -900.00f);]
      
movearm: MoveArm()
      
buildmap: BuildMap()

moveegg: MoveEgg()

startSort =N=> movearm

movearm =C=> buildmap

buildmap =C=> moveegg

#endstatemachine

startnode = startSort;
  }
};

#endif