
#import "Tracker.h"
#import "Actor.h"

@implementation Tracker

double normAngle(double theta)
{
	if (theta > M_PI)
		return normAngle(theta - 2.0*M_PI);
	else if (theta < -M_PI)
		return normAngle(theta + 2.0*M_PI);
	else
		return theta;
}


- initFor:aShip
{
	[super init];
	resolution = 0.1;
	ship = aShip;
	return self;
}

- track:target
{
	to = target;
	return [self update];
}

- ship
{
	return ship;
}

- target
{
	return ship;
}

- setResolution:(float) res
{
	resolution = res;
	return self;
}

- update
{
// Egads this is slopppy; plus it hardwires wrap around!
	if (to) {
		int n = N_AXIS;
		int off[] = {xOffset, yOffset};
		while (n--) {
			x[n] =  [ship distance:to axis:n];
			v[n] =  [ship velocity:to axis:n];
			if (x[n] > off[n])
				x[n] -= 2*off[n];
			else if (x[n] < -off[n])
				x[n] += 2*off[n];
		}
	} else {
		x[0] = x[1] = 0.0;
		v[0] = v[1] = 0.0;
	}
	return self;
}

- projectForward:(float)time
{
	int n = 2;
	while (n--)
		x[n] += n*v[n];
	return self;
}

-(float) angle
{
	register float at = atan2(x[1],x[0]);
	return normAngle(at - [ship angle]);
}

- (float) distance
{
    return sqrt(x[0]*x[0] + x[1]*x[1]);
}

- (float) velocity	
{
    return sqrt(v[0]*v[0] + v[1]*v[1]);
}

-(float) Vangle
{
	register float at = atan2(v[1],v[0]);
	return normAngle(at - [ship angle]);
}

/*
 * Control sequences
 */

- (BOOL) isFacing:(float)angle
{
	float target_angle = to->radius / [self distance];
	return (fabs(angle) < resolution) || (fabs(angle) < target_angle);
}


- (BOOL) turnTowards
{
	return [self turnTowards:[self angle]];
}

- (BOOL) turnTowards:(float)angle
{
	if ([self isFacing:angle]) {
		[ship setTurning:LEFT down:NO time:timeInMS];
		[ship setTurning:RIGHT down:NO time:timeInMS];
		return YES;
	} else {
		ROTATION LR = (angle > 0.0) ? LEFT : RIGHT;
		float norm = fabs(angle) / M_PI;
		[ship setTurning:LR down:YES time:timeInMS];
		if (norm < 0.5) {
			int rot = 1000 * norm; // scale amount to turn so don't overshoot
			[ship setTurning:LR down:NO time:timeInMS+rot];
		}
		return NO;
	}
}

- (BOOL) thrustTowards
{
	return [self thrustTowards:[self angle]];
}

- (BOOL) thrustTowards:(float)angle
{
	BOOL isFacing = [self turnTowards:angle];
	[ship setThrusting:isFacing time:timeInMS];
	return isFacing;
}

- (BOOL) matchVelocity
{
	if ([self thrustTowards:[self Vangle]] && ([self velocity] < 5.0)) {
		int	vscale = 1000 * [self velocity] * timeScale;
#ifdef DEBUG_MATCH
		printf("Match: %d for %f at %d\n",vscale, [self velocity], timeInMS);
#endif
		[ship setThrusting:NO time:timeInMS+vscale];
	}
	return [self isFacing:[self angle]];
}

- (BOOL) shieldsWithin:(float)distance
{
	BOOL flag = ([self distance] < distance);
	[ship setShields:flag];
	return flag;
}

- (BOOL) fireBetween:(float)far :(float)near
{
	float dist = [self distance];
	if (dist < far && dist > near) {
		[ship setShields:NO];
		[ship fire];
		return YES;
	}
	return NO;
}

- (BOOL) projectToHit
{
	extern float xw_bulletSpeed;
	float oldDist = 0;
	float d;
	int n = 0;
//	do {
		float t;
		d = [self distance] - oldDist;
	 	t = d / xw_bulletSpeed;
		[self projectForward:t];
//	} while (d > 10.0 && ++n < 10);
	return (n<10);
}


@end

@implementation Ship (Tracker)

- (float) distance:(Actor*)to axis:(AXIS)xy;
{
	return (xy == X_AXIS) ? to->x - x : to->y - y;
}

- (float) velocity:(Actor*)to axis:(AXIS)xy;
{
	return (xy == X_AXIS) ? to->xv - xv : to->yv - yv;
}

- (float) angle
{
	return theta + M_PI_2;
}

@end
