Index: trunk/code/projects/fp_math/fp_math.c
===================================================================
--- trunk/code/projects/fp_math/fp_math.c	(revision 1584)
+++ trunk/code/projects/fp_math/fp_math.c	(revision 1585)
@@ -5,10 +5,14 @@
 #include <serial.h>
 
 #define ABS(x) (x > 0 ? x : -x)
+#define FP_PI_OVER_TWO 102944
+#define FP_TWO_PI 411775
 
-#define TABLE_LENGTH 128
+#define TABLE_LENGTH 64
 #define TABLE_STEP 1621//.160757
 
+static inline int32_t trig_lookup(int32_t theta, int32_t* table);
+
 static int32_t linspace[TABLE_LENGTH] PROGMEM = { 
 	0, 1621, 3242, 4863, 6485, 8106, 9727, 11348, 
 	12969, 14590, 16212, 17833, 19454, 21075, 22696, 24317, 
@@ -17,18 +21,10 @@
 	51877, 53498, 55119, 56741, 58362, 59983, 61604, 63225, 
 	64846, 66468, 68089, 69710, 71331, 72952, 74573, 76195, 
 	77816, 79437, 81058, 82679, 84300, 85922, 87543, 89164, 
-	90785, 92406, 94027, 95648, 97270, 98891, 100512, 102133, 
-	103754, 105375, 106997, 108618, 110239, 111860, 113481, 115102, 
-	116724, 118345, 119966, 121587, 123208, 124829, 126451, 128072, 
-	129693, 131314, 132935, 134556, 136178, 137799, 139420, 141041, 
-	142662, 144283, 145904, 147526, 149147, 150768, 152389, 154010, 
-	155631, 157253, 158874, 160495, 162116, 163737, 165358, 166980, 
-	168601, 170222, 171843, 173464, 175085, 176707, 178328, 179949, 
-	181570, 183191, 184812, 186433, 188055, 189676, 191297, 192918, 
-	194539, 196160, 197782, 199403, 201024, 202645, 204266, 205887
+	90785, 92406, 94027, 95648, 97270, 98891, 100512, 102133
 };
 
-static int32_t cos_table[TABLE_LENGTH] PROGMEM = {
+static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {
 	65536, 65516, 65456, 65356, 65215, 65035, 64815, 64556, 
 	64257, 63919, 63541, 63125, 62670, 62176, 61645, 61076, 
 	60470, 59826, 59146, 58430, 57678, 56890, 56068, 55212, 
@@ -36,73 +32,68 @@
 	46053, 44886, 43691, 42470, 41222, 39949, 38652, 37331, 
 	35988, 34622, 33235, 31828, 30401, 28956, 27492, 26013, 
 	24517, 23006, 21481, 19943, 18393, 16831, 15260, 13679, 
-	12089, 10492, 8889, 7280, 5667, 4050, 2431, 811, 
-	-811, -2431, -4050, -5667, -7280, -8889, -10492, -12089, 
-	-13679, -15260, -16831, -18393, -19943, -21481, -23006, -24517, 
-	-26013, -27492, -28956, -30401, -31828, -33235, -34622, -35988, 
-	-37331, -38652, -39949, -41222, -42470, -43691, -44886, -46053, 
-	-47193, -48303, -49384, -50434, -51454, -52442, -53398, -54322, 
-	-55212, -56068, -56890, -57678, -58430, -59146, -59826, -60470, 
-	-61076, -61645, -62176, -62670, -63125, -63541, -63919, -64257, 
-	-64556, -64815, -65035, -65215, -65356, -65456, -65516, -65536
+	12089, 10492, 8889, 7280, 5667, 4050, 2431, 811
 };
 
-static int32_t sin_table[TABLE_LENGTH] PROGMEM = { 
-	0, 1621, 3241, 4859, 6474, 8085, 9691, 11292, 
-	12885, 14470, 16047, 17614, 19169, 20714, 22245, 23763, 
-	25267, 26755, 28226, 29680, 31117, 32534, 33931, 35307, 
-	36662, 37995, 39304, 40589, 41849, 43084, 44292, 45473, 
-	46627, 47751, 48847, 49913, 50948, 51952, 52924, 53864, 
-	54771, 55644, 56484, 57288, 58058, 58792, 59491, 60152, 
-	60777, 61365, 61915, 62428, 62902, 63338, 63735, 64093, 
-	64411, 64691, 64930, 65130, 65291, 65411, 65491, 65531, 
-	65531, 65491, 65411, 65291, 65130, 64930, 64691, 64411, 
-	64093, 63735, 63338, 62902, 62428, 61915, 61365, 60777, 
-	60152, 59491, 58792, 58058, 57288, 56484, 55644, 54771, 
-	53864, 52924, 51952, 50948, 49913, 48847, 47751, 46627, 
-	45473, 44292, 43084, 41849, 40589, 39304, 37995, 36662, 
-	35307, 33931, 32534, 31117, 29680, 28226, 26755, 25267, 
-	23763, 22245, 20714, 19169, 17614, 16047, 14470, 12885, 
-	11292, 9691, 8085, 6474, 4859, 3241, 1621, 0
-};
+//FIXME Lazy implementations of tangent and sine.
+int32_t fp_tan(int32_t theta) { 
+	return fp_div(fp_sin(theta), fp_cos(theta));
+}
 
+int32_t fp_sin(int32_t theta) { 
+	return fp_cos(theta + FP_PI_OVER_TWO);
+}
+
 //Quadratic interpolation.
 int32_t fp_cos(int32_t theta) { 
 	uint8_t n;	
+	int8_t negative;
 	int32_t x_n, x_np1, x_np2;
 	int32_t y_n, y_np1, y_np2;
-	int32_t dd_n, dd_np1, second_dd;
+	int32_t dd_n, dd_np1, second_dd, result;
 	
-	//Move theta into [0, pi]
-	theta = ABS(theta) % FP_TWO_PI;
+	//Move theta into [0, pi/2] w/ appropriate sign.
+	theta = ABS(theta) % FP_TWO_PI; 
+
 	if(theta > FP_PI) 
 		theta = FP_TWO_PI - theta;
+	
+	if(theta > FP_PI_OVER_TWO) {
+		theta = FP_PI - theta;
+		negative = 1;
+	}
 
 	//Find the nearest table values. FIXME
 	n = theta / TABLE_STEP;
-	while((x_np1 = pgm_read_dword(&linspace[n+1])) < theta) n++;
-	usb_puti(n);
+	while( n < TABLE_LENGTH - 1 && (x_np1 = pgm_read_dword(&linspace[n+1]))) 
+		n++;
 	
 	//theta is between x_{n} and x_{n+1}
-
-	//Address edge cases.	
-	if(n == 0) { 
-		//TODO 
-	}
 	
 	if(n == TABLE_LENGTH - 1) { 
-		//TODO 
+		//Perform linear interpolation, since we're close to zero anyway.
+		x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]);
+		y_n = pgm_read_dword(&fp_cos_table[TABLE_LENGTH - 1]);
+
+		result = fp_mult(fp_div(FP_PI_OVER_TWO - x_n, 0 - y_n), theta - x_n) + y_n;
+		return negative ? -result : result;
 	}
+
+	if(n == TABLE_LENGTH) { 
+		//We didn't find a value! Oh no!
+		usb_puts("fp_math: We couldn't find surrounding table values! \n\r
+				  This should never happen!!!\n\r\n\r");
+		return 0;
+	}
 	
 	//Address the general case. Quadratic interpolation.
 	//Load in the necessary values.
 	x_n = pgm_read_dword(&linspace[n]);
-	x_np1 = pgm_read_dword(&linspace[n + 1]);
 	x_np2  = pgm_read_dword(&linspace[n + 2]);
 	
-	y_n = pgm_read_dword(&cos_table[n]);
-	y_np1 = pgm_read_dword(&cos_table[n + 1]);
-	y_np2 = pgm_read_dword(&cos_table[n + 2]);
+	y_n = pgm_read_dword(&fp_cos_table[n]);
+	y_np1 = pgm_read_dword(&fp_cos_table[n + 1]);
+	y_np2 = pgm_read_dword(&fp_cos_table[n + 2]);
 
 	//Calculate first divided differences.
 	dd_n = fp_div(y_np1 - y_n, x_np1 - x_n);
@@ -111,8 +102,10 @@
 	//Calculate the second divided difference.
 	second_dd = fp_div(dd_np1 - dd_n, x_np2 - x_n);
 	
-	return fp_mult(fp_mult(second_dd, theta - x_n), theta - x_np1)  
-		   + fp_mult(dd_n, theta - x_n) + y_n;
+	result = fp_mult(fp_mult(second_dd, theta - x_n), theta - x_np1)  
+		   	+ fp_mult(dd_n, theta - x_n) + y_n;
+
+	return negative ? -result : result;
 }
 
 //FIXME I didn't write these functions very carefully...
Index: trunk/code/projects/fp_math/fp_math.h
===================================================================
--- trunk/code/projects/fp_math/fp_math.h	(revision 1584)
+++ trunk/code/projects/fp_math/fp_math.h	(revision 1585)
@@ -3,7 +3,6 @@
 #define _FP_MATH_H_
 
 #define FP_PI 205887
-#define FP_TWO_PI 411775
 
 #include <stdint.h>
 
