#include "Body.hpp"
#include <stdlib.h>
#include <iostream>
#include <cmath>
#include <cfloat>

namespace nbody {
	using namespace std;

	class Derivative {
		friend class Body;
	private:
		double dx[3];
		double dv[3];

		Derivative() {
			for (int i = 0; i < 3; i++) {
				this->dx[i] = 0.0;
				this->dv[i] = 0.0;
			}
		}

		~Derivative() {}
	};

	Body::Body() {
		for (int i = 0; i < 3; i++) {
			this->position[i] = 0.0;
			this->velocity[i] = 0.0;
			this->acceleration[i] = 0.0;
		}
		this->mass = 1.0;
		this->refinement = 0;
	}

	Body::Body(double positionX, double positionY, double positionZ) {
		for (int i = 0; i < 3; i++) {
			this->velocity[i] = 0.0;
			this->acceleration[i] = 0.0;
		}
		this->position[0] = positionX;
		this->position[1] = positionY;
		this->position[2] = positionZ;
		this->mass = 1.0;
		this->refinement = 0;
	}

	Body::Body(double positionX, double positionY, double positionZ, double velocityX, double velocityY, double velocityZ) {
		for (int i = 0; i < 3; i++) {
			this->acceleration[i] = 0.0;
		}
		this->position[0] = positionX;
		this->position[1] = positionY;
		this->position[2] = positionZ;
		this->velocity[0] = velocityX;
		this->velocity[1] = velocityY;
		this->velocity[2] = velocityZ;
		this->mass = 1.0;
		this->refinement = 0;
	}

	Body::Body(double positionX, double positionY, double positionZ, double weight) {
		for (int i = 0; i < 3; i++) {
			this->velocity[i] = 0.0;
			this->acceleration[i] = 0.0;
		}
		this->position[0] = positionX;
		this->position[1] = positionY;
		this->position[2] = positionZ;
		this->mass = weight;
		this->refinement = 0;
	}

	Body::Body(double positionX, double positionY, double positionZ, double velocityX, double velocityY, double velocityZ, double weight) {
		for (int i = 0; i < 3; i++) {
			this->acceleration[i] = 0.0;
		}
		this->position[0] = positionX;
		this->position[1] = positionY;
		this->position[2] = positionZ;
		this->velocity[0] = velocityX;
		this->velocity[1] = velocityY;
		this->velocity[2] = velocityZ;
		this->mass = weight;
		this->refinement = 0;
	}

	Body::~Body() {

	}

	Derivative Body::evaluate(double dt, Derivative d) {
		double velocity[3];
		Derivative output;

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

	void Body::integrate() {
		Derivative start;
		Derivative a, b, c, d;
		double dxdt[3];
		double dvdt[3];

		if (this->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]);
			this->position[i] = this->position[i] + dxdt[i] * timestep;
			this->velocity[i] = this->velocity[i] + dvdt[i] * timestep;
		}
		//this->print();
	}

	void Body::accumulateForceOnto(Body& to) {
		double distance2 = 0.0;

		distance2 += (this->position[0] - to.position[0]) * (this->position[0] - to.position[0]);
		distance2 += (this->position[1] - to.position[1]) * (this->position[1] - to.position[1]);
		distance2 += (this->position[2] - to.position[2]) * (this->position[2] - to.position[2]);
		if (fabs(distance2) < FLT_EPSILON) {
			distance2 = FLT_EPSILON;
		}
		double distance = sqrt(distance2);
		double mdist = -1.0 * ((this->mass * to.mass) / distance2);
		for (int i = 0; i < 3; i++) {
			double vec = (this->position[i] - to.position[i]) / distance;

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

	void Body::resetAcceleration() {
		for (int i = 0; i < 3; i++) {
			this->acceleration[i] = 0.0;
		}
	}

	bool Body::valid() {

		for (int i = 0; i < 3; i++) {
			if (this->position[i] < -FLT_MAX || this->position[i] > FLT_MAX) {
				return false;
			}
		}
		return this->mass >= 0.0;
	}

	void Body::print() {
		cout << "Position: " << this->position[0] << " " << this->position[1] << " " << this->position[2] << endl;
		cout << "Velocity: " << this->velocity[0] << " " << this->velocity[1] << " " << this->velocity[2] << endl;
		cout << "Acceleration: " << this->acceleration[0] << " " << this->acceleration[1] << " " << this->acceleration[2] << endl;
		cout << "Mass: " << this->mass << endl;
		cout << "Refinement: " << this->refinement << endl << endl;
	}
}