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


class BothSidesNow : public VisualRoutinesStateNode {
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, "blue");
      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 VisualRoutinesStateNode, public LGmixin {
  public:
    DisplayLGSoft () : VisualRoutinesStateNode("DisplayLGSoft"),
         LGmixin(), headMover() {}

    virtual void DoStart() {
      addMotion(headMover);

      /* Get all blobs from world, including landmarks */
      std::vector<Shape<BlobData> > allBlobs = select_type<BlobData>(localShS);
      
      Point bGrnObj, bOrngeObj;
      float bGrnSize = 0, bOrngeSize = 0;
      bool foundGrn = false, foundOrnge = 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() > bGrnSize){
	    cout << "\nFound a pink object." << endl;
	    bGrnSize = allBlobs[i].getData().getArea();
	    bGrnObj = allBlobs[i].getData().getCentroid();
	    foundGrn = true;
	  }
	}
	else if(IsColor("blue")(allBlobs[i])){
	  if(allBlobs[i].getData().getArea() > bOrngeSize){
	    cout << "\nFound a blue object." << endl;
	    bOrngeSize = allBlobs[i].getData().getArea();
	    bOrngeObj = allBlobs[i].getData().getCentroid();
	    foundOrnge = 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. */
      float lookX = (bGrnObj.coordX() + bOrngeObj.coordX())/2.0f;
      float lookY = (bGrnObj.coordY() + bOrngeObj.coordY())/2.0f;
      float lookZ = (bGrnObj.coordZ() + bOrngeObj.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 && foundOrnge){
	cout << "\nFound the blue 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("ms/lg/pic1.JPEG");
      }else {
	cout << "\nError: could not find one/both of the objects.\n" << endl;
	postStateCompletion();
	return;
      }

      cout << "Moving to points x, y and z." << endl;

      /* create point referencing the center of the two objects */
      Point moveTo = Point(lookX+250, lookY, lookZ, egocentric);
      
      /* Post state signal indicating point to move to */
      postStateSignal<Point>(moveTo);        
    }
 protected:
  MotionPtr<HeadPointerMC> headMover;
  };

  class GoToPoint : public StateNode {
  public:
    GoToPoint() : StateNode("GoToPoint") {}
    
    virtual void DoStartEvent(const EventBase &event) {
      const DataEvent<Point > *datev = dynamic_cast<const DataEvent<Point >*>(&event);
      if(datev){
	// get target point
	const Point &targetPoint = datev->getData();

	//check to make sure points correct
	cout << "x coord: " << targetPoint.coordX() << " y coord: " << targetPoint.coordY() << endl;

	//create PointData
	const PointData targetPointData(localShS, targetPoint);

	//create Shape of PointData to be used for the Pilot request
	Shape<PointData> targetShape(localShS, targetPointData);

	//request to move to the shape
	PilotRequest req(PilotRequest::gotoShape);

	req.targetShape = targetShape;
	
	unsigned int pilot_id = pilot.executeRequest(req);
	cout << "the pilot id: " << pilot_id << endl;
	//I tried:
	//erouter->addListener(this, EventBase::motmanEGID, pilot_id, EventBase::statusETID);
	//erouter->addListener(this, EventBase::locomotionEGID);
	//and various derivations thereof
      }
    }
    virtual void processEvent(const EventBase &ev){
      cout << "finished class id: " << ev.getClassTypeID() << endl;
      cout << "Finished walking to objects." << endl;
      //postStateCompletion();
    }
   };





  /*
  class GoToPoint : public WaypointWalkNode {
  public:
    GoToPoint() : WaypointWalkNode("GoToPoint") {}
    
    virtual void DoStartEvent(const EventBase &event) {
      const DataEvent<Point > *datev = dynamic_cast<const DataEvent<Point >*>(&event);
      if(datev){
	// get target point
	const Point &targetLoc = datev->getData();

	erouter->addListener(this,EventBase::locomotionEGID, EventBase::statusETID);

	getMC()->getWaypointList().clear();

	//check to make sure points correct
	cout << "x coord: " << targetLoc.coordX() << " y coord: " << targetLoc.coordY() << endl;

	AngSignPi bearing = atan2(targetLoc.coordY(),targetLoc.coordX());
	float const distance = targetLoc.distanceFrom(Point(0,0));
	cout << "Pilot egocentric gotoShape: rotate by " << float(bearing)*180/M_PI << " deg., then forward "
	     << distance << " mm" << endl;
	getMC()->addEgocentricWaypoint(0,0,bearing,true,0.001);
	getMC()->addEgocentricWaypoint(distance/1000,0,0,true,0.02);  // convert distance from mm to meters
	getMC()->go();
	while(getMC()->isActive())
	  cout << "active" << endl;
	cout << "done" << endl;
      }
    }
      virtual void processEvent(const EventBase &ev){
      cout << "finished class id: " << ev.getClassTypeID() << endl;
      cout << "Finished walking to objects." << endl;
      //postStateCompletion();
      }
      };*/



  /*class TurnAround : public XWalkNode {
  public:
    TurnAround() : XWalkNode("TurnAround", 1, 1, 3) {}
    virtual void DoStart() {
      cout << "Turning around" << endl;
      getMC()->setTargetVelocity(1,1,3);
    }
    };*/


  /*
  class GoToPoint : public XWalkNode {
  public:
    GoToPoint() : XWalkNode("GoToPoint",0,0,0) {}

    virtual void DoStartEvent(const EventBase &event) {
      const DataEvent<Point > *datev = dynamic_cast<const DataEvent<Point >*>(&event);
      if ( datev ) {
	const Point &target = datev->getData();
       
	const float tx = target.coordX(), ty = target.coordY();
	
	const float robotDiameter = 25.4 * 14;  // 14 inch robot diameter in mm
	
	const float distance = sqrt(tx*tx + ty*ty);
	
	const float bearing = atan2(ty,tx);
	
	cout << "Target=" << target << "   Distance=" << distance
	     << " mm,    Bearing=" << bearing*180/M_PI << " deg\n";
	getMC()->setTargetDisplacement(distance-robotDiameter, 0, bearing, 20);
      }
    }
    };*/



    /*
    MapBuilderRequest req(MapBuilderRequest::localMap);
    req.searchArea = gazePt;
    req.maxDist = 1500;
    req.pursueShapes = true;
		req.numSamples = 5;
		req.motionSettleTime = 2000;

    req.objectColors[lineDataType].insert(yellow_index);
    req.occluderColors[lineDataType].insert(blue_index);
    req.occluderColors[lineDataType].insert(pink_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 SeeAndSend : public VisualRoutinesStateNode, public LGmixin {
  public:
    SeeAndSend () : VisualRoutinesStateNode("SeeAndSend"),
         LGmixin(), headMover() {}

    virtual void DoStart() {
      /* Point head at the target position 
      headMover->lookAtPoint(lookX, lookY, lookZ);
      /* Take a picture and send the image 
      uploadCameraImage("ms/lg/pic1.JPEG");
    }
 protected:
  MotionPtr<HeadPointerMC> headMover;
  };  
*/

  BothSidesNow() :  VisualRoutinesStateNode("BothSidesNow") {}
  
  virtual void setup() {
#statemachine
  startnode: FindObjects() =MAP=> 
      DisplayLGSoft() =S<Point >=> 
      GoToPoint() =C=> FindObjects()
#endstatemachine
      }
};

DATAEVENT_IMPLEMENTATION(Point, unsigned int);

#endif