1 files changed, 22 insertions, 13 deletions

diff --git a/fw/table.c b/fw/table.c
index c78d5e8..0e65b83 100644
--- a/fw/table.c
+++ b/fw/table.c
@@ -43,40 +43,49 @@ tabRead(const Table *tab, U8 k, U32 *key, U16 *val) {
 
 Status
 tabLookup(const Table *tab, Number key, U16 *val) {
-	U8 k;
-	Number tkey;
+	U8 row;
+	Number tkey1, tkey2;
 	U16 tval1, tval2;
 	Status status;
 	int ord;
 
 	// Search for key
-	for (k = 0u; (k < TAB_ROWS-1u); k++) {
-		status = tabRead(tab, k, &tkey.u32, &tval1);
+	for (row = 0u; row < (TAB_ROWS-1u); row++) {
+		status = tabRead(tab, row, &tkey1.u32, &tval1);
 		if (status != OK) {
 			return FAIL;
 		}
 
-		status = u32ToNum(tkey.u32, key.type, &tkey);
+		status = u32ToNum(tkey1.u32, key.type, &tkey1);
 		if (status != OK) {
 			return FAIL;
 		}
 
-		status = cmpNum(key, tkey, &ord);
+		status = cmpNum(key, tkey1, &ord);
 		if (status != OK) {
 			return FAIL;
 		}
 		if (ord == 0) { // found exact key
 			*val = tval1; 
 			return OK;
-		} else if (ord > 0) { // interpolate
-			status = tabRead(tab, k+1u, &tkey.u32, &tval2);
-			if (status != OK) {
-				return FAIL;
+		} else if (ord < 0) { // key < tkey
+			if (row > 0u) { // not first row
+				// Interpolate with previous row
+				status = tabRead(tab, row-1u, &tkey2.u32, &tval2);
+				if (status != OK) {
+					return FAIL;
+				}
+				status = u32ToNum(tkey2.u32, key.type, &tkey2);
+				if (status != OK) {
+					return FAIL;
+				}
+				*val = (tval1 + tval2) / 2u; // TODO: linear interpolation
+			} else { // key < key of first row
+				*val = tval1; // use first row value
 			}
-			*val = (tval1 + tval2) / 2u; 
 			return OK;
-		} else { // less
-			// continue
+		} else { // key > tkey
+			// continue until key <= tkey
 		}
 	}