#include <iostream>
#include <cmath>
#include <algorithm>
#include <cstdlib>
#include "Box.hpp"

namespace nbody {
	//extend box to form cube
	void Box::extendToCube() {
		const auto maxL = maxSidelength();
		const auto diag = max - min;
		const auto middle = min + (diag / 2);

		const auto normalizedDiag = diag / Norm(diag);

		const auto halfCubeDiag = normalizedDiag * (maxL / 2);

		min = middle - halfCubeDiag;
		max = middle + halfCubeDiag;
	}

	//extend for bodies
	void Box::extendForBodies(const std::vector<Body>& bodies) {
		for (const auto& body : bodies) {
			min.MinComponents(body.position);
			max.MaxComponents(body.position);
		}
	}

	//extract bodies within box
	std::vector<Body> Box::extractBodies(std::vector<Body>& bodies) const {
		auto i = std::stable_partition(std::begin(bodies), std::end(bodies), [&](const Body& b) {return !(min <= b.position && b.position <= max); });
		std::vector<Body> result{ i, std::end(bodies) };
		bodies.erase(i, std::end(bodies));
		return result;
	}

	//copy bodies within box
	std::vector<Body> Box::copyBodies(const std::vector<Body>& bodies) const {
		std::vector<Body> result;
		std::copy_if(std::begin(bodies), std::end(bodies), std::back_inserter(result), [&](const Body& body) {
			return min <= body.position && body.position <= max;
		});
		return result;
	}

	//check for body inside box
	bool isContained(const Body& body, const Box& box) {
		return !(box.min > body.position || body.position > box.max);
	}

	//check for box inside box
	bool isContained(const Box& inner, const Box& outer) {
		return !(outer.min > inner.min || inner.max > outer.max);
	}

	//box volume
	double Box::volume() const {
		if (!isValid()) {
			return -1.0;
		}
		auto diagonal = max - min;
		return diagonal.X * diagonal.Y * diagonal.Z;
	}

	double Box::maxSidelength() const {
		if (!isValid()) {
			return -1.0;
		}
		auto diagonal = max - min;
		return std::max({ diagonal.X, diagonal.Y, diagonal.Z });
	}

	bool Box::isCorrectBox() const {
		if (!isValid()) {
			std::cout << "inverted bb\n";
			return false;
		}
		return true;
	}

	bool Box::isValid() const {
		return max > min;
	}

	void Box::printBB(std::size_t parallelId) const {
		std::cout << parallelId << ": min :" << min;
		std::cout << parallelId << ": max :" << max;
		std::cout << '\n';
	}


	//distance from nearest box order to position
	double Box::distanceToPosition(const Vec3& position) const {
		if (!isValid()) {
			return std::numeric_limits<double>::max();
		}

		bool isInside{ min <= position && position <= max };

		if (isInside) {
			return 0.0;
		} else {
			auto nextpos = position;
			nextpos.ClampComponents(min, max);
			return Distance(nextpos, position);
		}
	}

	//box - box distance
	double Box::distanceToBox(const Box& box2) const {
		double length = 0.0;

		if (!isValid() || !box2.isValid()) {
			return std::numeric_limits<double>::max();
		}

		for (std::size_t i = 0; i < 3; i++) {
			double elem;

			if (box2.min[i] < min[i] && box2.max[i] < min[i]) {
				elem = min[i] - box2.max[i];
			} else if (box2.min[i] > max[i] && box2.max[i] > max[i]) {
				elem = box2.min[i] - max[i];
			} else {
				elem = 0.0;
			}
			length += elem * elem;
		}
		return std::sqrt(length);
	}

	//determine octree subboxes
	std::vector<Box> Box::octreeSplit() const {
		if (!isValid()) {
			return std::vector<Box>{};
		}

		const auto diag = max - min;
		const auto mid = min + (diag / 2);

		return std::vector<Box> { 
			{ { min.X, min.Y, min.Z }, { mid.X, mid.Y, mid.Z } },
			{ { mid.X, min.Y, min.Z }, { max.X, mid.Y, mid.Z } },
			{ { min.X, mid.Y, min.Z }, { mid.X, max.Y, mid.Z } },
			{ { mid.X, mid.Y, min.Z }, { max.X, max.Y, mid.Z } },
			{ { min.X, min.Y, mid.Z }, { mid.X, mid.Y, max.Z } },
			{ { mid.X, min.Y, mid.Z }, { max.X, mid.Y, max.Z } },
			{ { min.X, mid.Y, mid.Z }, { mid.X, max.Y, max.Z } },
			{ { mid.X, mid.Y, mid.Z }, { max.X, max.Y, max.Z } }};
	}

	//split box into two across longest side
	std::vector<Box> Box::splitLongestSide() const {
		std::vector<Box> result;
		std::size_t longestIndex = 0;
		double longestSide = -1.0;

		if (!isValid()) {
			return result;
		}
		for (std::size_t i = 0; i < 3; i++) {
			if (max[i] - min[i] > longestSide) {
				longestSide = max[i] - min[i];
				longestIndex = i;
			}
		}
		double middle = min[longestIndex] + (max[longestIndex] - min[longestIndex]) / 2.0;
		result.push_back(*this);
		result.back().max[longestIndex] = middle;
		result.push_back(*this);
		result.back().min[longestIndex] = middle;
		return result;
	}

	//check for position in box
	bool Box::contained(const Vec3& position) const {
		if (!isValid()) {
			return false;
		}
		return min <= position && position <= max;
	}

	//extend box by box
	void Box::extend(const Box& extender) {
		if (!extender.isValid()) {
			return;
		}
		min.MinComponents(extender.min);
		max.MaxComponents(extender.max);
	}

	//extend box by body
	void Box::extend(const Body& extender) {
		min.MinComponents(extender.position);
		max.MaxComponents(extender.position);
	}

} // namespace nbody
