#include "Trajectory.h"

#include <algorithm>

Trajectory::Trajectory() {

}

Trajectory::Trajectory(Eigen::Vector3d& H, Eigen::Vector3d& A, Eigen::Vector3d& B) {
	init(H, A, B);
}

Trajectory::Trajectory(Eigen::Vector3d& H, Eigen::Vector3d& A, Eigen::Vector3d& B, double deltaHA, double deltaAB, double deltaBH) {
	init(H, A, B, deltaHA, deltaAB, deltaBH);
}

void Trajectory::init(Eigen::Vector3d& H, Eigen::Vector3d& A, Eigen::Vector3d& B) {
	init(H, A, B, 10, 2, 10);
}

void Trajectory::init(Eigen::Vector3d& H, Eigen::Vector3d& A, Eigen::Vector3d& B, double deltaHA, double deltaAB, double deltaBH) {
	this->H = H;
	this->A = A;
	this->B = B;
	this->deltaHA = deltaHA;
	this->deltaAB = deltaAB;
	this->deltaBH = deltaBH;

	HA = A - H;
	AB = B - A;
	BH = H - B;

	currentDistance = 0;
	distHA = HA.norm();
	distAB = AB.norm();
	distBH = BH.norm();
	totalDistance = distHA + distAB + distBH;

	currentPos = H;

	// Angle between HA vector and xy plane
	double alpha = atan2(A.y() - B.y(), A.x() - B.x());

	// Set orientation of robot during trajectory
	double zRotation = alpha;
	double yRotation = 0;
	double xRotation = M_PI; // 180 degrees

	// Create quaternion from unit angles
	Eigen::AngleAxisd zAngle(zRotation, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd yAngle(yRotation, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd xAngle(xRotation, Eigen::Vector3d::UnitX());

	// Robot orientation should point in the direction of HA vector
	orientation = zAngle*yAngle*xAngle;

	// constant that prevents floating point errors from affecting proximity calculations
	eps = 0.01;

	endPoint = false;
}

bool Trajectory::hasNext() {
	return currentDistance < totalDistance;
}

Eigen::Vector3d Trajectory::next() {
	// Obtain the points which the current location of the robot
	// is in between based on the current distance
	double dist = currentDistance;

	// Check if we are past point A
	if (dist >= distHA) {
		dist -= distHA;
		// Check if we are past point B
		if (dist >= distAB) {
			dist -= distAB;
			// Check if we have reached point H again
			if (dist >= distBH) {
				currentDistance = totalDistance + eps;
				return H;
			} else {
				return moveBH(dist);
			}
		} else {
			return moveAB(dist);
		}
	} else {
		return moveHA(dist);
	}
}

Eigen::Vector3d Trajectory::moveHA(double dist) {
	return move(H, HA, A, dist, deltaHA, distHA);
}

Eigen::Vector3d Trajectory::moveAB(double dist) {
	return move(A, AB, B, dist, deltaAB, distAB);
}

Eigen::Vector3d Trajectory::moveBH(double dist) {
	return move(B, BH, H, dist, deltaBH, distBH);
}

Eigen::Vector3d Trajectory::move(Eigen::Vector3d& initial, Eigen::Vector3d& diff,
  Eigen::Vector3d& max, double dist, double delta, double maxDist) {
	// New distance which robot will move to, unless it exceeds max
	// distance of this interval in which case we move to end point
	double newDist = dist + delta;

	if (newDist < maxDist) {
		// Interpolate betwween start and end points
	    distanceDelta = newDist - dist;
	    currentDistance += distanceDelta;
	    double interp = dist/maxDist;
	    currentPos = initial+interp*diff;
		endPoint = false;
	} else {
		// Move directly to end point
		distanceDelta = maxDist - dist;
		currentDistance += distanceDelta;
		currentDistance += eps;
		currentPos = max;
		endPoint = true;
	}

	// New position of robot
	return currentPos;
}

Eigen::Quaterniond Trajectory::getOrientation() {
	return orientation;
}

double Trajectory::getDeltaTime() { // get the time required for the robot to move to the next position in seconds
	return distanceDelta/ROBOT_SPEED;
}

bool Trajectory::pointA() {
	return endPoint && (currentDistance >= distHA) && (currentDistance < distHA + distAB);
}

bool Trajectory::pointB() {
	return endPoint && (currentDistance >= distHA + distAB) && (currentDistance < totalDistance);
}