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

#include "LocalizationNode.h"
#include "DualCoding/DualCoding.h"
#include "Events/DataEvent.h"
#include "Motion/MotionPtr.h"
#include "Behaviors/StateMachine.h"
#include "Planners/Navigation/ShapeSpacePlanner.h"

DATAEVENT_IMPLEMENTATION(Point, void*);

// should have provided a map already in the world space
class FollowPathNode : public StateNode {
private:
  // distance in mm that we can get within a given point and be "there"
  static const float ALLOWABLE_DISTANCE_ERROR = 50;
  static const float ALLOWABLE_ANGULAR_ERROR = 0.15;

  // access to localization node
  LocalizationNode * localizer;

  // the final target
  DualCoding::Point target;
  // path remaining to target
  std::vector<DualCoding::Point> remainingPath;

  // plan the path and fill in the remaining path
  class PlanPath : public VisualRoutinesStateNode {
  private:
    LocalizationNode *localizer;
    std::vector<DualCoding::Point> &path;
    const DualCoding::Point &target;

  public:
    PlanPath(const std::string& name, LocalizationNode *_localizer,
	     std::vector<DualCoding::Point> &_path, const DualCoding::Point &_target)
      : VisualRoutinesStateNode(name), localizer(_localizer),
	path(_path), target(_target)
    {}

    virtual void DoStart() {
      std::cout << "Planning path to point: " << target << std::endl;

      ShapeSpacePlanner planner = ShapeSpacePlanner(worldShS, localizer->getWorldBounds(), 255);

      // get current location
      DualCoding::Point position = theAgent->getCentroid();
      path = planner.planPath(position, target);

      NEW_SHAPE(plannedPath, PolygonData,
		new PolygonData(worldShS, path, false));
      plannedPath->setColor(rgb(0,0,255));

      postStateCompletion();
    }

  private:
    PlanPath(const PlanPath&); //!< don't call (copy constructor)
    PlanPath& operator=(const PlanPath&); //!< don't call (assignment operator)
  };

  // check location
  class CheckLocation : public VisualRoutinesStateNode {
  private:
    std::vector<DualCoding::Point> &path;

  public:
    CheckLocation(const std::string& name, std::vector<DualCoding::Point> &_path)
      : VisualRoutinesStateNode(name), path(_path)
    {}

    virtual void DoStart() {
      // check to see if we're close to the next waypoint
      DualCoding::Point position = theAgent->getCentroid();

      std::cout << "Checking current location: " << position << std::endl;
      std::cout << "against next point: " << path[0] << std::endl;

      while (position.xyDistanceFrom(path[0]) < ALLOWABLE_DISTANCE_ERROR) {
	std::cout << "Point " << path[0] << " reached." << std::endl;
	path.erase(path.begin(),path.begin()+1);

	if (path.size() == 0) {
	  postStateCompletion();
	  return;
	}
      }

      fmat::Column<4> pos = position.getCoords();
      fmat::Column<4> next = path[0].getCoords();

      fmat::SubVector<2> heading = fmat::SubVector<2>(next) - fmat::SubVector<2>(pos);
      AngSignPi angle = (atan2(heading[1], heading[0]) - theAgent->getOrientation());

      cout << "Heading to next point: " << heading << endl;
      // are we facing the next point?
      if (fabs(angle) > ALLOWABLE_ANGULAR_ERROR) {
	cout << "Turning to face next point!" << endl;
	
	cout << "Angle is: " << angle << endl;
	postStateSignal(WalkRequest(0, 0, 0, 0, 0, fmin(angle, 0.5)));
      } else {
	cout << "Advancing to next point!" << endl;
	heading *= fmin( 100.0/heading.norm(), 1.0 );
	postStateSignal(WalkRequest(0, heading.norm(), 0, 0, 0, 0));
      }
    }
  };

public:
  FollowPathNode(const std::string &name, LocalizationNode * _localizer)
    : StateNode(name), localizer(_localizer),
      target(), remainingPath()
  {
    if (!localizer) {
      std::cout << "Warning: localizer access pointer is null in FollowPathNode!" << std::endl;
    }
  }

  virtual void DoStartEvent(const EventBase &event)
  {
    const DataEvent<Point> *de = dynamic_cast<const DataEvent<Point>*>(&event);
    target = (de->getData());
  }

  virtual void setup() {
    MotionPtr<XWalkMC> walker;
    MotionManager::MC_ID walkID = addMotion(walker);
#statemachine
  startnode: SpeechNode($, "following path") =T(5000)=>
      plan: PlanPath($, localizer, remainingPath, target) =C=>
      check: CheckLocation($, remainingPath) =S<WalkRequest>=>
      approach: XWalkNode[setMC(walkID)] =C=>
      check =C=> SpeechNode($, "we have arrived")
            
#endstatemachine
      }

  static std::string getClassDescription() {
    return "Follow a path generated using Planners/Navigation/ShapeSpacePlanner";
  }

private:
  FollowPathNode(const FollowPathNode&); //!< don't call (copy constructor)
  FollowPathNode& operator=(const FollowPathNode&); //!< don't call (assignment operator)
};

#endif