/*************************************************************************/
/*  broad_phase_2d_hash_grid.cpp                                         */
/*************************************************************************/
/*                       This file is part of:                           */
/*                           GODOT ENGINE                                */
/*                    http://www.godotengine.org                         */
/*************************************************************************/
/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
/*                                                                       */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the       */
/* "Software"), to deal in the Software without restriction, including   */
/* without limitation the rights to use, copy, modify, merge, publish,   */
/* distribute, sublicense, and/or sell copies of the Software, and to    */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions:                                             */
/*                                                                       */
/* The above copyright notice and this permission notice shall be        */
/* included in all copies or substantial portions of the Software.       */
/*                                                                       */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
/*************************************************************************/
#include "broad_phase_2d_hash_grid.h"
#include "globals.h"

#define LARGE_ELEMENT_FI 1.01239812

void BroadPhase2DHashGrid::_pair_attempt(Element *p_elem, Element *p_with) {

	Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with);

	ERR_FAIL_COND(p_elem->_static && p_with->_static);

	if (!E) {

		PairData *pd = memnew(PairData);
		p_elem->paired[p_with] = pd;
		p_with->paired[p_elem] = pd;
	} else {
		E->get()->rc++;
	}
}

void BroadPhase2DHashGrid::_unpair_attempt(Element *p_elem, Element *p_with) {

	Map<Element *, PairData *>::Element *E = p_elem->paired.find(p_with);

	ERR_FAIL_COND(!E); //this should really be paired..

	E->get()->rc--;

	if (E->get()->rc == 0) {

		if (E->get()->colliding) {
			//uncollide
			if (unpair_callback) {
				unpair_callback(p_elem->owner, p_elem->subindex, p_with->owner, p_with->subindex, E->get()->ud, unpair_userdata);
			}
		}

		memdelete(E->get());
		p_elem->paired.erase(E);
		p_with->paired.erase(p_elem);
	}
}

void BroadPhase2DHashGrid::_check_motion(Element *p_elem) {

	for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) {

		bool pairing = p_elem->aabb.intersects(E->key()->aabb);

		if (pairing != E->get()->colliding) {

			if (pairing) {

				if (pair_callback) {
					E->get()->ud = pair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, pair_userdata);
				}
			} else {

				if (unpair_callback) {
					unpair_callback(p_elem->owner, p_elem->subindex, E->key()->owner, E->key()->subindex, E->get()->ud, unpair_userdata);
				}
			}

			E->get()->colliding = pairing;
		}
	}
}

void BroadPhase2DHashGrid::_enter_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) {

	Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI); //use magic number to avoid floating point issues
	if (sz.width * sz.height > large_object_min_surface) {
		//large object, do not use grid, must check against all elements
		for (Map<ID, Element>::Element *E = element_map.front(); E; E = E->next()) {
			if (E->key() == p_elem->self)
				continue; // do not pair against itself
			if (E->get().owner == p_elem->owner)
				continue;
			if (E->get()._static && p_static)
				continue;

			_pair_attempt(p_elem, &E->get());
		}

		large_elements[p_elem].inc();
		return;
	}

	Point2i from = (p_rect.pos / cell_size).floor();
	Point2i to = ((p_rect.pos + p_rect.size) / cell_size).floor();

	for (int i = from.x; i <= to.x; i++) {

		for (int j = from.y; j <= to.y; j++) {

			PosKey pk;
			pk.x = i;
			pk.y = j;

			uint32_t idx = pk.hash() % hash_table_size;
			PosBin *pb = hash_table[idx];

			while (pb) {

				if (pb->key == pk) {
					break;
				}

				pb = pb->next;
			}

			bool entered = false;

			if (!pb) {
				//does not exist, create!
				pb = memnew(PosBin);
				pb->key = pk;
				pb->next = hash_table[idx];
				hash_table[idx] = pb;
			}

			if (p_static) {
				if (pb->static_object_set[p_elem].inc() == 1) {
					entered = true;
				}
			} else {
				if (pb->object_set[p_elem].inc() == 1) {

					entered = true;
				}
			}

			if (entered) {

				for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {

					if (E->key()->owner == p_elem->owner)
						continue;
					_pair_attempt(p_elem, E->key());
				}

				if (!p_static) {

					for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {

						if (E->key()->owner == p_elem->owner)
							continue;
						_pair_attempt(p_elem, E->key());
					}
				}
			}
		}
	}

	//pair separatedly with large elements

	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {

		if (E->key() == p_elem)
			continue; // do not pair against itself
		if (E->key()->owner == p_elem->owner)
			continue;
		if (E->key()->_static && p_static)
			continue;

		_pair_attempt(E->key(), p_elem);
	}
}

void BroadPhase2DHashGrid::_exit_grid(Element *p_elem, const Rect2 &p_rect, bool p_static) {

	Vector2 sz = (p_rect.size / cell_size * LARGE_ELEMENT_FI);
	if (sz.width * sz.height > large_object_min_surface) {

		//unpair all elements, instead of checking all, just check what is already paired, so we at least save from checking static vs static
		for (Map<Element *, PairData *>::Element *E = p_elem->paired.front(); E; E = E->next()) {

			_unpair_attempt(p_elem, E->key());
		}

		if (large_elements[p_elem].dec() == 0) {
			large_elements.erase(p_elem);
		}
		return;
	}

	Point2i from = (p_rect.pos / cell_size).floor();
	Point2i to = ((p_rect.pos + p_rect.size) / cell_size).floor();

	for (int i = from.x; i <= to.x; i++) {

		for (int j = from.y; j <= to.y; j++) {

			PosKey pk;
			pk.x = i;
			pk.y = j;

			uint32_t idx = pk.hash() % hash_table_size;
			PosBin *pb = hash_table[idx];

			while (pb) {

				if (pb->key == pk) {
					break;
				}

				pb = pb->next;
			}

			ERR_CONTINUE(!pb); //should exist!!

			bool exited = false;

			if (p_static) {
				if (pb->static_object_set[p_elem].dec() == 0) {

					pb->static_object_set.erase(p_elem);
					exited = true;
				}
			} else {
				if (pb->object_set[p_elem].dec() == 0) {

					pb->object_set.erase(p_elem);
					exited = true;
				}
			}

			if (exited) {

				for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {

					if (E->key()->owner == p_elem->owner)
						continue;
					_unpair_attempt(p_elem, E->key());
				}

				if (!p_static) {

					for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {

						if (E->key()->owner == p_elem->owner)
							continue;
						_unpair_attempt(p_elem, E->key());
					}
				}
			}

			if (pb->object_set.empty() && pb->static_object_set.empty()) {

				if (hash_table[idx] == pb) {
					hash_table[idx] = pb->next;
				} else {

					PosBin *px = hash_table[idx];

					while (px) {

						if (px->next == pb) {
							px->next = pb->next;
							break;
						}

						px = px->next;
					}

					ERR_CONTINUE(!px);
				}

				memdelete(pb);
			}
		}
	}

	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {
		if (E->key() == p_elem)
			continue; // do not pair against itself
		if (E->key()->owner == p_elem->owner)
			continue;
		if (E->key()->_static && p_static)
			continue;

		//unpair from large elements
		_unpair_attempt(p_elem, E->key());
	}
}

BroadPhase2DHashGrid::ID BroadPhase2DHashGrid::create(CollisionObject2DSW *p_object, int p_subindex) {

	current++;

	Element e;
	e.owner = p_object;
	e._static = false;
	e.subindex = p_subindex;
	e.self = current;
	e.pass = 0;

	element_map[current] = e;
	return current;
}

void BroadPhase2DHashGrid::move(ID p_id, const Rect2 &p_aabb) {

	Map<ID, Element>::Element *E = element_map.find(p_id);
	ERR_FAIL_COND(!E);

	Element &e = E->get();

	if (p_aabb == e.aabb)
		return;

	if (p_aabb != Rect2()) {

		_enter_grid(&e, p_aabb, e._static);
	}

	if (e.aabb != Rect2()) {

		_exit_grid(&e, e.aabb, e._static);
	}

	e.aabb = p_aabb;

	_check_motion(&e);

	e.aabb = p_aabb;
}
void BroadPhase2DHashGrid::set_static(ID p_id, bool p_static) {

	Map<ID, Element>::Element *E = element_map.find(p_id);
	ERR_FAIL_COND(!E);

	Element &e = E->get();

	if (e._static == p_static)
		return;

	if (e.aabb != Rect2())
		_exit_grid(&e, e.aabb, e._static);

	e._static = p_static;

	if (e.aabb != Rect2()) {
		_enter_grid(&e, e.aabb, e._static);
		_check_motion(&e);
	}
}
void BroadPhase2DHashGrid::remove(ID p_id) {

	Map<ID, Element>::Element *E = element_map.find(p_id);
	ERR_FAIL_COND(!E);

	Element &e = E->get();

	if (e.aabb != Rect2())
		_exit_grid(&e, e.aabb, e._static);

	element_map.erase(p_id);
}

CollisionObject2DSW *BroadPhase2DHashGrid::get_object(ID p_id) const {

	const Map<ID, Element>::Element *E = element_map.find(p_id);
	ERR_FAIL_COND_V(!E, NULL);
	return E->get().owner;
}
bool BroadPhase2DHashGrid::is_static(ID p_id) const {

	const Map<ID, Element>::Element *E = element_map.find(p_id);
	ERR_FAIL_COND_V(!E, false);
	return E->get()._static;
}
int BroadPhase2DHashGrid::get_subindex(ID p_id) const {

	const Map<ID, Element>::Element *E = element_map.find(p_id);
	ERR_FAIL_COND_V(!E, -1);
	return E->get().subindex;
}

template <bool use_aabb, bool use_segment>
void BroadPhase2DHashGrid::_cull(const Point2i p_cell, const Rect2 &p_aabb, const Point2 &p_from, const Point2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices, int &index) {

	PosKey pk;
	pk.x = p_cell.x;
	pk.y = p_cell.y;

	uint32_t idx = pk.hash() % hash_table_size;
	PosBin *pb = hash_table[idx];

	while (pb) {

		if (pb->key == pk) {
			break;
		}

		pb = pb->next;
	}

	if (!pb)
		return;

	for (Map<Element *, RC>::Element *E = pb->object_set.front(); E; E = E->next()) {

		if (index >= p_max_results)
			break;
		if (E->key()->pass == pass)
			continue;

		E->key()->pass = pass;

		if (use_aabb && !p_aabb.intersects(E->key()->aabb))
			continue;

		if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to))
			continue;

		p_results[index] = E->key()->owner;
		p_result_indices[index] = E->key()->subindex;
		index++;
	}

	for (Map<Element *, RC>::Element *E = pb->static_object_set.front(); E; E = E->next()) {

		if (index >= p_max_results)
			break;
		if (E->key()->pass == pass)
			continue;

		if (use_aabb && !p_aabb.intersects(E->key()->aabb)) {
			continue;
		}

		if (use_segment && !E->key()->aabb.intersects_segment(p_from, p_to))
			continue;

		E->key()->pass = pass;
		p_results[index] = E->key()->owner;
		p_result_indices[index] = E->key()->subindex;
		index++;
	}
}

int BroadPhase2DHashGrid::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {

	pass++;

	Vector2 dir = (p_to - p_from);
	if (dir == Vector2())
		return 0;
	//avoid divisions by zero
	dir.normalize();
	if (dir.x == 0.0)
		dir.x = 0.000001;
	if (dir.y == 0.0)
		dir.y = 0.000001;
	Vector2 delta = dir.abs();

	delta.x = cell_size / delta.x;
	delta.y = cell_size / delta.y;

	Point2i pos = (p_from / cell_size).floor();
	Point2i end = (p_to / cell_size).floor();

	Point2i step = Vector2(SGN(dir.x), SGN(dir.y));

	Vector2 max;

	if (dir.x < 0)
		max.x = (Math::floor(pos.x) * cell_size - p_from.x) / dir.x;
	else
		max.x = (Math::floor(pos.x + 1) * cell_size - p_from.x) / dir.x;

	if (dir.y < 0)
		max.y = (Math::floor(pos.y) * cell_size - p_from.y) / dir.y;
	else
		max.y = (Math::floor(pos.y + 1) * cell_size - p_from.y) / dir.y;

	int cullcount = 0;
	_cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount);

	bool reached_x = false;
	bool reached_y = false;

	while (true) {

		if (max.x < max.y) {

			max.x += delta.x;
			pos.x += step.x;
		} else {

			max.y += delta.y;
			pos.y += step.y;
		}

		if (step.x > 0) {
			if (pos.x >= end.x)
				reached_x = true;
		} else if (pos.x <= end.x) {

			reached_x = true;
		}

		if (step.y > 0) {
			if (pos.y >= end.y)
				reached_y = true;
		} else if (pos.y <= end.y) {

			reached_y = true;
		}

		_cull<false, true>(pos, Rect2(), p_from, p_to, p_results, p_max_results, p_result_indices, cullcount);

		if (reached_x && reached_y)
			break;
	}

	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {

		if (cullcount >= p_max_results)
			break;
		if (E->key()->pass == pass)
			continue;

		E->key()->pass = pass;

		//		if (use_aabb && !p_aabb.intersects(E->key()->aabb))
		//			continue;

		if (!E->key()->aabb.intersects_segment(p_from, p_to))
			continue;

		p_results[cullcount] = E->key()->owner;
		p_result_indices[cullcount] = E->key()->subindex;
		cullcount++;
	}

	return cullcount;
}

int BroadPhase2DHashGrid::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) {

	pass++;

	Point2i from = (p_aabb.pos / cell_size).floor();
	Point2i to = ((p_aabb.pos + p_aabb.size) / cell_size).floor();
	int cullcount = 0;

	for (int i = from.x; i <= to.x; i++) {

		for (int j = from.y; j <= to.y; j++) {

			_cull<true, false>(Point2i(i, j), p_aabb, Point2(), Point2(), p_results, p_max_results, p_result_indices, cullcount);
		}
	}

	for (Map<Element *, RC>::Element *E = large_elements.front(); E; E = E->next()) {

		if (cullcount >= p_max_results)
			break;
		if (E->key()->pass == pass)
			continue;

		E->key()->pass = pass;

		if (!p_aabb.intersects(E->key()->aabb))
			continue;

		//		if (!E->key()->aabb.intersects_segment(p_from,p_to))
		//			continue;

		p_results[cullcount] = E->key()->owner;
		p_result_indices[cullcount] = E->key()->subindex;
		cullcount++;
	}
	return cullcount;
}

void BroadPhase2DHashGrid::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {

	pair_callback = p_pair_callback;
	pair_userdata = p_userdata;
}
void BroadPhase2DHashGrid::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {

	unpair_callback = p_unpair_callback;
	unpair_userdata = p_userdata;
}

void BroadPhase2DHashGrid::update() {
}

BroadPhase2DSW *BroadPhase2DHashGrid::_create() {

	return memnew(BroadPhase2DHashGrid);
}

BroadPhase2DHashGrid::BroadPhase2DHashGrid() {

	hash_table_size = GLOBAL_DEF("physics_2d/bp_hash_table_size", 4096);
	hash_table_size = Math::larger_prime(hash_table_size);
	hash_table = memnew_arr(PosBin *, hash_table_size);

	cell_size = GLOBAL_DEF("physics_2d/cell_size", 128);
	large_object_min_surface = GLOBAL_DEF("physics_2d/large_object_surface_treshold_in_cells", 512);

	for (int i = 0; i < hash_table_size; i++)
		hash_table[i] = NULL;
	pass = 1;

	current = 0;
}

BroadPhase2DHashGrid::~BroadPhase2DHashGrid() {

	for (int i = 0; i < hash_table_size; i++) {
		while (hash_table[i]) {
			PosBin *pb = hash_table[i];
			hash_table[i] = pb->next;
			memdelete(pb);
		}
	}

	memdelete_arr(hash_table);
}

/* 3D version of voxel traversal:

public IEnumerable<Point3D> GetCellsOnRay(Ray ray, int maxDepth)
{
    // Implementation is based on:
    // "A Fast Voxel Traversal Algorithm for Ray Tracing"
    // John Amanatides, Andrew Woo
    // http://www.cse.yorku.ca/~amana/research/grid.pdf
    // http://www.devmaster.net/articles/raytracing_series/A%20faster%20voxel%20traversal%20algorithm%20for%20ray%20tracing.pdf

    // NOTES:
    // * This code assumes that the ray's position and direction are in 'cell coordinates', which means
    //   that one unit equals one cell in all directions.
    // * When the ray doesn't start within the voxel grid, calculate the first position at which the
    //   ray could enter the grid. If it never enters the grid, there is nothing more to do here.
    // * Also, it is important to test when the ray exits the voxel grid when the grid isn't infinite.
    // * The Point3D structure is a simple structure having three integer fields (X, Y and Z).

    // The cell in which the ray starts.
    Point3D start = GetCellAt(ray.Position);        // Rounds the position's X, Y and Z down to the nearest integer values.
    int x = start.X;
    int y = start.Y;
    int z = start.Z;

    // Determine which way we go.
    int stepX = Math.Sign(ray.Direction.X);
    int stepY = Math.Sign(ray.Direction.Y);
    int stepZ = Math.Sign(ray.Direction.Z);

    // Calculate cell boundaries. When the step (i.e. direction sign) is positive,
    // the next boundary is AFTER our current position, meaning that we have to add 1.
    // Otherwise, it is BEFORE our current position, in which case we add nothing.
    Point3D cellBoundary = new Point3D(
	x + (stepX > 0 ? 1 : 0),
	y + (stepY > 0 ? 1 : 0),
	z + (stepZ > 0 ? 1 : 0));

    // NOTE: For the following calculations, the result will be Single.PositiveInfinity
    // when ray.Direction.X, Y or Z equals zero, which is OK. However, when the left-hand
    // value of the division also equals zero, the result is Single.NaN, which is not OK.

    // Determine how far we can travel along the ray before we hit a voxel boundary.
    Vector3 tMax = new Vector3(
	(cellBoundary.X - ray.Position.X) / ray.Direction.X,    // Boundary is a plane on the YZ axis.
	(cellBoundary.Y - ray.Position.Y) / ray.Direction.Y,    // Boundary is a plane on the XZ axis.
	(cellBoundary.Z - ray.Position.Z) / ray.Direction.Z);    // Boundary is a plane on the XY axis.
    if (Single.IsNaN(tMax.X)) tMax.X = Single.PositiveInfinity;
    if (Single.IsNaN(tMax.Y)) tMax.Y = Single.PositiveInfinity;
    if (Single.IsNaN(tMax.Z)) tMax.Z = Single.PositiveInfinity;

    // Determine how far we must travel along the ray before we have crossed a gridcell.
    Vector3 tDelta = new Vector3(
	stepX / ray.Direction.X,                    // Crossing the width of a cell.
	stepY / ray.Direction.Y,                    // Crossing the height of a cell.
	stepZ / ray.Direction.Z);                    // Crossing the depth of a cell.
    if (Single.IsNaN(tDelta.X)) tDelta.X = Single.PositiveInfinity;
    if (Single.IsNaN(tDelta.Y)) tDelta.Y = Single.PositiveInfinity;
    if (Single.IsNaN(tDelta.Z)) tDelta.Z = Single.PositiveInfinity;

    // For each step, determine which distance to the next voxel boundary is lowest (i.e.
    // which voxel boundary is nearest) and walk that way.
    for (int i = 0; i < maxDepth; i++)
    {
	// Return it.
	yield return new Point3D(x, y, z);

	// Do the next step.
	if (tMax.X < tMax.Y && tMax.X < tMax.Z)
	{
	    // tMax.X is the lowest, an YZ cell boundary plane is nearest.
	    x += stepX;
	    tMax.X += tDelta.X;
	}
	else if (tMax.Y < tMax.Z)
	{
	    // tMax.Y is the lowest, an XZ cell boundary plane is nearest.
	    y += stepY;
	    tMax.Y += tDelta.Y;
	}
	else
	{
	    // tMax.Z is the lowest, an XY cell boundary plane is nearest.
	    z += stepZ;
	    tMax.Z += tDelta.Z;
	}
    }

    */