/*
	Copyright 2014 Michele "King_DuckZ" Santullo

	This file is part of CloonelJump.

	CloonelJump 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.

	CloonelJump 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 CloonelJump.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "collider.hpp"
#include "horzcollisionbar.hpp"
#include "line.hpp"
#include "vectypes.hpp"
#include "collisionbarset.hpp"
#include "vectorwrapper/vectorops.hpp"
#include "vectormath.hpp"
#include <vector>
#include <ciso646>
#include <cassert>
#include <algorithm>

#if defined(WITH_VERBOSE_COLLIDER) && !defined(NDEBUG)
#define VERBOSE_COLLIDER
#include <iostream>
#endif

namespace cloonel {
	namespace {
		typedef std::vector<const CollisionBar*> CollisionBarListType;
		typedef std::vector<std::pair<Collider::GroupIDType, CollisionBarListType>> CollisionBarGroupListType;
		typedef std::vector<const CollisionBarSet*> CollisionBarSetListType;
		typedef std::vector<std::pair<Collider::GroupIDType, CollisionBarSetListType>> CollisionBarSetsListType;

		const CollisionBarListType g_emptyBars;
		const CollisionBarSetListType g_emptySets;

		///----------------------------------------------------------------------
		///----------------------------------------------------------------------
		const CollisionBarListType& findGroup (const CollisionBarGroupListType& parList, Collider::GroupIDType parID) {
			for (const auto& itm : parList) {
				if (itm.first == parID) {
					return itm.second;
				}
			}
			return g_emptyBars;
		}

#if !defined(NDEBUG)
		///----------------------------------------------------------------------
		///----------------------------------------------------------------------
		const CollisionBarSetListType& findGroup (const CollisionBarSetsListType& parList, Collider::GroupIDType parID) {
			for (const auto& itm : parList) {
				if (itm.first == parID) {
					return itm.second;
				}
			}
			return g_emptySets;
		}
#endif

#if !defined(NDEBUG)
		///----------------------------------------------------------------------
		///----------------------------------------------------------------------
		bool isGroupRegistered (const CollisionBarGroupListType& parList, Collider::GroupIDType parID) {
			const CollisionBarListType& result = findGroup(parList, parID);
			return &g_emptyBars != &result;
		}
#endif

#if !defined(NDEBUG)
		///----------------------------------------------------------------------
		///----------------------------------------------------------------------
		bool isGroupRegistered (const CollisionBarSetsListType& parList, Collider::GroupIDType parID) {
			const CollisionBarSetListType& result = findGroup(parList, parID);
			return &g_emptySets != &result;
		}
#endif

		///----------------------------------------------------------------------
		///----------------------------------------------------------------------
		template <typename L>
		typename L::value_type::second_type& findOrAddGroup (L& parList, Collider::GroupIDType parID) {
			for (auto& itm : parList) {
				if (itm.first == parID) {
					return itm.second;
				}
			}
			parList.push_back(std::make_pair(parID, typename L::value_type::second_type()));
			assert(parList.back().first == parID);
			return parList.back().second;
		}

		///----------------------------------------------------------------------
		///----------------------------------------------------------------------
		void collide (float parDeltaT, const CollisionBarListType& parGroup1, const CollisionBarListType& parGroup2) {
			Line<float, 2> overlap;
			for (const auto& bar1 : parGroup1) {
				assert(bar1);
				for (const auto& bar2 : parGroup2) {
					assert(bar2);
					if (bar2 == bar1)
						continue;

					if (Collide(parDeltaT, *bar1, *bar2, overlap)) {
#if defined(VERBOSE_COLLIDER)
						std::cout << "Collider: Collision ";
						std::cout << "between " << bar1 << " and " << bar2 << "\n";
#endif
						const auto& offs1 = bar1->GetOffset();
						const auto& offs2 = bar2->GetOffset();
						float2 dir1, dir2;
						if (offs1 == float2(0.0f) and offs2 == float2(0.0f)) {
							dir1 = dir2 = float2(0.0f);
						}
						else if (offs1 == float2(0.0f)) {
							dir2 = normalized(offs2);
							dir1 = -1.0f * dir2;
						}
						else if (offs2 == float2(0.0f)) {
							dir1 = normalized(offs1);
							dir2 = -1.0f * dir1;
						}
						else {
							dir1 = normalized(offs1);
							dir2 = normalized(offs2);
						}
						bar1->InvokeCallback(overlap, dir1);
						bar2->InvokeCallback(overlap, dir2);
					}
				}
			}
		}
	} //unnamed namespace

	///--------------------------------------------------------------------------
	///--------------------------------------------------------------------------
	Collider::Collider() {
	}

	///--------------------------------------------------------------------------
	///--------------------------------------------------------------------------
	Collider::~Collider() noexcept {
	}

	///--------------------------------------------------------------------------
	///--------------------------------------------------------------------------
	void Collider::RunCollisionTests (float parDeltaT) const {
		CollisionBarGroupListType collectedGroups;
		//For each CollisionBarSet list+ID
		for (const auto& set : m_collisionBarSets) {
			//Get or make a list for the currend ID
			CollisionBarListType& newList = findOrAddGroup(collectedGroups, set.first);
			//For each CollisionBarSet with the current ID
			for (auto setObject : set.second) {
#if !defined(NDEBUG)
				const size_t oldListSize = newList.size();
				const CollisionBar* const lastCollisionBar = (oldListSize == 0 ? nullptr : newList.back());
#endif
				//Get the collision bars in the current list
				setObject->CopyBars(newList);

				//Make sure the CollisionBarSet didn't tamper with elements already in the list
				assert(oldListSize <= newList.size());
				assert(0 == oldListSize or newList[oldListSize - 1] == lastCollisionBar);
			}
		}

		for (const auto& relationship : m_relationships) {
			const CollisionBarListType& group1 = findGroup(m_collisionBars, relationship.first);
			const CollisionBarListType& group2 = (relationship.first == relationship.second ? group1 : findGroup(m_collisionBars, relationship.second));
			const CollisionBarListType& group1a = findGroup(collectedGroups, relationship.first);
			const CollisionBarListType& group2a = (relationship.first == relationship.second ? group1a : findGroup(collectedGroups, relationship.second));

			collide(parDeltaT, group1, group2);
			collide(parDeltaT, group1, group2a);
			collide(parDeltaT, group1a, group2);
			collide(parDeltaT, group1a, group2a);
		}
	}

	///--------------------------------------------------------------------------
	///--------------------------------------------------------------------------
	void Collider::TieGroups (GroupIDType parGroup1, GroupIDType parGroup2) {
#if defined(VERBOSE_COLLIDER)
		std::cout << "Collider: Tying group " << parGroup1 << " to " << parGroup2 << " for collision tests\n";
#endif
		assert(m_relationships.end() == std::find(m_relationships.begin(), m_relationships.end(), std::make_pair(parGroup1, parGroup2)));
		assert(m_relationships.end() == std::find(m_relationships.begin(), m_relationships.end(), std::make_pair(parGroup2, parGroup1)));
		assert(isGroupRegistered(m_collisionBars, parGroup1) or isGroupRegistered(m_collisionBarSets, parGroup1));
		assert(isGroupRegistered(m_collisionBars, parGroup2) or isGroupRegistered(m_collisionBarSets, parGroup2));
		m_relationships.push_back(std::make_pair(parGroup1, parGroup2));
	}

	///--------------------------------------------------------------------------
	///--------------------------------------------------------------------------
	void Collider::RegisterBar (GroupIDType parGroup, const CollisionBar* parBar) {
#if defined(VERBOSE_COLLIDER)
		std::cout << "Collider: Registering bar " << parBar << " in group " << parGroup << "\n";
#endif
		CollisionBarListType& group = findOrAddGroup(m_collisionBars, parGroup);
		assert(isGroupRegistered(m_collisionBars, parGroup));
		assert(parBar);
		group.push_back(parBar);
	}

	///--------------------------------------------------------------------------
	///--------------------------------------------------------------------------
	void Collider::UnregisterBar (GroupIDType parGroup, const CollisionBar* parBar) {
		assert(isGroupRegistered(m_collisionBars, parGroup));
		CollisionBarListType& group = findOrAddGroup(m_collisionBars, parGroup);
		auto itdele = std::find(group.begin(), group.end(), parBar);
		assert(itdele != group.end());
		group.erase(itdele);
	}

	///--------------------------------------------------------------------------
	///--------------------------------------------------------------------------
	void Collider::Reset() noexcept {
		m_relationships.clear();
		m_collisionBars.clear();
		m_collisionBarSets.clear();
	}

	///--------------------------------------------------------------------------
	///--------------------------------------------------------------------------
	void Collider::RegisterBarSet (GroupIDType parGroup, const CollisionBarSet* parSet) {
#if defined(VERBOSE_COLLIDER)
		std::cout << "Collider: Registering bar set " << parSet << " in group " << parGroup << "\n";
#endif
		CollisionBarSetListType& group = findOrAddGroup(m_collisionBarSets, parGroup);
		assert(isGroupRegistered(m_collisionBarSets, parGroup));
		assert(parSet);
		group.push_back(parSet);
	}

	///--------------------------------------------------------------------------
	///--------------------------------------------------------------------------
	void Collider::UnregisterBarSet (GroupIDType parGroup, const CollisionBarSet* parSet) {
		assert(isGroupRegistered(m_collisionBarSets, parGroup));
		CollisionBarSetListType& group = findOrAddGroup(m_collisionBarSets, parGroup);
		auto itdele = std::find(group.begin(), group.end(), parSet);
		assert(itdele != group.end());
		group.erase(itdele);
	}
} //namespace cloonel

#if defined(VERBOSE_COLLIDER)
#undef VERBOSE_COLLIDER
#endif