/// Copyright (c) 2010 Ruben Penalva. All rights reserved.
/// Redistribution and use in source and binary forms, with or without modification, 
/// are permitted provided that the following conditions are met:
///    1. Redistributions of source code must retain the above copyright notice, this 
///    list of conditions and the following disclaimer.
///    2. Redistributions in binary form must reproduce the above copyright notice,
///    this list of conditions and the following disclaimer in the documentation and/or
///    other materials provided with the distribution.
///    3. The name of the author may not be used to endorse or promote products derived
///    from this software without specific prior written permission.
///
/// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES,
/// INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 
/// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, 
/// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 
/// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
/// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
/// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 
/// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "FlockTask.h"

#include "Matrix4x4.h"

#include <math.h>
#include <boost\math\constants\constants.hpp>

namespace
{
	bool IsInNeighborhood(const TestFrameWork::Vector3& position, const TestFrameWork::Vector3& direction, 
						  const TestFrameWork::Vector3& neighborPosition, float maxDistance, float visionAngle)
	{
		TestFrameWork::Vector3 p(position - neighborPosition);
		float pLength = p.Length();
		if (pLength > maxDistance)
		{
			return false;
		}

		return acosf(direction.Dot(p) / (direction.Length() * pLength)) < visionAngle;
	}

	TestFrameWork::Vector3 EvaluateTrefoilKnot(float t)
	{
		return TestFrameWork::Vector3((2 + cosf(t * 3.0f / 2.0f)) * cosf(t) * 10.0f,
									  (2 + cosf(t * 3.0f / 2.0f)) * sinf(t) * 10.0f,
									  sinf(t * 3.0f / 2.0f) * 10.0f);
	}
}

using namespace TaskBasedFlockSim;


FlockTask::FlockTask(const Flock& flock, Flock& flockBuffer, 
					 size_t begin, size_t end, float elapsedTime)	:	m_flock(flock), m_flockBuffer(flockBuffer),
																		m_begin(begin), m_end(end), 
																		m_elapsedTime(elapsedTime)
{
}

FlockTask::~FlockTask(void)
{
}

void FlockTask::DoWork(void)
{
	const Positions& oldPositions = m_flock.GetPositions();
	const Velocities& oldVelocities = m_flock.GetVelocities();
	const Angles& oldAngles = m_flock.GetAngles();
	 
	Positions& positions = m_flockBuffer.GetPositions();
	Velocities& velocities = m_flockBuffer.GetVelocities();
	Angles& angles = m_flockBuffer.GetAngles();

	size_t boidsCount = m_flock.GetBoidsCount();

	for (size_t i = m_begin; i < m_end; ++i)
	{
		const TestFrameWork::Vector3& oldPosition(oldPositions[i]);
		const TestFrameWork::Vector3& oldVelocity(oldVelocities[i]);
		const float& oldAngle(oldAngles[i]);

		TestFrameWork::Vector3 separation;
		TestFrameWork::Vector3 cohesion;
		TestFrameWork::Vector3 alignment;

		size_t outsidersCount = 0;
		size_t insidersCount = 0;
		for (size_t j = 0; j < boidsCount; ++j)
		{
			if (j == i)
			{
				continue;
			}
			
			const TestFrameWork::Vector3& neighborPosition = oldPositions[j];
			TestFrameWork::Vector3 direction2Neighbor(oldPosition - neighborPosition);
			const float distance2Neighbor = direction2Neighbor.Length();
			if (distance2Neighbor < m_flock.GetSeparationDistance())
			{
				if (distance2Neighbor != 0.0f)
				{
					/// Separation
					separation += (Normalize(direction2Neighbor) * (1.0f / distance2Neighbor));					
					++insidersCount;
				}
			}
			else if (distance2Neighbor < m_flock.GetGroupDistance())
			{
				/// Alignment
				alignment += oldVelocities[j] - oldVelocity;

				/// Cohesion
				cohesion += neighborPosition;
				++outsidersCount;
			}
		}

		/// Separation
		if (insidersCount)
		{
			separation /= insidersCount;
		}

		if (outsidersCount)
		{
			/// Cohesion
			cohesion /= outsidersCount;

			/// Alignment
			alignment /= outsidersCount;
		}

		/// Seek cohesion target
		TestFrameWork::Vector3 desiredV(cohesion - oldPosition);
		desiredV.Normalize();
		desiredV *= m_flock.GetMaxSpeed();
		cohesion = desiredV - oldVelocity;
		

		TestFrameWork::Vector3 steeringVelocity(m_flock.GetSeparationDistance() * separation + 
												m_flock.GetCohesionWeight() * cohesion +
												m_flock.GetAlignmentWeight() * alignment);
				
		/// Target -> trefoil knot
		angles[i] = oldAngle + m_flock.GetAngleIncrement() * m_elapsedTime;
		TestFrameWork::Vector3 target(EvaluateTrefoilKnot(angles[i]));
		
		/// Seek trefoil knot target
		TestFrameWork::Vector3 desiredV2Trefoil(target - oldPosition);
		desiredV2Trefoil.Normalize();
		desiredV2Trefoil *= m_flock.GetMaxSpeed();
		TestFrameWork::Vector3 seekTarget = desiredV2Trefoil - oldVelocity;

		seekTarget *= m_flock.GetTrefoilKnotPathWeight();

		TestFrameWork::Vector3 velocity = TestFrameWork::Truncate(oldVelocity + steeringVelocity + seekTarget, m_flock.GetMaxSpeed());
		velocities[i] = velocity;
		positions[i] = oldPosition + velocity * m_elapsedTime;		
		assert(IsFinite(velocity));
	}
}
