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

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->weight = 1.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->weight = 1.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->weight = 1.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->weight = weight;
	}

	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->weight = weight;
	}

	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];

		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;
		}
	}

	double Body::getPosition(int index) {
		return this->position[index];
	}

	double Body::getMass() {
		return this->weight;
	}

	void Body::setPosition(int index, double value) {
		this->position[index] = value;
	}

	void Body::setMass(double value) {
		this->weight = value;
	}


	void Body::print() {
		cout << this->position[0] << " " << this->position[1] << " " << this->position[2] << endl;
	}
}