/*
	Copyright 2016, 2017 Michele "King_DuckZ" Santullo

	This file is part of MyCurry.

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

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

#include "grid_raytrace.hpp"
#include "worldgrid.hpp"
#include "vectorwrapper/vectorops.hpp"
#include "tileproperty.hpp"
#include <cassert>
#include <ciso646>
#include <cstdint>
#include <cmath>
#include <cfloat>
#include <algorithm>
#include <cfloat>

namespace curry {
#if !defined(BUILD_TESTING)
	namespace {
#endif
		float inv_length (const vec2f& parVec) {
			return 1.0f / std::sqrt(parVec.x() * parVec.x() + parVec.y() * parVec.y());
		}

		vec2f abs (const vec2f& parVec) {
			return vec2f(std::abs(parVec.x()), std::abs(parVec.y()));
		}

		float segment_intersection (const vec2f& parA, const vec2f& parDirA, const vec2f& parB, const vec2f& parDirB) {
			const auto& p = parA;
			const auto& r = parDirA;
			const auto& q = parB;
			const auto& s = parDirB;

			const auto r_cross_s = cross(r, s);
			if (std::abs(r_cross_s) <= FLT_EPSILON) {
				if (std::abs(cross(q - p, r)) <= FLT_EPSILON) {
					assert(std::abs(dot(r, r)) > FLT_EPSILON);
					auto t0 = dot(q - p, r) / dot(r, r);
					auto t1 = dot(q - p + s, r) / dot(r, r);
					if (dot(s, r) < 0.0f)
						std::swap(t0, t1);
					assert(t0 <= t1);
					if (0.0f <= t0)
						return t0;
					else if (t1 <= 1.0f)
						return t1;
					else
						return INFINITY;
				}
				else {
					return INFINITY;
				}
			}

			assert(not (std::abs(r_cross_s) <= FLT_EPSILON));
			const auto inv_r_cross_s = 1.0f / r_cross_s;
			const auto t = cross(q - p, s) * inv_r_cross_s;
			//const auto u = cross(q - p, r) * inv_r_cross_s;

			return t;
			//if (0.0f <= t and t <= 1.0f and 0.0f <= u and u <= 1.0f)
				//return t;
			//else
				//return INFINITY;
		}

		vec2f frac (vec2f parVal) {
			return parVal - vec2f(std::floor(parVal.x()), std::floor(parVal.y()));
		}

		template <typename T> int sgn (T val) {
			return (T(0) < val) - (val < T(0));
		}
#if !defined(BUILD_TESTING)
	} //unnamed namespace
#endif

	//see:
	//http://stackoverflow.com/questions/24679963/precise-subpixel-line-drawing-algorithm-rasterization-algorithm
	void for_each_voxel_under_segment (const vec2f& parFrom, const vec2f& parTo, vec2us parObjSize, const WorldGrid& parWorld, std::function<bool(vec2us)> parFunc) {
		const vec2f tile_size = vector_cast<vec2f>(parWorld.tile_size());
		//in this simplified case everything should be part of the world grid
		assert(parFrom >= vec2f(0.0f));
		assert(parTo >= vec2f(0.0f));
		assert(parFrom / tile_size <= vector_cast<vec2f>(parWorld.world_size()));
		assert(parTo / tile_size <= vector_cast<vec2f>(parWorld.world_size()));

		//float t = 0.0f;
		const vec2f& u = parFrom;
		const vec2f v = parTo - parFrom;

		const auto delta = tile_size / abs(v);
		const auto fr = frac(u / tile_size);
		assert(fr.x() >= 0.0f and fr.x() < 1.0f);
		assert(fr.y() >= 0.0f and fr.y() < 1.0f);

		//see:
		//http://stackoverflow.com/questions/12367071/how-do-i-initialize-the-t-variables-in-a-fast-voxel-traversal-algorithm-for-ray#12370474
		const vec2i step(sgn(v.x()), sgn(v.y()));
		vec2f max;
		if (step.x() > 0)
			max.x() = delta.x() * (1.0f - fr.x());
		else if (step.x() < 0)
			max.x() = delta.x() * fr.x();
		else
			max.x() = FLT_MAX;
		if (step.y() > 0)
			max.y() = delta.y() * (1.0f - fr.y());
		else if (step.y() < 0)
			max.y() = delta.y() * fr.y();
		else
			max.y() = FLT_MAX;

		vec2us curr_tile = pixel_to_world_tile(parWorld, u);
		const vec2us last_tile = pixel_to_world_tile(parWorld, parTo);
		while (parFunc(curr_tile) and last_tile != curr_tile) {
			if (max.x() < max.y()) {
				assert(step.x());
				assert(
					std::max(static_cast<uint16_t>(curr_tile.x() + step.x()), last_tile.x()) -
					std::min(static_cast<uint16_t>(curr_tile.x() + step.x()), last_tile.x()) <
					std::max(curr_tile.x(), last_tile.x()) -
					std::min(curr_tile.x(), last_tile.x())
				);
				max.x() += delta.x();
				curr_tile.x() = static_cast<uint16_t>(curr_tile.x() + step.x());
			}
			else {
				assert(step.y());
				assert(
					std::max(static_cast<uint16_t>(curr_tile.y() + step.y()), last_tile.y()) -
					std::min(static_cast<uint16_t>(curr_tile.y() + step.y()), last_tile.y()) <
					std::max(curr_tile.y(), last_tile.y()) -
					std::min(curr_tile.y(), last_tile.y())
				);
				max.y() += delta.y();
				curr_tile.y() = static_cast<uint16_t>(curr_tile.y() + step.y());
			}
		}
	}
} //namespace curry