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

static float phy_calc_fake_cond(float *fake_cond, size_t *x, size_t *widths,
                                size_t *dims, float dt)
{
	size_t i;

	for (i = 0; i < 3; i++) {
		float f;

		if (x[i] <= widths[i])
			f = 1.0f - (float)(x[i] - 1) / widths[i];
		else if (x[i] >= dims[i] - widths[i] - 1)
			f = (float)(x[i] + widths[i] - dims[i] + 2) / widths[i];
		else
			continue;

		fake_cond[i] = EPS_ZERO / (2 * dt) * pow(f, 3);
	}
}

void phy_sim_reset_aux(phy_sim *sim)
{
	size_t x[3], i;
	phy_field_info *fi = &sim->field_info;

	for (x[2] = 1; x[2] < fi->dims[2] - 1; x[2]++)
	for (x[1] = 1; x[1] < fi->dims[1] - 1; x[1]++)
	for (x[0] = 1; x[0] < fi->dims[0] - 1; x[0]++) {
		phy_field_aux_point *aux;

		aux = sim->aux + x[2] * fi->zstr1 + x[1] * fi->ystr1 +
			x[0] * fi->xstr1;

		aux->eps = 1.0f / EPS_ZERO;
		aux->mu = -1.0f / MU_ZERO;

		v3_set(aux->int_curl_E, 0, 0, 0);
		v3_set(aux->int_curl_H, 0, 0, 0);

		phy_calc_fake_cond(aux->fake_cond, x, sim->pml_widths,
		                   fi->dims, sim->time_delta);
	}
}

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

	phy_sim_reset_aux(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->dims[0] = 50;
	fi->dims[1] = 50;
	fi->dims[2] = 50;
	fi->spacing = 10e+9f / max3(fi->dims[0], fi->dims[1], fi->dims[2]);
	sim->time_delta = 0.1;

	printf("phy_sim_create: Courant number is %f\n",
	       3 * LIGHT_SPEED * sim->time_delta / fi->spacing);

	sim->pml_widths[0] = fi->dims[0] / 5;
	sim->pml_widths[1] = fi->dims[1] / 5;
	sim->pml_widths[2] = fi->dims[2] / 5;

	fi->xstr = 3;
	fi->ystr = fi->xstr * fi->dims[0];
	fi->zstr = fi->ystr * fi->dims[1];
	fi->size = fi->zstr * fi->dims[2];

	fi->xstr1 = 1;
	fi->ystr1 = fi->xstr1 * fi->dims[0];
	fi->zstr1 = fi->ystr1 * fi->dims[1];
	fi->size1 = fi->zstr1 * fi->dims[2];

	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->aux = malloc(fi->size1 * sizeof(phy_field_aux_point));
	if (!sim->aux) {
		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_update_aux_H(phy_sim *sim)
{
	phy_field_info *fi = &sim->field_info;
	size_t x, y, z;

	for (z = 1; z < fi->dims[2] - 1; z++)
	for (y = 1; y < fi->dims[1] - 1; y++)
	for (x = 1; x < fi->dims[0] - 1; x++) {
		size_t offs, offs_px, offs_py, offs_pz;
		phy_field_aux_point *aux;
		float *E = sim->fields[1].E, *H = sim->fields[1].H;

		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;

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

		aux->curl_E[0] = (E[offs_py+2] - E[offs+2] -
		                  E[offs_pz+1] + E[offs+1]) / fi->spacing;
		aux->curl_E[1] = (E[offs_pz+0] - E[offs+0] -
		                  E[offs_px+2] + E[offs+2]) / fi->spacing;
		aux->curl_E[2] = (E[offs_px+1] - E[offs+1] -
		                  E[offs_py+0] + E[offs+0]) / fi->spacing;

		v3_acc(aux->int_H, H + offs);
		v3_acc(aux->int_curl_E, aux->curl_E);
	}
}

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

	for (z = 1; z < fi->dims[2] - 1; z++)
	for (y = 1; y < fi->dims[1] - 1; y++)
	for (x = 1; x < fi->dims[0] - 1; x++) {
		size_t offs, offs_nx, offs_ny, offs_nz;
		phy_field_aux_point *aux;
		float *E = sim->fields[1].E, *H = sim->fields[1].H;

		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;

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

		aux->curl_H[0] = (H[offs+2] - H[offs_ny+2] -
		                  H[offs+1] + H[offs_nz+1]) / fi->spacing;
		aux->curl_H[1] = (H[offs+0] - H[offs_nz+0] -
		                  H[offs+2] + H[offs_nx+2]) / fi->spacing;
		aux->curl_H[2] = (H[offs+1] - H[offs_nx+1] -
		                  H[offs+0] + H[offs_ny+0]) / fi->spacing;

		v3_acc(aux->int_E, E + offs);
		v3_acc(aux->int_curl_H, aux->curl_H);
	}
}

void phy_sim_update_em_H(phy_sim *sim)
{
	phy_field_info *fi = &sim->field_info;
	size_t x, y, z;
	float *Hn, *Hp, dt;

	Hn = sim->fields[2].H;
	Hp = sim->fields[0].H;
	dt = sim->time_delta;

	for (z = 1; z < fi->dims[2] - 1; z++)
	for (y = 1; y < fi->dims[1] - 1; y++)
	for (x = 1; x < fi->dims[0] - 1; x++) {
		size_t offs;
		phy_field_aux_point *aux;

		offs = x * fi->xstr + y * fi->ystr + z * fi->zstr;
		aux = sim->aux + x * fi->xstr1 + y * fi->ystr1 +
		      z * fi->zstr1;

		Hn[offs+0] = Hp[offs+0] + dt * aux->mu * aux->curl_E[0];
		Hn[offs+1] = Hp[offs+1] + dt * aux->mu * aux->curl_E[1];
		Hn[offs+2] = Hp[offs+2] + dt * aux->mu * aux->curl_E[2];
	}
}

void phy_sim_update_em_E(phy_sim *sim)
{
	phy_field_info *fi = &sim->field_info;
	size_t x, y, z;
	float *En, *Ep, dt;

	En = sim->fields[2].E;
	Ep = sim->fields[0].E;
	dt = sim->time_delta;

	for (z = 1; z < fi->dims[2] - 1; z++)
	for (y = 1; y < fi->dims[1] - 1; y++)
	for (x = 1; x < fi->dims[0] - 1; x++) {
		size_t offs;
		phy_field_aux_point *aux;

		offs = x * fi->xstr + y * fi->ystr + z * fi->zstr;
		aux = sim->aux + x * fi->xstr1 + y * fi->ystr1 +
		         z * fi->zstr1;

		En[offs+0] = Ep[offs+0] + dt * aux->eps * aux->curl_H[0];
		En[offs+1] = Ep[offs+1] + dt * aux->eps * aux->curl_H[1];
		En[offs+2] = Ep[offs+2] + dt * aux->eps * aux->curl_H[2];
	}
}

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->dims[0]/2;
	y = fi->dims[1]/2;
	z = fi->dims[2]/2;
	E = sim->fields[2].E + z * fi->zstr + y * fi->ystr +
		     x * fi->xstr;

	E[0] += exp(-40.0f * pow(sim->time - 3, 2)) * 50;

/*
	x = 10;
	for (y = 1; y < fi->dims[1] - 1; y++)
	for (z = 1; z < fi->dims[2] - 1; z++) {
		E = sim->fields[2].E + z * fi->zstr + y * fi->ystr +
		     x * fi->xstr;

		E[0] += sin(sim->time * 3) * 1;
		E[1] += 0;
		E[2] += 0;
	}*/
}

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

	x = 0;
	for (y = 1; y < fi->dims[1] - 1; y++)
	for (z = 1; z < fi->dims[2] - 1; z++) {
		H = sim->fields[2].H + z * fi->zstr + y * fi->ystr +
		     x * fi->xstr;

		H[0] += 0;
		H[1] += 0;
		H[2] += sin(sim->time + sim->time_delta / 2)  / IMPEDANCE_ZERO * 100;
	}*/
}


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

	start_time = get_time();

	phy_sim_update_aux_H(sim);
	phy_sim_update_em_H(sim);
	phy_sim_add_sources_H(sim);
	phy_sim_update_aux_E(sim);
	phy_sim_update_em_E(sim);
	phy_sim_add_sources_E(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_sim_debug(phy_sim *sim, size_t *coords)
{
	phy_field_info *fi = &sim->field_info;
	size_t offs1;
	phy_field_aux_point *aux;

	offs1 = coords[2] * fi->zstr1 + coords[1] * fi->ystr1 +
	        coords[0] * fi->xstr1;

	aux = sim->aux + offs1;

	printf("===[ phy_sim_debug ]===\n");
	printf("x = [%zu %zu %zu], offs1=%zu\n", coords[0], coords[1],
	       coords[2], offs1);
	printf("CE = [%E %E %E]\n", aux->curl_E[0], aux->curl_E[1],
	       aux->curl_E[2]);
	printf("CH = [%E %E %E]\n", aux->curl_H[0], aux->curl_H[1],
	       aux->curl_H[2]);
	printf("ICE = [%E %E %E]\n", aux->int_curl_E[0], aux->int_curl_E[1],
	       aux->int_curl_E[2]);
	printf("ICH = [%E %E %E]\n", aux->int_curl_H[0], aux->int_curl_H[1],
	       aux->int_curl_H[2]);
	printf("FC = [%E %E %E]\n", aux->fake_cond[0], aux->fake_cond[1],
	       aux->fake_cond[2]);
	printf("=======================\n");
}

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;

	case PHY_CMD_DEBUG:
		phy_sim_debug(sim, data);
		free(data);
		break;
	}
}

int phy_thread(phy_sim *sim)
{
	int 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");
	return 0;
}