//-*-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"
#include <math.h>

using namespace DualCoding;

//! Moves the arm and the camera to the left side
class StartPosition : public StateNode {
public:
  StartPosition() : StateNode("StartPosition"),
	      armMover(SharedObject<ArmMC>()),
	      headMover() {}
  
  virtual void DoStart() {
    addMotion(armMover);
    addMotion(headMover);
    armMover->setMaxSpeed(0, 0.6);
    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");

    //create search area polygon
    vector<Point> gazePts;
    gazePts.push_back(Point(220,80,-100));
    gazePts.push_back(Point(350,200,-100));
    gazePts.push_back(Point(350,-100,-100));
    gazePts.push_back(Point(220,-80,-100));
    gazePts.push_back(Point(400,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.numSamples = 5;
    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(blue_index);
    req.objectColors[ellipseDataType].insert(pink_index);
    req.occluderColors[ellipseDataType].insert(yellow_index);
    req.occluderColors[ellipseDataType].insert(green_index);
    req.occluderColors[ellipseDataType].insert(orange_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 StateNode, VRmixin {
public: 
  MoveEgg() : StateNode("MoveEgg"), VRmixin(),
	      armMover(){}
  virtual void DoStart() {
    //add armMover and listener for events
    addMotion(armMover);
    erouter->addListener(this, EventBase::motmanEGID, armMover->getID(), EventBase::deactivateETID);

    float lineX, lineY, lineZ, blobDiffX, blobDiffY, offsetX, offsetY;

    //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 midPointBlob, midPointBlueLine, midPointPinkLine;
    //make sure blobs and lines reasonable distance away from the gripper
    float minDistBlob = 200, minDistBlue = 650, minDistPink = 650;
    bool isPink;

    //find the centroid of the blob which is the minimum distance away from the arm
    for(int i = 0; i < (int)allBlobs.size(); i++){
      Point centroid = allBlobs[i].getData().getCentroid();
      Point gripperFrame = Point(target);
      float d_blob2arm = centroid.distanceFrom(gripperFrame);
      cout << "\ndistance from blob to arm: " << d_blob2arm << endl;
      if(minDistBlob > d_blob2arm){
	minDistBlob = d_blob2arm;
	midPointBlob = centroid;
	blobDiffX = centroid.coordX() - gripperFrame.coordX();
	blobDiffY = centroid.coordY() - gripperFrame.coordY();
	if(IsColor("blue")(allBlobs[i]))
	  isPink = false;
	else //since we only parsed two colors for ellipses other color must be pink
	  isPink = true;
      }
    }

    //find the pink and blue lines closest to the arm
    for(int i = 0; i < (int)allLines.size(); i++){
      Point centroid = allLines[i].getData().getCentroid();
      float d_line2arm = centroid.distanceFrom(Point(target));
      if(IsColor("blue")(allLines[i]) && (minDistBlue > d_line2arm)){
	minDistBlue = d_line2arm;
	midPointBlueLine = centroid;
	cout << "see blue line" << " dx: " << centroid.coordX() << " dy: " << centroid.coordY() << " dz: " << centroid.coordZ() << endl;
      }else if(IsColor("pink")(allLines[i]) && (minDistPink > d_line2arm)){
	minDistPink = d_line2arm;
	midPointPinkLine = centroid;
	cout << "see pink line" << " dx: " << centroid.coordX() << " dy: " << centroid.coordY() << " dz: " << centroid.coordZ() << endl;
      }
    }

    //check if lines and blob exist within reasonable distance
    if(minDistBlue == 650){
      cout << "You tricked me!  There is no blue line nearby :(." << endl;
      postStateCompletion();
    } else if (minDistPink == 650){
      cout << "You tricked me! There is no pink line nearby :(." << endl;
      postStateCompletion();
    } else if (minDistBlob == 200){
      cout << "You tricked me!  There is no egg in the gripper :(." << endl;
      postStateCompletion();
    }else {
      
      //move the egg to the corresponding line
      //float lineX, lineY, lineZ;
      if(isPink){ //move blob to line of same color
	lineX = midPointPinkLine.coordX();
	lineY = midPointPinkLine.coordY();
	lineZ = target[2];
      }else {
	lineX = midPointBlueLine.coordX();
	lineY = midPointBlueLine.coordY();
	lineZ = target[2];
      }
      cout << "\nChosen line: " << " dx: " << lineX << " dy: " << lineY << " dz: " << lineZ << endl;
      //find x and y coordinates of endpoint of vector (from base frame to line mid) extended 50mm up
      float absX = fabs(lineX);
      float absY = fabs(lineY);
      offsetX = 50*(absX/(absX+absY));
      offsetY = 50*(absY/(absX+absY));
      armMover->setMaxSpeed(0,0.2);
      cout << "Blob offsets: " << " dx: " << blobDiffX << " dy " << blobDiffY << endl;
      cout << "Midpt extensions: " << " dx: " << offsetX << " dy " << offsetY << endl;
      bool possible = armMover->moveToPoint(lineX-blobDiffX+offsetX, lineY-blobDiffY-offsetY-30, lineZ);
      if(!possible)
	sndman->speak("No kinematic solution, trying anyway.");
    }
  }
  virtual void processEvent(const EventBase &) {
    fmat::Transform Tgripper = kine->linkToBase(GripperFrameOffset);
    fmat::Column<3> target = fmat::SubVector<3>(&Tgripper(0,3));
    cout << "posting state completion" << endl;
    postStateCompletion();
  }
protected:
  MotionPtr<ArmMC> armMover;
};

class SortingObjects : public StateNode, VRmixin {
public:
  SortingObjects(): StateNode("SortingObjects"), VRmixin() {}

  virtual void setup() {

#statemachine
    
startSort: StateNode
      
movearm: StartPosition()

buildmap: BuildMap()
      
moveegg: MoveEgg()

wait: StateNode
      
startSort =N=> movearm
      
movearm =E(buttonEGID, RobotInfo::GreenButOffset, activateETID)=> buildmap
      
buildmap =C=> moveegg

moveegg =C=> movearm
      
#endstatemachine

startnode = startSort;
  }
};

#endif