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

#include "Ball.h"
#include "World.h"
#include <cmath>
#include <iostream>
#include <GL/glut.h>

using namespace std;

// Lighting Constants
const float Ball::SHININESS = 80.0;
const float Ball::SPECULAR_COLOR[] = {1.0,1.0,1.0,1.0};

// Physics Constants
const double Ball::STOP_DISTANCE = 0.05;
//const double Ball::ROLL_VELOCITY = 0.2;
const double Ball::ROLL_DISTANCE = 0.05;
const double Ball::COLLISION_TIME = 0.001;

const double Ball::DEF_RADIUS = 0.25;

const double Ball::DEF_MIN_Y = -100;

Ball::Ball(double radius, Tuple position, Tuple velocity, Tuple color)
: radius_(radius),
  position_(position),
  velocity_(velocity),
  color_(color),
  rolls_(),
  history_(),
  stopped_(false),
  rollDebug_(false),
  MIN_Y(DEF_MIN_Y)
{
	lastStopPos_ = position_;
}

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

/* draw
 *
 * Draw a sphere representing that ball at the ball's position.
 */
void Ball::draw(bool light) const
{
	glPushMatrix();
	glTranslatef(position_.x(), position_.y(), position_.z());
	if (light) // light is on
	{
		GLfloat matShininess[] = {SHININESS};
		GLfloat ambNDiff[] = {color_.x(), color_.y(), color_.z(), 1.0};
		glMaterialfv(GL_FRONT, GL_SPECULAR, SPECULAR_COLOR);
		glMaterialfv(GL_FRONT, GL_SHININESS, matShininess);
		glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, ambNDiff);
	}
	else // light is off
	{
		glColor3f(color_.x(), color_.y(), color_.z());
	}

	glutSolidSphere(radius_, BALL_DETAIL, BALL_DETAIL);
	glPopMatrix();
}

/* calcPath
 *
 * Determine the ball's new position after small time step dt.  This function
 * is not responsible for handling collisions, but is used by the collision
 * handler.
 */
const Path Ball::calcPath(double dt) const
{
	return Path(position_, position_ + dt * velocity_);
}

/* clearAll
 *
 * Empty set of existing rolls and historoy.  Used when reloading the world and
 * when swinging the golf club.
 */
void Ball::clearAll()
{
	rolls_.clear();
	history_.clear();
}

/* checkStopped
 *
 * Check to see if the ball's position has changed significantly over the last
 * several time steps; if not, set the velocity to zero.  This function should
 * only be called once per timestep.
 */
const bool Ball::checkStopped()
{
	if (stopped_)
		return true;

	if (history_.size() >= STOP_CHECKS && rolls_.size() > 0)
	{
		// Check if the ball has moved significantly in its recent trajectory
		bool moved = false;
		for (list<Tuple>::const_iterator i = history_.begin();
			 i != history_.end(); ++i)
		{
			if ((position_ - (*i)).length() > STOP_DISTANCE)
				moved = true;
		}

		if (!moved)
		{
			if (rollDebug_)
				cout << "Stopped" << endl;

			// Record the stopped status and do housekeeping
			stopped_ = true;
			velocity_ = Tuple();
			lastStopPos_ = position_;
			return true;
		}
	}

	// Add the current position to the list
	history_.push_front(position_);

	// Trim the list down to standard size
	while (history_.size() > STOP_CHECKS)
		history_.pop_back();

	return false;
}

/* checkRolling
 *
 * Test a set of obstructions (and the existing set of rolls) for rolling.  If
 * any changes in rolling are found, rolls_ is updated and the ball's velocity
 * is adjusted.  This function assumes that the ball is in contact with all
 * listed obstructions (it should be called after collisions are detected with
 * the listed obstructions, or with no input to test the existing rolls).
 */
const bool Ball::checkRolling(const vector<Obstruction*>& testRolls)
{
	// Collect all potential rolls into one vector
	vector<Obstruction*> temp = rolls_;
	temp.insert(temp.end(), testRolls.begin(), testRolls.end());

	// Clear out the old rolls (they may come right back in the next step)
	rolls_.clear();

	// Check for new rolls
	for (vector<Obstruction*>::const_iterator i = temp.begin();
		 i != temp.end(); ++i)
	{
		Tuple normal = (*i)->calcNormal(position_);

		// Check if the ball has moved significantly away from the obstruction
		//   in its recent trajectory
		bool moved = false;
		for (list<Tuple>::const_iterator j = history_.begin();
			 j != history_.end(); ++j)
		{
			if ((*i)->pathIntersect(
				 Path(*j, *j - normal * ROLL_DISTANCE),
				 radius_) > 1.0)
				moved = true;
		}

		if (!moved)
		{
			if (velocity_.dot(normal) < 0.0)
				velocity_ -= normal * velocity_.dot(normal);
			rolls_.push_back(*i);
		}
	}

	return (rolls_.size() > 0);
}

const bool Ball::checkBottomedOut()
{
	if (position_.y() < MIN_Y)
	{
		stopped_ = true;
		position_ = lastStopPos_;
		velocity_ = Tuple(); // reset velocity
		clearAll(); // reset rolling and history
		cout << "You have gone off the edge!\n"
			 << "I will move you at your previous location.\n\n";
		return true;
	}
	return false;
}

/* doCollision
 *
 * Find the average normal of the given obstructions and reflect the ball's
 * velocity through it.  Also, enforce damping and friction.  This function
 * assumes that the ball is properly positioned (at the point of collilsion).
 */
void Ball::doCollision(const vector<Obstruction*>& collisions)
{
	// First check whether the ball has collided with anything
	if (collisions.size() > 0)
	{
		// Calculate the effective normal of all collided objects, and
		//   simultaneously find the greatest encountered damping and friction
		double maxDamping = 0.0;
		double maxFriction = 0.0;
		Tuple normal;
		for (vector<Obstruction*>::const_iterator i = collisions.begin();
			 i != collisions.end(); ++i)
		{
			normal += (*i)->calcNormal(position());
			if ((*i)->damping() > maxDamping)
				maxDamping = (*i)->damping();
			if ((*i)->friction() > maxFriction)
				maxFriction = (*i)->friction();
		}
		normal /= collisions.size();

		// Redirect velocity
		velocity_ -= 2 * (normal * velocity_.dot(normal));

		// Implement damping
		Tuple normalV = normal * velocity_.dot(normal);
		velocity_ =
			(1.0 - maxDamping) * normalV +
			pow(1.0 - maxFriction, COLLISION_TIME) * (velocity_ - normalV);

		// Check for rolling against newly collided objects
		if (checkRolling(collisions))
		{
			if (rollDebug_)
				cout << "Rolling" << endl;
		}
		else
		{
			if (rollDebug_)
				cout << "Not rolling" << endl;
		}
	}
}

/* doFriction
 *
 * Respond to friction due to rolling.  This function finds the greatest
 * friction acting on the ball, raises it to the power of time, and then
 * adjusts the ball's velocity.  This function assumes that the ball is
 * properly positioned and that it's velocity is appropriate for rolling.
 */
void Ball::doFriction(double dt)
{
	// First check whether the ball is rolling
	if (rolls_.size() > 0)
	{
		// Determine the greatest friction acting on the ball
		double maxFriction = 0.0;
		for (vector<Obstruction*>::const_iterator i = rolls_.begin();
			 i != rolls_.end(); ++i)
		{
			if ((*i)->friction() > maxFriction)
				maxFriction = (*i)->friction();
		}

		// Adjust the veolicty (note that this velocity *should* be
		//   perpendicular to the normals of the obstructions above)
		velocity_ *= pow(1.0 - maxFriction, dt);
	}
}

/* impulse
 *
 * Accelate the ball over dt, taking rolling into account.  If the ball is
 * rolling on any surface, any acceleration in opposition of the normals will
 * be ignored.
 */
void Ball::impulse(double dt, const Tuple& acceleration)
{
	// Clear the stopped status
	if (stopped_)
	{
		stopped_ = false;
		history_.clear();
	}

	// Calculate the change in velocity
	velocity_ += dt * acceleration;

	// For each of the obstructions the ball is rolling on, cancel any changes
	//   in velocity directed into the obstruction
	for (vector<Obstruction*>::const_iterator i = rolls_.begin();
		 i != rolls_.end(); ++i)
	{
		Tuple normal = (*i)->calcNormal(position_);
		if (velocity_.dot(normal) < 0.0)
			velocity_ -= normal * velocity_.dot(normal);
	}
}