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

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

namespace nbody {
	Body::Body(Vec3 position_,
		Vec3 velocity_,
		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 integration
	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<double>::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 Body::isValid() const {
		return position.IsValid() &&
			velocity.IsValid() &&
			acceleration.IsValid() &&
			validDouble(mass) &&
			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> bodies;
		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;
			bodies.push_back({
				{ px, py, pz },
				{ vx, vy, vz },
				{ 0.0, 0.0, 0.0 },
				mass,
				false,
				id++
			});
		}
		return bodies;
	}
} // namespace nbody
