/*
 * Copyright (C) 2017  Christopher J. Howard
 *
 * This file is part of Antkeeper Source Code.
 *
 * Antkeeper Source Code is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Antkeeper Source Code is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Antkeeper Source Code.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "agent.hpp"

Agent::Agent():
	navmeshTriangle(nullptr),
	barycentricPosition(0.0f),
	position(0.0f),
	forward(0, 0, -1),
	up(0, 1, 0),
	right(1, 0, 0),
	rotation(1, 0, 0, 0),
	wanderDirection(0, 0, -1),
	velocity(0.0f)
{}

void Agent::applyForce(const Vector3& force)
{
	acceleration += force;
}

void Agent::updateVelocity()
{
	// Limit acceleration
	acceleration = limit(acceleration / mass, maxAcceleration);
	
	// Add acceleration to velocity and limit
	velocity = limit(velocity + acceleration, maxSpeed);
	
	// Reset acceleration to zero
	acceleration = Vector3(0.0f);
}

Vector3 Agent::wander(float dt)
{
	// Calculate center of wander circle
	Vector3 wanderCircleCenter = position + forward * wanderCircleDistance;
	
	// Calculate wander force
	Vector3 target = wanderCircleCenter + wanderDirection * wanderCircleRadius;
	
	// Rotate wander direction by a random displacement angle
	float displacement = frand(-wanderRate * 0.5f, wanderRate * 0.5f);
	wanderDirection = glm::normalize(glm::angleAxis(displacement, up) * wanderDirection);
	
	return seek(target);
}

Vector3 Agent::seek(const Vector3& target) const
{
	Vector3 desiredVelocity = glm::normalize(target - position) * maxSpeed;
	return desiredVelocity - velocity;
}

Vector3 Agent::flee(const Vector3& target) const
{
	Vector3 desiredVelocity = glm::normalize(position - target) * maxSpeed;
	return desiredVelocity - velocity;
}

Vector3 Agent::containment(const Vector3& probe) const
{
	std::vector<Navmesh::Step> traversal;
	Navmesh::traverse(navmeshTriangle, barycentricPosition, probe, &traversal);
	
	if (traversal.empty())
	{
		return Vector3(0.0f);
	}
	
	const Navmesh::Step& step = traversal.back();
	
	// If not on edge or on connected edge
	if (step.edge == nullptr || step.edge->symmetric != nullptr)
	{
		return Vector3(0.0f);
	}
	
	/*
	// Calculate difference between probe position and position on edge
	Vector3 end = cartesian(step.end,
		step.triangle->edge->vertex->position,
		step.triangle->edge->next->vertex->position,
		step.triangle->edge->previous->vertex->position);
	
	Vector3 difference = probe - end;
	
	float depth = 0.0f;
	if (nonzero(difference))
	{
		depth = glm::length(difference);
	}
	*/
	
	// Calculate edge normal
	const Vector3& a = step.edge->vertex->position;
	const Vector3& b = step.edge->next->vertex->position;
	Vector3 ab = glm::normalize(b - a);
	Vector3 edgeNormal = glm::cross(up, ab);
	
	// Calculate reflection vector of forward vector and edge normal
	//Vector3 reflection = glm::reflect(forward, edgeNormal);
	
	/*
	Vector3 target = cartesian(step.end,
		step.triangle->edge->vertex->position,
		step.triangle->edge->next->vertex->position,
		step.triangle->edge->previous->vertex->position) + reflection * 0.1f;
	*/
	
	//std::cout << "reflection: " << reflection.x << ", " << reflection.y << ", " << reflection.z << std::endl;
	
	return edgeNormal;
}

Vector3 Agent::separation(const std::list<Agent*>& neighbors) const
{
	Vector3 force(0.0f);
	
	for (Agent* neighbor: neighbors)
	{
		Vector3 difference = position - neighbor->position;
		
		float distanceSquared = glm::dot(difference, difference);
		if (distanceSquared > 0.0f && distanceSquared < separationRadiusSquared)
		{			
			force += difference * (1.0f / distanceSquared);
		}
	}
	
	if (nonzero(force))
	{
		force = glm::normalize(force);
	}
	
	return force;
}

void Agent::setPosition(Navmesh::Triangle* triangle, const Vector3& position)
{
	// Update navmesh triangle and position
	navmeshTriangle = triangle;
	barycentricPosition = position;
	
	// Convert navmesh-space barycentric position to world-space cartesian position
	const Vector3& a = triangle->edge->vertex->position;
	const Vector3& b = triangle->edge->next->vertex->position;
	const Vector3& c = triangle->edge->previous->vertex->position;
	this->position = cartesian(position, a, b, c);
}

void Agent::setOrientation(const Vector3& newForward, const Vector3& newUp)
{
	// Calculate alignment quaternion
	Quaternion alignment = glm::rotation(up, newUp);
	
	// Rebuild vector basis
	forward = newForward;
	right = glm::normalize(glm::cross(newUp, forward));
	up = glm::cross(forward, right);
	
	// Calculate rotation quaternion from vector basis
	rotation = glm::normalize(glm::quat_cast(Matrix3(right, up, forward)));
	
	// Align wander direction
	wanderDirection = glm::normalize(project_on_plane(alignment * wanderDirection, Vector3(0.0f), up));
}

void Agent::setMaxSpeed(float speed)
{
	maxSpeed = speed;
}

void Agent::setVelocity(const Vector3& velocity)
{
	this->velocity = velocity;
}

void Agent::setWanderCircleDistance(float distance)
{
	wanderCircleDistance = distance;
}

void Agent::setWanderCircleRadius(float radius)
{
	wanderCircleRadius = radius;
}

void Agent::setWanderRate(float angle)
{
	wanderRate = angle;
}

void Agent::setSeparationRadius(float radius)
{
	separationRadius = radius;
	separationRadiusSquared = separationRadius * separationRadius;
}

/** EXAMPLE USAGE
Vector3 wanderForce = wander(dt) * wanderWeight;
Vector3 fleeForce = flee(mouse) * fleeWeight;
Vector3 steerForce = wanderForce + fleeForce;
steer(steerForce);
**/