#include <iostream>

void _euler() {
	float t = 0;
	float dt = 1;

	float velocity = 0;
	float position = 0;
	float force = 10;
	float mass = 1;

	while ( t <= 10 )
	{
		std::cout << t << " " << position << " " << velocity << std::endl;
		position = position + velocity * dt;
		velocity = velocity + ( force / mass ) * dt;
		t = t + dt;
	}
}

struct State
{
	float x;          // position
	float v;          // velocity
};

struct Derivative
{
	float dx;          // derivative of position: velocity
	float dv;          // derivative of velocity: acceleration
};

float acceleration(const State &state, float t)
{
/*	const float k = 10;
	const float b = 1;
	return -k * state.x - b*state.v;*/
	return 10;
}

Derivative evaluate(const State &initial, float t, float dt, const Derivative &d)
{
	State state;
	state.x = initial.x + d.dx*dt;
	state.v = initial.v + d.dv*dt;

	Derivative output;
	output.dx = state.v;
	output.dv = acceleration(state, t+dt);
	return output;
}

void _rk4Integrate(State &state, float t, float dt)
{
	Derivative a = evaluate(state, t, 0.0f, Derivative());
	Derivative b = evaluate(state, t+dt*0.5f, dt*0.5f, a);
	Derivative c = evaluate(state, t+dt*0.5f, dt*0.5f, b);
	Derivative d = evaluate(state, t+dt, dt, c);

	const float dxdt = 1.0f/6.0f * (a.dx + 2.0f*(b.dx + c.dx) + d.dx);
	const float dvdt = 1.0f/6.0f * (a.dv + 2.0f*(b.dv + c.dv) + d.dv);

	state.x = state.x + dxdt * dt;
	state.v = state.v + dvdt * dt;
}

void _rk4() {
    float t = 0;
    float dt = 1;

	State state = { 0, 0 }; // initial position and velocity
    float force = 10;
    float mass = 1;

    while ( t <= 10 )
    {
		std::cout << t << " " << state.x << " " << state.v << std::endl;
		_rk4Integrate(state, t, dt);
		t = t + dt;
    }
}

int main(int argc, char *argv[]) {
	//_euler();
	_rk4();

	return 0;
}