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

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 = 50;
	fi->height = 50;
	fi->depth = 50;
	fi->spacing = 1.5e9 / max3(fi->width, fi->height, fi->depth);

	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 = calloc(fi->size, sizeof(float));
		sim->fields[i].H = calloc(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 = calloc(fi->size1, sizeof(float));
	sim->field_mu = calloc(fi->size1, sizeof(float));

	phy_sim_compute_const_fields(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_1(phy_field_info *fi, float dt, float *mul_const,
                    const float *L0, float *L2, const float *R)
{
	size_t x, y, z;

#define AT(F, x, y, z) ((F) + (z) * fi->zstr + (y) * fi->ystr + (x) * fi->xstr)
#define AT1(F, x, y, z) ((F)[(z) * fi->zstr1 + (y) * fi->ystr1 + \
                         (x) * fi->xstr1])

	// X
	for (z = 1; z < fi->depth - 1; z++)
	for (y = 1; y < fi->height - 1; y++)
	for (x = 0; x < fi->width; x++) {
		float dRzdy, dRydz, dLxdt;

		dRzdy = AT(R, x, y + 1, z)[2] - AT(R, x, y - 1, z)[2];
		dRzdy /= 2 * fi->spacing;

		dRydz = AT(R, x, y, z + 1)[1] - AT(R, x, y, z - 1)[1];
		dRydz /= 2 * fi->spacing;

		dLxdt = AT1(mul_const, x, y, z) * (dRzdy - dRydz);

		AT(L2, x, y, z)[0] = AT(L0, x, y, z)[0] + dLxdt * dt;
	}

	// Y
	for (z = 1; z < fi->depth - 1; z++)
	for (y = 0; y < fi->height; y++)
	for (x = 1; x < fi->width - 1; x++) {
		float dRxdz, dRzdx, dLydt;

		dRxdz = AT(R, x, y, z + 1)[0] - AT(R, x, y, z - 1)[0];
		dRxdz /= 2 * fi->spacing;

		dRzdx = AT(R, x + 1, y, z)[2] - AT(R, x - 1, y, z)[2];
		dRzdx /= 2 * fi->spacing;

		dLydt = AT1(mul_const, x, y, z) * (dRxdz - dRzdx);

		AT(L2, x, y, z)[1] = AT(L0, x, y, z)[1] + dLydt * dt;
	}

	// Z
	for (z = 0; z < fi->depth; z++)
	for (y = 1; y < fi->height - 1; y++)
	for (x = 1; x < fi->width - 1; x++) {
		float dRydx, dRxdy, dLzdt;

		dRydx = AT(R, x + 1, y, z)[1] - AT(R, x - 1, y, z)[1];
		dRydx /= 2 * fi->spacing;

		dRxdy = AT(R, x, y + 1, z)[0] - AT(R, x, y - 1, z)[0];
		dRxdy /= 2 * fi->spacing;

		dLzdt = AT1(mul_const, x, y, z) * (dRydx - dRxdy);

		AT(L2, x, y, z)[2] = AT(L0, x, y, z)[2] + dLzdt * dt;
	}
#undef AT
#undef AT1
}

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

	for (z = 0; z < fi->depth; z++)
	for (y = 0; y < fi->height; y++)
	for (x = 0; x < fi->width; x++) {
		float *E, *H, fx, fy, fz, chuj;

		fx = (float)x / fi->width * 2 - 1;
		fy = (float)y / fi->height * 2 - 1;
		fz = (float)z / fi->depth * 2 - 1;

		E = sim->fields[2].E + z * fi->zstr + y * fi->ystr + x * fi->xstr;
		H = sim->fields[2].H + z * fi->zstr + y * fi->ystr + x * fi->xstr;

		chuj = (0.5 - sqrt(fx * fx + fy * fy + fz * fz)) / 0.5;
		if (chuj < 0)
			chuj = 0;
		else
			chuj = 1;

		chuj *= sin(sim->time * 3) * 1;
		//chuj *= exp(-10.0f * pow(sim->time - 0.5, 2)) * 2;
		E[0] += chuj;
	}
}

void phy_sim_step(phy_sim *sim, float dt) {
	phy_field_info *fi = &sim->field_info;
	phy_field_em *fields = sim->fields, tmp;
	size_t i;

	phy_sim_add_sources(sim);

	// note: only pointers are rotated, not the entire fields
	SDL_mutexP(sim->rotate_lock);
	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));
	SDL_mutexV(sim->rotate_lock);

	phy_sim_step_1(fi, dt, sim->field_eps,
	               fields[0].E, fields[2].E, fields[1].H);
	phy_sim_step_1(fi, dt, sim->field_mu,
	               fields[0].H, fields[2].H, fields[1].E);

	sim->time += dt;
}

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:
		phy_sim_step(sim, 0.1);
		break;
	}
}

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

	while (1) {
		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);
			}

			phy_sim_step(sim, 0.1);
		} 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");
}