#include "header.h"

// table for inverse tangent (of form 512 = 90 degrees)
short InvTanLut[] = {
   0,   3,   5,   8,  10,  13,  15,  18,  20,  23,  25,  28,  31,  33,  36,  38,  
  41,  43,  46,  48,  51,  53,  56,  58,  61,  63,  66,  69,  71,  74,  76,  79,  
  81,  84,  86,  89,  91,  94,  96,  99, 101, 104, 106, 108, 111, 113, 116, 118, 
 121, 123, 126, 128, 131, 133, 136, 138, 140, 143, 145, 148, 150, 152, 155, 157, 
 160, 162, 164, 167, 169, 172, 174, 176, 179, 181, 183, 186, 188, 190, 193, 195, 
 197, 200, 202, 204, 207, 209, 211, 214, 216, 218, 220, 223, 225, 227, 229, 232, 
 234, 236, 238, 241, 243, 245, 247, 249, 252, 254, 256, 258, 260, 262, 265, 267,
 269, 271, 273, 275, 277, 279, 282, 284, 286, 288, 290, 292, 294, 296, 298, 300, 
 302, 304, 306, 308, 310, 312, 314, 316, 318, 320, 322, 324, 326, 328, 330, 332, 
 334, 336, 338, 340, 342, 344, 346, 347, 349, 351, 353, 355, 357, 359, 360, 362, 
 364, 366, 368, 370, 371, 373, 375, 377, 379, 380, 382, 384, 386, 387, 389, 391, 
 393, 394, 396, 398, 399, 401, 403, 405, 406, 408, 410, 411, 413, 415, 416, 418, 
 419, 421, 423, 424, 426, 428, 429, 431, 432, 434, 435, 437, 439, 440, 442, 443, 
 445, 446, 448, 449, 451, 452, 454, 455, 457, 458, 460, 461, 463, 464, 466, 467, 
 469, 470, 471, 473, 474, 476, 477, 479, 480, 481, 483, 484, 486, 487, 488, 490, 
 491, 492, 494, 495, 496, 498, 499, 500, 502, 503, 504, 506, 507, 508, 509, 511
};


void GetHeight(void) {
	VECTOR a, b, norm;
	VERTEX v0, v1, v2;
	MATRIX tempMatrix;
	int planeConst, i, trackHeight, dh, slow=1, seg1, seg2;
	
	// Get height points x&z (4th never changes)
	ResetMatrix(tempMatrix.m);
	RotMatrixY(racer.rot.vy, &tempMatrix);
	SetVector(&racer.heightPoint[0], 0, 0, 64<<8);
	SetVector(&racer.heightPoint[1], -24<<8, 0, -16<<8);
	SetVector(&racer.heightPoint[2], 24<<8, 0, -16<<8);
	ApplyMatrixLV(&tempMatrix, &racer.heightPoint[0], &racer.heightPoint[0]);
	ApplyMatrixLV(&tempMatrix, &racer.heightPoint[1], &racer.heightPoint[1]);
	ApplyMatrixLV(&tempMatrix, &racer.heightPoint[2], &racer.heightPoint[2]);

	racer.heightOffTrack = 0;
	for (i=0; i<4; i++) {
		seg1 = racer.heightSegment[i]<<1;
		seg2 = ((racer.heightSegment[i]+1)%rawTrack->numSegments)<<1;
		if (racer.half[i]) {
			v0 = vertexLut[seg2+1];
			v1 = vertexLut[seg2];
			v2 = vertexLut[seg1+1];
			SetVector(&a, v1.vx-v0.vx, v1.vy-v0.vy, v1.vz-v0.vz);
			SetVector(&b, v0.vx-v2.vx, v0.vy-v2.vy, v0.vz-v2.vz);
		}
		else {
			v0 = vertexLut[seg1+1];
			v1 = vertexLut[seg1];
			v2 = vertexLut[seg2];
			SetVector(&a, v1.vx-v0.vx, v1.vy-v0.vy, v1.vz-v0.vz);
			SetVector(&b, v2.vx-v1.vx, v2.vy-v1.vy, v2.vz-v1.vz);
		}
		
		// Get normal to current segment
		CrossProduct(&a, &b, &norm);
		// Find planar constant
		planeConst = v0.vx*norm.vx + v0.vy*norm.vy + v0.vz*norm.vz;
		
		// Work out height at each point
		trackHeight = (planeConst - norm.vx*((racer.heightPoint[i].vx+racer.pos.vx)>>8) - norm.vz*((racer.heightPoint[i].vz+racer.pos.vz)>>8))/norm.vy;
		trackHeight <<= 8;
		dh = racer.height[i] - trackHeight;
		if (dh < (6<<8)) {
			if (dh < 0) {
				racer.heightVel[i] = 0;
				racer.height[i] = trackHeight+(3<<8);
				if (slow) {
					racer.speed >>= 1;
					racer.vel.vx >>= 1;
					racer.vel.vy >>= 1;
					racer.vel.vz >>= 1;
					slow = 0;
				}
			}
			dh = (6<<8);
		}
		racer.heightOffTrack += dh;
		// Front is heavier than rear
		if (i == 0)
			racer.heightVel[i] += ((32*8<<16)/dh) - (8<<8);
		else
			racer.heightVel[i] += ((32*7<<16)/dh) - (7<<8);
		racer.height[i] += racer.heightVel[i];
		racer.heightVel[i] = (racer.heightVel[i]*3)>>2;
	}
	// Average 4 heights
	racer.heightOffTrack >>= 2;
	racer.tilt += (pad.leftStickH*3-racer.tilt)>>2;
	// Get craft orientation from height points
	racer.rot.vx = FindRotation(racer.height[3], racer.height[0], 64<<8);
	racer.rot.vz = FindRotation(racer.height[2], racer.height[1], 48<<8)+racer.tilt;
}


int TrackCollide(void) {
	Line a, b;
	VERTEX *v0, *v1, *v2, *v3;
	MATRIX tempMatrix;
	int i, loopAgain, dx, dz, seg1, seg2, bonk=0, trackAng, actualSpeed, velAng;
	
	// Get collision point segment numbers
	for (i=0; i<3; i++) {
		do {
			loopAgain = 0;
			seg1 = racer.collisionSegment[i]<<1;
			seg2 = ((racer.collisionSegment[i]+1)%rawTrack->numSegments)<<1;
			v0 = &vertexLut[seg1];
			v1 = &vertexLut[seg1+1];
			v2 = &vertexLut[seg2];
			v3 = &vertexLut[seg2+1];
			a.sx = (racer.pos.vx+racer.vel.vx+racer.collisionPoint[i].vx)>>8;
			a.sy = (racer.pos.vz+racer.vel.vz+racer.collisionPoint[i].vz)>>8;
			a.ex = (v0->vx+v1->vx+v2->vx+v3->vx)>>2;
			a.ey = (v0->vz+v1->vz+v2->vz+v3->vz)>>2;
			
			b.sx = v2->vx;
			b.sy = v2->vz;
			b.ex = v3->vx;
			b.ey = v3->vz;
		
			if (LineCollision(a, b)) {
				racer.collisionSegment[i] = (racer.collisionSegment[i]+1)%rawTrack->numSegments;
				loopAgain = 1;
			}
			else {
				b.sx = v0->vx;
				b.sy = v0->vz;
				b.ex = v1->vx;
				b.ey = v1->vz;
				if (LineCollision(a, b)) {
					racer.collisionSegment[i]--;
					if (racer.collisionSegment[i] < 0)
						racer.collisionSegment[i] = rawTrack->numSegments-1;
					loopAgain = 1;
				}
	
			}
			// Check for collision with sides
			b.sx = v0->vx;
			b.sy = v0->vz;
			b.ex = v2->vx;
			b.ey = v2->vz;
			if (LineCollision(a, b)) {
				dx = b.ex-b.sx;
				dz = b.ey-b.sy;
				bonk = 1;
				tempMatrix.m[0][0] = 0;
				tempMatrix.m[2][0] = 16<<8;
				trackAng = InvTan(dz, dx);
				RotMatrixY(trackAng+1024, &tempMatrix);
				racer.pos.vx += tempMatrix.m[0][0];
				racer.pos.vz += tempMatrix.m[2][0];
				loopAgain = 1;
			}
			b.sx = v1->vx;
			b.sy = v1->vz;
			b.ex = v3->vx;
			b.ey = v3->vz;
			if (LineCollision(a, b)) {
				dx = b.ex-b.sx;
				dz = b.ey-b.sy;
				bonk = 1;
				tempMatrix.m[0][0] = 0;
				tempMatrix.m[2][0] = 16<<8;
				trackAng = InvTan(dz, dx);
				RotMatrixY(trackAng-1024, &tempMatrix);
				racer.pos.vx += tempMatrix.m[0][0];
				racer.pos.vz += tempMatrix.m[2][0];
				loopAgain = 1;
			}
		} while (loopAgain);
	}
	if (bonk) {
		// Get new velocity
		dx = racer.vel.vx;
		dz = racer.vel.vz;
		velAng = InvTan(dz, dx);
		if (velAng-trackAng > 2047)
			trackAng += 4096;
		else {
			if (trackAng-velAng > 2047)
				velAng += 4096;
		}
		actualSpeed = IntSqrt(dx*dx + dz*dz);
		tempMatrix.m[0][0] = 0;
		tempMatrix.m[2][0] = actualSpeed;
		
		// Angle of incedence = trackAng - velAng
		// Angle of reflectance = trackAng + (Angle of incedence)
		RotMatrixY((trackAng<<1)-velAng, &tempMatrix);
		// If difference between angle of velocity and angle of track
		// is small (approx 12 degrees) then don't slow down
		if (Abs(velAng-trackAng) < 160) {
			racer.vel.vx = tempMatrix.m[0][0];
			racer.vel.vz = tempMatrix.m[2][0];
			bonk = 2; // return value for small bonk
		}
		else {
			racer.vel.vx = tempMatrix.m[0][0]>>2;
			racer.vel.vz = tempMatrix.m[2][0]>>2;
			racer.speed >>= 1;
		}
	}

	// Get height point segment numbers
	for (i=0; i<4; i++) {
		do {
			loopAgain = 0;
			seg1 = racer.heightSegment[i]<<1;
			seg2 = ((racer.heightSegment[i]+1)%rawTrack->numSegments)<<1;
			v0 = &vertexLut[seg1];
			v1 = &vertexLut[seg1+1];
			v2 = &vertexLut[seg2];
			v3 = &vertexLut[seg2+1];
			a.sx = (racer.pos.vx+racer.vel.vx+racer.heightPoint[i].vx)>>8;
			a.sy = (racer.pos.vz+racer.vel.vz+racer.heightPoint[i].vz)>>8;
			a.ex = (v0->vx+v1->vx+v2->vx+v3->vx)>>2;
			a.ey = (v0->vz+v1->vz+v2->vz+v3->vz)>>2;
			
			b.sx = v2->vx;
			b.sy = v2->vz;
			b.ex = v3->vx;
			b.ey = v3->vz;
		
			if (LineCollision(a, b)) {
				racer.heightSegment[i] = (racer.heightSegment[i]+1)%rawTrack->numSegments;
				loopAgain = 1;
			}
			else {
				b.sx = v0->vx;
				b.sy = v0->vz;
				b.ex = v1->vx;
				b.ey = v1->vz;
				if (LineCollision(a, b)) {
					racer.heightSegment[i]--;
					if (racer.heightSegment[i] < 0)
						racer.heightSegment[i] = rawTrack->numSegments-1;
					loopAgain = 1;
				}
	
			}
		} while (loopAgain);
		// Get which half of segment each point is on
		a.ex = (v0->vx+v1->vx)>>1;
		a.ey = (v0->vz+v1->vz)>>1;
		b.sx = v1->vx;
		b.sy = v1->vz;
		b.ex = v2->vx;
		b.ey = v2->vz;
		if (LineCollision(a, b))
			racer.half[i] = 1;
		else
			racer.half[i] = 0;
	}
	
	return bonk;
}


// Check if lines a & b collide (excluding endpoints)
int LineCollision(Line a, Line b) {
	long swapTemp;
	long ay, by, cy, dy;
	
	// Ensure coords are defined in correct order
	if (a.sx > a.ex) {
		swapTemp=a.sx; a.sx=a.ex; a.ex=swapTemp;
		swapTemp=a.sy; a.sy=a.ey; a.ey=swapTemp;
	}
	if (b.sx > b.ex) {
		swapTemp=b.sx; b.sx=b.ex; b.ex=swapTemp;
		swapTemp=b.sy; b.sy=b.ey; b.ey=swapTemp;
	}
	
	// Clip lines
	if (a.sx < b.sx) {
		if (a.ex <= b.sx)
			return 0;
		ay = ((a.ey-a.sy)*(b.sx-a.sx)) / (a.ex-a.sx) + a.sy;
		cy = b.sy;
		if (b.ex < a.ex) {
			by = ((a.ey-a.sy)*(b.ex-a.sx)) / (a.ex-a.sx) + a.sy;
			dy = b.ey;
		}
		else {
			by = a.ey;
			dy = ((b.ey-b.sy)*(a.ex-b.sx)) / (b.ex-b.sx) + b.sy;
		}
	}
	else {
		if (b.ex <= a.sx)
			return 0;
		ay = a.sy;
		cy = ((b.ey-b.sy)*(a.sx-b.sx)) / (b.ex-b.sx) + b.sy;
		if (a.ex < b.ex) {
			by = a.ey;
			dy = ((b.ey-b.sy)*(a.ex-b.sx)) / (b.ex-b.sx) + b.sy;
		}
		else {
			by = ((a.ey-a.sy)*(b.ex-a.sx)) / (a.ex-a.sx) + a.sy;
			dy = b.ey;
		}
	}
	
	// Don't accept collision if lines are touching
	if (ay==cy || by==dy)
		return 0;
	
	// Check that they cross
	return (Sgn(cy-ay) != Sgn(dy-by));
}


u_long IntSqrt(u_long n) {
    u_long r=0, s, v=n;
    
    
    SqrtStep(15); SqrtStep(14); SqrtStep(13); SqrtStep(12);
    SqrtStep(11); SqrtStep(10); SqrtStep(9); SqrtStep(8);
    SqrtStep(7); SqrtStep(6); SqrtStep(5); SqrtStep(4);
    SqrtStep(3); SqrtStep(2); SqrtStep(1); SqrtStep(0);
    
    return r;
}


long FindRotation(long height1, long height2, long length) {
	register int delta, delta2;  

	if (height1 == height2)
		return(0);

	if (height1 > height2) {
		delta= (height1 - height2);
		delta2=InvTan(length, delta);		
		return ((long)(delta2 & 4095)); 	
	
	}
	else {
		delta = (height2 - height1);
		delta2 = InvTan(length, delta);	
		return ((long)4096-(delta2 & 4095));
	}
}


// A modified version of some code that I found 
long InvTan(long x, long y) {
	long t;

	if (x==0 && y==0)
		return 0;
	
	if (Abs(x) == Abs(y)) {
		if (x > 0)
			return (y<0 ? 3584 : 512);
		return (y<0 ? 2560 : 1536);
	}

	if (Abs(x) > Abs(y)) {
		t = (y<<8)/x;

		if (t>=0)
			return InvTanLut[t&255] + (x<0 ? 2048 : 0);

		return (x<0 ? 2048 : 4096) - InvTanLut[(-t)&255];
	}

	t = (x<<8)/y;
	if (t>=0)
		return (y<0 ? 3072 : 1024) - InvTanLut[t&255];

	return InvTanLut[(-t) & 255] + (y < 0 ? 3072 : 1024);
}