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

static void phy_calc_fc(float *fc, size_t *x, size_t *widths,
                        size_t *dims, float dt, float offset)
{
	size_t i;

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

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

		fc[i] = EPS_ZERO / (2 * dt) * sin(0.5f * M_PI * f);
	}
}

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[1] *= 1.0f / eps;
	uec[2] *= 1.0f / eps;
	uec[3] *= 1.0f / eps;
} 


static float phy_sim_csg_subsample(phy_csg_step *stack, float *x)
{
	phy_csg_step *step;
	float rv = 1.0f;

	for (step = stack; step; step = step->next) {
		switch (step->shape) {
		case CSG_SPHERE:
			if (sq(x[0] - step->x[0]) + sq(x[1] - step->x[1]) +
			    sq(x[2] - step->x[2]) > sq(step->d[0]))
				goto next_step;
			break;

		case CSG_CYLINDER_Z:
			if (x[2] > step->x[2] + step->d[1] ||
			    x[2] < step->x[2] - step->d[1] ||
			    sq(x[0] - step->x[0]) + sq(x[1] - step->x[1]) >
			    sq(step->d[0]))
				goto next_step;
		}

		rv = step->value;

	next_step:
		;
	}

	return rv;
}

static float phy_sim_csg_sample(phy_csg_step *stack, size_t *x)
{
	size_t dx, dy, dz;
	float total = 0.0f;

	for (dz = 0; dz < 2; dz++)
	for (dy = 0; dy < 2; dy++)
	for (dx = 0; dx < 2; dx++) {
		vec3_t xf;

		v3_set(xf, x[0] + 0.5f * dx, x[1] + 0.5f * dy,
		       x[2] + 0.5f * dz);

		total += phy_sim_csg_subsample(stack, xf);
	}
	
	return total / 8.0f;
}

static void phy_sim_zero_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));

	sim->eps_min = sim->eps_max = 1.0f;

	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_H = {0, 0, 0}, fc_E = {0, 0, 0};

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

		aux->eps = phy_sim_csg_sample(sim->csg_stack, x);

		if (aux->eps > sim->eps_max)
			sim->eps_max = aux->eps;
		if (aux->eps < sim->eps_min)
			sim->eps_min = aux->eps;

		aux->mu = 1;

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

		phy_calc_fc(fc_H, x, sim->pml_widths, fi->dims, dt, 0.0f);
		phy_calc_fc(fc_E, x, sim->pml_widths, fi->dims, dt, 0.5f);

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

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

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

	sim->pml_widths[0] = con_var_get_size("pml_x");
	sim->pml_widths[1] = con_var_get_size("pml_y");
	sim->pml_widths[2] = con_var_get_size("pml_z");

	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_zero_aux(sim);

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

int phy_sim_create(phy_sim *sim)
{
	memset(sim, 0, sizeof(phy_sim));

	sim->rotate_lock = SDL_CreateMutex();
	assert(sim->rotate_lock);
	itc_chan_create(&sim->ctl);

	con_var_set("width", "50");
	con_var_set("height", "50");
	con_var_set("depth", "50");
	con_var_set("scale", "7e+9"); 
	con_var_set("time_delta", "0.1");
	con_var_set("pml_x", "0");
	con_var_set("pml_y", "0");
	con_var_set("pml_z", "0");

	con_var_set("src_z", "10"); 
	con_var_set("src_angle", "0");
                              
	con_var_set("csg_shape", "sphere");
	con_var_set("csg_v", NULL);

	con_var_set("csg_x", NULL);
	con_var_set("csg_y", NULL);
	con_var_set("csg_z", NULL);
	con_var_set("csg_r", NULL);
	con_var_set("csg_h", NULL);

	phy_sim_create_fields(sim);

	return 0;
}

void phy_sim_destroy(phy_sim *sim)
{
        phy_sim_csg_clear(sim);
	phy_sim_destroy_fields(sim);
	SDL_DestroyMutex(sim->rotate_lock);
	itc_chan_destroy(&sim->ctl);
}

static void phy_sim_configure(phy_sim *sim)
{
	phy_field_info *fi = &sim->field_info;

	fi->spacing = con_var_get_float("scale") /
	              min3(fi->dims[0], fi->dims[1], fi->dims[2]);
	sim->time_delta = con_var_get_float("time_delta");

	con_var_set("courant", va("%f", 3 * LIGHT_SPEED * sim->time_delta /
	                          fi->spacing));

	sim->source_xyplane.z = con_var_get_size("src_z");
	sim->source_xyplane.Px = cos(DEG2RAD(con_var_get_float("src_angle")));
	sim->source_xyplane.Py = sin(DEG2RAD(con_var_get_float("src_angle")));
	sim->source_xyplane.enabled = true;

}

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

	fi = &sim->field_info;

	fi->dims[0] = con_var_get_size("width");
	fi->dims[1] = con_var_get_size("height");
	fi->dims[2] = con_var_get_size("depth");

	printf("phy_sim_create_fields: %zux%zux%zu\n",
	       fi->dims[0], fi->dims[1], fi->dims[2]);

	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_fields: out of memory\n");
			goto error;
		}
	}

	sim->aux = malloc(fi->size1 * sizeof(phy_field_aux_point));
	if (!sim->aux) {
		con_printf("phy_sim_create_fields: out of memory\n");
		goto error;
	}

	phy_sim_configure(sim);
	phy_sim_zero(sim);

	sim->running = false;
	sim->valid = true;

	return 0;

error:
	phy_sim_destroy_fields(sim);
	return 1;
}

void phy_sim_destroy_fields(phy_sim *sim)
{
	size_t i;

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

	free(sim->aux);
	sim->aux = NULL;

	sim->valid = false;
}

static float gauss(phy_source_xyplane *s, float t)
{
	return sin(t); //exp(-s->lambda * sq(t - s->time));
}

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

	if (!sim->source_xyplane.enabled)
		return;

	z = sim->source_xyplane.z;

	delay = fi->spacing / (2.0f * LIGHT_SPEED)
		- sim->time_delta / 2.0f + 0.35;

	/*delay = - fi->spacing / (2.0f * LIGHT_SPEED)
	        + sim->time_delta / 2.0f;*/

	src_A = gauss(&sim->source_xyplane, sim->time + delay) * 1;
	src_x = -sim->source_xyplane.Py * src_A;
	src_y = sim->source_xyplane.Px * src_A;

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

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

		aux->curl_H[0] += src_y / fi->spacing;
		aux->curl_H[1] -= src_x / fi->spacing;
	}
}

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

	if (!sim->source_xyplane.enabled)
		return;

	z = sim->source_xyplane.z - 1;

	src_A = gauss(&sim->source_xyplane, sim->time) * 1;
	src_x = sim->source_xyplane.Px * src_A;
	src_y = sim->source_xyplane.Py * src_A;

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

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

		aux->curl_E[0] += src_y / fi->spacing;
		aux->curl_E[1] -= src_x / fi->spacing;
	}
}

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;

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

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

	phy_sim_add_sources_E(sim);
}

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;

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

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

	phy_sim_add_sources_H(sim);
}

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

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

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_update_aux_E(sim);
	phy_sim_update_em_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_csg_push(phy_sim *sim)
{
	char *shape_str;
	phy_csg_shape shape;
	phy_csg_step *step;

	shape_str = con_var_get("csg_shape");

	shape = CSG_SPHERE;

	if (shape_str) {
		if (!strcmp(shape_str, "cylinder-z"))
			shape = CSG_CYLINDER_Z;
	}

	step = malloc(sizeof(phy_csg_step));
	assert(step);

	step->next = sim->csg_stack;
	sim->csg_stack = step;

	step->shape = shape;
	step->value = con_var_get_float("csg_v");

	switch (shape) {
	case CSG_SPHERE:
		step->x[0] = con_var_get_float("csg_x");
		step->x[1] = con_var_get_float("csg_y");
		step->x[2] = con_var_get_float("csg_z");
		step->d[0] = con_var_get_float("csg_r");
		break;

	case CSG_CYLINDER_Z:
		step->x[0] = con_var_get_float("csg_x");
		step->x[1] = con_var_get_float("csg_y");
		step->x[2] = con_var_get_float("csg_z");
		step->d[0] = con_var_get_float("csg_r");
		step->d[1] = con_var_get_float("csg_h");
		break;
	}
}

void phy_sim_csg_clear(phy_sim *sim)
{
	phy_csg_step *step, *next;

	for (step = sim->csg_stack; step; step = next) {
		next = step->next;
		free(step);
	}
}

void phy_sim_debug(phy_sim *sim, size_t *coords)
{
	phy_field_info *fi = &sim->field_info;
	phy_field_aux_point *aux;
	float *E, *H, Emag, Hmag;

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

	E = sim->fields[1].E + coords[2] * fi->zstr + coords[1] * fi->ystr +
	      coords[0] * fi->xstr;
	H = sim->fields[1].H + coords[2] * fi->zstr + coords[1] * fi->ystr +
	      coords[0] * fi->xstr;

	Emag = sqrt(sq(E[0]) + sq(E[1]) + sq(E[2]));
	Hmag = sqrt(sq(H[0]) + sq(H[1]) + sq(H[2]));

	printf("===[ phy_sim_debug ]===\n");
	printf("x = [%zu %zu %zu], E=%p, H=%p, aux=%p\n", coords[0], coords[1],
	       coords[2], E, H, 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("Emag = %E, Hmag = %E, Z = %E\n", Emag, Hmag, Emag/Hmag);
	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_ZERO:
		phy_sim_zero(sim);
		break;

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

	case PHY_CMD_RESET:
		SDL_LockMutex(sim->rotate_lock);
		if (sim->field_info.dims[0] == con_var_get_size("width") &&
		    sim->field_info.dims[1] == con_var_get_size("height") &&
		    sim->field_info.dims[2] == con_var_get_size("depth")) {
			phy_sim_configure(sim);
			phy_sim_zero(sim);
		} else {
			phy_sim_destroy_fields(sim);
			phy_sim_create_fields(sim);
		}
		SDL_UnlockMutex(sim->rotate_lock);
		break;

	case PHY_CMD_CSG_PUSH:
		phy_sim_csg_push(sim);
		break;

	case PHY_CMD_CSG_CLEAR:
		phy_sim_csg_clear(sim);
		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;
}