#include "BarnesHutTree.hpp"
#include "Node.hpp"
#include <iostream>

namespace nbody {
	using namespace std;

	BarnesHutTree::BarnesHutTree() {
		this->maxLeafBodies = 16;
	}

	BarnesHutTree::~BarnesHutTree() {
	}

	vector<Box> BarnesHutTree::splitBB(Node* node) {
		return node->getBB().octreeSplit();
	}

	int BarnesHutTree::numberOfChildren() {
		return 8;
	}

	void BarnesHutTree::update() {
		//iterate for updating representatives
		Node* current = this->nodes->prev;
		while (current != this->nodes) {
			current->representative.mass = 0.0;
			current->representative.position[0] = 0.0;
			current->representative.position[1] = 0.0;
			current->representative.position[2] = 0.0;
			if (current->leaf) {
				for (unsigned int i = 0; i < current->bodies.size(); i++) {
					current->representative.mass += current->bodies[i].mass;
					for (int j = 0; j < 3; j++) {
						current->representative.position[j] += current->bodies[i].position[j] * current->bodies[i].mass;
					}
				}
			} else {
				Node* child = current->next;

				do {
					current->representative.mass += child->representative.mass;
					for (int j = 0; j < 3; j++) {
						current->representative.position[j] += child->representative.position[j] * child->representative.mass;
					}
					child = child->nextSibling;
				} while (child != NULL);
			}
			for (int j = 0; j < 3; j++) {
				current->representative.position[j] /= current->representative.mass;
			}
			current = current->prev;
		}
	}

	void BarnesHutTree::split(Node* current) {
		vector<Box> subboxes = BarnesHutTree::splitBB(current);
		current->leaf = false;
		Node* after = current->next;

		for (vector<Box>::iterator it = subboxes.begin(); it != subboxes.end(); it++) {
			Node* child = new Node(current->tree);

			pthread_rwlock_wrlock(&child->lock);
			child->parent = current;
			child->bb = *it;
			child->bodies = it->copyBodies(current->bodies);
			child->nextSibling = NULL;
			child->prevSibling = NULL;
			after->insertBefore(child);
			if (it != subboxes.begin()) {
				child->prev->nextSibling = child;
				child->prevSibling = child->prev;
				child->prev->afterSubtree = child;
			}
			//this->control.toProcess.push(child);
			//cout << "storing " << child << endl;
		}
		after->prev->afterSubtree = current->afterSubtree;
		current->bodies.clear();
		for (Node* n = current->next; n != NULL; n = n->nextSibling) {
			pthread_rwlock_unlock(&n->lock);
		}
	}

	void BarnesHutTree::splitNode(Node* current) {
		//this->control.toProcess.pop();
		//cout << "storing " << current << endl;
		if (current->isSplitable()) {
			split(current);
		}
		//cout << "removing " << this->control.toProcess.top() << endl;
		//this->control.toProcess.pop();
	}

	void BarnesHutTree::build(vector<Body> bodies, Box domain) {
		Node* current;

		this->clean();
		if (bodies.empty()) return;
		//insert root node
		this->nodes->insertAfter(new Node(this));
		current = this->nodes->next;
		//assign bodies to root node
		current->bodies = bodies;
		//setup proper bounding box
		current->bb = domain;
		current->extendBBforBodies();
		current->extendBBtoCube();
		current->afterSubtree = current->next;
		//this->control.toProcess.push(current);
		//iterate over existing boxes and split if it contains too much bodies
		/*
		while (current != this->nodes) {
			this->splitNode(current);
			current = current->next;
		}
		*/
		BarnesHutTree::splitTree(this->nodes->next);
		this->update();
	}

	void BarnesHutTree::build(vector<Body> bodies) {
		Box bb;

		bb.extendForBodies(bodies);
		this->build(bodies, bb);
	}

	void BarnesHutTree::mergeLET(vector<Body> bodies) {
		//put all new bodies into fitting leaves, walk through tree and split
		Node* current;

		for (vector<Body>::iterator it = bodies.begin(); it != bodies.end(); it++) {
			current = this->nodes->next;
			while (!current->leaf) {
				Node* child = current->next;

				while (child != NULL && !child->getBB().contained(it->position)) {
					child = child->nextSibling;
				}
				if (child == NULL) {
					cout << it - bodies.begin() << " Error: not contained: " << it->position[0] << " " << it->position[1] << " " << it->position[2] << endl;
				} else {
					current = child;
				}
			}
			current->bodies.push_back(*it);
			current->bodies.back().refinement = true;
		}
		current = this->nodes->next;
		//this->control.toProcess.push(current);
		while (current != this->nodes) {
			this->splitNode(current);
			current = current->next;
		}
		this->update();
	}

	/*
	void BarnesHutTree::splitTree(Node* root) {
		bool toSplitLeft = false;
		Node* current = root;

		do {
			toSplitLeft = false;
			while (current != root->prev) {
				if (!pthread_rwlock_tryrdlock(&current->lock)) {
					//read lock acquired
					if (current->isSplitable()) {
						pthread_rwlock_unlock(&current->lock);
						if (!pthread_rwlock_trywrlock(&current->lock)) {
							//write lock acquired
							split(current);
							pthread_rwlock_unlock(&current->lock);
						}
						toSplitLeft = true;
					} else {
						pthread_rwlock_unlock(&current->lock);
					}
				} else {
					toSplitLeft = true;
				}
			}
		} while (toSplitLeft);
	}
	*/

	void BarnesHutTree::splitTree(Node* root) {
		bool toSplitLeft = false;
		Node* current = root;

		do {
			toSplitLeft = false;
			while (current != root->prev) {
				if (current->isSplitable()) {
					split(current);
					toSplitLeft = true;
				}
				current = current->next;
			}
		} while (toSplitLeft);
	}
}
