#ifndef PATHPLANNER_H
#define PATHPLANNER_H

#include <math.h>

#define DEBUG_NAVIGATION_TO_TARGET 0
#define DEBUG_NAVIGATION_FROM_BASE 0
#define DEBUG_ODOMETRY 0


class IPCInterface;
class Vector2D;
class Robot;
class EPuckBoardController;

class PathPlanner {
 public :


  PathPlanner(Robot* robot_, EPuckBoardController* boardController_, IPCInterface *IPCROSVICON);
  ~PathPlanner();

  void OnRobotPositionUpdated(); //should be called every time robot moves, recalculates internal vectors

  Vector2D* GetNavigationVectorToBase();
  Vector2D* GetNavigationVectorAwayFromBase();
  Vector2D* GetNavigationVectorToWorksite();

  bool GetIsRobotOutOfBoundaries();
  bool GetIsRobotInsideBase();

  double GetDistanceToBase();
  double GetDistanceToWorksite();
  void ForgetWorksite();
  void SetRelVectorToWorksiteFromGlobalCoords(double globalX_, double globalY_);
  bool GetHasValidData();

  Vector2D* GetRelativeVectorToBase() { return relVectorToBase; }
  Vector2D* GetRelativeVectorToWorksite() { return relVectorToWorksite; }
  Vector2D* GetGlobalPosition() { return currentGlobalPosition; }

  Vector2D* globalWorksiteCoord;

 private :

  Robot* robot;
  IPCInterface *IPCROSVICON;
  EPuckBoardController *epuckBoardController;

  Vector2D* GetNavigationVectorTo(Vector2D* targetVec_);
  Vector2D* ConvertVectorToLocalCoordinateSystem(Vector2D* vector_, bool isNavigationToTarget_);
  Vector2D* CalculateRelativeVectorTo(Vector2D* vector_);
  Vector2D* CalculateRelativeVectorTo(Vector2D* vector_, bool isNavigationToTarget_);

  Vector2D* prevGlobalPosition;
  double prevGlobalRotation;
  Vector2D* currentGlobalPosition;
  Vector2D* relAccelerationVector; // in relative local coordinates, where X is rotation, Y is movement
  Vector2D* relVectorToBase;
  Vector2D* relVectorToWorksite;

  bool rotateTowardsTargetBehindByLeft;

  int angleToTargetChangeCounter;



};

#endif
