//!
//! \file SpringSystem.cpp
//! \brief Implementation of the SpringSystem class
//!
//! \author Austin McGee amcgee@digipen.edu
//! \date April 23, 2006
//!
//! Copyright 2006 DigiPen (USA) Corporation, all rights reserved.
//!
////////////////////////////////////////////////////////////////////////////////

#include "SpringSystem.h"
#include "Vector3D.h"
#include <limits>

/*------------------------------------------------------------------------------
SpringSystem
------------------------------------------------------------------------------*/
/*!	Default constructor.  Builds a 3x3x3 cube of springs that are connected
	to their sides and diagonally.  Two root anchors are placed above the cube
	so that it can be held up and not fally continually.
*/
SpringSystem::SpringSystem( void )
{
	// color
	color_ = D3DCOLOR_XRGB( 255, 0, 0 );

	// create all of the mass points
	for ( int i = -1; i < numAnchors_ - 1; ++i )
	{
		for ( int j = -1; j < numAnchors_ - 1; ++j )
		{
			for ( int k = -1; k < numAnchors_ - 1; ++k )
			{
				anchors_[i+1][j+1][k+1].position_ = Vector3D( i*10.f, j*10.f, k*10.f );
			}
		}
	}

	// create all of the springs from the mass points
	for ( int i = 0; i < numAnchors_; ++i )
	{
		for ( int j = 0; j < numAnchors_; ++j )
		{
			for ( int k = 0; k < numAnchors_; ++k )
			{
				// now that we're inside the spring mass points, check the
				// surrounding points to try to make springs
				for ( int x = -1; x < numAnchors_ - 1; ++x )
				{
					for ( int y = -1; y < numAnchors_ - 1; ++y )
					{
						for ( int z = -1; z < numAnchors_ - 1; ++z )
						{
							// make sure we're inside the bounds of the box
							if ( i + x < 0 || i + x >= numAnchors_ ||
								 j + y < 0 || j + y >= numAnchors_ ||
								 k + z < 0 || k + z >= numAnchors_ )
								 continue;

							// don't try to make a spring with itself
							if ( x == 0 && y == 0 && z == 0 )
								continue;

							bool found = false;
							// check to see if we've already added this spring
							for ( unsigned spring = 0; spring < springs_.size(); ++spring )
							{
								if ( springs_[spring].a0 == &anchors_[i][j][k] &&
									 springs_[spring].a1 == &anchors_[i+x][j+y][k+z] ||
									 springs_[spring].a1 == &anchors_[i][j][k] &&
									 springs_[spring].a0 == &anchors_[i+x][j+y][k+z] )
								{
									found = true;
									break;
								}
							}
							// if we didn't find it, let's add it
							if ( !found )
							{
								Spring newspring;
								newspring.AddConstraint( &anchors_[i][j][k], &anchors_[i+x][j+y][k+z] );
								springs_.push_back( newspring );
							}
						}
					}
				}
			}
		}
	}

	// now let's create the root anchors
	firstRoot_.position_ = Vector3D( -5.f, 15.f, 0.f );
	firstRoot_.moveable_ = true;

	secondRoot_.position_ = Vector3D( 5.f, 15.f, 0.f );
	secondRoot_.moveable_ = true;

	// let's connect the root anchors to the cube
	for ( int i = 0; i < numAnchors_; ++i )
	{
		for ( int k = 0; k < numAnchors_; ++k )
		{
			Spring newspring, newspring2;
			// make the anchor points connect to the top of the cube
			newspring.AddConstraint( &anchors_[i][2][k], &firstRoot_ );
			newspring2.AddConstraint( &anchors_[i][2][k], &secondRoot_ );

			// push back the new springs
			springs_.push_back( newspring );
			springs_.push_back( newspring2 );
		}
	}

}

/*------------------------------------------------------------------------------
~SpringSystem
------------------------------------------------------------------------------*/
/*!	Normal destructor.
*/
SpringSystem::~SpringSystem( void )
{
	// do nothing
}

/*------------------------------------------------------------------------------
Update
------------------------------------------------------------------------------*/
/*!	Updates the spring system.

	\param time	the amount of time since the last frame passed
*/
void SpringSystem::Update( const float time )
{
	/* 
	Fs = Force of spring constant
	Ks = Spring constant
	l  = length inbetween anchor points
	r  = resting length of spring
	Fd = Dampening force
	Kd = Spring dampening
	v1 = velocity of first anchor point
	v2 = velocity of second anchor point
	L  = length vector inbetween two anchor points
	F1 = Force exerted on body 1
	*/
	Vector3D gravity( 0.f, -9.8f, 0.f );

	// calculate out all of the new forces on the springs
	for ( unsigned i = 0; i < springs_.size(); ++i )
	{
		AnchorPoint *a0 = springs_[i].a0;
		AnchorPoint *a1 = springs_[i].a1;

		float Ks = 8.0f;
		float Kd = 0.7f;
		Vector3D L = a0->position_ - a1->position_;
		float Fs = Ks * ( L.Length() - springs_[i].resting_ );
		float Fd = ( Kd / L.Length() ) * DotProduct( a0->oldDirection_ - a1->oldDirection_, L );
		float scalar = ( -( Fs + Fd ) / L.Length() );
		Vector3D F1 = L * scalar;

		a0->newDirection_ += F1;
		a1->newDirection_ += F1 * -1;
	}

	// now we need to update all of the positions
	for ( unsigned i = 0; i < numAnchors_; ++i )
	{
		for ( unsigned j = 0; j < numAnchors_; ++j )
		{
			for ( unsigned k = 0; k < numAnchors_; ++k )
			{
				if ( anchors_[i][j][k].moveable_ )
					anchors_[i][j][k].oldDirection_ = ( anchors_[i][j][k].newDirection_ + gravity ) * time;
				anchors_[i][j][k].position_ += anchors_[i][j][k].oldDirection_;
				anchors_[i][j][k].newDirection_ = Vector3D( 0.f, 0.f, 0.f );
			}
		}
	}
}

/*------------------------------------------------------------------------------
Draw
------------------------------------------------------------------------------*/
/*!	Draws all of the springs

	\param line	the line list
	\param lineCount the number of lines
*/
void SpringSystem::Draw( Point *line, unsigned &lineCount )
{
	for ( unsigned i = 0; i < springs_.size(); ++i )
	{
		line[lineCount++] = Point( springs_[i].a0->position_.x, springs_[i].a0->position_.y, springs_[i].a0->position_.z, color_ );
		line[lineCount++] = Point( springs_[i].a1->position_.x, springs_[i].a1->position_.y, springs_[i].a1->position_.z, color_ );
	}
}

/*------------------------------------------------------------------------------
MoveAnchors
------------------------------------------------------------------------------*/
/*!	Moves the two anchors.

	\param vec direction to move the anchor
	\param first which anchor to move
*/
void SpringSystem::MoveAnchors( const Vector3D vec, const bool first )
{
	// if we're moving the first anchor
	if ( first )
	{
		firstRoot_.position_ += vec;
	}
	// if we're moving the second anchor
	else
	{
		secondRoot_.position_ += vec;
	}
}