led_errorcode(ERROR_NOXBEE);

			return;

		}

		slavesensors_exitAT();

	}

	_delay_ms(2);

	#ifdef DEBUG_SELECTNODE

	serial0_sendString("Selected ");

	serial0_sendChar(nodeIndex + 0x30);

	serial0_sendString("\r\n");

	#endif

	serial0_ion();

	return;

}

void slavesensors_selectlogger() 

{

	if(loggerIndex != 255) {

		slavesensors_selectnode(loggerIndex);

	}	

}

void slavesensors_exitAT() 

{

	// Exit AT

	serial0_sendString("ATCN");

	serial0_sendChar(0x0D);

	_delay_ms(2);

	//// Wait until we get "OK"

	//int atStart = time_millis();

	//while(!serial0_hasChar()) {

		//if(time_millis() - atStart > 500) {

			//led_errorcode(ERROR_EXITAT);

			//wdt_reset();

			//return 1;

		//}

	//};

	xbeeIsOk();

}

// Enter AT mode. Leaves "OK" on the buffer.

int slavesensors_enterAT() 

{

	// Delay guard time

	_delay_ms(2);

	serial0_ioff(); // interrupts MUST be off

	// Enter AT mode

	serial0_sendChar('+'); // Enter AT mode

	serial0_sendChar('+');

	serial0_sendChar('+');

	_delay_ms(2);

	// Wait 1ms until we get "OK"

	//int atStart = time_millis();

	//while(!serial0_hasChar()) {

		//if(time_millis() - atStart > 500) {

			//led_errorcode(ERROR_ATFAIL);

			//wdt_reset();

			//return 1;

		//}

	//};	

	return xbeeIsOk();

}

int xbeeIsOk() 

{

	char* tmppntr = serial0_readLine();

	if(strcmp(tmppntr, "OK") == 0)

	{

		return 0;

	}

	else

	{

		led_errorcode(ERROR_NOXBEE);

		return 1;

	}

}

bool slavesensors_dataReady() {

	return dataReady;

}

bool slavesensors_isrequesting() 

{

	return requesting;	

}

void slavesensors_startprocess() 

{

	requesting = true;

	slavesensors_request();		

}

// TODO: inline. static.

void slavesensors_request() 

{

	if(currentSlave == loggerIndex) {

		currentSlave++;

	}

	slavesensors_selectnode(currentSlave);

	serial_sendCommand("@"); // Request data!

	slavesensors_selectlogger();

}

uint8_t numReadingsToExpect = 0; // number of values that the slave is about to send (testing)

// TODO: needs to skip logger!

void slavesensors_process(uint8_t parseResult) 

{

	if(!requesting) {

		// we got a command when we didn't request anything. probably skip it.

		return;

	}

	// TODO: timeout. If we're at NODATA for a long time and we are requesting, that's an issue.

	// TODO: If we time out, WE NEED TO RESET THE PARSER. It could be in a bad state.

	else if(parseResult == PARSERESULT_NODATA) {

		// Wait for data

	}

	// Finished reception of a message (one sensor data value). If not finished, send out command to get the next one

	else if(parseResult == PARSERESULT_PARSEOK)

	{

		#ifdef DEBUG_GETSLAVEDATA

		char debug[50];

		snprintf(debug, 50, "Slave %u sensor %u of total nodes %u\r\n", currentSlave, currentSlaveSensor,nodeCount);

		serial0_sendString(debug);

		#endif

		// We got some data, let's handle it

		// ASCII payload

		uint8_t len = getPayloadLength();

		uint8_t* load = getPayload();

		uint8_t type = getPayloadType();

		uint16_t parsedVal = atoi(load);

		// Special case for slave telling us how many things we're about to get		

		if(type + 0x30 == '@') {

			#ifdef DEBUG_GETSLAVEDATA

			serial0_sendString("Got an awesome count!\r\n");

			serial0_sendChar(parsedVal + 0x30);

			serial0_sendString("\r\n");

			#endif

			numReadingsToExpect = parsedVal;

			currentSlaveSensor = 0;

			requesting = true;

		}

		else {

			// Store data in structure

			sensordata_set(currentSlave,type,parsedVal);

			#ifdef DEBUG_GETSLAVEDATA

			serial0_sendString("Stored some sexy data!\r\n");

			#endif 

			// If we finished all sensors for all slaves

			if(currentSlave >= (nodeCount-1) && currentSlaveSensor >= (numReadingsToExpect-1))

			{

				#ifdef DEBUG_GETSLAVEDATA

				serial0_sendString("We got all data for all slaves!\r\n");

				#endif

				dataReady = true;

				currentSlave = 0;

				currentSlaveSensor = 0;

				requesting = false;

			}

			// If we finished up one slave, go to the next

			else if(currentSlaveSensor >= (numReadingsToExpect-1)) 

			{

				#ifdef DEBUG_GETSLAVEDATA

				serial0_sendString("Finished up one slave, go to another.\r\n");

				#endif

				currentSlave++;

				currentSlaveSensor = 0;

				requesting = true;

				slavesensors_request();

			}

			// If we haven't finished a slave (or all of them), just get the next sensor of the current slave

			else

			{

				#ifdef DEBUG_GETSLAVEDATA

				serial0_sendString("Give me another sensor value...");

				#endif

				// request data for the current sensor of the current slave

				currentSlaveSensor++;

				requesting = true;

				//slavesensors_request();	 slaves now send all values at once, we don't need to keep requesting

			}

		}

	}

	// If fail, try retransmit. Or we could skip and hit it next time.

	// TODO: Maximum number of retransmissions

	else if(parseResult == PARSERESULT_FAIL) {

		if(requesting) {

			slavesensors_request();	// re-request

		}			

	}

	else if(parseResult == PARSERESULT_STILLPARSING)

	{

		return; // do nothing

	}

	else {

		// something is terribly wrong!

		return;

	}

}