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

namespace nbody {
	void initBox(Box& box) {
		std::fill(std::begin(box.min), std::end(box.min), FLT_MAX); //TODO(steinret) numeric limits
		std::fill(std::begin(box.max), std::end(box.max), FLT_MIN);
	}

	//extend box to form cube
	void extendToCube(Box& box) {
		int longestSide = -1;
		double sidelength = 0.0;

		for (int i = 0; i < 3; i++) {
			if (box.max[i] - box.min[i] >= sidelength) {
				longestSide = i;
				sidelength = box.max[i] - box.min[i];
			}
		}
		if (longestSide == -1) {
			return;
		}
		for (int i = 0; i < 3; i++) {
			if (i != longestSide) {
				double extend = (sidelength - (box.max[i] - box.min[i])) / 2.0;

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

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

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

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

	//copy bodies within box
	std::vector<Body> copyBodies(Box box, std::vector<Body> bodies) { //TODO(steinret) std::copy_if ??
		std::vector<Body> result;

		for (auto& body : bodies) {
			if (body.position[0] >= box.min[0] && body.position[0] <= box.max[0] &&
					body.position[1] >= box.min[1] && body.position[1] <= box.max[1] &&
					body.position[2] >= box.min[2] && body.position[2] <= box.max[2]) {
				result.push_back(body);
			}
		}
		return result;
	}

	//check for body inside box
	bool isContained(Body body, Box box) {
		for (int 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(Box inner, Box outer) {
		for (int 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 volume(Box box) {
		if (!isValid(box)) {
			return -1.0;
		}
		double result = 1.0;

		for (int i = 0; i < 3; i++) {
			result *= box.max[i] - box.min[i];
		}
		return result;
	}

	double maxSidelength(Box box) {
		double maxVal = 0.0;

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

	bool isCorrectBox(Box box) {
		for (int i = 0; i < 3; i++) {
			if (box.max[i] < box.min[i]) {
				std::cout << "inverted bb\n";
				return false;
			}
		}
		return true;
	}

	bool isValid(Box box) {
		for (int i = 0; i < 3; i++) {
			if (box.max[i] < box.min[i]) {
				return false;
			}
		}
		return true;
	}

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

	//check for box/sphere overlap
	bool overlapsSphere(Box box, double* sphereCenter, double sphereRadius) {
		double dmin = 0.0;

		if (!isValid(box)) {
			return false;
		}
		for (int i = 0; i < 3; i++) {
			if (sphereCenter[i] < box.min[i]) {
				double dist = sphereCenter[i] - box.min[i];

				dmin += dist * dist;
			} else if (sphereCenter[i] > box.max[i]) {
				double dist = sphereCenter[i] - box.max[i];

				dmin += dist * dist;
			}
		}
		return dmin <= sphereRadius * sphereRadius;
	}

	//distance from nearest box order to position
	double distanceToPosition(Box box, double* position) {
		int inside = 0;
		double nextPosition[3] = {position[0], position[1], position[2]};

		if (!isValid(box)) {
			return DBL_MAX;
		}
		for (int i = 0; i < 3; i++) {
			if (nextPosition[i] < box.min[i]) {
				nextPosition[i] = box.min[i];
			} else if (nextPosition[i] > box.max[i]) {
				nextPosition[i] = box.max[i];
			} else {
				inside++;
			}
		}
		if (inside == 3) {
			return 0.0;
		} else {
			double dist = 0.0;

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

	//box - box distance
	double distanceToBox(Box box1, Box box2) {
		double length = 0.0;

		if (!isValid(box1) || !isValid(box2)) {
			return DBL_MAX;
		}
		for (int i = 0; i < 3; i++) {
			double elem;

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

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

		if (!isValid(box)) {
			return result;
		}
		for (unsigned int i = 0; i < 8; i++) {
			Box current = box;

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

				if (i & (1 << j)) {
					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> splitLongestSide(Box box) {
		std::vector<Box> result;
		int longestIndex = -1;
		double longestSide = -1.0;

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

	//check for position in box
	bool contained(Box box, std::array<double, 3> position) {
		if (!isValid(box)) {
			return false;
		}
		for (int i = 0; i < 3; i++) {
			if (position[i] < box.min[i] || position[i] > box.max[i]) {
				return false;
			}
		}
		return true;
	}

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

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

}
