/* Edge.cpp
 *
 * Definition of the Edge class.
 */

#include "Edge.h"
#include <cmath>
#include <GL/glut.h>
#include <assert.h>

Edge::Edge(Vertex* v1, Vertex* v2)
: Obstruction(),
  v1_(v1),
  v2_(v2)
{
	// Make sure pointers are non-null
	assert(v1_ && v2_);

	// Relative vectors for each of the vertices (i.e., distances from v1)
	Tuple rv1 = Tuple();
	Tuple rv2 = Tuple(v2_->v() - v1_->v());

	// Find the standardized vertices
	sv1_ = Tuple();
	sv2_ = Tuple(rv2.length(), 0.0, 0.0);

	// Record the direction for later use in computing normals
	direction_ = rv2.norm();

	/* We must now calculate the rotation matrix used to transform paths when
	 * testing for intersection.  We will do this by composing two rotations
	 * about the axes (or one in the case that rv2 is straight up).  After
	 * computing each rotation, we'll transform the edge's vertices to make it
	 * easier to compute the next rotation.
	 */

	// Check if rv2 is straight up; if so, we shouldn't rotate on the y-axis
	Tuplex yRotation;
	Tuple yv1;
	Tuple yv2;
	if (rv2.x() != 0.0 || rv2.z() != 0.0)
	{
		// Compute a and b, where a + bi is a unit complex constant of rotation
		//   for rotating rv2 such that it's in the xy-plane
		double a1 =
			Tuple(1.0, 0.0, 0.0).dot(Tuple(rv2.x(), 0.0, rv2.z()).norm());
		double b1 =
			Tuple(0.0, 0.0, 1.0).dot(Tuple(rv2.x(), 0.0, rv2.z()).norm());

		// Define a matrix to compute complex multiplcation (a + bi) * (x + zi)
		yRotation = Tuplex(
			Tuple(a1, 0.0, b1),
			Tuple(0.0, 1.0, 0.0),
			Tuple(-b1, 0.0, a1));

		// Rotate the edge about the y-axis
		yv1 = yRotation * rv1;
		yv2 = yRotation * rv2;
	}
	else
	{
		// No rotation necessary
		yRotation = Tuplex(
			Tuple(1.0, 0.0, 0.0),
			Tuple(0.0, 1.0, 0.0),
			Tuple(0.0, 0.0, 1.0));

		yv1 = rv1;
		yv2 = rv2;
	}

	// Compute a and b, where a + bi is a unit complex constant of rotation
	//   for rotating rv2 such that it's along the x-axis
	// Note, this rotaion should be negative, so negate b
	double a2 =
		Tuple(1.0, 0.0, 0.0).dot(Tuple(yv2.x(), yv2.y(), 0.0).norm());
	double b2 =
		-Tuple(0.0, 1.0, 0.0).dot(Tuple(yv2.x(), yv2.y(), 0.0).norm());

	// Define a matrix to compute complex multiplcation (a + bi) * (x + yi)
	Tuplex zRotation(
		Tuple(a2, -b2, 0.0),
		Tuple(b2, a2, 0.0),
		Tuple(0.0, 0.0, 1.0));

	// Compose the rotations
	rotation_ = zRotation * yRotation;
}

Edge::~Edge()
{
	// Nothing to do
}

/* draw
 *
 * This is here just in case we ever decide to draw edges.
 */
void Edge::draw(bool light) const
{
	(void)light; // Don't use lighting setting b/c there is nothing to draw
}

/* pathIntersect
 *
 * Determines if this edge is on the given path and returns the Beta value,
 * (the percent of path completed before collision).
 */
const double Edge::pathIntersect(
	const Path& path,
	double radius) const
{
	/* Here we'll use the quadratic equation to determine if and where the ball
	 * collides with the edge.  Actually, we're checking whether the center
	 * of the ball intersects a cyllinder about the x-axis with a radius
	 * equal to the ball's radius plus a BUFFER.
	 */

	// Standardize input path to match standardized edge vertices
	Path sPath = path.translate(sv1_ - v1_->v()).rotate(rotation_);

	// Define a few helpful variables to clarify the following code
	double y1 = sPath.endPos().y();
	double z1 = sPath.endPos().z();
	double y0 = sPath.begPos().y();
	double z0 = sPath.begPos().z();

	// Make sure the ball is not moving parallel to the line
	if (y0 != y1 || z0 != z1)
	{
		// We'll fill this with the distance along the path where the ball
		//   touches the buffer about the x-axis
		double beta;

		// If the ball starts in the buffer, collide instantly; otherwise
		//   just collide when the ball reaches the buffer
		double r0 = sqrt(y0 * y0 + z0 * z0);
		if (r0 < radius + BUFFER)
		{
			// Define variables for the quadratic equation
			double a =
				(y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0);
			double b =
				2 * (y0 * (y1 - y0) + z0 * (z1 - z0));
			double c =
				y0 * y0 + z0 * z0 - radius * radius;

			// Check the determinant; if negative, then no collision
			double det = b * b - 4 * a * c;
			if (det >= 0.0)
			{
				// Collide no later than now (0.0)
				beta = min((-b - sqrt(det)) / (2 * a), 0.0);
			}
			else
			{
				// Add EPSILON to err on the safe side (no collision)
				beta = 1.0 + EPSILON;
			}
		}
		else
		{
			// Define variables for the quadratic equation
			double a =
				(y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0);
			double b =
				2 * (y0 * (y1 - y0) + z0 * (z1 - z0));
			double c =
				y0 * y0 + z0 * z0 - (radius + BUFFER) * (radius + BUFFER);

			// Check the determinant; if negative, then no collision
			double det = b * b - 4 * a * c;
			if (det >= 0.0)
			{
				// We only care about lowest beta, so consider only -sqrt(det)
				beta = (-b - sqrt(det)) / (2 * a);
			}
			else
			{
				// Add EPSILON to err on the safe side (no collision)
				beta = 1.0 + EPSILON;
			}
		}

		// Make sure beta is in range
		if (beta >= 0.0 && beta < 1.0)
		{
			// Find where the ball touches the x-axis
			Tuple touchPos =
				sPath.begPos() + beta * (sPath.endPos() - sPath.begPos());

			// Check if the path moves through the edge
			if (touchPos.x() > 0 && touchPos.x() < sv2_.x())
			{
				return beta;
			}
		}
	}

	// Add EPSILON to err on the safe side (no collision)
	return 1.0 + EPSILON;
}

/* calcNormal
 *
 * Returns the normal of the given edge relative to a given point, which is the
 * direction of the line perpendicular to the edge that goes through the point.
 */
const Tuple Edge::calcNormal(const Tuple& point) const
{
	return direction_.cross(point - v1_->v()).cross(direction_).norm();
}