#include <iostream>
#include <unistd.h>
#include "Node.hpp"
#include "BarnesHutTreeThreaded.hpp"
#include "PthreadSimulation.hpp"

namespace nbody {
	PthreadSimulation::PthreadSimulation(string inputFile) : Simulation() {
		this->iterations = 1;
		if (!this->readInputData(inputFile)) {
			cerr << "input file error" << endl;
			exit(-1);
		}
		int numThreads = sysconf(_SC_NPROCESSORS_ONLN);
		for (int i = 0; i < numThreads; i++) {
			pthread_t thread;

			this->threads.push_back(thread);
		}
		pthread_barrier_init(&this->barrier, NULL, this->threads.size());
		this->tree = new BarnesHutTreeThreaded(0);
		this->tree->simulation = this;
	}

	PthreadSimulation::~PthreadSimulation() {
		pthread_barrier_destroy(&this->barrier);
		delete this->tree;
		this->tree = NULL;
	}

	void PthreadSimulation::setIterations(int iterations) {
		this->iterations = iterations;
	}

	int PthreadSimulation::getNumberOfProcesses() {
		return this->threads.size();
	}

	int PthreadSimulation::getProcessId() {
		return PthreadSimulation::getThreadId(pthread_self(), this->threads);
	}

	int PthreadSimulation::getThreadId(pthread_t thread, vector<pthread_t> threads) {
		if (threads.empty()) {
			return -1;
		}
		vector<pthread_t>::iterator it = threads.begin();
		int id = 0;

		while (*it != thread && it != threads.end()) {
			id++;
			it++;
		}
		return (it != threads.end()) ? id : -1;
	}

	void* PthreadSimulation::run(void* data) {
		NodesToProcess* toProcess = (NodesToProcess*) data;
		int threadId = PthreadSimulation::getThreadId(pthread_self(), *toProcess->threads);

		for (int i = 0; i < toProcess->iterations; i++) {
			pthread_mutex_lock(&toProcess->mutex);
			cout << "Thread " << threadId << ": Iteration " << i << ": Build tree ..." << endl;
			pthread_mutex_unlock(&toProcess->mutex);
			while (!BarnesHutTreeThreaded::hasNodeProcessingFinished(toProcess)) {
				Node* current = BarnesHutTreeThreaded::getNodeToProcess(toProcess);

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

						while (child != NULL) {
							BarnesHutTreeThreaded::addNodeToProcess(child, toProcess);
							child = child->nextSibling;
						}
					}
				}
				BarnesHutTreeThreaded::checkNodeProcessingFinished(toProcess);
			}
			pthread_mutex_lock(&toProcess->mutex);
			cout << "Thread " << threadId << ": Iteration " << i << ": Compute forces ..." << endl;
			pthread_mutex_unlock(&toProcess->mutex);
			while (!BarnesHutTreeThreaded::hasNodeProcessingFinished(toProcess)) {
				Node* current = BarnesHutTreeThreaded::getNodeToProcess(toProcess);

				if (current != NULL) {
					for (vector<Body>::iterator it = current->bodies.begin(); it != current->bodies.end(); it++) {
						if (!it->refinement) {
							toProcess->tree->accumulateForceOnto(*it);
						}
					}
				}
				BarnesHutTreeThreaded::checkNodeProcessingFinished(toProcess);
			}
			pthread_barrier_wait(toProcess->barrier);
			vector<Body> extractBodies;

			extractBodies.clear();
			for (Node* n = toProcess->tree->nodes->next; n != toProcess->tree->nodes; n = n->next) {
				if (n->lock.writeLock()) {
					n->extractLocalBodiesTo(extractBodies);
					n->lock.unLock();
				}
			}
			pthread_mutex_lock(&toProcess->mutex);
			cout << "Thread " << threadId << ": Iteration " << i << ": Move bodies ..." << endl;
			pthread_mutex_unlock(&toProcess->mutex);
			for (vector<Body>::iterator it = extractBodies.begin(); it != extractBodies.end(); it++) {
				integrate(*it);
			}
			pthread_mutex_lock(&toProcess->mutex);
			toProcess->bodies->insert(toProcess->bodies->end(), extractBodies.begin(), extractBodies.end());
			pthread_mutex_unlock(&toProcess->mutex);
			if (pthread_barrier_wait(toProcess->barrier) == PTHREAD_BARRIER_SERIAL_THREAD) {
				cout << "Thread " << threadId << ": Iteration " << i << ": Cleanup of shared memory tree ..." << endl;
				Box domain;

				toProcess->tree->clean();
				initBox(domain);
				toProcess->tree->init(*(toProcess->bodies), domain);
				toProcess->tree->clearNodesToProcess();
				toProcess->tree->nodesToProcess.toProcess.push_back(toProcess->tree->nodes->next);
				toProcess->bodies->clear();
			}
			pthread_barrier_wait(toProcess->barrier);
		}
		pthread_exit(NULL);
	}

	void PthreadSimulation::run() {
		Box domain;
		BarnesHutTreeThreaded* bhtTree = (BarnesHutTreeThreaded*) this->tree;

		initBox(domain);
		bhtTree->init(this->bodies, domain);
		this->bodies.clear();
		bhtTree->clearNodesToProcess();
		bhtTree->nodesToProcess.tree = bhtTree;
		bhtTree->nodesToProcess.barrier = &this->barrier;
		bhtTree->nodesToProcess.iterations = this->iterations;
		bhtTree->nodesToProcess.bodies = &this->bodies;
		bhtTree->nodesToProcess.threads = &this->threads;
		bhtTree->nodesToProcess.toProcess.push_back(bhtTree->nodes->next);
		for (int i = 0; i < this->getNumberOfProcesses(); i++) {
			pthread_create(&this->threads[i], NULL, PthreadSimulation::run, &bhtTree->nodesToProcess);
		}
		for (int i = 0; i < this->getNumberOfProcesses(); i++) {
			void* retVal;

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

}
