/*
 * 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 "ant.hpp"
#include "colony.hpp"
#include "pheromone-matrix.hpp"
#include <cmath>

float FRAMES_PER_SECOND = 60;
float TIMESTEP = 1.0f / FRAMES_PER_SECOND;
float ANT_LENGTH = 0.5f; // 0.5 cm, head to abdomen (not including legs / antennae)
float ANT_COLLISION_RADIUS = ANT_LENGTH * 1.25f;
float RECEPTOR_RADIUS = 0.4f;
float RECEPTOR_SEPARATION = 0.882f;
float RECEPTOR_DISTANCE = 0.588f;
float MOUTH_DISTANCE = 0.2646f;
float BITE_RADIUS = 0.0294f;
float FOOD_PARTICLE_RADIUS = 0.1176f;
float MAX_RECEPTOR_NOISE = 0.05f; // essentially an epsilon
float MAX_EXCITEMENT = 1.0f;
float MAX_PHEROMONE_TURNING_ANGLE = glm::radians(8.5f);
float MIN_WALK_TIME = 0.5f; // seconds
float MAX_WALK_TIME = 8.0f; // seconds
float MIN_REST_TIME = 0.15f;
float MAX_REST_TIME = 0.7f;
float MIN_CHEW_TIME = 0.25f;
float MAX_CHEW_TIME = 0.5f;
float DEEXCITEMENT_FACTOR = 0.999f; // This should probably always be less than the evaporation factor
float CALM_FACTOR = 0.995f;
float MAX_WALK_FORCE = 1.5;
float MAX_PANIC_FORCE = 0.1029f;
float MAX_WALK_SPEED = 3.0f; // cm/s
float MAX_PANIC_SPEED = 8.82f; // cm/s
float PANIC_RADIUS = 7.35f;
float WANDER_CIRCLE_DISTANCE = 0.441f;
float WANDER_CIRCLE_RADIUS = 0.0294f;
float MAX_WANDER_ANGLE = 0.15f;

inline float fwrap(float angle, float limit)
{
    return angle - std::floor(angle / limit) * limit;
}

Ant::Ant(Colony* colony):
	colony(colony),
	state(Ant::State::IDLE),
	transform(Transform::getIdentity()),
	pose(nullptr)
{
	pose = new Pose(colony->getAntModel()->getSkeleton());
	pose->reset();
	pose->concatenate();
	
	modelInstance.setModel(colony->getAntModel());
	modelInstance.setPose(pose);
	
	animationTime = frand(0.0f, 60.0f);
	
	velocity = Vector3(0);
	acceleration = Vector3(0);
	wanderDirection = getForward();
	excitement = MAX_EXCITEMENT;
}

Ant::~Ant()
{
	delete pose;
}

void Ant::animate()
{
	colony->getTripodGaitAnimation()->animate(pose, animationTime);
	pose->concatenate();
	animationTime = fwrap(animationTime + 4.0f, colony->getTripodGaitAnimation()->getEndTime());
}

void Ant::suspend(const Vector3& suspensionPoint, const Quaternion& suspensionRotation)
{
	transform.translation = suspensionPoint;
	transform.rotation = suspensionRotation;
	modelInstance.setTransform(transform);
}

void Ant::move(const Vector3& velocity)
{
	std::vector<Navmesh::Step> traversal;
	Navmesh::traverse(getNavmeshTriangle(), getBarycentricPosition(), velocity, &traversal);
	
	if (!traversal.empty())
	{
		const Navmesh::Step& step = traversal.back();
		
		if (step.start != step.end)
		{
			if (step.triangle != getNavmeshTriangle())
			{
				Quaternion alignment = glm::rotation(getNavmeshTriangle()->normal, step.triangle->normal);
				Vector3 newForward = glm::normalize(project_on_plane(alignment * getForward(), Vector3(0.0f), step.triangle->normal));
				
				setOrientation(newForward, step.triangle->normal);
			}
		}
		
		setPosition(step.triangle, step.end);
	}
}

void Ant::turn(float angle)
{
	// Rotate forward vector
	Vector3 newForward = glm::normalize(glm::angleAxis(angle, getUp()) * getForward());
	setOrientation(newForward, getUp());
}

void Ant::update(float dt)
{
	float probeLateralOffset = 0.1f;
	float probeForwardOffset = 0.3f;
	
	animate();
	
    // Calculate positions of receptors
	receptorL = getPosition() + getForward() * RECEPTOR_DISTANCE;
	receptorR = receptorL;
	receptorL -= getRight() * RECEPTOR_SEPARATION * 0.5f;
	receptorR += getRight() * RECEPTOR_SEPARATION * 0.5f;
	
	// Steering
	if (state == Ant::State::WANDER)
	{
		//setWanderCircleDistance(4.0f);
		//setWanderCircleRadius(0.3f);
		//setWanderRate(glm::radians(90.0f));
		//setSeparationRadius(0.5f);
		//setMaxSpeed(0.025f);
		
		// Calculate wander force
		Vector3 wanderForce = wander() * 1.5f;
		Vector3 followForce = follow() * 3.0f;
		
		// Setup containment probes
		//Vector3 leftProbe = getForward() * probeForwardOffset - getRight() * probeLateralOffset;
		//Vector3 rightProbe = getForward() * probeForwardOffset + getRight() * probeLateralOffset;
		
		// Calculate containment force
		//Vector3 containmentForce = containment(leftProbe) + containment(rightProbe);
		
		// Determine neighbors
		//float neighborhoodSize = 2.0f;
		//AABB neighborhoodAABB(getPosition() - Vector3(neighborhoodSize * 0.5f), getPosition() + Vector3(neighborhoodSize * 0.5f));
		//std::list<Agent*> neighbors;
		//colony->queryAnts(neighborhoodAABB, &neighbors);
				
		// Calculate separation force
		//Vector3 separationForce = separation(neighbors);
		
		applyForce(wanderForce);
		applyForce(followForce);
		
		float maxSpeed = MAX_WALK_SPEED * TIMESTEP;
		
		// Limit acceleration
		float accelerationMagnitudeSquared = glm::dot(acceleration, acceleration);
		if (accelerationMagnitudeSquared > MAX_WALK_FORCE * MAX_WALK_FORCE)
		{
			acceleration = glm::normalize(acceleration) * MAX_WALK_FORCE;
		}
		
		// Accelerate
		velocity += acceleration;
		
		// Limit speed
		float speedSquared = glm::dot(velocity, velocity);
		if (speedSquared > maxSpeed * maxSpeed)
		{
			velocity = glm::normalize(velocity) * maxSpeed;
		}
		
		Vector3 direction = glm::normalize(velocity);
		setOrientation(direction, getUp());
		
		// Deposit pheromones
		Vector2 position2D = Vector2(getPosition().x, getPosition().z);
		colony->getHomingMatrix()->deposit(position2D, excitement);
		excitement *= DEEXCITEMENT_FACTOR;
		
		// Move ant
		move(velocity);
	}
	else if (state == Ant::State::IDLE)
	{
		velocity = Vector3(0.0f);
		
		// Move ant
		move(velocity);
	}
	
	// Update transform
	if (state == Ant::State::WANDER || state == Ant::State::IDLE)
	{
		transform.translation = getPosition();
		transform.rotation = getRotation();
			
		// Update model instance
		modelInstance.setTransform(transform);
	}
}

void Ant::setState(Ant::State state)
{
	this->state = state;
}

Vector3 Ant::seek(const Vector3& target)
{
	Vector3 steer(0.0f);
	Vector3 difference = target - getPosition();
	
	float distanceSquared = glm::dot(difference, difference);
	if (distanceSquared > 0.0f)
	{
		float maxForce = MAX_WALK_FORCE;
		
		steer = glm::normalize(difference) * maxForce - velocity;
	}
	
	return steer;
}

Vector3 Ant::flee(const Vector3& target)
{
	return -seek(target);
}

Vector3 Ant::wander()
{
	// Determine center of wander circle
	Vector3 center = getPosition() + getForward() * WANDER_CIRCLE_DISTANCE;
	
	// Calculate wander target
	Vector3 target = center + wanderDirection * WANDER_CIRCLE_RADIUS;
	
	// Rotate wander direction by a random displacement angle
	float displacement = frand(-MAX_WANDER_ANGLE, MAX_WANDER_ANGLE);
	wanderDirection = glm::normalize(glm::angleAxis(displacement, getUp()) * wanderDirection);
	
	return seek(target);
}

Vector3 Ant::follow()
{
	const PheromoneMatrix* pheromoneMatrix = colony->getRecruitmentMatrix();
	
	Vector2 receptorL2D = Vector2(receptorL.x, receptorL.z);
	Vector2 receptorR2D = Vector2(receptorR.x, receptorR.z);

	float signalL = pheromoneMatrix->query(receptorL2D, RECEPTOR_RADIUS);
	signalL += frand(0.0f, MAX_RECEPTOR_NOISE);

	float signalR = pheromoneMatrix->query(receptorR2D, RECEPTOR_RADIUS);
	signalR += frand(0.0f, MAX_RECEPTOR_NOISE);
	
	if (signalL + signalR > 0.0f)
	{
		float angle = -MAX_PHEROMONE_TURNING_ANGLE * ((signalL - signalR) / (signalL + signalR));
		
		Vector3 steer = glm::normalize(glm::angleAxis(angle, getUp()) * getForward());
		return steer;
	}

	return Vector3(0.0f);
}

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