#include "mathpi.h"
#include "sheep2.h"



static BLOCK *get_block_at(STATE *state, int x, int y)
{
	if (x >= 0 && x < state->l->w && y >= 0 && y < state->l->h)
		return state->block[y][x];

	return NULL;
}



static void point_collision(SHEEP *sheep, float x, float y, float z)
{
	float xd = sheep->x - x;
	float yd = sheep->y - y;
	float zd = sheep->z - z;
	float d = xd*xd + yd*yd + zd*zd;
	if (d >= 0.0001f && d < SHEEP_R*SHEEP_R) {
		d = sqrt(d);
		xd /= d;
		yd /= d;
		zd /= d;
		d = SHEEP_R - d;
		sheep->x += xd * d;
		sheep->y += yd * d;
		sheep->z += zd * d;
		d = sheep->xv * xd + sheep->yv * yd + sheep->zv * zd;
		if (d < 0) {
			d *= -1.9f;
			sheep->xv += xd * d;
			sheep->yv += yd * d;
			sheep->zv += zd * d;
		}
	}
}



static void edge_collision(float u0, float v0, float *u, float *v, float *uv, float *vv)
{
	float ud = *u - u0;
	float vd = *v - v0;
	float d = ud*ud + vd*vd;
	if (d >= 0.0001f && d < SHEEP_R*SHEEP_R) {
		d = sqrt(d);
		ud /= d;
		vd /= d;
		d = SHEEP_R - d;
		*u += ud * d;
		*v += vd * d;
		d = *uv * ud + *vv * vd;
		if (d < 0) {
			d *= -1.9f;
			*uv += ud * d;
			*vv += vd * d;
		}
	}
}



static void arbitrary_edge_collision(SHEEP *sheep, float x0, float y0, float z0, float xb, float yb, float zb)
{
	float xa = sheep->x - x0, ya = sheep->y - y0, za = sheep->z - z0;
	float xc = ya*zb - za*yb;
	float yc = za*xb - xa*zb;
	float zc = xa*yb - ya*xb;
	float xd = yb*zc - zb*yc;
	float yd = zb*xc - xb*zc;
	float zd = xb*yc - yb*xc;
	float d = xd*xd + yd*yd + zd*zd;
	if (d >= 0.0001f && d < SHEEP_R*SHEEP_R) {
		d = sqrt(d);
		xd /= d;
		yd /= d;
		zd /= d;
		d = SHEEP_R - d;
		sheep->x += xd * d;
		sheep->y += yd * d;
		sheep->z += zd * d;
		d = sheep->xv * xd + sheep->yv * yd + sheep->zv * zd;
		if (d < 0) {
			d *= -1.9f;
			sheep->xv += xd * d;
			sheep->yv += yd * d;
			sheep->zv += zd * d;
		}
	}
}



static void arbitrary_finite_edge_collision(SHEEP *sheep, float x0, float y0, float z0, float xb, float yb, float zb, float len)
{
	float xa = sheep->x - x0, ya = sheep->y - y0, za = sheep->z - z0;
	float d = xa*xb + ya*yb + za*zb;
	if (d <= 0)
		point_collision(sheep, x0, y0, z0);
	else if (d >= len)
		point_collision(sheep, x0+xb, y0+yb, z0+zb);
	else
		arbitrary_edge_collision(sheep, x0, y0, z0, xb, yb, zb);
}



static void left_slope_collision(SHEEP *sheep, int x, int y, float z)
{
	if (sheep->y < y)
		arbitrary_finite_edge_collision(sheep, x, y  , z, 0.707106781187f, 0, 0.707106781187f, 1.41421356237f);
	else if (sheep->y > y+1)
		arbitrary_finite_edge_collision(sheep, x, y+1, z, 0.707106781187f, 0, 0.707106781187f, 1.41421356237f);
	else if (sheep->x <= x - SHEEP_R * 0.707106781187f)
		edge_collision(x, z, &sheep->x, &sheep->z, &sheep->xv, &sheep->zv);
	else if (sheep->x >= x + 1.0f - SHEEP_R * 0.707106781187f)
		edge_collision(x+1, z+1.0f, &sheep->x, &sheep->z, &sheep->xv, &sheep->zv);
	else {
		z -= x - sheep->x;
		z += SHEEP_R * 1.41421356237f;
		if (sheep->z < z) {
			sheep->z = z;
			{ float t = sheep->xv; sheep->xv = sheep->zv * 0.9f; sheep->zv = t * 0.9f; }
		}
	}
}



static void right_slope_collision(SHEEP *sheep, int x, int y, float z)
{
	x++;
	if (sheep->y < y)
		arbitrary_finite_edge_collision(sheep, x, y  , z, -0.707106781187f, 0, 0.707106781187f, 1.41421356237f);
	else if (sheep->y > y+1)
		arbitrary_finite_edge_collision(sheep, x, y+1, z, -0.707106781187f, 0, 0.707106781187f, 1.41421356237f);
	else if (sheep->x >= x + SHEEP_R * 0.707106781187f)
		edge_collision(x, z, &sheep->x, &sheep->z, &sheep->xv, &sheep->zv);
	else if (sheep->x <= x - 1.0f + SHEEP_R * 0.707106781187f)
		edge_collision(x-1, z+1.0f, &sheep->x, &sheep->z, &sheep->xv, &sheep->zv);
	else {
		z -= sheep->x - x;
		z += SHEEP_R * 1.41421356237f;
		if (sheep->z < z) {
			sheep->z = z;
			{ float t = sheep->xv; sheep->xv = sheep->zv * -0.9f; sheep->zv = t * -0.9f; }
		}
	}
}



static void away_slope_collision(SHEEP *sheep, int x, int y, float z)
{
	if (sheep->x < x)
		arbitrary_finite_edge_collision(sheep, x  , y, z, 0, 0.707106781187f, 0.707106781187f, 1.41421356237f);
	else if (sheep->x > x+1)
		arbitrary_finite_edge_collision(sheep, x+1, y, z, 0, 0.707106781187f, 0.707106781187f, 1.41421356237f);
	else if (sheep->y <= y - SHEEP_R * 0.707106781187f)
		edge_collision(y, z, &sheep->y, &sheep->z, &sheep->yv, &sheep->zv);
	else if (sheep->y >= y + 1.0f - SHEEP_R * 0.707106781187f)
		edge_collision(y+1, z+1.0f, &sheep->y, &sheep->z, &sheep->yv, &sheep->zv);
	else {
		z -= y - sheep->y;
		z += SHEEP_R * 1.41421356237f;
		if (sheep->z < z) {
			sheep->z = z;
			{ float t = sheep->yv; sheep->yv = sheep->zv * 0.9f; sheep->zv = t * 0.9f; }
		}
	}
}



static void towards_slope_collision(SHEEP *sheep, int x, int y, float z)
{
	y++;
	if (sheep->x < x)
		arbitrary_finite_edge_collision(sheep, x  , y, z, 0, -0.707106781187f, 0.707106781187f, 1.41421356237f);
	else if (sheep->x > x+1)
		arbitrary_finite_edge_collision(sheep, x+1, y, z, 0, -0.707106781187f, 0.707106781187f, 1.41421356237f);
	else if (sheep->y >= y + SHEEP_R * 0.707106781187f)
		edge_collision(y, z, &sheep->y, &sheep->z, &sheep->yv, &sheep->zv);
	else if (sheep->y <= y - 1.0f + SHEEP_R * 0.707106781187f)
		edge_collision(y-1, z+1.0f, &sheep->y, &sheep->z, &sheep->yv, &sheep->zv);
	else {
		z -= sheep->y - y;
		z += SHEEP_R * 1.41421356237f;
		if (sheep->z < z) {
			sheep->z = z;
			{ float t = sheep->yv; sheep->yv = sheep->zv * -0.9f; sheep->zv = t * -0.9f; }
		}
	}
}



int update_sheep(STATE *state) /* Returns nonzero on no more sheep. */
{
	SHEEP *sheep;

	/* Step 1: create a new sheep if the time is right. */
	if (state->sheep_left > 0) {
		if (--state->blower_time <= 0) {
			sheep = malloc(sizeof(*sheep));
			if (sheep) {
				BLOWER *blower = &state->l->blower[state->blower_next];
				sheep->next = state->sheep;
				if (sheep->next) sheep->next->prev = sheep;
				sheep->prev = NULL;
				state->sheep = sheep;
				sheep->x = blower->x + 0.5f - xmove[blower->d] * 0.25f;
				sheep->y = blower->y + 0.5f - ymove[blower->d] * 0.25f;
				sheep->z = blower->z + 0.5f;
				sheep->xv = xmove[blower->d] * 0.002f;
				sheep->yv = ymove[blower->d] * 0.002f;
				sheep->zv = 0;
			}
			state->sheep_left--;
			state->blower_next++;
			if (state->blower_next >= state->l->n_blowers) state->blower_next = 0;
			state->blower_time = state->l->sheep_interval;
		}
	} else if (!state->sheep && --state->fade == 0)
		return 1;

	/* Step 2: propagate sheep positions. */
	for (sheep = state->sheep; sheep; sheep = sheep->next) {
		sheep->zv -= 0.00007f;
		sheep->zv *= 0.9999f;
		sheep->x += sheep->xv;
		sheep->y += sheep->yv;
		sheep->z += sheep->zv;
	}

	/* Step 3: sort sheep by z position. */

	/* Step 4: do sheep-sheep collision detection. */

	/* Step 5: do sheep-player collision detection. */

	/* Step 6: do sheep-level collision detection. */
	sheep = state->sheep;
	while (sheep) {
		int x = (int)floor(sheep->x);
		int y = (int)floor(sheep->y);

		BLOCK *block = get_block_at(state, x, y);
		if (block) {
			float z = block->z;
			if (BLOCK_CAN_SUPPORT(block->type)) {
				z += 1.0f + SHEEP_R;
				if (sheep->z < z) {
					sheep->z = z;
					if (sheep->zv < 0) sheep->zv *= -0.9f;
					continue;
				}
			} else {
				switch (block->type) {
					case BLOCK_SLOPE(DIR_LEFT):    z +=         sheep->x - x ; break;
					case BLOCK_SLOPE(DIR_RIGHT):   z += 1.0f - (sheep->x - x); break;
					case BLOCK_SLOPE(DIR_AWAY):    z +=         sheep->y - y ; break;
					case BLOCK_SLOPE(DIR_TOWARDS): z += 1.0f - (sheep->y - y); break;
				}
				z += SHEEP_R * 1.41421356237f; /* Vertical displacement at contact */
				if (sheep->z < z) {
					sheep->z = z;
					switch (block->type) {
						case BLOCK_SLOPE(DIR_LEFT):    { float t = sheep->xv; sheep->xv = sheep->zv *  0.9f; sheep->zv = t *  0.9f; } break;
						case BLOCK_SLOPE(DIR_RIGHT):   { float t = sheep->xv; sheep->xv = sheep->zv * -0.9f; sheep->zv = t * -0.9f; } break;
						case BLOCK_SLOPE(DIR_AWAY):    { float t = sheep->yv; sheep->yv = sheep->zv *  0.9f; sheep->zv = t *  0.9f; } break;
						case BLOCK_SLOPE(DIR_TOWARDS): { float t = sheep->yv; sheep->yv = sheep->zv * -0.9f; sheep->zv = t * -0.9f; } break;
					}
				}
			}
		} else if (sheep->z < SHEEP_R) {
			SHEEP *old = sheep;
			sheep = old->next;
			if (old->next) old->next->prev = old->prev;
			if (old->prev) old->prev->next = old->next; else state->sheep = old->next;
			free(old);
			if (x >= 0 && x < state->l->w && y >= 0 && y < state->l->h && state->l->hole[y][x])
				state->sheep_saved++;
			else
				state->sheep_lost++;
			continue;
		}

		block = get_block_at(state, x-1, y);
		if (block) {
			if (sheep->z < block->z) {
				if (sheep->x < x + SHEEP_R) {
					sheep->x = x + SHEEP_R;
					if (sheep->xv < 0) sheep->xv *= -0.9f;
				}
			} else if (block->type == BLOCK_SLOPE(DIR_RIGHT)) {
				right_slope_collision(sheep, x-1, y, block->z);
			} else if (block->type == BLOCK_SLOPE(DIR_AWAY) && sheep->z > block->z + sheep->y - y && sheep->z < block->z + sheep->y - y + 1.41421356237f) {
				arbitrary_edge_collision(sheep, x, y  , block->z, 0,  0.707106781187f, 0.707106781187f);
			} else if (block->type == BLOCK_SLOPE(DIR_TOWARDS) && sheep->z > block->z + y+1 - sheep->y && sheep->z < block->z + y+1 - sheep->y + 1.41421356237f) {
				arbitrary_edge_collision(sheep, x, y+1, block->z, 0, -0.707106781187f, 0.707106781187f);
			} else { /* including BLOCK_SLOPE(DIR_LEFT) */
				if (sheep->z < block->z+1.0f) {
					if (sheep->x < x + SHEEP_R) {
						sheep->x = x + SHEEP_R;
						if (sheep->xv < 0) sheep->xv *= -0.9f;
					}
				} else if (sheep->z < block->z+1.0f+SHEEP_R)
					edge_collision(x, block->z+1.0f, &sheep->x, &sheep->z, &sheep->xv, &sheep->zv);
			}
		}

		block = get_block_at(state, x+1, y);
		if (block) {
			if (sheep->z < block->z) {
				if (sheep->x > x+1 - SHEEP_R) {
					sheep->x = x+1 - SHEEP_R;
					if (sheep->xv > 0) sheep->xv *= -0.9f;
				}
			} else if (block->type == BLOCK_SLOPE(DIR_LEFT)) {
				left_slope_collision(sheep, x+1, y, block->z);
			} else if (block->type == BLOCK_SLOPE(DIR_AWAY) && sheep->z > block->z + sheep->y - y && sheep->z < block->z + sheep->y - y + 1.41421356237f) {
				arbitrary_edge_collision(sheep, x+1, y  , block->z, 0,  0.707106781187f, 0.707106781187f);
			} else if (block->type == BLOCK_SLOPE(DIR_TOWARDS) && sheep->z > block->z + y+1 - sheep->y && sheep->z < block->z + y+1 - sheep->y + 1.41421356237f) {
				arbitrary_edge_collision(sheep, x+1, y+1, block->z, 0, -0.707106781187f, 0.707106781187f);
			} else { /* including BLOCK_SLOPE(DIR_RIGHT) */
				if (sheep->z < block->z+1.0f) {
					if (sheep->x > x+1 - SHEEP_R) {
						sheep->x = x+1 - SHEEP_R;
						if (sheep->xv > 0) sheep->xv *= -0.9f;
					}
				} else if (sheep->z < block->z+1.0f+SHEEP_R)
					edge_collision(x+1, block->z+1.0f, &sheep->x, &sheep->z, &sheep->xv, &sheep->zv);
			}
		}

		block = get_block_at(state, x, y-1);
		if (block) {
			if (sheep->z < block->z) {
				if (sheep->y < y + SHEEP_R) {
					sheep->y = y + SHEEP_R;
					if (sheep->yv < 0) sheep->yv *= -0.9f;
				}
			} else if (block->type == BLOCK_SLOPE(DIR_TOWARDS)) {
				towards_slope_collision(sheep, x, y-1, block->z);
			} else if (block->type == BLOCK_SLOPE(DIR_LEFT) && sheep->z > block->z + sheep->x - x && sheep->z < block->z + sheep->x - x + 1.41421356237f) {
				arbitrary_edge_collision(sheep, x  , y, block->z,  0.707106781187f, 0, 0.707106781187f);
			} else if (block->type == BLOCK_SLOPE(DIR_RIGHT) && sheep->z > block->z + x+1 - sheep->x && sheep->z < block->z + x+1 - sheep->x + 1.41421356237f) {
				arbitrary_edge_collision(sheep, x+1, y, block->z, -0.707106781187f, 0, 0.707106781187f);
			} else { /* including BLOCK_SLOPE(DIR_AWAY) */
				if (sheep->z < block->z+1.0f) {
					if (sheep->y < y + SHEEP_R) {
						sheep->y = y + SHEEP_R;
						if (sheep->yv < 0) sheep->yv *= -0.9f;
					}
				} else if (sheep->z < block->z+1.0f+SHEEP_R)
					edge_collision(y, block->z+1.0f, &sheep->y, &sheep->z, &sheep->yv, &sheep->zv);
			}
		}

		block = get_block_at(state, x, y+1);
		if (block) {
			if (sheep->z < block->z) {
				if (sheep->y > y+1 - SHEEP_R) {
					sheep->y = y+1 - SHEEP_R;
					if (sheep->yv > 0) sheep->yv *= -0.9f;
				}
			} else if (block->type == BLOCK_SLOPE(DIR_AWAY)) {
				away_slope_collision(sheep, x, y+1, block->z);
			} else if (block->type == BLOCK_SLOPE(DIR_LEFT) && sheep->z > block->z + sheep->x - x && sheep->z < block->z + sheep->x - x + 1.41421356237f) {
				arbitrary_edge_collision(sheep, x  , y+1, block->z,  0.707106781187f, 0, 0.707106781187f);
			} else if (block->type == BLOCK_SLOPE(DIR_RIGHT) && sheep->z > block->z + x+1 - sheep->x && sheep->z < block->z + x+1 - sheep->x + 1.41421356237f) {
				arbitrary_edge_collision(sheep, x+1, y+1, block->z, -0.707106781187f, 0, 0.707106781187f);
			} else { /* including BLOCK_SLOPE(DIR_TOWARDS) */
				if (sheep->z < block->z+1.0f) {
					if (sheep->y > y+1 - SHEEP_R) {
						sheep->y = y+1 - SHEEP_R;
						if (sheep->yv > 0) sheep->yv *= -0.9f;
					}
				} else if (sheep->z < block->z+1.0f+SHEEP_R)
					edge_collision(y+1, block->z+1.0f, &sheep->y, &sheep->z, &sheep->yv, &sheep->zv);
			}
		}

		block = get_block_at(state, x-1, y-1);
		if (block) {
			if (sheep->z < block->z)
				edge_collision(x, y, &sheep->x, &sheep->y, &sheep->xv, &sheep->yv);
			else if (block->type == BLOCK_SLOPE(DIR_RIGHT))
				right_slope_collision(sheep, x-1, y-1, block->z);
			else if (block->type == BLOCK_SLOPE(DIR_TOWARDS))
				towards_slope_collision(sheep, x-1, y-1, block->z);
			else if (sheep->z < block->z+1.0f)
				edge_collision(x, y, &sheep->x, &sheep->y, &sheep->xv, &sheep->yv);
			else
				point_collision(sheep, x, y, block->z+1.0f);
		}

		block = get_block_at(state, x+1, y-1);
		if (block) {
			if (sheep->z < block->z)
				edge_collision(x+1, y, &sheep->x, &sheep->y, &sheep->xv, &sheep->yv);
			else if (block->type == BLOCK_SLOPE(DIR_LEFT))
				left_slope_collision(sheep, x+1, y-1, block->z);
			else if (block->type == BLOCK_SLOPE(DIR_TOWARDS))
				towards_slope_collision(sheep, x-1, y-1, block->z);
			else if (sheep->z < block->z+1.0f)
				edge_collision(x+1, y, &sheep->x, &sheep->y, &sheep->xv, &sheep->yv);
			else
				point_collision(sheep, x+1, y, block->z+1.0f);
		}

		block = get_block_at(state, x-1, y+1);
		if (block) {
			if (sheep->z < block->z)
				edge_collision(x, y+1, &sheep->x, &sheep->y, &sheep->xv, &sheep->yv);
			else if (block->type == BLOCK_SLOPE(DIR_RIGHT))
				right_slope_collision(sheep, x-1, y+1, block->z);
			else if (block->type == BLOCK_SLOPE(DIR_AWAY))
				away_slope_collision(sheep, x-1, y+1, block->z);
			else if (sheep->z < block->z+1.0f)
				edge_collision(x, y+1, &sheep->x, &sheep->y, &sheep->xv, &sheep->yv);
			else
				point_collision(sheep, x, y+1, block->z+1.0f);
		}

		block = get_block_at(state, x+1, y+1);
		if (block) {
			if (sheep->z < block->z)
				edge_collision(x+1, y+1, &sheep->x, &sheep->y, &sheep->xv, &sheep->yv);
			else if (block->type == BLOCK_SLOPE(DIR_LEFT))
				left_slope_collision(sheep, x+1, y+1, block->z);
			else if (block->type == BLOCK_SLOPE(DIR_AWAY))
				away_slope_collision(sheep, x-1, y+1, block->z);
			else if (sheep->z < block->z+1.0f)
				edge_collision(x+1, y+1, &sheep->x, &sheep->y, &sheep->xv, &sheep->yv);
			else
				point_collision(sheep, x+1, y+1, block->z+1.0f);
		}

		sheep = sheep->next;
	}

	return 0;
}
