#include "common.h"
#include "itc.h"
#include "physics.h"

void phy_sim_destroy(phy_sim *sim)
{
	size_t i;

	SDL_DestroyMutex(sim->rotate_lock);
 
	itc_chan_destroy(&sim->ctl);

	for (i = 0; i < 3; i++) {
		free(sim->fields[i].E);
		free(sim->fields[i].H);
	}
}

void phy_sim_reset(phy_sim *sim)
{
	size_t i;
	phy_field_info *fi;

	fi = &sim->field_info;

	for (i = 0; i < 3; i++) {
		memset(sim->fields[i].E, 0, fi->size * sizeof(float));
		memset(sim->fields[i].H, 0, fi->size * sizeof(float));
	}

	memset(sim->field_eps, 0, fi->size1 * sizeof(float));
	memset(sim->field_eps, 0, fi->size1 * sizeof(float));
	phy_sim_compute_const_fields(sim);

	sim->time = 0;
	sim->running = false;
	sim->frame_index = get_time();
}

int phy_sim_create(phy_sim *sim)
{
	size_t i;
	phy_field_info *fi;

	memset(sim, 0, sizeof(phy_sim));

	fi = &sim->field_info;

	fi->width = 100;
	fi->height = 100;
	fi->depth = 100;
	fi->spacing = 1.5e10f / max3(fi->width, fi->height, fi->depth);
	sim->time_delta = 0.2;

	fi->xstr = 3;
	fi->ystr = fi->xstr * fi->width;
	fi->zstr = fi->ystr * fi->height;
	fi->size = fi->zstr * fi->depth;

	fi->xstr1 = 1;
	fi->ystr1 = fi->xstr1 * fi->width;
	fi->zstr1 = fi->ystr1 * fi->height;
	fi->size1 = fi->zstr1 * fi->depth;

	for (i = 0; i < 3; i++) { 
		sim->fields[i].E = malloc(fi->size * sizeof(float));
		sim->fields[i].H = malloc(fi->size * sizeof(float));
		if (!sim->fields[i].E || !sim->fields[i].H) {
			con_printf("phy_sim_create: out of memory\n");
			goto error;
		}
	}

	sim->field_eps = malloc(fi->size1 * sizeof(float));
	sim->field_mu = malloc(fi->size1 * sizeof(float));
	if (!sim->field_eps || !sim->field_mu) {
		con_printf("phy_sim_create: out of memory\n");
		goto error;
	}

	phy_sim_reset(sim);

	itc_chan_create(&sim->ctl);
	sim->running = false;

	sim->rotate_lock = SDL_CreateMutex();
	assert(sim->rotate_lock);
	return 0;

error:
	phy_sim_destroy(sim);
	return 1;
}

void phy_sim_compute_const_fields(phy_sim *sim)
{
	size_t x, y, z;
	phy_field_info *fi = &sim->field_info;

	for (z = 0; z < fi->depth; z++)
	for (y = 0; y < fi->height; y++)
	for (x = 0; x < fi->width; x++) {
		float *eps, *mu;

		eps = sim->field_eps + z * fi->zstr1 + y * fi->ystr1 +
		      x * fi->xstr1;
		mu = sim->field_mu + z * fi->zstr1 + y * fi->ystr1 +
		     x * fi->xstr1;

		*eps = 1.129409066837282e+11;
		*mu = -7.957747154594767e+5;
	}
}

void phy_sim_step_E(phy_field_info *fi, float *mul_const, float dt,
                    float *En, float *Ep, float *H)
{
	size_t x, y, z;
	float dx = fi->spacing;

	for (z = 1; z < fi->depth - 1; z++)
	for (y = 1; y < fi->height - 1; y++)
	for (x = 1; x < fi->width - 1; x++) {
		size_t offs, offs_nx, offs_ny, offs_nz;
		size_t offs1;
		float dExdt, dEydt, dEzdt;

		offs = x * fi->xstr + y * fi->ystr + z * fi->zstr;
		offs_nx = offs - fi->xstr;
		offs_ny = offs - fi->ystr;
		offs_nz = offs - fi->zstr;

		offs1 = x * fi->xstr1 + y * fi->ystr1 + z * fi->zstr1;

		dExdt = (H[offs+2] - H[offs_ny+2] - H[offs+1] + H[offs_nz+1]);
		dEydt = (H[offs+0] - H[offs_nz+0] - H[offs+2] + H[offs_nx+2]);
		dEzdt = (H[offs+1] - H[offs_nx+1] - H[offs+0] + H[offs_ny+0]);

		En[offs+0] = Ep[offs+0] + dt * mul_const[offs1] * dExdt / dx;
		En[offs+1] = Ep[offs+1] + dt * mul_const[offs1] * dEydt / dx;
		En[offs+2] = Ep[offs+2] + dt * mul_const[offs1] * dEzdt / dx;
	}
}

void phy_sim_step_H(phy_field_info *fi, float *mul_const, float dt,
                    float *Hn, float *Hp, float *E)
{
	size_t x, y, z;
	float dx = fi->spacing;

	for (z = 1; z < fi->depth - 1; z++)
	for (y = 1; y < fi->height - 1; y++)
	for (x = 1; x < fi->width - 1; x++) {
		size_t offs, offs_px, offs_py, offs_pz;
		size_t offs1;
		float dHxdt, dHydt, dHzdt;

		offs = x * fi->xstr + y * fi->ystr + z * fi->zstr;
		offs_px = offs + fi->xstr;
		offs_py = offs + fi->ystr;
		offs_pz = offs + fi->zstr;

		offs1 = x * fi->xstr1 + y * fi->ystr1 + z * fi->zstr1;

		dHxdt = (E[offs_py+2] - E[offs+2] - E[offs_pz+1] + E[offs+1]);
		dHydt = (E[offs_pz+0] - E[offs+0] - E[offs_px+2] + E[offs+2]);
		dHzdt = (E[offs_px+1] - E[offs+1] - E[offs_py+0] + E[offs+0]);

		Hn[offs+0] = Hp[offs+0] + dt * mul_const[offs1] * dHxdt / dx;
		Hn[offs+1] = Hp[offs+1] + dt * mul_const[offs1] * dHydt / dx;
		Hn[offs+2] = Hp[offs+2] + dt * mul_const[offs1] * dHzdt / dx;
	}
}

void phy_sim_add_sources_E(phy_sim *sim)
{
	phy_field_info *fi = &sim->field_info;
	float *E;
	size_t x, y, z;

	x = fi->width / 2;
	y = fi->height / 2;
	z = fi->depth / 2;

	E = sim->fields[2].E + z * fi->zstr + y * fi->ystr + x * fi->xstr;
	E[0] += sin(sim->time * 0.5) *
	        exp(-0.1f * pow(sim->time - 3.0f, 2)) * 50;
}

void phy_sim_add_sources_H(phy_sim *sim)
{
	phy_field_info *fi = &sim->field_info;
	float *H;
	size_t x, y, z;

	x = fi->width / 2;
	y = fi->height / 2;
	z = fi->depth / 2;

	H = sim->fields[2].H + z * fi->zstr + y * fi->ystr + x * fi->xstr;
	H[1] -= exp(-0.1f * pow(sim->time - 5.0f - sim->time_delta / 2, 2)) * 5;
}

void phy_sim_step(phy_sim *sim) {
	phy_field_info *fi = &sim->field_info;
	phy_field_em *fields = sim->fields, tmp;
	int64_t start_time;

	start_time = get_time();

	phy_sim_step_E(fi, sim->field_mu, sim->time_delta,
	               fields[2].E, fields[0].E, fields[1].H);
	phy_sim_add_sources_E(sim);
	phy_sim_step_H(fi, sim->field_eps, sim->time_delta,
	               fields[2].H, fields[0].H, fields[1].E);
	phy_sim_add_sources_H(sim);

	SDL_mutexP(sim->rotate_lock);

	// note: only pointers are rotated, not the entire fields
	memcpy(&tmp, fields, sizeof(phy_field_em));
	memcpy(fields, fields + 1, sizeof(phy_field_em));
	memcpy(fields + 1, fields + 2, sizeof(phy_field_em));
	memcpy(fields + 2, &tmp, sizeof(phy_field_em));
	sim->time += sim->time_delta;

	sim->frame_index = get_time(); // for renderer
	sim->step_real_time = get_time() - start_time;

	SDL_mutexV(sim->rotate_lock);
}

void phy_run_command(phy_sim *sim, int number, void *data)
{
	switch (number) {
	case PHY_CMD_PAUSE:
		sim->running = false;
		break;

	case PHY_CMD_RESUME:
		sim->running = true;
		break;

	case PHY_CMD_STEP:
		sim->running = false;
		phy_sim_step(sim);
		break;

	case PHY_CMD_RESET:
		phy_sim_reset(sim);
		break;
	}
}

int phy_thread(phy_sim *sim)
{
	int rv, cmd_num;
	void *cmd_data;

	while (1) {
cont_main_loop:
		if (sim->running) {
			while (!itc_chan_pop(&sim->ctl, &cmd_num, &cmd_data)) {
				if (cmd_num == PHY_CMD_QUIT)
					goto out;

				phy_run_command(sim, cmd_num, cmd_data);

				if (!sim->running)
					goto cont_main_loop;
			}

			phy_sim_step(sim);
		} else {
			// do it only once per iteration
			if (!itc_chan_pop_block(&sim->ctl, &cmd_num,
		                                &cmd_data)) {
				if (cmd_num == PHY_CMD_QUIT)
					goto out;

				phy_run_command(sim, cmd_num, cmd_data);
			}
		}
	}

out:

	printf("phy_thread: quitting\n");
}