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

namespace nbody {
	Body::Body(const std::array<double, 3>& position_,
		const std::array<double, 3>& velocity_,
		const std::array<double, 3>& acceleration_,
		double mass_,
		bool refinement_,
		std::size_t id_) :
		id(id_),
		position(position_),
		velocity(velocity_),
		acceleration(acceleration_),
		mass(mass_),
		refinement(refinement_) {}

	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 (std::size_t 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 (std::size_t 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 (std::size_t 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(std::size_t parallelId) const {
		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);

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

			double mass, px, py, pz, vx, vy, vz;
			std::size_t id = 0;
			//not all input files have velocity, so initialize properly
			vx = vy = vz = 0.0;
			iss >> mass >> px >> py >> pz >> vx >> vy >> vz;
			Body b{
				{{px, py, pz}},
				{{vx, vy, vz}},
				{{0.0, 0.0, 0.0}},
				mass,
				false,
				id++
			};
			result.push_back(b);
		}
		return result;
	}


	void printBodies(std::size_t parallelId, const std::vector<Body>& bodies) {
		for (const 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(); });
	}
} // namespace nbody
