void steering_Wandering( Vector2* pos, Vector2* vel, Vector2* currWanderTarget, float radius, float offset, float displacement, Vector2* newWanderTargetOut, Vector2* desiredVelocityOut )
{
	assert( pos != NULL );
	assert( vel != NULL );
	assert( desiredVelocityOut != NULL );
	assert( currWanderTarget != NULL );
	assert( newWanderTargetOut != NULL );
	assert( radius > 0.0f );

	// we'll use the circle method, we'll need a target that is stored as an offset from the current position

	// find circle center
	Vector2 circleCenter = ( *vel );
	vec2_Normalize( &circleCenter );
	vec2_Scale( &circleCenter, offset, &circleCenter );

	// create displaced target
	(*newWanderTargetOut) = vec2( rand_GetToleranceFloat( NULL, 0.0f, displacement ), rand_GetToleranceFloat( NULL, 0.0f, displacement ) );
	vec2_Add( currWanderTarget, newWanderTargetOut, newWanderTargetOut );

	// project displaced target onto circle
	Vector2 diff;
	vec2_Subtract( newWanderTargetOut, &circleCenter, &diff );
	vec2_Normalize( &diff );
	vec2_AddScaled( &circleCenter, &diff, radius, newWanderTargetOut );

	( *desiredVelocityOut ) = ( *newWanderTargetOut );
	vec2_Normalize( desiredVelocityOut );
}
void steering_Flee( Vector2* pos, Vector2* target, Vector2* out )
{
	assert( pos != NULL );
	assert( target != NULL );
	assert( out != NULL );

	vec2_Subtract( pos, target, out );
	vec2_Normalize( out );
}
void steering_Arrive( Vector2* pos, Vector2* target, float innerRadius, float outerRadius, Vector2* out )
{
	assert( pos != NULL );
	assert( target != NULL );
	assert( out != NULL );
	assert( innerRadius <= outerRadius );

	vec2_Subtract( target, pos, out );
	float dist = vec2_Normalize( out );

	float speed = inverseLerp( innerRadius, outerRadius, dist );
	vec2_Scale( out, speed, out );
}
static void drawVehicles( )
{
	for( size_t i = 0; i < sb_Count( sbVehicles ); ++i ) {
		debugRenderer_Circle( 1, sbVehicles[i].pos, sbVehicles[i].radius, sbVehicles[i].clr );

		// draw line from the center to the edge for the facing
		Vector2 facing;
		vec2_NormalFromRot( sbVehicles[i].rotRad, &facing );
		vec2_AddScaled( &( sbVehicles[i].pos ), &facing, sbVehicles[i].radius, &facing );
		debugRenderer_Line( 1, sbVehicles[i].pos, facing, sbVehicles[i].clr );

		// draw a line outward from the edge for the velocity direction
		Vector2 normalVel = sbVehicles[i].vel;
		Vector2 velLineStart;
		Vector2 velLineEnd;
		float mag = vec2_Normalize( &normalVel );
		vec2_AddScaled( &( sbVehicles[i].pos ), &normalVel, sbVehicles[i].radius, &velLineStart );
		vec2_AddScaled( &velLineStart, &normalVel, ( mag / sbVehicles[i].maxSpeed ) * sbVehicles[i].radius, &velLineEnd );
		debugRenderer_Line( 1, velLineStart, velLineEnd, sbVehicles[i].clr );
	}
}
static void vehiclePhysics( float dt )
{
	for( size_t i = 0; i < sb_Count( sbVehicles ); ++i ) {

		vec2_AddScaled( &( sbVehicles[i].pos ), &( sbVehicles[i].vel ), dt, &( sbVehicles[i].pos ) );
		vec2_AddScaled( &( sbVehicles[i].pos ), &( sbVehicles[i].linearAccel ), 0.5f * dt * dt, &( sbVehicles[i].pos ) );

		sbVehicles[i].rotRad += ( sbVehicles[i].rotSpdRad * dt ) + ( 0.5f * dt * dt * sbVehicles[i].angularAccelRad );

		vec2_AddScaled( &( sbVehicles[i].vel ), &( sbVehicles[i].linearAccel ), dt, &( sbVehicles[i].vel ) );
		if( vec2_MagSqrd( &(sbVehicles[i].vel) ) > ( sbVehicles[i].maxSpeed * sbVehicles[i].maxSpeed ) ) {
			vec2_Normalize( &(sbVehicles[i].vel) );
			vec2_Scale( &(sbVehicles[i].vel), sbVehicles[i].maxSpeed, &(sbVehicles[i].vel) );
		}

		sbVehicles[i].rotSpdRad += sbVehicles[i].angularAccelRad * dt;
		if( fabsf( sbVehicles[i].rotSpdRad ) > sbVehicles[i].maxRotSpeedRad ) {
			sbVehicles[i].rotSpdRad = sbVehicles[i].maxRotSpeedRad * sign( sbVehicles[i].rotSpdRad );
		}

		sbVehicles[i].pos.x = clamp( bordersMin.x + sbVehicles[i].radius, bordersMax.x - sbVehicles[i].radius, sbVehicles[i].pos.x );
		sbVehicles[i].pos.y = clamp( bordersMin.y + sbVehicles[i].radius, bordersMax.y - sbVehicles[i].radius, sbVehicles[i].pos.y );
	}
}
void ai_getDistances(int player, AI_Distances *distances) {
	enum { eFront = 0, eLeft, eRight, eBackleft, eMax };
	segment2 segments[eMax];
	vec2 v, vPos;
	Data *data = game->player[player].data;
	int dirLeft = (data->dir + 3) % 4;
	int dirRight = (data->dir + 1) % 4;
	int i, j;
	float *front = &distances->front;
	float *right = &distances->right;
	float *left = &distances->left;
	float *backleft = &distances->backleft;

	getPositionFromIndex(vPos.v + 0, vPos.v + 1, player);

	for(i = 0; i < eMax; i++) {
		vec2_Copy(&segments[i].vStart, &vPos);
	}

	segments[eFront].vDirection.v[0] = (float) dirsX[data->dir];
	segments[eFront].vDirection.v[1] = (float) dirsY[data->dir];
	segments[eLeft].vDirection.v[0] = (float) dirsX[dirLeft];
	segments[eLeft].vDirection.v[1] = (float) dirsY[dirLeft];
	segments[eRight].vDirection.v[0] = (float) dirsX[dirRight];
	segments[eRight].vDirection.v[1] = (float) dirsY[dirRight];
	segments[eBackleft].vDirection.v[0] = (float) dirsX[dirLeft] - dirsX[data->dir];
	segments[eBackleft].vDirection.v[1] = (float) dirsY[dirLeft] - dirsY[data->dir];
	vec2_Normalize(&segments[eBackleft].vDirection,
								&segments[eBackleft].vDirection);
	*front = FLT_MAX;
	*left = FLT_MAX;
	*right = FLT_MAX;
	*backleft = FLT_MAX;

	// loop over all segment
	for(i = 0; i < game->players; i++) {
		segment2 *wall = game->player[i].data->trails;
		if(game->player[i].data->trail_height < TRAIL_HEIGHT)
			continue;

		for(j = 0; j < game->player[i].data->trailOffset + 1; j++) {
			float t1, t2;
			if(i == player && j == game->player[i].data->trailOffset)
				break;
			if(segment2_Intersect(&v, &t1, &t2, segments + eFront, wall) &&
				 t1 > 0 && t1 < *front && t2 >= 0 && t2 <= 1)
				*front = t1;
			if(segment2_Intersect(&v, &t1, &t2, segments + eLeft, wall) &&
				 t1 > 0 && t1 < *left && t2 >= 0 && t2 <= 1)
				*left = t1;
			if(segment2_Intersect(&v, &t1, &t2, segments + eRight, wall) &&
				 t1 > 0 && t1 < *right && t2 >= 0 && t2 <= 1)
				*right = t1;
			if(segment2_Intersect(&v, &t1, &t2, segments + eBackleft, wall) &&
				 t1 > 0 && t1 < *backleft && t2 >= 0 && t2 <= 1)
				*backleft = t1;
			wall++;
		}
	}
	for(i = 0; i < game2->level->nBoundaries; i++) {
		float t1, t2;
		segment2* wall = game2->level->boundaries + i;
		if(segment2_Intersect(&v, &t1, &t2, segments + eFront, wall) &&
			 t1 > 0 && t1 < *front && t2 >= 0 && t2 <= 1)
			*front = t1;
		if(segment2_Intersect(&v, &t1, &t2, segments + eLeft, wall) &&
			 t1 > 0 && t1 < *left && t2 >= 0 && t2 <= 1)
			*left = t1;
		if(segment2_Intersect(&v, &t1, &t2, segments + eRight, wall) &&
			 t1 > 0 && t1 < *right && t2 >= 0 && t2 <= 1)
			*right = t1;
		if(segment2_Intersect(&v, &t1, &t2, segments + eBackleft, wall) &&
			 t1 > 0 && t1 < *backleft && t2 >= 0 && t2 <= 1)
			*backleft = t1;
	}
	
	// update debug render segments
	{
		AI *ai = game->player[player].ai;
		vec2_Copy(&ai->front.vStart, &vPos);
		vec2_Copy(&ai->left.vStart, &vPos);
		vec2_Copy(&ai->right.vStart, &vPos);
		vec2_Copy(&ai->backleft.vStart, &vPos);
		
		ai->front.vDirection.v[0] = *front * dirsX[data->dir];
		ai->front.vDirection.v[1] = *front * dirsY[data->dir];
		ai->left.vDirection.v[0] = *left * dirsX[dirLeft];
		ai->left.vDirection.v[1] = *left * dirsY[dirLeft];
		ai->right.vDirection.v[0] = *right * dirsX[dirRight];
		ai->right.vDirection.v[1] = *right * dirsY[dirRight];
		ai->backleft.vDirection.v[0] = (float) (dirsX[dirLeft] - dirsX[data->dir]);
		ai->backleft.vDirection.v[1] = (float) (dirsY[dirLeft] - dirsY[data->dir]);
		vec2_Normalize(&ai->backleft.vDirection,
									&ai->backleft.vDirection);
		vec2_Scale(&ai->backleft.vDirection, 
							&ai->backleft.vDirection,
							*backleft);
	}
		
	// printf("%.2f, %.2f, %.2f\n", *front, *right, *left);
	return;
}
void ai_getConfig(int player, int target,
										AI_Configuration *config) {

	Data *data;
	
	getPositionFromIndex(config->player.vStart.v + 0,
											 config->player.vStart.v + 1,
											 player);
	getPositionFromIndex(config->opponent.vStart.v + 0,
											 config->opponent.vStart.v + 1,
											 target);
	
	data = game->player[player].data;
	config->player.vDirection.v[0] = dirsX[ data->dir ] * data->speed;
	config->player.vDirection.v[1] = dirsY[ data->dir ] * data->speed;

	data = game->player[target].data;
	config->opponent.vDirection.v[0] = dirsX[ data->dir ] * data->speed;
	config->opponent.vDirection.v[1] = dirsY[ data->dir ] * data->speed;
	
	// compute sector
	{
		vec2 diff;
		vec3 v1, v2, v3;
		vec3 up = { { 0, 0, 1 } };
		float cosphi;
		float phi;
		int i;

		vec2_Sub(&diff, &config->player.vStart, &config->opponent.vStart);
		v1.v[0] = diff.v[0];
		v1.v[1] = diff.v[1];
		v1.v[2] = 0;

		v2.v[0] = config->opponent.vDirection.v[0];
		v2.v[1] = config->opponent.vDirection.v[1];
		v2.v[2] = 0;

		vec3_Normalize(&v1, &v1);
		vec3_Normalize(&v2, &v2);

		vec3_Cross(&v3, &v1, &v2);
		vec3_Normalize(&v3, &v3);
	
		cosphi = vec3_Dot(&v1, &v2);
		nebu_Clamp(&cosphi, -1, 1);
		phi = (float) acos(cosphi);
		if(vec3_Dot(&v3, &up) > 0)
			phi = 2 * (float) M_PI - phi;
	
		for(i = 0; i < 8; i++) {
			phi -= (float) M_PI / 4;
			if(phi < 0) {
				config->location = i;
				break;
			}
		}
	}
	// compute intersection
	{
		segment2 seg1;
		segment2 seg2;
		seg1.vStart = config->opponent.vStart;
		seg1.vDirection = config->opponent.vDirection;
		seg2.vStart = config->player.vStart;
		vec2_Orthogonal( &seg2.vDirection, &config->opponent.vDirection );
		vec2_Normalize( &seg2.vDirection, &seg2.vDirection );
		vec2_Scale( &seg2.vDirection, 
							 &seg2.vDirection,
							 vec2_Length( &config->player.vDirection )
							 );
							 
		segment2_Intersect( &config->intersection, 
												&config->t_opponent, &config->t_player,
												&seg1, &seg2 );
		if(config->t_player < 0)
			config->t_player *= -1;
	}
	
}