#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <ctime>

using namespace std;

/* car properties. */
struct car_t
{
	double position_x;
	double velocity_x;
	double desired_velocity;
	double desired_time_headway;
	double max_acceleration;
	double desired_deceleration;
	double acceleration_exponent;
	double jam_distance;
	double vehicle_length;
	double net_distance;
	double approaching_rate;
};

double position_derivative(car_t o)
{
	return o.velocity_x;
}

double velocity_derivative(car_t o)
{
	double term_a = exp(o.acceleration_exponent * log(o.velocity_x / o.desired_velocity));

	double term_b_numerator_a = o.jam_distance + (o.velocity_x * o.desired_time_headway);

	double term_b_numerator_b = (o.velocity_x * o.approaching_rate) / (2 * sqrt(o.max_acceleration * o.desired_deceleration));

	double term_b_numerator_sum = term_b_numerator_a + term_b_numerator_b;

	double term_b_dividend = term_b_numerator_sum / o.net_distance;

	double term_b = exp(2 * log(term_b_dividend));

	return (o.max_acceleration * (1 - term_a - term_b));
}

int main (int argc, char **argv)
{
	srand(time(NULL));

	int n_cars = 10;
	double n_steps = 3600;
	double current_time = 0;
	double time_step = 1;
	double road_length = 1000;

	/* loop over the car_ts to set the defaults in an array
     of car_t parameter structures. */
	car_t cars[n_cars];
	for (int i = 0; i < n_cars; i++) {
		/*
		 * distribute the car_ts over the length of the road
		 * equally. we don't know the net distace or the
		 * approaching rate yet, so we leave these alone
		 * for now.
		 */
		car_t car;
		car.position_x = ((double)i / (double)n_cars) * road_length;
		car.velocity_x = 20.0;
		car.desired_velocity = 60.0;
		car.desired_time_headway = 1.5;
		car.max_acceleration = 1.0;
		car.desired_deceleration = 3.0;
		car.acceleration_exponent = 4.0;
		car.jam_distance = 2.0;
		car.vehicle_length = 5.0;

		cars[i] = car;
	}

	/* positions and velocities files. */
	FILE *positions, *velocities;
	positions = fopen("positions.txt", "a");
	velocities = fopen("velocities.txt", "a");

	/* the phase point is set up so we can now start the loop. */
	int next_car;
	double k1, k2, k3, k4;
	double m1, m2, m3, m4;
	double velocity_orig;
	for (int i = 0; i < n_steps; i++) {
		/* print the timestep to file. */
		fprintf (positions, "%.6f ", (float)current_time);
		fprintf (velocities, "%.6f ", (float)current_time);

		/* build the current net distance and approaching rate. */
		for (int i = 0; i < n_cars; i++) {
			/* print the positions and velocities to file. */
			fprintf(positions, "%.6f ", (float)cars[i].position_x);
			fprintf(velocities, "%.6f ", (float)cars[i].velocity_x);

			/*
			 * define the next car_t with periodic boundary conditions
			 * so the first car_t in the loop is seen by the last.
			 */
			next_car = (i + 1) % n_cars;

			cars[i].net_distance = cars[next_car].position_x - cars[i].position_x - cars[next_car].vehicle_length;

			/*
			 * if the distance is less than 0, then the next car_t has
			 * been moved across the periodic boundary.
			 */
			if (cars[i].net_distance < 0)
				cars[i].net_distance += road_length;

			cars[i].approaching_rate = cars[i].velocity_x - cars[next_car].velocity_x;
		}

		/* begin runge-kutta order four. */
		for (int i = 0; i < n_cars; i++) {
			/*
			 * store the original values of the velocity
			 * so it can be modified from and restored
			 * during the rk procedure. the positions do
			 * do not appear in the derivatives and
			 * therefore need not be treated here.
			 */
			velocity_orig = cars[i].velocity_x;

			k1 = position_derivative(cars[i]);
			m1 = velocity_derivative(cars[i]);

			cars[i].velocity_x = velocity_orig + (0.5 * m1 * time_step);

			k2 = position_derivative(cars[i]);
			m2 = velocity_derivative(cars[i]);

			cars[i].velocity_x = velocity_orig + (0.5 * m2 * time_step);

			k3 = position_derivative(cars[i]);
			m3 = velocity_derivative(cars[i]);

			cars[i].velocity_x = velocity_orig + (m3 * time_step);

			k4 = position_derivative(cars[i]);
			m4 = velocity_derivative(cars[i]);

			/* finally update the variables. */
			current_time += time_step;
			cars[i].position_x += ((k1 + (2 * k2) + (2 * k3) + k4)) * (time_step / 6);
			cars[i].velocity_x = velocity_orig + ((m1 + (2 * m2) + (2 * m3) + m4)) * (time_step / 6);

			/*
			 * if the car_t has moved beyond the end of the road,
			 * move it back to the beginning.
			 */

			if (cars[i].position_x > road_length)
				cars[i].position_x -= road_length;
		}

		/* we're done with this step's output so print a new line in output files. */
		fprintf(positions, "\n");
		fprintf(velocities, "\n");
	}

	/* close file pointers. */
	fclose(positions);
	fclose(velocities);

	return 0;
}