#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 void phy_calc_fc(float *fc, 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;

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

static void phy_calc_uec_H(float *uec, float dt, float fc0, float fc1,
                           float fc2, float mu)
{
	float tmp;

	tmp = 1.0f / dt + (fc1 + fc2) / (2 * EPS_ZERO) +
	      fc1 * fc2 * dt / sq(2.0f * EPS_ZERO);
	uec[0] = 1.0f / tmp * (1.0f / dt - (fc1 + fc2) / (2 * EPS_ZERO)
	         - fc1 * fc2 * dt / sq(2.0f * EPS_ZERO));
	uec[1] = -1.0f / tmp * LIGHT_SPEED / mu;
	uec[2] = -1.0f / tmp * LIGHT_SPEED * dt / EPS_ZERO * fc0 / mu;
	uec[3] = -1.0f / tmp * dt / sq(EPS_ZERO) * fc1 * fc2;
} 

static void phy_calc_uec_E(float *uec, float dt, float fc0, float fc1,
                           float fc2, float eps)
{
	float tmp;

	tmp = 1.0f / dt + (fc1 + fc2) / (2 * EPS_ZERO) +
	      fc1 * fc2 * dt / sq(2.0f * EPS_ZERO);
	uec[0] = 1.0f / tmp * (1.0f / dt - (fc1 + fc2) / (2 * EPS_ZERO)
	         - fc1 * fc2 * dt / sq(2.0f * EPS_ZERO));
	uec[1] = LIGHT_SPEED / tmp;
	uec[2] = 1.0f / tmp * LIGHT_SPEED * dt * fc0 / EPS_ZERO;
	uec[3] = 1.0f / tmp * dt / sq(EPS_ZERO) * fc1 * fc2;

	// equations above are for the D field and E = 1/eps * D
                               /*
	uec[0] *= 1.0f / eps;
	uec[1] *= 1.0f / eps;
	uec[2] *= 1.0f / eps;
	uec[3] *= 1.0f / eps;*/
} 

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

	memset(sim->aux, 0, fi->size1 * sizeof(phy_field_aux_point));

	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;
		vec3_t fc = {0, 0, 0};

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

		aux->eps = EPS_ZERO;
		aux->mu = MU_ZERO;

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

		phy_calc_fc(fc, x, sim->pml_widths, fi->dims, dt);
                   /*
		for (size_t i = 0; i < 3; i++) {
			aux->uec_H[i][0] = 1;
			aux->uec_H[i][1] = -dt / MU_ZERO;
			aux->uec_H[i][2] = 0;
			aux->uec_H[i][3] = 0;
			aux->uec_E[i][0] = 1;
			aux->uec_E[i][1] = +dt / EPS_ZERO;
			aux->uec_E[i][2] = 0;
			aux->uec_E[i][3] = 0;
		}*/

		phy_calc_uec_H(aux->uec_H[0], dt, fc[0], fc[1], fc[2], 1);
		phy_calc_uec_H(aux->uec_H[1], dt, fc[1], fc[0], fc[2], 1);
		phy_calc_uec_H(aux->uec_H[2], dt, fc[2], fc[0], fc[1], 1);

		phy_calc_uec_E(aux->uec_E[0], dt, fc[0], fc[1], fc[2], 1);
		phy_calc_uec_E(aux->uec_E[1], dt, fc[1], fc[0], fc[2], 1);
		phy_calc_uec_E(aux->uec_E[2], dt, fc[2], fc[0], fc[1], 1);
	}
}

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

	#if 1
	sim->pml_widths[0] = fi->dims[0] / 5;
	sim->pml_widths[1] = fi->dims[1] / 5;
	sim->pml_widths[2] = fi->dims[2] / 5;
	#else
	sim->pml_widths[0] = 0;
	sim->pml_widths[1] = 0;
	sim->pml_widths[2] = 0;
	#endif

	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;

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

	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, i;
		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;

		for (i = 0; i < 3; i++)
			Hn[offs+i] = aux->uec_H[i][0] * Hp[offs+i] +
			             aux->uec_H[i][1] * aux->curl_E[i] +
			             aux->uec_H[i][2] * aux->int_curl_E[i] +
			             aux->uec_H[i][3] * aux->int_H[i];

		//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;

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

	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, i;
		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;

		for (i = 0; i < 3; i++)
			En[offs+i] = aux->uec_E[i][0] * Ep[offs+i] +
			             aux->uec_E[i][1] * aux->curl_H[i] +
			             aux->uec_E[i][2] * aux->int_curl_H[i] +
			             aux->uec_E[i][3] * aux->int_E[i];

		//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, aux=%p\n", coords[0], coords[1],
	       coords[2], offs1, aux);
	printf("eps = %f, mu = %f\n", aux->eps, aux->mu);
	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("UEC_E = [[%E %E %E %E] [%E %E %E %E] [%E %E %E %E]]\n",
	       aux->uec_E[0][0], aux->uec_E[0][1], aux->uec_E[0][2], aux->uec_E[0][3],
	       aux->uec_E[1][0], aux->uec_E[1][1], aux->uec_E[1][2], aux->uec_E[1][3],
	       aux->uec_E[2][0], aux->uec_E[2][1], aux->uec_E[2][2], aux->uec_E[2][3]);
	printf("UEC_H = [[%E %E %E %E] [%E %E %E %E] [%E %E %E %E]]\n",
	       aux->uec_H[0][0], aux->uec_H[0][1], aux->uec_H[0][2], aux->uec_H[0][3],
	       aux->uec_H[1][0], aux->uec_H[1][1], aux->uec_H[1][2], aux->uec_H[1][3],
	       aux->uec_H[2][0], aux->uec_H[2][1], aux->uec_H[2][2], aux->uec_H[2][3]);
	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;
}