#ifndef _TRAJECTORY_H
#define _TRAJECTORY_H

#include <ImFusion/Base/Mat.h>

#define ROBOT_SPEED 50 // Speed of robot in mm/s

/* Emits points along the path H->A->B->H where H is the initial position of the robot,
and A and B are keypoints that the robot should move between.
*/
class Trajectory
{
public:
	/* Create a new instance without initializing the trajectory */
	Trajectory();
	/* Create a new instance and initialize the trajectory with default parameters for movement interval */
	Trajectory(Eigen::Vector3d& H, Eigen::Vector3d& A, Eigen::Vector3d& B);
	/* Create a new instance and initialize the trajectory */
	Trajectory(Eigen::Vector3d& H, Eigen::Vector3d& A, Eigen::Vector3d& B, double deltaHA, double deltaAB, double deltaBH);
	/* Initialize the trajectory with default parameters for movement interval */
	void init(Eigen::Vector3d& H, Eigen::Vector3d& A, Eigen::Vector3d& B);
	/* Initialize the trajectory */
	void init(Eigen::Vector3d& H, Eigen::Vector3d& A, Eigen::Vector3d& B,
		double deltaHA, double deltaAB, double deltaBH);
	/* Whether or not the trajectory has a next location */
	bool hasNext();
	/* Get the next location in the trajectory */
	Eigen::Vector3d next();
	/* Helper method for interpolating between H and A */
	Eigen::Vector3d moveHA(double dist);
	/* Helper method for interpolating between A and B */
    Eigen::Vector3d moveAB(double dist);
	/* Helper method for interpolating between B and H */
    Eigen::Vector3d moveBH(double dist);
	/* Helper method for moving to the next position given the key points
	associated with the trajectory and the distance to be moved */
	Eigen::Vector3d move(Eigen::Vector3d& initial, Eigen::Vector3d& diff,
		Eigen::Vector3d& max, double dist, double delta, double maxDist);
	/* Get the current orientation */
	Eigen::Quaterniond getOrientation();
	/* Get the time required for the robot to move to the next point */
	double getDeltaTime();
	/* Whether or not we are at point A for first time */
	bool pointA();
	/* Whether or not we are at point B for first time */
	bool pointB();
	/* Starting point of robot */
	Eigen::Vector3d H;
	/* First key point of trajectory */
	Eigen::Vector3d A;
	/* Second key point of trajectory */
	Eigen::Vector3d B;
protected:
	/* Vector from H to A */
	Eigen::Vector3d HA;
	/* Vector from A to B */
	Eigen::Vector3d AB;
	/* Vector from B to H */
	Eigen::Vector3d BH;
	/* Current position of trajectory */
	Eigen::Vector3d currentPos;
	/* Current orientation of trajectory */
	Eigen::Quaterniond orientation;
	/* Distance from H to A */
	double distHA;
	/* Distance from A to B */
	double distAB;
	/* Distance from B to H */
	double distBH;
	/* Current distance from starting point of trajectory */
	double currentDistance;
	/* Total distance of trajectory */
	double totalDistance;
	/* Change in position when moving from H to A */
	double deltaHA;
	/* Change in position when moving from A to B */
	double deltaAB;
	/* Change in position when moving from B to H */
	double deltaBH;
	/* Last change in position */
	double distanceDelta;
	/* Small constant used to prevent floating point errors */
	double eps;
	/* Whether or not we are at an end point */
	bool endPoint;
};

#endif