//-*-c++-*-
#ifndef INCLUDED_BothSidesNow_h_
#define INCLUDED_BothSidesNow_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 "Wireless/LGmixin.h"
#include "Sound/SoundManager.h"

using namespace DualCoding;

static float lookX = 0;
static float lookY = 0;
static float lookZ = 0;
static float xVel = 0;
static float yVel = 0;

class BothSidesNow : public StateNode, public VRmixin {
public:
  
  class FindObjects : public MapBuilderNode {
  public:
    FindObjects() : MapBuilderNode("FindObjects",MapBuilderRequest::localMap) {}
    
    virtual void DoStart() {

      /* Create polygon area to look at */
      vector<Point> gazePts;
      gazePts.push_back(Point(200,-10,-100));
      gazePts.push_back(Point(1000,0,-100)); //objects assumed to be in front of Chiara
      gazePts.push_back(Point(220,-10,-100));
      localShS.clear();
      NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, gazePts, false));

      mapreq.maxDist = 2500;
      mapreq.searchArea = gazePoly;
      mapreq.pursueShapes = false;
      mapreq.numSamples = 5;
      mapreq.motionSettleTime = 2000;
      mapreq.addObjectColor(blobDataType, "green");
      mapreq.addObjectColor(blobDataType, "pink");
    }
  };

  /* This node finds the centroids of the objects which, since they are above the ground,
   * will indicate a point that is farther away in space than the two individual objects. */
  class DisplayLGSoft : public StateNode, public VRmixin, public LGmixin {
  public:
    DisplayLGSoft () : 
      StateNode("DisplayLGSoft"), VRmixin(), LGmixin(), headMover(), walker() {}
    
    virtual void DoStart() {
      addMotion(headMover);
      addMotion(walker);

      /* Get all blobs from world, including landmarks */
      std::vector<Shape<BlobData> > allBlobs = select_type<BlobData>(localShS);
      
      Point bGrnObj, bPinkObj;
      float bGrnSize = 0, bPinkSize = 0;
      bool foundGrn = false, foundPink = false;
      
      /* find the largest green and orange blobs */
      for(int i = 0; i < (int)allBlobs.size(); i++){
	if(IsColor("pink")(allBlobs[i])){
	  if(allBlobs[i].getData().getArea() > bPinkSize){
	    cout << "\nFound a pink object." << endl;
	    bPinkSize = allBlobs[i].getData().getArea();
	    bPinkObj = allBlobs[i].getData().getCentroid();
	    foundPink = true;
	  }
	}
	else if(IsColor("green")(allBlobs[i])){
	  if(allBlobs[i].getData().getArea() > bGrnSize){
	    cout << "\nFound a green object." << endl;
	    bGrnSize = allBlobs[i].getData().getArea();
	    bGrnObj = allBlobs[i].getData().getCentroid();
	    foundGrn = true;
	  }
	}
      }
      
      /* Average the coordinates to find the new location to go to.
       * Note that since the objects are assumed to be a good height
       * above the ground there is no need to add a greater distance. */
      lookX = (bGrnObj.coordX() + bPinkObj.coordX())/2.0f;
      lookY = (bGrnObj.coordY() + bPinkObj.coordY())/2.0f;
      lookZ = (bGrnObj.coordZ() + bPinkObj.coordZ())/2.0f;
      xVel = 20;
      yVel = 20;
      
      /* If found the objects move head to landmarks and take
       * a snapshot of the scene.  Then display image on looking glass. */
      if(foundGrn && foundPink){
	cout << "\nFound the green and pink objects.\n" << endl;
	headMover->lookAtPoint(lookX, lookY, lookZ);
	while(headMover->isDirty()){
	  //cout << "camera is dirty" << endl;
	}
	cout << "Camera reached point.  Printing coordinates." << endl;
	cout << "ptx: " << lookX << " pty: :" << lookY << " ptz: " << lookZ << endl;
	cout << "Uploading the camera image." << endl;
	uploadCameraImage("pic1.JPEG");
      }else {
	cout << "\nError: could not find one/both of the objects.\n" << endl;
      }

      cout << "Setting to absolute values... " << endl;
      lookX = abs(lookX);
      lookY = abs(lookY);
      lookZ = abs(lookZ);

      cout << "Moving to points x, y and z." << endl;
      cout << "x: " << lookX << " y: " << lookY << " z: " << lookZ << endl;
      cout << " x vel: " << xVel << " y vel: " << yVel << endl;

      
	
    }
  protected:
    MotionPtr<HeadPointerMC> headMover;
    MotionPtr<XWalkMC> walker;
  };

  BothSidesNow() :  StateNode("BothSidesNow"), VRmixin() {}
  
  virtual void setup() {
    MotionPtr<XWalkMC> walker;
    addMotion(walker);
    MotionManager::MC_ID walkID = walker->getID();
    cout << "Walker ID is: " << walkID << endl;

#statemachine
  startnode: FindObjects() =MAP=> 
  DisplayLGSoft()

#endstatemachine
				}
};

#endif