#include "Body.hpp"
#include <stdlib.h>
#include <iostream>
#include <sstream>
#include <fstream>
#include <cmath>
#include <limits>
#include <cstring>
#include <cstdio>
#include <algorithm>
#include <cmath>

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

	//helper method for intergration
	Derivative evaluate(Body body, double dt, Derivative d) {
		double velocity[3];
		Derivative output;

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

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

		if (body.refinement) {
			return;
		}
		a = evaluate(body, 0.0, start);
		b = evaluate(body, timestep * 0.5, a);
		c = evaluate(body, timestep * 0.5, b);
		d = evaluate(body, 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]);
			body.position[i] = body.position[i] + dxdt[i] * timestep;
			body.velocity[i] = body.velocity[i] + dvdt[i] * timestep;
		}
	}

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

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

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

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

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

	void printBody(int parallelId, Body body) {
		std::cout << parallelId << " " << body.id << " Position: " << body.position[0] << " " << body.position[1] << " " << body.position[2] << '\n';
		std::cout << parallelId << " " << body.id << " Velocity: " << body.velocity[0] << " " << body.velocity[1] << " " << body.velocity[2] << '\n';
		std::cout << parallelId << " " << body.id << " Acceleration: " << body.acceleration[0] << " " << body.acceleration[1] << " " << body.acceleration[2] << '\n';
		std::cout << parallelId << " " << body.id << " Mass: " << body.mass << '\n';
		std::cout << parallelId << " " << body.id << " Refinement: " << body.refinement << '\n' << '\n';
	}

	//parse input files
	std::vector<Body> dubinskiParse(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, std::vector<Body> bodies) {
		for (auto& body : bodies) {
			printBody(parallelId, body);
		}
	}

	bool valid(std::vector<Body> bodies) {
		return std::all_of(std::begin(bodies), std::end(bodies), validBody);
	}
}
