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

namespace nbody {
	//extend box to form cube
	void Box::extendToCube() {
		std::size_t longestSide = std::numeric_limits<std::size_t>::max();
		double sidelength = 0.0;

		for (std::size_t i = 0; i < 3; i++) {
			if (max[i] - min[i] >= sidelength) {
				longestSide = i;
				sidelength = max[i] - min[i];
			}
		}
		if (longestSide == std::numeric_limits<std::size_t>::max()) {
			return;
		}
		for (std::size_t i = 0; i < 3; i++) {
			if (i != longestSide) {
				double extend = (sidelength - (max[i] - min[i])) / 2.0;

				min[i] -= extend;
				max[i] += extend;
			}
		}
	}

	//extend for bodies
	void Box::extendForBodies(const std::vector<Body>& bodies) {
		for (const auto& body : bodies) {
			for (std::size_t i = 0; i < 3; i++) {
				min[i] = std::min(body.position[i], min[i]);
				max[i] = std::max(body.position[i], max[i]);
			}
		}
	}

	//extract bodies within box
	std::vector<Body> Box::extractBodies(std::vector<Body>& bodies) const {
		std::vector<Body> result;
		auto it = std::begin(bodies);

		while (it != std::end(bodies)) {
			if (it->position[0] >= min[0] && it->position[0] <= max[0] &&
					it->position[1] >= min[1] && it->position[1] <= max[1] &&
					it->position[2] >= min[2] && it->position[2] <= max[2]) {
				result.push_back(*it);
				it = bodies.erase(it);
			} else {
				it++;
			}
		}
		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 body.position[0] >= min[0] && body.position[0] <= max[0] &&
				body.position[1] >=min[1] && body.position[1] <= max[1] &&
				body.position[2] >=min[2] && body.position[2] <= max[2];
		});
		return result;
	}

	//check for body inside box
	bool isContained(const Body& body, const Box& box) {
		for (std::size_t i = 0; i < 3; i++) {
			if (body.position[i] < box.min[i] || body.position[i] > box.max[i]) {
				return false;
			}
		}
		return true;
	}

	//check for box inside box
	bool isContained(const Box& inner, const Box& outer) {
		for (std::size_t i = 0; i < 3; i++) {
			if (inner.min[i] < outer.min[i] || inner.max[i] > outer.max[i]) {
				return false;
			}
		}
		return true;
	}

	//box volume
	double Box::volume() const {
		if (!isValid()) {
			return -1.0;
		}
		double result = 1.0;

		for (std::size_t i = 0; i < 3; i++) {
			result *= max[i] - min[i];
		}
		return result;
	}

	double Box::maxSidelength() const {
		double maxVal = 0.0;

		if (!isValid()) {
			return -1.0;
		}
		for (std::size_t i = 0; i < 3; i++) {
			maxVal = std::max(max[i] - min[i], maxVal);
		}
		return maxVal;
	}

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

	bool Box::isValid() const {
		for (std::size_t i = 0; i < 3; i++) {
			if (max[i] < min[i]) {
				return false;
			}
		}
		return true;
	}

	void Box::printBB(std::size_t parallelId) const {
		std::cout << parallelId << ": min ";
		for (std::size_t i = 0; i < 3; i++) {
			std::cout << ": " << min[i] << " ";
		}
		std::cout << parallelId << ": max ";
		for (std::size_t i = 0; i < 3; i++) {
			std::cout << max[i] << " ";
		}
		std::cout << '\n';
	}


	//distance from nearest box order to position
	double Box::distanceToPosition(const std::array<double, 3>& position) const {
		std::size_t inside = 0;
		std::array<double, 3> nextPosition = position;

		if (!isValid()) {
			return std::numeric_limits<double>::max();
		}
		for (std::size_t i = 0; i < 3; i++) {
			if (nextPosition[i] < min[i]) {
				nextPosition[i] = min[i];
			} else if (nextPosition[i] > max[i]) {
				nextPosition[i] = max[i];
			} else {
				inside++;
			}
		}
		if (inside == 3) {
			return 0.0;
		} else {
			double dist = 0.0;

			for (std::size_t i = 0; i < 3; i++) {
				dist += (nextPosition[i] - position[i]) * (nextPosition[i] - position[i]);
			}
			return sqrt(dist);
		}
	}

	//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 sqrt(length);
	}

	//determine octree subboxes
	std::vector<Box> Box::octreeSplit() const {
		std::vector<Box> result;

		if (!isValid()) {
			return result;
		}
		for (std::size_t i = 0; i < 8; i++) {
			Box current = *this;

			for (std::size_t j = 0; j < 3; j++) {
				double middle = current.min[j] + (current.max[j] - current.min[j]) / 2.0;

				//TODO(steinret): unroll the loop, construct explicitly for better readability
				if ((i & (1 << j)) != 0) {
					current.min[j] = middle;
				} else {
					current.max[j] = middle;
				}
			}
			result.push_back(current);
		}
		return result;
	}

	//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 std::array<double, 3>& position) const {
		if (!isValid()) {
			return false;
		}
		for (std::size_t i = 0; i < 3; i++) {
			if (position[i] < min[i] || position[i] > max[i]) {
				return false;
			}
		}
		return true;
	}

	//extend box by box
	void Box::extend(const Box& extender) {
		if (!extender.isValid()) {
			return;
		}
		for (std::size_t i = 0; i < 3; i++) {
			min[i] = std::min(min[i], extender.min[i]);
			max[i] = std::max(max[i], extender.max[i]);
		}
	}

	//extend box by body
	void Box::extend(const Body& extender) {
		for (std::size_t i = 0; i < 3; i++) {
			min[i] = std::min(min[i], extender.position[i]);
			max[i] = std::max(max[i], extender.position[i]);
		}
	}

} // namespace nbody
