#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <limits>
#include <sstream>

#include "Body.hpp"
#include "Vec3.hpp"

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

	void Body::resetAcceleration() {
		acceleration = Vec3{ 0.0 };
	}

	//helper method for intergration
	Derivative Body::evaluate(double dt, const Derivative& d) const {
		return Derivative{
			velocity + d.dv * dt,
			acceleration
		};
	}

	//integrating acceleration -> velocity -> position
	void Body::integrate() {
		if (refinement) {
			return;
		}
		const auto a = evaluate(0.0, Derivative{});
		const auto b = evaluate(timestep * 0.5, a);
		const auto c = evaluate(timestep * 0.5, b);
		const auto d = evaluate(timestep, c);

		const auto dxdt = Vec3{ 1.0 / 6.0 } *(a.dx + Vec3{ 2.0 } *(b.dx + c.dx) + d.dx);
		const auto dvdt= Vec3{ 1.0 / 6.0 } * (a.dv + Vec3{ 2.0 } * (b.dv + c.dv) + d.dv);
		position = position + dxdt * timestep;
		velocity = velocity + dvdt * timestep;
	}

	//gravitational force computation
	void Body::accumulateForceOntoBody(const Body& from) {
		const auto distance2 = std::max<double>(
			SquaredDistance(from.position, position), 
			std::numeric_limits<float>::epsilon());
		const auto distance = sqrt(distance2);
		const auto mdist = -1.0 * ((from.mass * mass) / distance2);

		const auto vec = (from.position - position) / distance;
		acceleration += vec * mdist;
	}

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

	bool Body::isValid() const {
		if (!position.IsValid()) return false;
		if (!velocity.IsValid()) return false;
		if (!acceleration.IsValid()) 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 << '\n';
		std::cout << parallelId << " " << id << " Velocity: " << velocity << '\n';
		std::cout << parallelId << " " << id << " Acceleration: " << acceleration << '\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
