#include <climits>
#include <cfloat>
#include <algorithm>
#include <iostream>
#include "BarnesHutTreeThreaded.hpp"
#include "Node.hpp"
#include "PthreadSimulation.hpp"

namespace nbody {
	using namespace std;

	BarnesHutTreeThreaded::BarnesHutTreeThreaded(int parallelId) : BarnesHutTree(parallelId) {
		pthread_mutex_init(&this->nodesToProcess.mutex, nullptr);
	}

	BarnesHutTreeThreaded::~BarnesHutTreeThreaded() {
		pthread_mutex_destroy(&this->nodesToProcess.mutex);
	}

	void BarnesHutTreeThreaded::addNodeToProcess(Node* node, NodesToProcess* store) {
		pthread_mutex_lock(&store->mutex);
		store->toProcess.push_back(node);
		pthread_mutex_unlock(&store->mutex);
	}

	Node* BarnesHutTreeThreaded::getNodeToProcess(NodesToProcess* store) {
		Node* result;

		pthread_mutex_lock(&store->mutex);
		if (store->toProcess.empty()) {
			result = nullptr;
		} else {
			result = store->toProcess.back();
			store->toProcess.pop_back();
			vector<pthread_t>::iterator it = std::find(store->processing.begin(), store->processing.end(), pthread_self());
			if (it == store->processing.end()) {
				store->processing.push_back(pthread_self());
			}
		}
		pthread_mutex_unlock(&store->mutex);
		return result;
	}

	void BarnesHutTreeThreaded::checkNodeProcessingFinished(NodesToProcess* store) {
		pthread_mutex_lock(&store->mutex);
		if (store->toProcess.empty()) {
			vector<pthread_t>::iterator it = std::find(store->processing.begin(), store->processing.end(), pthread_self());
			if (it != store->processing.end()) {
				store->processing.erase(it);
			}
		}
		pthread_mutex_unlock(&store->mutex);
	}

	bool BarnesHutTreeThreaded::hasNodeProcessingFinished(NodesToProcess* store) {
		bool finished;

		pthread_mutex_lock(&store->mutex);
		finished = store->processing.empty() && store->toProcess.empty();
		pthread_mutex_unlock(&store->mutex);
		return finished;
	}

	void BarnesHutTreeThreaded::updateNode(Node* current) {
		current->representative.id = ULONG_MAX;
		current->representative.mass = 0.0;
		current->representative.position[0] = 0.0;
		current->representative.position[1] = 0.0;
		current->representative.position[2] = 0.0;
		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;
			}
		}
		for (int j = 0; j < 3; j++) {
			if (current->representative.mass > FLT_EPSILON) {
				current->representative.position[j] /= current->representative.mass;
			} else {
				current->representative.position[j] = 0.0;
			}
		}
	}

	void* BarnesHutTreeThreaded::build(void* data) {
		NodesToProcess* toProcess = (NodesToProcess*) data;

		while (!BarnesHutTreeThreaded::hasNodeProcessingFinished(toProcess)) {
			Node* current = BarnesHutTreeThreaded::getNodeToProcess(toProcess);

			if (current != nullptr) {
				BarnesHutTreeThreaded::updateNode(current);
				if (BarnesHutTree::splitNode(current)) {
					Node* child = current->next;

					while (child != nullptr) {
						BarnesHutTreeThreaded::addNodeToProcess(child, toProcess);
						child = child->nextSibling;
					}
				}
			}
			BarnesHutTreeThreaded::checkNodeProcessingFinished(toProcess);
		}
		pthread_exit(nullptr);
	}

	void* BarnesHutTreeThreaded::computeMove(void* data) {
		NodesToProcess* toProcess = (NodesToProcess*) data;
		vector<Body*> localBodies;

		while (!BarnesHutTreeThreaded::hasNodeProcessingFinished(toProcess)) {
			Node* current = BarnesHutTreeThreaded::getNodeToProcess(toProcess);

			if (current != nullptr) {
				for (vector<Body>::iterator it = current->bodies.begin(); it != current->bodies.end(); it++) {
					if (!it->refinement) {
						toProcess->tree->accumulateForceOnto(*it);
						localBodies.push_back(&(*it));
					}
				}
			}
			BarnesHutTreeThreaded::checkNodeProcessingFinished(toProcess);
		}
		while (!localBodies.empty()) {
			integrate(*(localBodies.back()));
			localBodies.pop_back();
		}
		pthread_exit(nullptr);
	}

	void BarnesHutTreeThreaded::clearNodesToProcess() {
		this->nodesToProcess.processing.clear();
		this->nodesToProcess.toProcess.clear();
	}

	void BarnesHutTreeThreaded::build(vector<Body> bodies, Box domain) {
		this->init(bodies, domain);
		this->clearNodesToProcess();
		this->nodesToProcess.toProcess.push_back(this->nodes->next);
		for (int i = 0; i < this->simulation->getNumberOfProcesses(); i++) {
			pthread_create((&((PthreadSimulation*) this->simulation)->threads[i]), nullptr, BarnesHutTreeThreaded::build, &this->nodesToProcess);
		}
		for (int i = 0; i < this->simulation->getNumberOfProcesses(); i++) {
			void* retVal;

			pthread_join(((PthreadSimulation*) this->simulation)->threads[i], &retVal);
		}
		if (this->isCorrect()) {
			cout << "correct" << endl;
		}
	}

	void BarnesHutTreeThreaded::computeMove() {
		//accumulate forces for whole tree (local particles) and move particles
		this->nodesToProcess.tree = this;
		for (Node* n = this->nodes->next; n != this->nodes; n = n->next) {
			if (n->leaf && !n->bodies.empty()) {
				this->nodesToProcess.toProcess.push_back(n);
			}
		}
		for (int i = 0; i < this->simulation->getNumberOfProcesses(); i++) {
			pthread_create((&((PthreadSimulation*) this->simulation)->threads[i]), nullptr, BarnesHutTreeThreaded::computeMove, &this->nodesToProcess);
		}
		for (int i = 0; i < this->simulation->getNumberOfProcesses(); i++) {
			void* retVal;

			pthread_join(((PthreadSimulation*) this->simulation)->threads[i], &retVal);
		}
	}
}
