//-*-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"

#include <string>

using namespace DualCoding;

class BothSidesNow : public StateNode, public VRmixin {
public:
  
  /* Use a MapBuilderNode to find the objects in the image */
  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, "orange");
    }
  };

  /* Get the coordinates of the center of the two objects */
  class GetCoords : public StateNode, public VRmixin {
  public:
    GetCoords() : StateNode("GetCoords"), VRmixin() {}

    virtual void DoStart() {

      /* Get all blobs from world, including landmarks */
      std::vector<Shape<BlobData> > allBlobs = select_type<BlobData>(localShS);
      
      Point bGrnObj, bOrangeObj;
      float bGrnSize = 0, bOrangeSize = 0;
      bool foundGrn = false, foundOrange = false;
      
      /* find the largest green and orange blobs */
      for(int i = 0; i < (int)allBlobs.size(); i++){
	if(IsColor("orange")(allBlobs[i])){
	  if(allBlobs[i].getData().getArea() > bOrangeSize){
	    cout << "\nFound an orange object." << endl;
	    bOrangeSize = allBlobs[i].getData().getArea();
	    bOrangeObj = allBlobs[i].getData().getCentroid();
	    foundOrange = 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 move head to. */
      float lookX = (bGrnObj.coordX() + bOrangeObj.coordX())/2.0f;
      float lookY = (bGrnObj.coordY() + bOrangeObj.coordY())/2.0f;
      float lookZ = (bGrnObj.coordZ() + bOrangeObj.coordZ())/2.0f;
      
      /* If found the objects move head to landmarks and take
       * a snapshot of the scene.  Then display image on looking glass. */
      if(foundGrn && foundOrange){
	cout << "\nFound the green and orange objects.\n" << endl;
	Point moveTo = Point(lookX, lookY, lookZ, egocentric);
	postStateSignal<Point >(moveTo);
      } else {
	cout << "\nError: could not find one/both of the objects.\n" << endl;
      }
    }
  };
    
  /* Internal class that moves the head to the center of the two objects */
  class MoveHead : public HeadPointerNode, public VRmixin, public LGmixin {
  public:
    int inst;
    float tx, ty, tz;
    MoveHead(int instg) : HeadPointerNode("MoveHead"), VRmixin(), 
			  LGmixin(), inst(instg), tx(0), ty(0), tz(0) {}

    virtual void DoStartEvent(const EventBase &event) {
      const DataEvent<Point > *datev = dynamic_cast<const DataEvent<Point >*>(&event);
      if ( datev ) {
	const Point &target = datev->getData();
	tx = target.coordX(); 
	ty = target.coordY(); 
	tz = target.coordZ();
	getMC()->lookAtPoint(tx, ty, tz);
	erouter->addListener(this, EventBase::motmanEGID, 12, EventBase::statusETID);
      }
    }
    virtual void processEvent(const EventBase &ev){
      cout << "Camera reached point.  Printing coordinates." << endl;
      cout << "ptx: " << tx << " pty: :" << ty << " ptz: " << tz << endl;
      cout << "Uploading the camera image." << endl;

      // Upload the camera image and display the image file
      if(inst){
	uploadCameraImage("point2.jpg");
	displayImageFile("point2.jpg");
      } else {
	uploadCameraImage("point1.jpg");
	displayImageFile("point1.jpg");
      }
      /* Set the x and y velocities to 20 mm */
      float xVel = 20;
      float yVel = 20;
      float sum = abs(tx)+abs(ty);

      /* Add weighted 550 cm to the x and y coordinates to move past the object */
      tx += 550*(tx/sum);
      ty += 550*(ty/sum);

      /* If already on other side, just post state completion.  Otherwise post a walk request */
      if(inst){
	postStateCompletion();
      } else {
	WalkRequest req(xVel, tx, yVel, ty, 0, 0);
	postStateSignal<WalkRequest>(req);
      }
    }
  };
   
  /* Internal class to display an HTML file showing both of the images */
  class DisplayHTML : public StateNode, public VRmixin, public LGmixin {
  public:
    DisplayHTML() : StateNode("DisplayHTML"), VRmixin(), 
			     LGmixin() {}
    virtual void DoStart() {
      /* Display an HTML file showing two images */
      displayHtmlText("<html><body><p>Test showing two images</p>"
		      "<img src=\"point1.jpg\"><img src=\"point2.jpg\">"
		      "</body></html>");
    }
  };

  BothSidesNow() :  StateNode("BothSidesNow"), VRmixin() {}
  virtual void setup() {
    MotionPtr<XWalkMC> walker;
    addMotion(walker);
    MotionManager::MC_ID walkID = walker->getID();
#statemachine
  startnode: FindObjects() =MAP=> 
      GetCoords() =S<Point >=>
      MoveHead(0) =S<WalkRequest >=>
      XWalkNode =C=>
      XWalkNode($, 0, 0, 0, 0, 0.1,-3.14)[setMC(walkID);] =C=>
      FindObjects() =MAP=> 
      GetCoords() =S<Point >=>
      MoveHead(1) =C=>
      DisplayHTML()
#endstatemachine
      }
};

/* This is needed for passing points as state signals. */
DATAEVENT_IMPLEMENTATION(Point, unsigned int);

#endif