diff --git a/src/application/application_definitions.h b/src/application/application_definitions.h
index 3aa5dc4fbc9bea827226839a5377a56496b4249e..a1389593139117e62e2905dd0dd4709f7aa1353f 100644
--- a/src/application/application_definitions.h
+++ b/src/application/application_definitions.h
@@ -144,8 +144,12 @@ enum
 #define BLINK_FRAME_CRTL_AND_ADDRESS    (BLINK_FRAME_SOURCE_ADDRESS + BLINK_FRAME_CTRLP) //10 bytes
 #define BLINK_FRAME_LEN_BYTES           (BLINK_FRAME_CRTL_AND_ADDRESS + BLINK_FRAME_CRC)
 
-#define UWB_LIST_SIZE				    (10)	//maximum number of UWBs to range with (valid options are 0 through 253)//TODO check what works based on data sizes needed for INF packets and 16 or 64 bit addres
-#define UWB_COMM_TIMEOUT                3000    //ms	//TODO maybe make this a function of other defines?
+#define UWB_LIST_SIZE				    10	//the maximum size of the UWB network
+											//do not use a number larger than 80 if USING_64BIT_ADDR==1 and 127 if USING_64BIT_ADDR==0
+											//these are the largest UWB network sizes that the firmware will support.
+											//USING_64BIT_ADDR==1: Limiting factor is the max length of the INF messages
+											//USING_64BIT_ADDR==0: Limiting factor is the max power of 2 that fits in a uint8 (framelength is stored as uint8)
+#define UWB_COMM_TIMEOUT                3000    //ms	//TODO make this a function of UWB_LIST_SIZE, twice the max framelengt? maybe this shouldn't be a define?
 
 //UWB_LIST entry types
 #define UWB_LIST_SELF					0 //entry for self in list
@@ -154,8 +158,8 @@ enum
 #define UWB_LIST_TWICE_HIDDEN			3 //uwb in list that is active, out of range, and a hidden neighbor is slotted to range with
 #define UWB_LIST_INACTIVE               4 //uwb in list that is not active (could have previously been neighbor, hidden, or twice hidden)
 
-#define BLINK_SLEEP_DELAY					0     //ms //how long the tag should sleep after blinking
-#define POLL_SLEEP_DELAY					25     //ms //how long the tag should sleep after ranging
+#define BLINK_SLEEP_DELAY				0     //ms //how long the tag should sleep after blinking
+#define POLL_SLEEP_DELAY				25     //ms //how long the tag should sleep after ranging
 
 
 
@@ -165,15 +169,15 @@ enum
 // NOTE: the maximum RX timeout is ~ 65ms
 // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 
-#define INST_DONE_WAIT_FOR_NEXT_EVENT   1   //this signifies that the current event has been processed and instance is ready for next one
+#define INST_DONE_WAIT_FOR_NEXT_EVENT   	1   //this signifies that the current event has been processed and instance is ready for next one
 #define INST_DONE_WAIT_FOR_NEXT_EVENT_TO    2   //this signifies that the current event has been processed and that instance is waiting for next one with a timeout
-                                        //which will trigger if no event coming in specified time
-#define INST_NOT_DONE_YET               0   //this signifies that the instance is still processing the current event
+                                        		//which will trigger if no event coming in specified time
+#define INST_NOT_DONE_YET               	0   //this signifies that the instance is still processing the current event
 
 // Function code byte offset (valid for all message types).
 #define FCODE                               0               // Function code is 1st byte of messageData
 
-//INF message byte offsets
+//INF message byte offsets					//TODO remove the +6
 #define TDMA_TSFS                           1+6				// offset to put time since TDMA frame start in the INF message
 #define TDMA_TSFS_REBASE					7+6				// offset to put whether the receiving UWB needs to rebase its TDMA frame to the transmitted TSFS
 #define TDMA_NUMN							8+6				// offset to put the number of this UWB's neighbors in the INF message
@@ -196,16 +200,6 @@ enum
 #define SYNC_FRAMELENGTH					1				// offset to put the framelength in the sync message
 #define SYNC_TSFS							2				// offset to put the time since TDMA frame start in the sync message
 
-//TODO remove below
-// Ranging init message byte offsets. Composed of tag short address, anchor
-// response delay and tag response delay.
-//#define RNG_INIT_TAG_SHORT_ADDR_LO 1
-//#define RNG_INIT_TAG_SHORT_ADDR_HI 2
-//#define RNG_INIT_ANC_RESP_DLY_LO 3
-//#define RNG_INIT_ANC_RESP_DLY_HI 4
-//#define RNG_INIT_TAG_RESP_DLY_LO 5
-//#define RNG_INIT_TAG_RESP_DLY_HI 6
-
 // Response delay values coded in ranging init message.
 // This is a bitfield composed of:
 //   - bits 0 to 14: value
diff --git a/src/application/dw_main.c b/src/application/dw_main.c
index b49a499a2bbba994947e84001e17d6e5cd8a1646..c4f079a8f2551d73de69ed85cf3ce326b6f54d06 100644
--- a/src/application/dw_main.c
+++ b/src/application/dw_main.c
@@ -302,7 +302,7 @@ int dw_main(void)
 {
     int i = 0;
     double range_result = 0;
-    uint64 range_addr = 0;
+//    uint64 range_addr = 0;
     int canSleep;
 
     //LCD variables
@@ -579,8 +579,8 @@ int dw_main(void)
 			memset(dataseq1, ' ', LCD_BUFF_LEN);
 			uint64 addr = instance_get_addr();
 
-			uint8 num_neighbors = instfindnumactiveneighbors(inst);
-			uint8 num_hidden = instfindnumactivehidden(inst);
+			uint8 num_neighbors = instfindnumneighbors(inst);
+			uint8 num_hidden = instfindnumhidden(inst);
 			char status[10];
 
 			if(instance_mode == DISCOVERY)
diff --git a/src/application/instance.c b/src/application/instance.c
index 9751dc1c9f79a66408412d5619ee70507371fe21..3c75779dce12f2164ca8dcc58acfbe085a970716 100644
--- a/src/application/instance.c
+++ b/src/application/instance.c
@@ -395,11 +395,11 @@ void send_statetousb(instance_data_t *inst, struct TDMAHandler *tdma_handler)
         usbdebugdataprev_size = usbdebugdata_size;
         memcpy(usbdebugdataprev, usbdebugdata, usbdebugdata_size);
 
-        int num_neighbors = instfindnumactiveneighbors(inst);
-		uint8 debug_msg[150];
-		int n = sprintf((char*)&debug_msg[0], "mode: %s, num_neighbors %d, discovery_mode %s", get_instanceModes_string(inst->mode), num_neighbors, get_discovery_modes_string(tdma_handler->discovery_mode));
-		send_usbmessage(&debug_msg[0], n);
-		usb_run();
+//        int num_neighbors = instfindnumneighbors(inst);
+//		uint8 debug_msg[150];
+//		int n = sprintf((char*)&debug_msg[0], "mode: %s, num_neighbors %d, discovery_mode %s", get_instanceModes_string(inst->mode), num_neighbors, get_discovery_modes_string(tdma_handler->discovery_mode));
+//		send_usbmessage(&debug_msg[0], n);
+//		usb_run();
 
     }
 }
@@ -624,10 +624,11 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
 
 
 
-          	//select a TX action, return if TRUE if we should move on to another state
+          	//select a TX action, return TRUE if we should move on to another state
         	if(tdma_handler->tx_select(tdma_handler) == TRUE)
         	{
         		dwt_forcetrxoff();
+
         	}
         	else
         	{
@@ -954,7 +955,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
 
             //set the delayed rx on time (the response message will be sent after this delay)
             dwt_setrxaftertxdelay(inst->txToRxDelayTag_sy*0); //TODO fix this! remove *0
-            dwt_setrxtimeout((uint16)inst->fwtoTime_sy*0);    //TODO fix this! remove *2
+            dwt_setrxtimeout((uint16)inst->fwtoTime_sy*2);    //TODO fix this! remove *2
 
             //response is expected
             inst->wait4ack = DWT_RESPONSE_EXPECTED;
@@ -1317,6 +1318,18 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
             inst->testAppState = TA_RX_WAIT_DATA;   // let this state handle it
             inst->rxCheckOnTime = portGetTickCnt() + RX_CHECK_ON_PERIOD;
 
+
+//            //see if tx is actually enabled
+//			//read SYS_STATE, getting second byte
+//			uint8 regval = dwt_read8bitoffsetreg(SYS_STATE_ID,1);
+//			//get the first 5 bytes
+//			regval &= 0x1F;
+//			uint8 debug_msg[200];
+//			int n = sprintf((char *)&debug_msg, "RX STATUS: regval %u", regval);
+//			send_usbmessage(&debug_msg[0], n);
+//			usb_run();
+
+
             // end case TA_RXE_WAIT, don't break, but fall through into the TA_RX_WAIT_DATA state to process it immediately.
             if(message == 0) 
             {
@@ -1499,6 +1512,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
 							{
 								if(tdma_handler->uwbListTDMAInfo[0].slotsLength == 0)
 								{
+									inst->mode = DISCOVERY;
 									tdma_handler->set_discovery_mode(tdma_handler, WAIT_INF_REG, time_now);
 								}
 							}
@@ -1650,39 +1664,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
 
 							break;
 						}//RTLS_DEMO_MSG_INF
-//                        case RTLS_DEMO_MSG_INF_SUG :
-//						{
-//							//TODO: process differently depending on if we are or aren't in discovery
-//
-//							//process the INF_SUG packet and return to RX
-//							uint8 debug_msg[100];
-//							int n = sprintf((char*)&debug_msg[0], "process RTLS_DEMO_MSG_INF_SUG :");
-//							send_usbmessage(&debug_msg[0], n);
-//							usb_run();
-//
-////							uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
-////
-////							//synchronise the frames
-////							tdma_handler->frame_sync(tdma_handler, messageData, dw_event->rxLength, srcIndex, FS_ADOPT);
-////							//initialize collection of tdma info, clear any previously stored info
-////							tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_ALL_COPY);
-////							//set discovery mode to EXIT
-////							tdma_handler->set_discovery_mode(tdma_handler, EXIT, time_now); //TODO do I really need EXIT?
-//
-//
-//							uint8 srcIndex = instgetuwblistindex(inst, &srcAddr[0], inst->addrByteSize);
-//
-//							//TODO implement
-////							tdma_handler->process_sug_msg(tdma_handler, messageData, &srcAddr);
-//							tdma_handler->process_inf_msg(tdma_handler, messageData, srcIndex, CLEAR_LISTED_COPY);
-//
-//							//TODO add frame sync!
-//
-//							//stay in RX wait for next frame...
-//							inst->testAppState = TA_RXE_WAIT ;              // wait for next frame
-//
-//							break;
-//						}//RTLS_DEMO_MSG_INF_SUG
                         case RTLS_DEMO_MSG_TAG_POLL:
                         {
                             if(dw_event->typePend == DWT_SIG_TX_PENDING)
@@ -1848,7 +1829,6 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
 									}
 								}
 
-//								inst->lastRangeTimeStamp[inst->uwbToRangeWith] = portGetTickCnt();
 								tdma_handler->uwbListTDMAInfo[inst->uwbToRangeWith].lastRange = portGetTickCnt();
 
 								uint8 debug_msg[100];
@@ -1857,9 +1837,7 @@ int testapprun(instance_data_t *inst, struct TDMAHandler *tdma_handler, int mess
 								send_usbmessage(&debug_msg[0], n);
 								usb_run();
 
-
-								//TODO remember that below is for testing out if its better to send INF as last message
-//								inst->testAppState = TA_TXINF_WAIT_SEND;
+								tdma_handler->firstPollComplete = TRUE;
 								inst->testAppState = TA_TX_SELECT;
 								inst->previousState = TA_INIT;
 								inst->nextState = TA_INIT;
diff --git a/src/application/instance.h b/src/application/instance.h
index 04c3ba9843a2a8b6f8462c4a3ffd8f0f91b426ba..c3d5149be3cb1f22cbc376b8f2ab4727cb9af9a6 100644
--- a/src/application/instance.h
+++ b/src/application/instance.h
@@ -127,12 +127,12 @@ typedef struct
 	int lateTX;
 	int lateRX;
 
-    double adist[RTD_MED_SZ] ;
-    double adist4[4] ;
-    double longTermRangeSum ;
-    int longTermRangeCount ;
-    int tofIndex ;
-    int tofCount ;
+//    double adist[RTD_MED_SZ] ;
+//    double adist4[4] ;
+//    double longTermRangeSum ;
+//    int longTermRangeCount ;
+//    int tofIndex ;
+//    int tofCount ;
 
     uint8 newRangeUWBIndex; //index for most recent ranging exchange
     int newRange;
@@ -156,10 +156,10 @@ typedef struct
 //	uint8 uwbNumActive[UWB_LIST_SIZE];		//number of TAGs each tracked ANCHOR is actively ranging with. //TODO remove?
 
     // keep track of when final messages so we can drop uwbs that we haven't communicated with in a while
-    uint32 lastCommTimeStamp[UWB_LIST_SIZE]; //TODO move into tdma_handler?
-    uint32 lastHiddenTimeStamp[UWB_LIST_SIZE];
-    uint32 lastTwiceHiddenTimeStamp[UWB_LIST_SIZE];
-    uint8 uwbTimeout[UWB_LIST_SIZE] ;		//TODO remove and use list type instead
+//    uint32 lastCommTimeStamp[UWB_LIST_SIZE]; //TODO move into tdma_handler?
+//    uint32 lastHiddenTimeStamp[UWB_LIST_SIZE];
+//    uint32 lastTwiceHiddenTimeStamp[UWB_LIST_SIZE];
+//    uint8 uwbTimeout[UWB_LIST_SIZE] ;		//TODO remove and use list type instead
 
 //    uint32 lastRangeTimeStamp[UWB_LIST_SIZE];
 
@@ -214,9 +214,10 @@ int instaddactivateuwbinlist(instance_data_t *inst, uint8 *uwbAddr);
 int instcheckactiveuwbinlist(instance_data_t *inst, uint8 *uwbAddr);
 int instfindfirstactiveuwbinlist(instance_data_t *inst, uint8 startindex);
 int instfindnumactiveuwbinlist(instance_data_t *inst);
-int instfindnumactiveneighbors(instance_data_t *inst);
-int instfindnumactivehidden(instance_data_t *inst);
+int instfindnumneighbors(instance_data_t *inst);
+int instfindnumhidden(instance_data_t *inst);
 int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr, uint8 addrByteSize);
+void instremoveuwb(instance_data_t *inst, uint8 uwb_index);
 
 
 void instance_readaccumulatordata(void);
diff --git a/src/application/instance_common.c b/src/application/instance_common.c
index b9b6dd9a3ba0e8ce80e88dfbb215a040c06fc247..ac1cb51f1b7cc8de31a1d504e6a9186bfa3895ab 100644
--- a/src/application/instance_common.c
+++ b/src/application/instance_common.c
@@ -296,7 +296,7 @@ int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr, uint8 addrByteSiz
             {
                 memcpy(&inst->uwbList[i][0], &uwbAddr[0], addrByteSize) ;
                 inst->uwbListLen = i + 1 ;
-				inst->uwbTimeout[i] = 1;
+//				inst->uwbTimeout[i] = 1;
 				return i;
             }
         }
@@ -309,142 +309,195 @@ int instgetuwblistindex(instance_data_t *inst, uint8 *uwbAddr, uint8 addrByteSiz
     return 255;
 }
 
-
 // -------------------------------------------------------------------------------------------------------------------
 //
-// function to add new UWBs to the uwb list or remove timeout status from UWBs already in uwb list
+// function to remove the UWB at a given index from our list. Take care when calling this inside of a loop iterating over the uwbList!
 //
 // -------------------------------------------------------------------------------------------------------------------
-// return 1 if UWB added to list or removed from timeout status 
-// return 0 if UWB not added list or removed from timeout status
-int instaddactivateuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
+// return index of UWB
+void instremoveuwb(instance_data_t *inst, uint8 uwb_index)
 {
-    uint8 i;
-    uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+	if(uwb_index > 0 && uwb_index < inst->uwbListLen)
+	{
+		if(inst->uwbToRangeWith == uwb_index)
+		{
+			uwb_index = 255;
+		}
+		else if(inst->uwbToRangeWith > uwb_index && inst->uwbToRangeWith < inst->uwbListLen)
+		{
+			inst->uwbToRangeWith--;
+		}
 
-    //add the new UWB to the list, if not already there and there is space
-    for(i=1; i<UWB_LIST_SIZE; i++)//0 reserved for self. timeout status not used for self
-    {
-        if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) != 0)
-        {
-            if(memcmp(&inst->uwbList[i][0], &blank[0], inst->addrByteSize) == 0) //blank entry
-            {
-                memcpy(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) ;
-                inst->uwbListLen = i + 1 ;
-				inst->uwbToRangeWith = i;
-				inst->uwbTimeout[i] = 0;
-				return 1;
-            }
-        }
-        else
-        {
-			if(inst->uwbTimeout[i])
+		if(inst->newRange == 1)
+		{
+			if(inst->newRangeUWBIndex == uwb_index)
 			{
-				//uwb has timed out, wake it up
-				inst->uwbToRangeWith = i;
-				inst->uwbTimeout[i] = 0;
-				return 1;
+				uwb_index = 0;
+				inst->newRange = 0;
 			}
-			else
+			else if(inst->newRangeUWBIndex > uwb_index && inst->newRangeUWBIndex < inst->uwbListLen)
 			{
-				//we already have this uwb in the list and it has not timed out
-				break; 
+				inst->newRangeUWBIndex--;
 			}
-        }
-    }
+		}
 
-    return 0;
-}
 
-// -------------------------------------------------------------------------------------------------------------------
-//
-// function to check if a UWB is already in our list and not in a timeout status
-//
-// -------------------------------------------------------------------------------------------------------------------
-// return index if UWB in list and not timed out
-// return 255 if UWB not in list or is but has timed out
-int instcheckactiveuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
-{
-    uint8 i;
-    uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+		for(int i=uwb_index; i<inst->uwbListLen-1; i++)
+		{
+			memcpy(&inst->uwbList[i][0], &inst->uwbList[i+1][0], inst->addrByteSize);
+			inst->tof[i] = inst->tof[i+1];
+			inst->idistance[i] = inst->idistance[i+1];
+			inst->idistanceraw[i] = inst->idistanceraw[i];
+		}
+		uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+		memcpy(&inst->uwbList[inst->uwbListLen-1][0], &blank[0], 8);
+		inst->tof[inst->uwbListLen-1] = 0;
+		inst->idistance[inst->uwbListLen-1] = 0;
+		inst->idistanceraw[inst->uwbListLen-1] = 0;
 
-    //add the new UWB to the list, if not already there and there is space
-    for(i=1; i<UWB_LIST_SIZE; i++)//0 reserved for self, timeout not used for self
-    {
-        if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) == 0)
-        {
-			if(inst->uwbTimeout[i])
-			{
-				//UWB in list, but timed out 
-				break;	
-			}
-			else
-			{
-				return i; //we already have this UWB in the list
-			}
-        }
-        else
-        {
-			if(memcmp(&inst->uwbList[i][0], &blank[0], 8) == 0) //blank entry
-            {
-                break;
-            }
-        }
-    }
+		//update the tdma_handler before reducing the size of the uwbList
+		tdma_handler.remove_uwbinfo(&tdma_handler, uwb_index);
 
-    return 255;
+		inst->uwbListLen--;
+	}
 }
 
+
 // -------------------------------------------------------------------------------------------------------------------
+////
+//// function to add new UWBs to the uwb list or remove timeout status from UWBs already in uwb list
+////
+//// -------------------------------------------------------------------------------------------------------------------
+//// return 1 if UWB added to list or removed from timeout status
+//// return 0 if UWB not added list or removed from timeout status
+//int instaddactivateuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
+//{
+//    uint8 i;
+//    uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
 //
-// function to find the first UWB in our list that is not in a timeout status, starting with the given index
+//    //add the new UWB to the list, if not already there and there is space
+//    for(i=1; i<UWB_LIST_SIZE; i++)//0 reserved for self. timeout status not used for self
+//    {
+//        if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) != 0)
+//        {
+//            if(memcmp(&inst->uwbList[i][0], &blank[0], inst->addrByteSize) == 0) //blank entry
+//            {
+//                memcpy(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) ;
+//                inst->uwbListLen = i + 1 ;
+//				inst->uwbToRangeWith = i;
+//				inst->uwbTimeout[i] = 0;
+//				return 1;
+//            }
+//        }
+//        else
+//        {
+//			if(inst->uwbTimeout[i])
+//			{
+//				//uwb has timed out, wake it up
+//				inst->uwbToRangeWith = i;
+//				inst->uwbTimeout[i] = 0;
+//				return 1;
+//			}
+//			else
+//			{
+//				//we already have this uwb in the list and it has not timed out
+//				break;
+//			}
+//        }
+//    }
 //
-// -------------------------------------------------------------------------------------------------------------------
-// return index for first UWB in list that is not timed out
-// return 255 if all UWBs in list (from the starting index) are timed out
-int instfindfirstactiveuwbinlist(instance_data_t *inst, uint8 startindex)
-{
-    uint8 i;
-    
-    for(i=startindex; i<inst->uwbListLen; i++)
-    {
-		if(!inst->uwbTimeout[i])
-		{
-			return i;
-		}
-    }
+//    return 0;
+//}
 
-    return 255;
-}
+//// -------------------------------------------------------------------------------------------------------------------
+////
+//// function to check if a UWB is already in our list and not in a timeout status
+////
+//// -------------------------------------------------------------------------------------------------------------------
+//// return index if UWB in list and not timed out
+//// return 255 if UWB not in list or is but has timed out
+//int instcheckactiveuwbinlist(instance_data_t *inst, uint8 *uwbAddr)
+//{
+//    uint8 i;
+//    uint8 blank[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+//
+//    //add the new UWB to the list, if not already there and there is space
+//    for(i=1; i<UWB_LIST_SIZE; i++)//0 reserved for self, timeout not used for self
+//    {
+//        if(memcmp(&inst->uwbList[i][0], &uwbAddr[0], inst->addrByteSize) == 0)
+//        {
+//			if(inst->uwbTimeout[i])
+//			{
+//				//UWB in list, but timed out
+//				break;
+//			}
+//			else
+//			{
+//				return i; //we already have this UWB in the list
+//			}
+//        }
+//        else
+//        {
+//			if(memcmp(&inst->uwbList[i][0], &blank[0], 8) == 0) //blank entry
+//            {
+//                break;
+//            }
+//        }
+//    }
+//
+//    return 255;
+//}
+
+//// -------------------------------------------------------------------------------------------------------------------
+////
+//// function to find the first UWB in our list that is not in a timeout status, starting with the given index
+////
+//// -------------------------------------------------------------------------------------------------------------------
+//// return index for first UWB in list that is not timed out
+//// return 255 if all UWBs in list (from the starting index) are timed out
+//int instfindfirstactiveuwbinlist(instance_data_t *inst, uint8 startindex)
+//{
+//    uint8 i;
+//
+//    for(i=startindex; i<inst->uwbListLen; i++)
+//    {
+//		if(!inst->uwbTimeout[i])
+//		{
+//			return i;
+//		}
+//    }
+//
+//    return 255;
+//}
 
 
 
-// -------------------------------------------------------------------------------------------------------------------
+//// -------------------------------------------------------------------------------------------------------------------
+////
+//// function to find the number of UWBs in our list that are not in a timeout status
+////
+//// -------------------------------------------------------------------------------------------------------------------
+//int instfindnumactiveuwbinlist(instance_data_t *inst)
+//{
+//    uint8 num = 0;
 //
-// function to find the number of UWBs in our list that are not in a timeout status
+//    for(int i=1; i<inst->uwbListLen; i++) //0 reserved for self, timeout status not applicable
+//    {
+//		if(!inst->uwbTimeout[i])
+//		{
+//			num++;
+//		}
+//    }
 //
-// -------------------------------------------------------------------------------------------------------------------
-int instfindnumactiveuwbinlist(instance_data_t *inst)
-{
-    uint8 num = 0;
-    
-    for(int i=1; i<inst->uwbListLen; i++) //0 reserved for self, timeout status not applicable
-    {
-		if(!inst->uwbTimeout[i])
-		{
-			num++;
-		}
-    }
-
-    return num;
-}
+//    return num;
+//}
 
 // -------------------------------------------------------------------------------------------------------------------
 //
 // function to find the number of neighbor UWBs in our list that are not in a timeout status
 //
 // -------------------------------------------------------------------------------------------------------------------
-int instfindnumactiveneighbors(instance_data_t *inst)
+int instfindnumneighbors(instance_data_t *inst)
 {
     uint8 num = 0;
 
@@ -465,7 +518,7 @@ int instfindnumactiveneighbors(instance_data_t *inst)
 // function to find the number of hidden neighbor UWBs in our list that are not in a timeout status
 //
 // -------------------------------------------------------------------------------------------------------------------
-int instfindnumactivehidden(instance_data_t *inst)
+int instfindnumhidden(instance_data_t *inst)
 {
     uint8 num = 0;
 
@@ -474,7 +527,6 @@ int instfindnumactivehidden(instance_data_t *inst)
 //		if(inst->uwbListType[i] == UWB_LIST_HIDDEN)
 		if(tdma_handler.uwbListTDMAInfo[i].connectionType == UWB_LIST_HIDDEN)
 		{
-
 			num++;
 		}
     }
@@ -499,15 +551,7 @@ void instclearuwbList(void)
 	
 	for(int i=0; i<UWB_LIST_SIZE; i++)
 	{
-		instance_data[instance].lastCommTimeStamp[i] = 0;
-//		instance_data[instance].lastRangeTimeStamp[i] = 0;
-		tdma_handler.uwbListTDMAInfo[i].lastRange = 0;
-		instance_data[instance].uwbTimeout[i] = 0;
-//		instance_data[instance].time_till_next_reported[i] = 0;
-		
 		memcpy(&instance_data[instance].uwbList[i][0], &blank[0], 8);
-//		instance_data[instance].uwbListType[i] = UWB_LIST_INACTIVE;
-		tdma_handler.uwbListTDMAInfo[i].connectionType = UWB_LIST_INACTIVE;
 	}
 }
 
@@ -579,16 +623,16 @@ void instanceclearcounts(void)
 
     instance_data[instance].frameSN = 0;
 
-    instance_data[instance].tofCount = 0 ;
-    instance_data[instance].tofIndex = 0 ;
+//    instance_data[instance].tofCount = 0 ;
+//    instance_data[instance].tofIndex = 0 ;
 
     instance_data[instance].txmsgcount = 0;
     instance_data[instance].rxmsgcount = 0;
     instance_data[instance].lateTX = 0;
     instance_data[instance].lateRX = 0;
 
-    instance_data[instance].longTermRangeSum  = 0;
-    instance_data[instance].longTermRangeCount  = 0;
+//    instance_data[instance].longTermRangeSum  = 0;
+//    instance_data[instance].longTermRangeCount  = 0;
 
     instclearuwbList();
 
@@ -610,8 +654,8 @@ int instance_init(void)
     instance_data[instance].ranging = 0;
     instance_data[instance].goToSleep = 0;
 
-    instance_data[instance].tofIndex = 0;
-    instance_data[instance].tofCount = 0;
+//    instance_data[instance].tofIndex = 0;
+//    instance_data[instance].tofCount = 0;
     for(uint8 i=0; i<UWB_LIST_SIZE; i++)
 	{
 		instance_data[instance].tof[i] = 0;
@@ -1322,6 +1366,16 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 		uwb_index = instgetuwblistindex(&instance_data[instance], &blink_address[0], instance_data[instance].addrByteSize);
 //		instance_data[instance].uwbListType[uwb_index] = UWB_LIST_NEIGHBOR;
 
+		if(uwb_index > 0 && uwb_index < UWB_LIST_SIZE)
+		{
+	//		instance_data[instance].uwbListType[uwb_index] = UWB_LIST_NEIGHBOR;
+			tdma_handler.uwbListTDMAInfo[uwb_index].connectionType = UWB_LIST_NEIGHBOR;
+	//		instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+			tdma_handler.uwbListTDMAInfo[uwb_index].lastCommNeighbor = time_now;
+//			instance_data[instance].uwbTimeout[uwb_index] = 0;
+		}
+
+
 
 //		uint8 debug_msg[100];
 ////		int n = sprintf((char*)&debug_msg[0], "RX CALLBACK RECEIVED: BLINK uwb_index: %d, uwbToRangeWith: %d ", uwb_index, instance_data[instance].uwbToRangeWith);
@@ -1334,12 +1388,15 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 		//must be a neighbor
 		uwb_index = instgetuwblistindex(&instance_data[instance], &dw_event.msgu.frame[srcAddr_index], instance_data[instance].addrByteSize);
 
-		//TODO maybe do this somewhere else...
-//		instance_data[instance].uwbListType[uwb_index] = UWB_LIST_NEIGHBOR;
-		tdma_handler.uwbListTDMAInfo[uwb_index].connectionType = UWB_LIST_NEIGHBOR;
-		instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-		instance_data[instance].uwbTimeout[uwb_index] = 0;
-
+		if(uwb_index > 0 && uwb_index < UWB_LIST_SIZE)
+		{
+			//TODO maybe do this somewhere else...
+	//		instance_data[instance].uwbListType[uwb_index] = UWB_LIST_NEIGHBOR;
+			tdma_handler.uwbListTDMAInfo[uwb_index].connectionType = UWB_LIST_NEIGHBOR;
+	//		instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+			tdma_handler.uwbListTDMAInfo[uwb_index].lastCommNeighbor = time_now;
+//			instance_data[instance].uwbTimeout[uwb_index] = 0;
+		}
 
 
 //		uint8 debug_msg[100];
@@ -1349,25 +1406,26 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 ////		uint16 *addr_ptr = &instance_data[instance].uwbShortAdd;
 ////		int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s,%u,xxxx,%p", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), my_addr, (void *) addr_ptr);
 ////		int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s,%u,xxxx", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), my_addr);
-//		int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s,%04X,xxxx", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), my_addr);
+//		int n = sprintf((char*)&debug_msg[0], "RX CB RX: DWT_SIG_RX_OKAY-%s,%04X,uwb_index: %04d, uwb_trw: %04d", get_msg_fcode_string(dw_event.msgu.frame[fcode_index]), my_addr,uwb_index, instance_data[instance].uwbToRangeWith);
 ////		int n = sprintf((char*)&debug_msg[0], "%u,%p", my_addr, (void *)addr_ptr);
 //		 send_usbmessage(&debug_msg[0], n);
 //		 usb_run();
 
 	}
 
-	if(uwb_index > UWB_LIST_SIZE - 1)
-	{
-		instance_data_t *inst = &instance_data[instance];
+//	if(uwb_index > UWB_LIST_SIZE - 1 || uwb_index <= 0)
+//	{
+//		instance_data_t *inst = &instance_data[instance];
 
-		uint8 debug_msg[100];
-		uint16 my_addr = instance_data[instance].uwbShortAdd;
-		int n = sprintf((char*)&debug_msg[0], "uwb_index:%u of %u",uwb_index, instance_data[instance].uwbListLen);
-		 send_usbmessage(&debug_msg[0], n);
-		 usb_run();
+//		uint8 debug_msg[100];
+//		uint16 my_addr = instance_data[instance].uwbShortAdd;
+//		int n = sprintf((char*)&debug_msg[0], "uwb_index:%u of %u",uwb_index, instance_data[instance].uwbListLen);
+//		 send_usbmessage(&debug_msg[0], n);
+//		 usb_run();
 
 		 //TODO solve this problem. other, (incorrect) addresses are making it into our UWB list for some reason...
-	}
+		 //think i took care of it, it was incorrect indexing in create/read INF messages
+//	}
 
 	bool accept_inf = FALSE;
 	//check if the incoming message indicates that we should range with the source UWB or just accept and process the message
@@ -1397,7 +1455,7 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 						if(tdma_handler.discovery_mode == WAIT_RNG_INIT)
 						{
 							//only accept RNG_INIT if no other active neighbors exist (and we are waiting for RNG_INIT)
-							int num_active = instfindnumactiveneighbors(&instance_data[instance]);
+							int num_active = instfindnumneighbors(&instance_data[instance]);
 							if(num_active <= 1)
 							{
 								instance_data[instance].uwbToRangeWith = uwb_index;
@@ -1432,7 +1490,7 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 				{
 					if(tdma_handler.discovery_mode == WAIT_INF_REG)
 					{
-						int num_neighbors = instfindnumactiveneighbors(&instance_data[instance]);
+						int num_neighbors = instfindnumneighbors(&instance_data[instance]);
 //						uint8 debug_msg[100];
 //						int n = sprintf((char*)&debug_msg[0], "num_neighbors %d", num_neighbors);
 //						send_usbmessage(&debug_msg[0], n);
@@ -1466,8 +1524,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 //			 int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: RNG_REPORT <- uwb_index %d", uwb_index);
 //			 send_usbmessage(&debug_msg[0], n);
 //			 usb_run();
-			instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-			instance_data[instance].uwbTimeout[uwb_index] = 0;
+//			instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+//			instance_data[instance].uwbTimeout[uwb_index] = 0;
 			place_event = 1;
 		}
 	}
@@ -1485,8 +1543,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 		instance_data[instance].timeofRxCallback_dwtime = (uint64)sys_time_arr[0] + ((uint64)sys_time_arr[1] << 8) + ((uint64)sys_time_arr[2] << 16) + ((uint64)sys_time_arr[3] << 24) + ((uint64)sys_time_arr[4] << 32);
 
 
-		instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-		instance_data[instance].uwbTimeout[uwb_index] = 0;
+//		instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+//		instance_data[instance].uwbTimeout[uwb_index] = 0;
 		place_event = 1;
 
 
@@ -1515,8 +1573,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 //				send_usbmessage(&debug_msg[0], n);
 //				usb_run();
 
-				instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-				instance_data[instance].uwbTimeout[uwb_index] = 0;
+//				instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+//				instance_data[instance].uwbTimeout[uwb_index] = 0;
 
 				uint16 frameLength = 0;
 
@@ -1643,8 +1701,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 			else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_ANCH_RESP)
 			{
 //				uint32 response_time = portGetTickCnt() - instance_data[instance].range_start;
-				instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-				instance_data[instance].uwbTimeout[uwb_index] = 0;
+//				instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+//				instance_data[instance].uwbTimeout[uwb_index] = 0;
 //				uint8 debug_msg[100];
 //				 int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: ANCH_RESP <- uwb_index %d", uwb_index);
 //				 send_usbmessage(&debug_msg[0], n);
@@ -1652,8 +1710,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 			}
 			else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_RNG_INIT)
 			{
-				instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-				instance_data[instance].uwbTimeout[uwb_index] = 0;
+//				instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+//				instance_data[instance].uwbTimeout[uwb_index] = 0;
 //				uint8 debug_msg[100];
 //				 int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: RNG_INIT <- uwb_index %d", uwb_index);
 //				 send_usbmessage(&debug_msg[0], n);
@@ -1661,8 +1719,8 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 			}
 			else if(dw_event.msgu.frame[fcode_index] == RTLS_DEMO_MSG_TAG_FINAL)
 			{
-				instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
-				instance_data[instance].uwbTimeout[uwb_index] = 0;
+//				instance_data[instance].lastCommTimeStamp[uwb_index] = time_now;
+//				instance_data[instance].uwbTimeout[uwb_index] = 0;
 //				uint8 debug_msg[100];
 //				 int n = sprintf((char*)&debug_msg[0], "RX CALLBACK ACCEPTED: TAG_FINAL <- uwb_index %d", uwb_index);
 //				 send_usbmessage(&debug_msg[0], n);
@@ -1709,6 +1767,11 @@ void instance_rxgoodcallback(const dwt_cb_data_t *rxd)
 
 	if(place_event)
 	{
+//		uint8 debug_msg[100];
+//		int n = sprintf((char*)&debug_msg[0], "event placed");
+//		send_usbmessage(&debug_msg[0], n);
+//		usb_run();
+
 		instance_putevent(dw_event);
 
 #if (DEEP_SLEEP == 1)
@@ -1839,78 +1902,94 @@ int instance_run(void)
 		message = 0;
 	}
 
-	//check if lastCommTimeStamp has expired for any of the uwbs in our list
-	for(int i=1; i < instance_data[instance].uwbListLen; i++)//0 reserved for self, timeout not applicable
+	uint32 time_now = portGetTickCnt();
+	//check timeouts. handle case where we timeout and no longer have any neighbors
+	if(tdma_handler.check_timeouts(&tdma_handler, time_now))
 	{
-		//TODO could also have a timer to put into DISCOVERY if we haven't spoken to anyone in a while
-		uint32 time_now = portGetTickCnt();
-
-		uint32 delta_t = get_dt32(instance_data[instance].lastCommTimeStamp[i], time_now);
-		if(delta_t > UWB_COMM_TIMEOUT) //TODO handle number wrapping
-		{
-//			if(instance_data[instance].uwbListType[i] == UWB_LIST_NEIGHBOR) //what about hidden?
-			if(tdma_handler.uwbListTDMAInfo[i].connectionType == UWB_LIST_NEIGHBOR) //what about hidden?
-			{
-//				instance_data[instance].uwbListType[i] = UWB_LIST_INACTIVE; //TODO release TDMA slots as well
-				tdma_handler.uwbListTDMAInfo[i].connectionType = UWB_LIST_INACTIVE; //TODO release TDMA slots as well
-				instance_data[instance].uwbTimeout[i] = 1;
-
-				//NEW
-				//if no more active neighbors exist, transition to DISCOVERY
-				uint8 numNeighbors = instfindnumactiveneighbors(&instance_data[instance]);
-				if(numNeighbors <= 0)
-				{
-					tdma_handler.tdma_free_all_slots(&tdma_handler);
-
+		tdma_handler.tdma_free_all_slots(&tdma_handler);
 
+		instance_data[instance].mode = DISCOVERY;						//TODO clear the TDMA slot information!
+		tdma_handler.discoveryStartTime = portGetTickCnt();
+		tdma_handler.enter_discovery_mode(&tdma_handler);
+		inst_processtxrxtimeout(&instance_data[instance]);
 
-//					uint8 debug_msg[100];
-//					int n = sprintf((char *)&debug_msg, "inst_processtxrxtimeout(inst) after free_all_slots");
-//					send_usbmessage(&debug_msg[0], n);
-//					usb_run();
-
-					instance_data[instance].mode = DISCOVERY;						//TODO clear the TDMA slot information!
-					tdma_handler.discoveryStartTime = portGetTickCnt();
-					tdma_handler.enter_discovery_mode(&tdma_handler);
-					inst_processtxrxtimeout(&instance_data[instance]);
-
-					instance_data[instance].canPrintInfo = 0;
-					instance_data[instance].ranging = 0;
-				}
-				else
-				{
-					//TODO check if able to reconfigure tdma assignements. shrink frame size, reassign emptied slots, etc
-					tdma_handler.uwblist_free_slots(&tdma_handler, i);
-				}
-
-
-//				 uint8 debug_msg[100];
-//				 int n = sprintf((char*)&debug_msg[0], "TIMEOUT: uwb %i", i);
-//				 send_usbmessage(&debug_msg[0], n);
-//				 usb_run();
-			}
-
-			if(instance_data[instance].uwbToRangeWith == i)
-			{	
-				instance_data[instance].uwbToRangeWith = 255;	
-				//NOTE this might need to be changed for TAG operations
-				if(instance_data[instance].mode == ANCHOR || instance_data[instance].mode == DISCOVERY) //TODO maybe send to TA_MODE_SELECT?
-				{
-					instance_data[instance].testAppState = TA_RXE_WAIT;
-				}
-				else
-				{
-//					 uint8 debug_msg[100];
-//					 int n = sprintf((char*)&debug_msg[0], "instance_run(void) TIMEOUT");
-//					 send_usbmessage(&debug_msg[0], n);
-//					 usb_run();
-					instance_data[instance].testAppState = TA_TX_SELECT;
-				}
-			}
-		}
-		
+		instance_data[instance].canPrintInfo = 0;
+		instance_data[instance].ranging = 0;
 	}
 
+//	//check if lastCommTimeStamp has expired for any of the uwbs in our list
+//	for(int i=1; i < instance_data[instance].uwbListLen; i++)//0 reserved for self, timeout not applicable
+//	{
+//		//TODO could also have a timer to put into DISCOVERY if we haven't spoken to anyone in a while
+//		uint32 time_now = portGetTickCnt();
+//
+//
+//		uint32 delta_t = get_dt32(instance_data[instance].lastCommTimeStamp[i], time_now);
+//		if(delta_t > UWB_COMM_TIMEOUT) //TODO handle number wrapping
+//		{
+////			if(instance_data[instance].uwbListType[i] == UWB_LIST_NEIGHBOR) //what about hidden?
+//			if(tdma_handler.uwbListTDMAInfo[i].connectionType == UWB_LIST_NEIGHBOR) //what about hidden?
+//			{
+////				instance_data[instance].uwbListType[i] = UWB_LIST_INACTIVE; //TODO release TDMA slots as well
+//				tdma_handler.uwbListTDMAInfo[i].connectionType = UWB_LIST_INACTIVE; //TODO release TDMA slots as well
+//				instance_data[instance].uwbTimeout[i] = 1;
+//
+//				//NEW
+//				//if no more active neighbors exist, transition to DISCOVERY
+//				uint8 numNeighbors = instfindnumactiveneighbors(&instance_data[instance]);
+//				if(numNeighbors <= 0)
+//				{
+//					tdma_handler.tdma_free_all_slots(&tdma_handler);
+//
+//
+//
+////					uint8 debug_msg[100];
+////					int n = sprintf((char *)&debug_msg, "inst_processtxrxtimeout(inst) after free_all_slots");
+////					send_usbmessage(&debug_msg[0], n);
+////					usb_run();
+//
+//					instance_data[instance].mode = DISCOVERY;						//TODO clear the TDMA slot information!
+//					tdma_handler.discoveryStartTime = portGetTickCnt();
+//					tdma_handler.enter_discovery_mode(&tdma_handler);
+//					inst_processtxrxtimeout(&instance_data[instance]);
+//
+//					instance_data[instance].canPrintInfo = 0;
+//					instance_data[instance].ranging = 0;
+//				}
+//				else
+//				{
+//					//TODO check if able to reconfigure tdma assignements. shrink frame size, reassign emptied slots, etc
+//					tdma_handler.uwblist_free_slots(&tdma_handler, i);
+//				}
+//
+//
+////				 uint8 debug_msg[100];
+////				 int n = sprintf((char*)&debug_msg[0], "TIMEOUT: uwb %i", i);
+////				 send_usbmessage(&debug_msg[0], n);
+////				 usb_run();
+//			}
+//
+//			if(instance_data[instance].uwbToRangeWith == i)
+//			{
+//				instance_data[instance].uwbToRangeWith = 255;
+//				//NOTE this might need to be changed for TAG operations
+//				if(instance_data[instance].mode == ANCHOR || instance_data[instance].mode == DISCOVERY) //TODO maybe send to TA_MODE_SELECT?
+//				{
+//					instance_data[instance].testAppState = TA_RXE_WAIT;
+//				}
+//				else
+//				{
+////					 uint8 debug_msg[100];
+////					 int n = sprintf((char*)&debug_msg[0], "instance_run(void) TIMEOUT");
+////					 send_usbmessage(&debug_msg[0], n);
+////					 usb_run();
+//					instance_data[instance].testAppState = TA_TX_SELECT;
+//				}
+//			}
+//		}
+//
+//	}
+
     if(done == INST_DONE_WAIT_FOR_NEXT_EVENT_TO) //we are in RX and need to timeout (Tag needs to send another poll if no Rx frame)
     {
     	if(instance_data[instance].mode == DISCOVERY)
diff --git a/src/application/tdma_handler.c b/src/application/tdma_handler.c
index 735feb075a52dbf7683a2291db9432f8caf5e6c2..e5c111811aceefb3087955e36e2a71aba3aee9c0 100644
--- a/src/application/tdma_handler.c
+++ b/src/application/tdma_handler.c
@@ -22,7 +22,9 @@ static bool slot_transition(struct TDMAHandler *this)
 		if(timeSinceSlotStart64 >= this->slotDuration_us) //TODO should i check frame separate from slot?
 		{
 			transition = TRUE;
-			this->pollSentThisSlot = FALSE;
+			this->firstPollSentThisSlot = FALSE;
+			this->firstPollComplete = FALSE;
+			this->secondPollSentThisSlot = FALSE;
 			this->infSentThisSlot = FALSE;
 
 			//we have transitioned into the next slot.
@@ -30,6 +32,7 @@ static bool slot_transition(struct TDMAHandler *this)
 
 //			uint64 timeSinceFrameStart64 = get_dt64(this->uwbFrameStartTimes64[0], time_now_us);
 			uint64 timeSinceFrameStart64 = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
+			//TODO this->uwbListTDMAInfo[0].frameStartTime is sometimes getting set to a time in the future... figure out!
 
 			uint64 frameDuration64 = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
 			if(timeSinceFrameStart64 >= frameDuration64)
@@ -113,7 +116,9 @@ static bool slot_transition(struct TDMAHandler *this)
 	else if(inst->mode == DISCOVERY)
 	{
 		this->infSentThisSlot = FALSE;
-		this->pollSentThisSlot = FALSE;
+		this->firstPollSentThisSlot = FALSE;
+		this->firstPollComplete = FALSE;
+		this->secondPollSentThisSlot = FALSE;
 	}
 
 	return transition;
@@ -125,6 +130,19 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
 {
 	instance_data_t *inst = instance_get_local_structure_ptr(0);
 
+//	uint8 debug_msg[100];
+//	int n = sprintf((char*)&debug_msg[0], "frame sync");
+//	send_usbmessage(&debug_msg[0], n);
+//	usb_run();
+
+	//do not process erroneous tsfs
+	//can happen if frame start time is shifted ahead of time_now_us in transmitting UWB
+	if(timeSinceFrameStart_us > 1000000000 || timeSinceFrameStart_us == 0)
+	{
+		return;
+	}
+
+
 	uint8 sys_time_arr[5] = {0, 0, 0, 0, 0};
 	dwt_readsystime(sys_time_arr);
 	uint64 dwt_time_now = 0;
@@ -159,17 +177,19 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
 //	this->uwbFrameStartTimes64[srcIndex] = timestamp_subtract64(time_now_us, hisTimeSinceFrameStart_us); //TODO consider applying the diff!
 	this->uwbListTDMAInfo[srcIndex].frameStartTime = timestamp_subtract64(time_now_us, hisTimeSinceFrameStart_us); //TODO consider applying the diff!
 
-	if(mode == FS_ADOPT) //TODO this might not be right! incoming framelength not always the same as ours!
+	if(mode == FS_COLLECT)
 	{
-//		this->uwbFrameStartTimes64[0] = this->uwbFrameStartTimes64[srcIndex];
-		this->uwbListTDMAInfo[0].frameStartTime = this->uwbListTDMAInfo[srcIndex].frameStartTime;
-		uint8 slot = hisTimeSinceFrameStart_us/this->slotDuration_us; //integer division rounded down
-//		this->lastSlotStartTime64 = this->uwbFrameStartTimes64[0] + (uint64)((this->slotDuration_us)*slot);
-		this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime + (uint64)((this->slotDuration_us)*slot);
+		return;
 	}
-	else if(mode == FS_COLLECT)
+	else if(mode == FS_ADOPT) //TODO this might not be right! incoming framelength not always the same as ours!
 	{
-		return;
+//		this->uwbFrameStartTimes64[0] = this->uwbFrameStartTimes64[srcIndex];
+//		this->uwbListTDMAInfo[0].frameStartTime = this->uwbListTDMAInfo[srcIndex].frameStartTime;
+//		uint8 slot = hisTimeSinceFrameStart_us/this->slotDuration_us; //integer division rounded down
+//		this->lastSlotStartTime64 = this->uwbFrameStartTimes64[0] + (uint64)((this->slotDuration_us)*slot);
+//		this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime + (uint64)((this->slotDuration_us)*slot);
+
+		this->uwbListTDMAInfo[0].frameStartTime = this->uwbListTDMAInfo[srcIndex].frameStartTime;//NOTE gets processed further at end of function
 	}
 	else// if(mode == FS_AVERAGE || mode == FS_EVAL) //TODO consider using a weighted average. perhaps use threshholds to determine if instead we should just use the ADOPT logic
 	{
@@ -177,69 +197,7 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
 //		uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbFrameStartTimes64[0], time_now_us);
 		uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
 
-		////OLD WAY
-
-//		if(this->uwbListTDMAInfo[0].framelength <= framelength)
-//		{
-//			uint64 hisTimeSinceFrameStartMod_us = hisTimeSinceFrameStart_us%myFramelengthDuration_us;
-//
-//			if(myTimeSinceFrameStart_us > hisTimeSinceFrameStartMod_us)
-//			{
-//				uint64 diff_us = myTimeSinceFrameStart_us - hisTimeSinceFrameStartMod_us;
-//
-//				uint8 debug_msg[100];
-//				 int n = sprintf((char*)&debug_msg[0], "add %llu", diff_us);
-//				 send_usbmessage(&debug_msg[0], n);
-//				 usb_run();
-//
-//				this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], diff_us/2);
-//				this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/2);
-//			}
-//			else
-//			{
-//				uint64 diff_us = hisTimeSinceFrameStartMod_us - myTimeSinceFrameStart_us;
-//
-//				uint8 debug_msg[100];
-//				 int n = sprintf((char*)&debug_msg[0], "subtract %llu", diff_us);
-//				 send_usbmessage(&debug_msg[0], n);
-//				 usb_run();
-//
-//				this->uwbFrameStartTimes64[0] = timestamp_subtract64(this->uwbFrameStartTimes64[0], diff_us/2);
-//				this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/2);
-//			}
-//		}
-//		else
-//		{
-//			uint64 hisFramelengthDuration_us = framelength*this->slotDuration_us;
-//			uint64 myTimeSinceFrameStartMod_us = myTimeSinceFrameStart_us%hisFramelengthDuration_us;
-//
-//			if(hisTimeSinceFrameStart_us > myTimeSinceFrameStartMod_us)
-//			{
-//				uint64 diff_us = hisTimeSinceFrameStart_us - myTimeSinceFrameStartMod_us;
-//
-//				uint8 debug_msg[100];
-//				 int n = sprintf((char*)&debug_msg[0], "subtract %llu", diff_us);
-//				 send_usbmessage(&debug_msg[0], n);
-//				 usb_run();
-//
-//				this->uwbFrameStartTimes64[0] = timestamp_subtract64(this->uwbFrameStartTimes64[0], diff_us/2);
-//				this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/2);
-//			}
-//			else
-//			{
-//				uint64 diff_us = myTimeSinceFrameStartMod_us - hisTimeSinceFrameStart_us;
-//
-//				uint8 debug_msg[100];
-//				 int n = sprintf((char*)&debug_msg[0], "add %llu", diff_us);
-//				 send_usbmessage(&debug_msg[0], n);
-//				 usb_run();
-//
-//				this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], diff_us/2);
-//				this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/2);
-//			}
-//		}
-
-		//NEW WAY SELF VS INCOMING
+		//SELF VS INCOMING
 
 		uint8 min_fl = this->uwbListTDMAInfo[0].framelength;
 		if(framelength < min_fl)
@@ -310,7 +268,7 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
 
 //			this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], diff_us/div);
 			this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, diff_us/div);
-			this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/div);
+//			this->lastSlotStartTime64 = timestamp_add64(this->lastSlotStartTime64, diff_us/div);
 		}
 		else
 		{
@@ -320,11 +278,27 @@ static void frame_sync(struct TDMAHandler *this, event_data_t *dw_event, uint8 f
 //			 usb_run();
 //			this->uwbFrameStartTimes64[0] = timestamp_subtract64(this->uwbFrameStartTimes64[0], diff_us/div);
 			this->uwbListTDMAInfo[0].frameStartTime = timestamp_subtract64(this->uwbListTDMAInfo[0].frameStartTime, diff_us/div);
-			this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/div);
+//			this->lastSlotStartTime64 = timestamp_subtract64(this->lastSlotStartTime64, diff_us/div);
 		}
+	}
 
+	uint64 myFrameDuration = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
+	uint64 myTimeSinceFrameStart_us = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us); //TODO what if added to be past time_now???
+	if(myTimeSinceFrameStart_us > 100000000)
+	{
+		//if this is a very large number, then the frame start time was likely moved head of time_now_us.
+		this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime - (uint64)(this->slotDuration_us);
+	}
+	else
+	{
+		while(myTimeSinceFrameStart_us >= myFrameDuration)
+		{
+			this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, myFrameDuration);
+			myTimeSinceFrameStart_us -= myFrameDuration;
+		}
 
-
+		uint8 slot = hisTimeSinceFrameStart_us/this->slotDuration_us; //integer division rounded down
+		this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime + (uint64)(this->slotDuration_us*slot);
 	}
 }
 
@@ -548,7 +522,7 @@ static bool tx_sync_msg(struct TDMAHandler *this)
 //}
 
 
-static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
+static bool tx_select(struct TDMAHandler *this)
 {
 //	uint8 debug_msg[100];
 //	int n = sprintf((char *)&debug_msg, "tx_select,");
@@ -559,12 +533,6 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
 
 	uint32 time_now = portGetTickCnt(); //TODO come back and perhaps use only one or the other
 	uint32 time_now_us = portGetTickCntMicro(); //TODO see time_now
-//	uint32 offset = 0;
-//	//force number wrapping in case the timestamp is getting close to wrapping
-//	if(time_now > 4000000000){
-//		offset = 1000000000;
-//	}
-//	uint32 time_now_offset = time_now + offset;
 
 	//DISCOVERY pauses for BLINK_DELAY <-added
 //	if(this->waitForInf == TRUE || this->waitForRngInit == TRUE)
@@ -623,8 +591,7 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
 				return TRUE;
 			}
 		}
-//		else if(this->discovery_mode == WAIT_SEND_SUG)//TODO left off here
-		else if(this->discovery_mode == SEND_SUG)//TODO left off here
+		else if(this->discovery_mode == SEND_SUG)
 		{
 //			uint8 debug_msg[100];
 //			int n = sprintf((char *)&debug_msg, "discovery send sug");
@@ -656,47 +623,70 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
 	}
 	else if(inst->mode == TAG)
 	{
-
-//		uint8 debug_msg[100];
-//		int n = sprintf((char *)&debug_msg, "tag");
-//		send_usbmessage(&debug_msg[0], n);
-//		usb_run();
-
 		//get time since slot start and make sure that's greater than delay
 		uint64 timeSinceSlotStart = get_dt64(this->lastSlotStartTime64, time_now_us);
 
-
 		//TAG pauses for INF_POLL_DELAY <-added at beginning of slot
-//		if(this->poll_delay(this, time_now_offset, offset) == TRUE)
 		if(timeSinceSlotStart <= this->slotStartDelay_us)
 		{
 			uwb_index = -1;
 		}
 		else
 		{
-			//check which neighbor UWB hasn't been ranged with
+			if(this->firstPollSentThisSlot == TRUE && this->secondPollSentThisSlot == FALSE && this->firstPollComplete == FALSE)
+			{
+				int test = 1;
+				test++;
+			}
+
+			//check which neighbor UWB hasn't been ranged with for the longest amount of time
 			uint32 timeSinceOldestRange = 0;
+			uint32 timeSinceSecondOldestRange = 0;
+			uint8 second_oldest_uwb_index = 255;
 			for(int i = 1; i < inst->uwbListLen; i++)//0 reserved for self
 			{
-//				if(inst->uwbListType[i] == UWB_LIST_NEIGHBOR)
 				if(this->uwbListTDMAInfo[i].connectionType == UWB_LIST_NEIGHBOR)
 				{
-//					uint32 timeSinceRange = get_dt32(inst->lastRangeTimeStamp[i], time_now);
 					uint32 timeSinceRange = get_dt32(this->uwbListTDMAInfo[i].lastRange, time_now);
-					if(timeSinceRange > timeSinceOldestRange)
+
+					if(timeSinceOldestRange == 0)
 					{
 						timeSinceOldestRange = timeSinceRange;
+						timeSinceSecondOldestRange = timeSinceRange;
+
 						uwb_index = i;
+						second_oldest_uwb_index = i;
+					}
+					else if(timeSinceRange > timeSinceOldestRange)
+					{
+						timeSinceSecondOldestRange = timeSinceOldestRange;
+						second_oldest_uwb_index = uwb_index;
+						timeSinceOldestRange = timeSinceRange;
+						uwb_index = i;
+					}
+					else if(timeSinceRange > timeSinceSecondOldestRange)
+					{
+						timeSinceSecondOldestRange = timeSinceRange;
+						second_oldest_uwb_index = i;
+					}
+					else if(timeSinceOldestRange == timeSinceSecondOldestRange)
+					{
+						timeSinceSecondOldestRange = timeSinceRange;
+						second_oldest_uwb_index = i;
 					}
 				}
 			}
 
-			if(uwb_index == 255 && inst->uwbListLen > 0) //TODO what is the purpose of this exactly?
+			if(this->firstPollSentThisSlot == TRUE && this->secondPollSentThisSlot == FALSE && this->firstPollComplete == FALSE)
+			{
+				uwb_index = second_oldest_uwb_index;
+			}
+
+			if(uwb_index == 255 && inst->uwbListLen > 1)
 			{
 				uwb_index = 1;
 			}
 		}
-
 	}
 	else
 	{
@@ -711,40 +701,33 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
 //	usb_run();
 
 
-	if(uwb_index < 1){
+	if(uwb_index < 1) //set to -1 when waiting for the slotStartDelay to pass
+	{
 		//do nothing
-		//TODO would be best to sleep until next possible poll and then select again...
-//		done = INST_DONE_WAIT_FOR_NEXT_EVENT; //TODO make sure that this is the right thing to do.
-
-//		debug_msg[100];
-//		n = sprintf((char *)&debug_msg, "1");
-//		send_usbmessage(&debug_msg[0], n);
-//		usb_run();
-
 		return FALSE;
-
-	}else if(uwb_index > 254){
+	}
+	else if(uwb_index > 254)
+	{
 		inst->testAppState = TA_TXBLINK_WAIT_SEND;
 		inst->uwbToRangeWith = (uint8)255;
-
-//		debug_msg[100];
-//		n = sprintf((char *)&debug_msg, "2");
-//		send_usbmessage(&debug_msg[0], n);
-//		usb_run();
-
-	}else{
-
+	}
+	else
+	{
 		uint8 fcode = RTLS_DEMO_MSG_INF_INIT;
 		if(memcmp(&inst->inf_msg.messageData[FCODE], &fcode, sizeof(uint8)) == 0)
 		{
 			this->infSentThisSlot = TRUE;
 			inst->testAppState = TA_TXINF_WAIT_SEND;
 		}
-		else if(this->pollSentThisSlot == FALSE)
+		else if(this->firstPollSentThisSlot == FALSE)
+		{
+			this->firstPollSentThisSlot = TRUE;
+			inst->testAppState = TA_TXPOLL_WAIT_SEND;
+		}
+		else if(this->secondPollSentThisSlot == FALSE && this->firstPollComplete == FALSE)
 		{
-			this->pollSentThisSlot = TRUE;
-			inst->testAppState = TA_TXPOLL_WAIT_SEND;//TODO modify any other related variables with this change if we keep it.
-													//in case the ranging event fails, dont just wait till end of slot, move on toe TA_TXINF_WAIT_SEND
+			this->secondPollSentThisSlot = TRUE;
+			inst->testAppState = TA_TXPOLL_WAIT_SEND; //select new uwb index!
 		}
 		else if(this->infSentThisSlot == FALSE)
 		{
@@ -756,13 +739,6 @@ static bool tx_select(struct TDMAHandler *this) //TODO handle unsuccessful add
 			return TRUE;
 		}
 
-
-//		debug_msg[100];
-//		n = sprintf((char *)&debug_msg, "3");
-//		send_usbmessage(&debug_msg[0], n);
-//		usb_run();
-
-
 		inst->uwbToRangeWith = (uint8)uwb_index;
 	}
 
@@ -806,8 +782,8 @@ static void populate_inf_msg(struct TDMAHandler *this, uint8 inf_msg_type)
 {
 	instance_data_t *inst = instance_get_local_structure_ptr(0);
 
-	int num_neighbors = instfindnumactiveneighbors(inst);
-	int num_hidden = instfindnumactivehidden(inst);
+	int num_neighbors = instfindnumneighbors(inst);
+	int num_hidden = instfindnumhidden(inst);
 //	uint32 time_now = portGetTickCnt();
 
 	//fcode
@@ -927,36 +903,26 @@ static void update_inf_tsfs(struct TDMAHandler *this)
 	instance_data_t *inst = instance_get_local_structure_ptr(0);
 	uint64 time_now_us = portGetTickCntMicro();
 	int msgDataIndex = TDMA_TSFS;
-//	uint64 timeSinceFrameStart64 = get_dt64(this->uwbFrameStartTimes64[0], time_now_us);
 	uint64 timeSinceFrameStart64 = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now_us);
 	uint64 frameDuration = this->slotDuration_us*this->uwbListTDMAInfo[0].framelength;
-	while(timeSinceFrameStart64 > frameDuration)
+
+	//if frameStartTime ahead of time_now_us, just send the erroneously large number over.
+	//the receiver will detect and disregard
+	if(timeSinceFrameStart64 < 10000000)
+	{
+		while(timeSinceFrameStart64 > frameDuration)
+		{
+			this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, frameDuration);
+			timeSinceFrameStart64 -= frameDuration;
+		}
+	}
+	else
 	{
-//		this->uwbFrameStartTimes64[0] = timestamp_add64(this->uwbFrameStartTimes64[0], frameDuration);
-		this->uwbListTDMAInfo[0].frameStartTime = timestamp_add64(this->uwbListTDMAInfo[0].frameStartTime, frameDuration);
-		timeSinceFrameStart64 -= frameDuration;
+		timeSinceFrameStart64 = 0;
 	}
 
 	memcpy(&inst->inf_msg.messageData[msgDataIndex], &timeSinceFrameStart64, 6);
 
-
-	uint64 tsinf = get_dt64(this->lastINFtx, time_now_us);
-	memcpy(&inst->inf_msg.messageData[1], &tsinf, 6);
-	this->lastINFtx = time_now_us;
-
-	//indicate whether the TSFS being transmitted is a rebase.
-//	memcpy(&inst->inf_msg.messageData[TDMA_TSFS_REBASE], &this->rebase_pending, sizeof(bool));
-
-//	uint8 debug_msg[100];
-//	int n = sprintf((char *)&debug_msg, "TSFS %llu", timeSinceFrameStart64	);
-//	send_usbmessage(&debug_msg[0], n);
-//	usb_run();
-
-//	uint8 sys_time_arr[5] = {0, 0, 0, 0, 0};
-//	dwt_readsystime(sys_time_arr);
-//	uint64 dwt_time_now = (uint64)sys_time_arr[0] + ((uint64)sys_time_arr[1] << 8) + ((uint64)sys_time_arr[2] << 16) + ((uint64)sys_time_arr[3] << 24) + ((uint64)sys_time_arr[4] << 32);
-//	uint64 timeSinceFrameStart_dwt = dwt_getdt(this->uwbFrameStartTimes_dwt[0], dwt_time_now);
-//	memcpy(&inst->inf_msg.messageData[100], &timeSinceFrameStart_dwt, 5);
 }
 
 
@@ -980,6 +946,12 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
 {
 	//NOTE: this function does not handle TDMA deconflict
 
+//	uint8 debug_msg[100];
+//	int n = sprintf((char*)&debug_msg[0], "process INF");
+//	 send_usbmessage(&debug_msg[0], n);
+//	 usb_run();
+
+
 	bool tdma_modified = FALSE;
 
 	uint32 time_now = portGetTickCnt();
@@ -1012,7 +984,6 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
 	}
 
 	instance_data_t *inst = instance_get_local_structure_ptr(0);
-//	inst->uwbListType[srcIndex] = UWB_LIST_NEIGHBOR;
 	this->uwbListTDMAInfo[srcIndex].connectionType = UWB_LIST_NEIGHBOR;
 
 	uint8 numNeighbors;
@@ -1044,7 +1015,6 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
 		tdma_modified = TRUE;
 	}
 
-//	msgDataIndex = TDMA_NUMS + 1;
 	//copy slot assignments for source UWB
 	info = &this->uwbListTDMAInfo[srcIndex];
 	if(framelength != info->framelength)
@@ -1115,15 +1085,14 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
 				this->uwbListTDMAInfo[uwb_index].connectionType = UWB_LIST_HIDDEN;
 			}
 
-			inst->lastHiddenTimeStamp[uwb_index] = time_now;
+//			inst->lastHiddenTimeStamp[uwb_index] = time_now;
+			this->uwbListTDMAInfo[uwb_index].lastCommHidden = time_now;
 		}
 
 		info = &this->uwbListTDMAInfo[uwb_index];
 		uwbListInMsg[uwb_index] = TRUE;
 
 
-
-
 		memcpy(&framelength, &messageData[msgDataIndex], sizeof(uint8));
 		msgDataIndex++;
 		memcpy(&numSlots, &messageData[msgDataIndex], sizeof(uint8));
@@ -1192,7 +1161,8 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
 				this->uwbListTDMAInfo[uwb_index].connectionType = UWB_LIST_TWICE_HIDDEN;
 			}
 
-			inst->lastTwiceHiddenTimeStamp[uwb_index] = time_now;
+//			inst->lastTwiceHiddenTimeStamp[uwb_index] = time_now;
+			this->uwbListTDMAInfo[uwb_index].lastCommTwiceHidden = time_now;
 		}
 
 		uwbListInMsg[uwb_index] = TRUE;
@@ -1294,76 +1264,17 @@ static bool process_inf_msg(struct TDMAHandler *this, uint8 *messageData, uint8
 		}
 	}
 
-	uint32 deltat = get_dt32(time_now, portGetTickCnt());
-
-	if(deltat > 1)
-	{
-		uint8 debug_msg[100];
-		int n = sprintf((char *)&debug_msg, "process_inf_time:%lu,%04X,xxxx", deltat, inst->uwbShortAdd);
-		send_usbmessage(&debug_msg[0], n);
-		usb_run();
-	}
-
-	return tdma_modified;
-
-	//handle TDMA frame sync
+//	uint32 deltat = get_dt32(time_now, portGetTickCnt());
 
-	//TODO keep this but turn back on, dw_event not declared...
-
-//	//account for the delay between transmission and reception of the INF message
-//	//TODO make this a function of the data rate as well!
-//	//NOTE experimentally found this number to be +- 1 millisecond for some reason. //TODO figure out why
-//	//experimentally found formula is 0.079667x + 0.85611
-//	uint32 txrx_delay = (uint32)(0.079667*(float)(dw_event->rxLength) + 0.85611); //TODO make this function of message length!
-//
-//
-//	tdma_handler->uwbFrameStartTimes[srcIndex] = time_now - (timeSinceFrameStart + txrx_delay);
-//
-//
-//	uint32 myFramelengthDuration = tdma_handler->framelength*tdma_handler->slotDuration;
-//	uint32 myTimeSinceFrameStart = get_dt32(tdma_handler->frameStartTime, time_now);
-//
-//	//NEW
-//	if(tdma_handler->framelength <= tdma_handler->uwbFramelengths[srcIndex])
+//	if(deltat > 1)
 //	{
-//		uint32 timeSinceFrameStartMod = (timeSinceFrameStart + txrx_delay)%myFramelengthDuration;
-//
-//		if(myTimeSinceFrameStart > timeSinceFrameStartMod)
-//		{
-//			uint32 diff = myTimeSinceFrameStart - timeSinceFrameStartMod;
-//			tdma_handler->frameStartTime += diff/2;
-//			tdma_handler->lastSlotStartTime += diff/2;
-//		}
-//		else
-//		{
-//			uint32 diff = timeSinceFrameStartMod - myTimeSinceFrameStart;
-//			tdma_handler->frameStartTime -= diff/2;
-//			tdma_handler->lastSlotStartTime -= diff/2;
-//
-//		}
-//	}
-//	else
-//	{
-//		uint32 hisFramelengthDuration = tdma_handler->uwbFramelengths[srcIndex]*tdma_handler->slotDuration;
-//		uint32 myTimeSinceFrameStartMod = (myTimeSinceFrameStart - txrx_delay)%hisFramelengthDuration;
-//
-//		if(timeSinceFrameStart > myTimeSinceFrameStartMod)
-//		{
-//			uint32 diff = timeSinceFrameStart - myTimeSinceFrameStartMod;
-//			tdma_handler->frameStartTime -= diff/2;
-//			tdma_handler->lastSlotStartTime -= diff/2;
-//		}
-//		else
-//		{
-//			uint32 diff = myTimeSinceFrameStartMod - timeSinceFrameStart;
-//			tdma_handler->frameStartTime += diff/2;
-//			tdma_handler->lastSlotStartTime += diff/2;
-//		}
+//		uint8 debug_msg[100];
+//		int n = sprintf((char *)&debug_msg, "process_inf_time:%lu,%04X,xxxx", deltat, inst->uwbShortAdd);
+//		send_usbmessage(&debug_msg[0], n);
+//		usb_run();
 //	}
 
-
-
-
+	return tdma_modified;
 }
 
 
@@ -2620,8 +2531,6 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
 //	 send_usbmessage(&debug_msg[0], n);
 //	 usb_run();
 
-
-
 	switch (discovery_mode)//TODO make sure all modes are captured here...
 	{
 		case WAIT_INF_REG:
@@ -2669,7 +2578,7 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
 			//keep track of which number each UWB belongs to
 			//then keep track of which was selected so we can iterate over it later in this function
 			uint8 num_sub_networks = 0;
-			uint8 sub_network_membership[UWB_LIST_SIZE] = {0};
+			uint8 sub_network_membership[UWB_LIST_SIZE] = {};
 			uint8 sub_network_selected = 0;
 			uint8 sub_network_members[UWB_LIST_SIZE-1] = {0}; //cannot be more subnetworks than other UWBs
 			uint64 sub_network_tsfs[UWB_LIST_SIZE-1] = {0};
@@ -2686,7 +2595,7 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
 //				if(inst->uwbListType[i] != UWB_LIST_NEIGHBOR)
 				if(this->uwbListTDMAInfo[i].connectionType != UWB_LIST_NEIGHBOR)
 				{
-					continue; //TODO consider a way to accoutn for hidden and twice hidden to determine which netowrk is larger
+					continue; //TODO consider a way to account for hidden and twice hidden to determine which network is larger
 				}
 
 				struct TDMAInfo *info_i = &this->uwbListTDMAInfo[i];
@@ -2694,6 +2603,12 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
 //				uint64 timeSinceFrameStart_us = get_dt64(this->uwbFrameStartTimes64[i], time_now_us);
 				uint64 timeSinceFrameStart_us = get_dt64(this->uwbListTDMAInfo[i].frameStartTime, time_now_us);
 
+				if(timeSinceFrameStart_us > 100000000)
+				{
+					sub_network_membership[i] = 255; //disregard
+					continue;
+				}
+
 				//if tsfs > framelengthDuration_us, reduce it by framelengthDuration_us
 
 				//test the ith UWB against the jth subnetwork
@@ -2701,7 +2616,6 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
 				{
 					uint64 diff_us = 0;
 
-
 					uint8 min_fl = info_i->framelength;
 					if(sub_network_base_framelength[j] < min_fl)
 					{
@@ -2711,7 +2625,7 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
 					uint64 min_framelengthDuration = min_fl*this->slotDuration_us;
 					uint64 diff_tsfs = 0;
 
-					if(timeSinceFrameStart_us <= sub_network_tsfs[j])//TODO left off here this appears to be the problem area, the subnetwork timings arent always lining up like they should
+					if(timeSinceFrameStart_us <= sub_network_tsfs[j])
 					{
 						diff_tsfs = sub_network_tsfs[j] - timeSinceFrameStart_us;
 					}
@@ -2732,37 +2646,6 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
 					}
 
 
-
-
-
-//					if(info_i->framelength <= sub_network_base_framelength[j])//TODO left off here this appears to be the problem area, the subnetwork timings arent always lining up like they should
-//					{
-//						uint64 sub_network_tsfs_us_mod = sub_network_tsfs[j]%framelengthDuration_us; //<--this doesn't really make sense...
-//
-//						if(timeSinceFrameStart_us > sub_network_tsfs_us_mod)
-//						{
-//							diff_us = timeSinceFrameStart_us - sub_network_tsfs_us_mod;
-//						}
-//						else
-//						{
-//							diff_us = sub_network_tsfs_us_mod - timeSinceFrameStart_us;
-//						}
-//					}
-//					else
-//					{
-//						uint64 sub_network_FramelengthDuration_us = sub_network_base_framelength[j]*this->slotDuration_us;
-//						uint64 timeSinceFrameStartMod_us = timeSinceFrameStart_us%sub_network_FramelengthDuration_us;
-//
-//						if(sub_network_tsfs[j] > timeSinceFrameStartMod_us)
-//						{
-//							diff_us = sub_network_tsfs[j] - timeSinceFrameStartMod_us;
-//						}
-//						else
-//						{
-//							diff_us = timeSinceFrameStartMod_us - sub_network_tsfs[j];
-//						}
-//					}
-
 					//if difference is below the threshold, it belongs to this subnetwork
 					//if not, it may belong to another one already listed,
 					//if not, create a new one...
@@ -2777,7 +2660,6 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
 						//reached the last listed sub_netowrk, list a new subnetwork.
 						sub_network_members[num_sub_networks] = 1;
 						sub_network_base_framelength[num_sub_networks] = this->uwbListTDMAInfo[i].framelength;
-//						sub_network_tsfs[num_sub_networks] = get_dt64(this->uwbFrameStartTimes64[i], time_now_us);
 						sub_network_tsfs[num_sub_networks] = get_dt64(this->uwbListTDMAInfo[i].frameStartTime, time_now_us);
 						sub_network_membership[num_sub_networks] = num_sub_networks;
 						num_sub_networks++;
@@ -2905,9 +2787,9 @@ static void set_discovery_mode(struct TDMAHandler *this, DISCOVERY_MODE discover
 //				this->uwbFrameStartTimes64[0] -= myFrameDuration;
 				this->uwbListTDMAInfo[0].frameStartTime -= myFrameDuration;
 			}
-//			uint8 slot = this->uwbFrameStartTimes64[0]/this->slotDuration_us; //integer division rounded down
-			uint8 slot = this->uwbListTDMAInfo[0].frameStartTime/this->slotDuration_us; //integer division rounded down
-//			this->lastSlotStartTime64 = this->uwbFrameStartTimes64[0] + (uint64)(this->slotDuration_us*slot);
+
+			uint64 myTimeSinceFrameStart = get_dt64(this->uwbListTDMAInfo[0].frameStartTime, time_now);
+			uint8 slot = myTimeSinceFrameStart/this->slotDuration_us; //integer division rounded down
 			this->lastSlotStartTime64 = this->uwbListTDMAInfo[0].frameStartTime + (uint64)(this->slotDuration_us*slot);
 
 //			this->lastSlotStartTime64 = this->uwbFrameStartTimes64[0];
@@ -2982,6 +2864,226 @@ static void check_discovery_mode_expiration(struct TDMAHandler *this)
 	}
 }
 
+
+static bool check_timeouts(struct TDMAHandler *this, uint32 time_now)
+{
+	instance_data_t *inst = instance_get_local_structure_ptr(0);
+	uint32 delta_t = 0;
+	bool rangingUWBTimeout = FALSE;
+	bool setInactive = FALSE;
+	bool updateINF = FALSE;
+	bool noNeighbors = FALSE;
+
+	for(int i=1; i < inst->uwbListLen; i++)//0 reserved for self, timeout not applicable
+	{
+		struct TDMAInfo *info = &this->uwbListTDMAInfo[i];
+
+		switch (info->connectionType)
+		{
+			case UWB_LIST_NEIGHBOR:
+			{
+				delta_t = get_dt32(info->lastCommNeighbor, time_now);
+
+				if(delta_t > UWB_COMM_TIMEOUT)
+				{
+					if(info->lastCommHidden != 0)
+					{
+						info->connectionType = UWB_LIST_HIDDEN;
+						updateINF = TRUE;
+					}
+					else if(info->lastCommTwiceHidden != 0)
+					{
+						info->connectionType = UWB_LIST_TWICE_HIDDEN;
+						updateINF = TRUE;
+					}
+					else
+					{
+						info->connectionType = UWB_LIST_INACTIVE;
+						updateINF = TRUE;
+						setInactive = TRUE;
+						this->free_slots(info);
+					}
+
+					if(instfindnumneighbors(inst) <= 0)
+					{
+						noNeighbors = TRUE;
+					}
+
+					if(inst->uwbToRangeWith == i)
+					{
+						rangingUWBTimeout = TRUE;
+					}
+				}
+
+				break;
+			}
+			case UWB_LIST_HIDDEN:
+			{
+				delta_t = get_dt32(info->lastCommHidden, time_now);
+
+				if(delta_t > UWB_COMM_TIMEOUT)
+				{
+					if(info->lastCommTwiceHidden != 0)
+					{
+						info->connectionType = UWB_LIST_TWICE_HIDDEN;
+						updateINF = TRUE;
+					}
+					else
+					{
+						info->connectionType = UWB_LIST_INACTIVE;
+						this->free_slots(info);
+						setInactive = TRUE;
+						updateINF = TRUE;
+					}
+				}
+
+				break;
+			}
+			case UWB_LIST_TWICE_HIDDEN:
+			{
+				delta_t = get_dt32(info->lastCommTwiceHidden, time_now);
+
+				if(delta_t > UWB_COMM_TIMEOUT)
+				{
+					info->connectionType = UWB_LIST_INACTIVE;
+					this->free_slots(info);
+					setInactive = TRUE;
+					updateINF = TRUE;
+				}
+
+				break;
+			}
+			case UWB_LIST_INACTIVE:
+			{
+				//TODO figure out the why this doesn't work.
+				//specifically when instremoveuwb(inst, i); and i--; are used
+
+
+//				//find the most recent comm...
+//				uint32 newest = 0;
+//				if(info->lastCommNeighbor != 0)
+//				{
+//					newest = info->lastCommNeighbor;
+//				}
+//
+//				if(info->lastCommHidden != 0)
+//				{
+//					if(info->lastCommHidden > newest)//TODO number wrapping
+//					{
+//						newest = info->lastCommHidden;
+//					}
+//				}
+//
+//				if(info->lastCommTwiceHidden != 0)
+//				{
+//					if(info->lastCommTwiceHidden > newest)//TODO number wrapping
+//					{
+//						newest = info->lastCommTwiceHidden;
+//					}
+//				}
+//
+//				if(newest != 0)
+//				{
+//					delta_t = get_dt32(newest, time_now);
+//
+//					if(delta_t > UWB_COMM_TIMEOUT*2)
+//					{
+//						uint64 uwb_addr = 0;
+//						uwb_addr = instance_get_uwbaddr(i);
+//						uint8 debug_msg[100];
+//						int n = sprintf((char*)&debug_msg[0], "REMOVE UWB: %d, %llX", i, uwb_addr);
+//						 send_usbmessage(&debug_msg[0], n);
+//						 usb_run();
+//						instremoveuwb(inst, i); //TODO finish testing to make sure it works like it should.
+//						i--; //TODO is this a probelm if i=1 then i=0?
+//
+//					}
+//				}
+
+
+				break;
+			}
+			default:
+			{
+				//invalid list type
+				break;
+			}
+		}
+	}
+
+	//one of the UWBs we were tracking became inactive,
+	//reoptimize our TDMA assignments and repopulate the inf message
+	if(setInactive == TRUE)
+	{
+		this->free_slots(&this->uwbListTDMAInfo[0]);
+		this->find_assign_slot(this);
+	}
+
+	//adjust the INF message to reflect any changes
+	if(updateINF == TRUE)
+	{
+		this->populate_inf_msg(this, RTLS_DEMO_MSG_INF_UPDATE);
+	}
+
+
+	if(rangingUWBTimeout == TRUE)
+	{
+		inst->uwbToRangeWith = 255;
+		if(inst->mode == ANCHOR || inst->mode == DISCOVERY)
+		{
+			inst->testAppState = TA_RXE_WAIT;
+		}
+		else
+		{
+			inst->testAppState = TA_TX_SELECT;
+		}
+	}
+
+	return noNeighbors;
+}
+
+
+static void remove_uwbinfo(struct TDMAHandler *this, uint8 uwb_index)
+{
+	instance_data_t *inst = instance_get_local_structure_ptr(0);
+
+	if(uwb_index > 0 && uwb_index < inst->uwbListLen)
+	{
+		this->free_slots(&this->uwbListTDMAInfo[uwb_index]);
+
+		for(int i=uwb_index; i<inst->uwbListLen-1; i++)
+		{
+			struct TDMAInfo *info_a = &this->uwbListTDMAInfo[i];
+			struct TDMAInfo *info_b = &this->uwbListTDMAInfo[i+1];
+
+			info_a->connectionType = info_b->connectionType;
+			info_a->lastCommNeighbor = info_b->lastCommNeighbor;
+			info_a->lastCommHidden = info_b->lastCommHidden;
+			info_a->lastCommTwiceHidden = info_b->lastCommTwiceHidden;
+			info_a->lastRange = info_b->lastRange;
+			info_a->frameStartTime = info_b->frameStartTime;
+			info_a->framelength = info_b->framelength;
+			info_a->slotsLength = info_b->slotsLength;
+			info_a->slots = info_b->slots;
+		}
+
+
+		struct TDMAInfo *info_end = &this->uwbListTDMAInfo[inst->uwbListLen-1]; //maybe this!
+		info_end->connectionType = UWB_LIST_INACTIVE;
+		info_end->lastCommNeighbor = 0;
+		info_end->lastCommHidden = 0;
+		info_end->lastCommTwiceHidden = 0;
+		info_end->lastRange = 0;
+		info_end->frameStartTime = 0;
+		info_end->framelength = MIN_FRAMELENGTH;
+		this->free_slots(info_end);
+	}
+}
+
+
+
+
+
 static void usb_dump_tdma(struct TDMAHandler *this)
 {
 	instance_data_t *inst = instance_get_local_structure_ptr(0);
@@ -3099,6 +3201,8 @@ static struct TDMAHandler new(){
 	ret.enter_discovery_mode = &enter_discovery_mode;
 	ret.set_discovery_mode = &set_discovery_mode;
 	ret.check_discovery_mode_expiration = &check_discovery_mode_expiration;
+	ret.check_timeouts = &check_timeouts;
+	ret.remove_uwbinfo = &remove_uwbinfo;
 	ret.usb_dump_tdma = &usb_dump_tdma;
 
 
@@ -3121,20 +3225,25 @@ static struct TDMAHandler new(){
 
 	for(int i = 0; i < UWB_LIST_SIZE; i++)
 	{
-		ret.uwbListTDMAInfo[i].uwbIndex = i;
+//		ret.uwbListTDMAInfo[i].uwbIndex = i;
 		ret.uwbListTDMAInfo[i].framelength = (uint8)MIN_FRAMELENGTH;
 		ret.uwbListTDMAInfo[i].slots = NULL;
 		ret.uwbListTDMAInfo[i].slotsLength = 0;
 		ret.uwbListTDMAInfo[i].frameStartTime = time_now_us;
 		ret.uwbListTDMAInfo[i].connectionType = UWB_LIST_INACTIVE;
+		ret.uwbListTDMAInfo[i].lastCommNeighbor = 0;
+		ret.uwbListTDMAInfo[i].lastCommHidden = 0;
+		ret.uwbListTDMAInfo[i].lastCommTwiceHidden = 0;
+		ret.uwbListTDMAInfo[i].lastRange = 0;
 	}
 	ret.uwbListTDMAInfo[0].connectionType = UWB_LIST_SELF;
 
-//    ret.uwbFrameStartTimes64[0] = time_now_us;
     ret.lastFST = time_now_us;
     ret.lastSlotStartTime64 = time_now_us;
     ret.infSentThisSlot = FALSE;
-    ret.pollSentThisSlot = FALSE;
+    ret.firstPollSentThisSlot = FALSE;
+    ret.firstPollComplete = FALSE;
+    ret.secondPollSentThisSlot = FALSE;
     ret.slotStartDelay_us = 4000;
     ret.frameSyncThreshold_us = 2*ret.slotStartDelay_us;
     ret.infMessageLength = 0;
@@ -3147,9 +3256,6 @@ static struct TDMAHandler new(){
     ret.collectInfDuration = 10000; //TODO change this back
 	ret.waitInfDuration = ret.collectInfDuration;
 
-	ret.lastINFtx = time_now_us;
-	ret.lastINFrx = time_now_us;
-
 	return ret;
 }
 
diff --git a/src/application/tdma_handler.h b/src/application/tdma_handler.h
index ec9eac97b08ae7076cfd4e6f9e399651411762cf..9e7001cbefc851c96fbefd474498e82fdc893774 100644
--- a/src/application/tdma_handler.h
+++ b/src/application/tdma_handler.h
@@ -7,13 +7,13 @@
 
 struct TDMAInfo
 {
-	//lastCommNeighbor
-	//lastCommHidden
-	//lastComm2Hidden
-	uint64 frameStartTime;
-	uint32 lastRange;
+//	uint8 uwbIndex; //TODO remove if unused
 	uint8 connectionType;       //UWB_LIST_SELF, UWB_LIST_NEIGHBOR, UWB_LIST_HIDDEN, UWB_LIST_INACTIVE
-	uint8 uwbIndex; //TODO remove if unused
+	uint32 lastCommNeighbor;	//milliseconds
+	uint32 lastCommHidden;		//milliseconds
+	uint32 lastCommTwiceHidden;		//milliseconds
+	uint32 lastRange;			//milliseconds
+	uint64 frameStartTime;		//microseconds
 	uint8 framelength;
 	uint8 slotsLength;
 	uint8 *slots;
@@ -47,17 +47,14 @@ struct TDMAHandler
 
 	//TODO use smaller data types where possible
 
-	uint64 lastINFtx;
-	uint64 lastINFrx;
-
-
-//	uint64 uwbFrameStartTimes64[UWB_LIST_SIZE];
 	uint64 lastFST;
 	uint64 lastSlotStartTime64;
 	uint32 slotDuration_ms;   //TODO make variable in duration based on UWB_LIST_SIZE
 	uint32 slotDuration_us;   //TODO make variable in duration based on UWB_LIST_SIZE
 	bool infSentThisSlot;
-	bool pollSentThisSlot;
+	bool firstPollSentThisSlot;
+	bool firstPollComplete;
+	bool secondPollSentThisSlot;
 	bool rebase_pending;
 	bool rebase_tx;
 	uint64 rebase_frameStartTime64; //TODO rename 64 to us!
@@ -81,7 +78,6 @@ struct TDMAHandler
 
     //class functions
 	bool (*slot_transition)(struct TDMAHandler *this);
-//	void (*frame_sync)(struct TDMAHandler *this, event_data_t *dw_event, uint8 *messageData, uint8 srcIndex, FRAME_SYNC_MODE mode);
 	void (*frame_sync)(struct TDMAHandler *this, event_data_t *dw_event, uint8 framelength, uint64 timeSinceFrameStart_us, uint8 srcIndex, FRAME_SYNC_MODE mode);
 	bool (*tx_sync_msg)(struct TDMAHandler *this);
 	void (*update_inf_tsfs)(struct TDMAHandler *this);
@@ -94,11 +90,9 @@ struct TDMAHandler
     void (*enter_discovery_mode)(struct TDMAHandler *this);
     void (*set_discovery_mode)(struct TDMAHandler *this, DISCOVERY_MODE mode, uint32 time_now);
     void (*check_discovery_mode_expiration)(struct TDMAHandler *this);
+    bool (*check_timeouts)(struct TDMAHandler *this, uint32 time_now);
+    void (*remove_uwbinfo)(struct TDMAHandler *this, uint8 uwb_index);
     void (*usb_dump_tdma)(struct TDMAHandler *this);
-
-
-    //TODO left off here. updating messageData as well as
-    //TODO revisit anything that works with messageData!!!
     bool (*slot_assigned)(struct TDMAInfo *info, uint8 slot);
     bool (*assign_slot)(struct TDMAInfo *info, uint8 slot, bool safeAssign);
     void (*free_slot)(struct TDMAInfo *info, uint8 slot);