#include "Body.hpp"
#include <iostream>
#include <sstream>
#include <fstream>
#include <limits>
#include <algorithm>
#include <cmath>

namespace nbody {
	void Body::resetAcceleration() {
		std::fill(std::begin(acceleration), std::end(acceleration), 0.0);
	}

	//helper method for intergration
	Derivative Body::evaluate(double dt, const Derivative& d) const {
		std::array<double, 3> tmp;
		Derivative output;

		for (int i = 0; i < 3; i++) {
			tmp[i] = velocity[i] + d.dv[i] * dt;
			output.dx[i] = tmp[i];
			output.dv[i] = acceleration[i];
		}
		return output;
	}

	//integrating acceleration -> velocity -> position
	void Body::integrate() {
		Derivative start;
		Derivative a, b, c, d;
		double dxdt[3];
		double dvdt[3];

		if (refinement) {
			return;
		}
		a = evaluate(0.0, start);
		b = evaluate(timestep * 0.5, a);
		c = evaluate(timestep * 0.5, b);
		d = evaluate(timestep, c);
		for (int i = 0; i < 3; i++) {
			dxdt[i] = 1.0 / 6.0 * (a.dx[i] + 2.0f * (b.dx[i] + c.dx[i]) + d.dx[i]);
			dvdt[i] = 1.0 / 6.0 * (a.dv[i] + 2.0f * (b.dv[i] + c.dv[i]) + d.dv[i]);
			position[i] = position[i] + dxdt[i] * timestep;
			velocity[i] = velocity[i] + dvdt[i] * timestep;
		}
	}

	//gravitational force computation
	void Body::accumulateForceOntoBody(const Body& from) {
		double distance2 = 0.0;

		distance2 += (from.position[0] - position[0]) * (from.position[0] - position[0]);
		distance2 += (from.position[1] - position[1]) * (from.position[1] - position[1]);
		distance2 += (from.position[2] - position[2]) * (from.position[2] - position[2]);
		distance2 = std::max<double>(distance2, std::numeric_limits<float>::epsilon());
		double distance = sqrt(distance2);
		double mdist = -1.0 * ((from.mass * mass) / distance2);
		for (int i = 0; i < 3; i++) {
			double vec = (from.position[i] - position[i]) / distance;

			acceleration[i] += vec * mdist;
		}
	}

	bool validDouble(double val) {
		return !std::isnan(val) && std::isfinite(val);
	}

	bool Body::isValid() const {
		if (!std::all_of(std::begin(position), std::end(position), validDouble)) return false;
		if (!std::all_of(std::begin(velocity), std::end(velocity), validDouble)) return false;
		if (!std::all_of(std::begin(acceleration), std::end(acceleration), validDouble)) return false;
		if (!validDouble(mass)) return false;
		return mass >= 0.0;
	}

	void Body::print(int parallelId) const { //TODO(steinret): do printing via put
		std::cout << parallelId << " " << id << " Position: " << position[0] << " " << position[1] << " " << position[2] << '\n';
		std::cout << parallelId << " " << id << " Velocity: " << velocity[0] << " " << velocity[1] << " " << velocity[2] << '\n';
		std::cout << parallelId << " " << id << " Acceleration: " << acceleration[0] << " " << acceleration[1] << " " << acceleration[2] << '\n';
		std::cout << parallelId << " " << id << " Mass: " << mass << '\n';
		std::cout << parallelId << " " << id << " Refinement: " << refinement << '\n' << '\n';
	}

	//parse input files
	std::vector<Body> dubinskiParse(const std::string& filename) {
		std::vector<Body> result;
		std::string line;
		std::ifstream infile(filename.c_str(), std::ifstream::in);
		double mass, px, py, pz, vx, vy, vz;
		unsigned long id = 0;

		while (std::getline(infile, line)) {
			std::istringstream iss(line);
			Body b;

			//not all input files have velocity, so initialize properly
			vx = vy = vz = 0.0;
			iss >> mass >> px >> py >> pz >> vx >> vy >> vz;
			b.position[0] = px;
			b.position[1] = py;
			b.position[2] = pz;
			b.velocity[0] = vx;
			b.velocity[1] = vy;
			b.velocity[2] = vz;
			b.acceleration[0] = 0.0;
			b.acceleration[1] = 0.0;
			b.acceleration[2] = 0.0;
			b.refinement = 0;
			b.mass = mass;
			b.id = id++;
			result.push_back(b);
		}
		return result;
	}


	void printBodies(int parallelId, const std::vector<Body>& bodies) {
		for (auto& body : bodies) {
			body.print(parallelId);
		}
	}

	bool valid(const std::vector<Body>& bodies) {
		return std::all_of(std::begin(bodies), std::end(bodies), [](const Body& b) { return b.isValid(); });
	}
}
