diff --git a/SerialGateway/SerialGateway.ino b/SerialGateway/SerialGateway.ino
index 2ce8685..c245b92 100644
--- a/SerialGateway/SerialGateway.ino
+++ b/SerialGateway/SerialGateway.ino
@@ -26,7 +26,7 @@
 #include <RF24.h>
 #include <MsTimer2.h>
 #include <PinChangeInt.h>
-#include <Gateway.h>  
+#include <MyGateway.h>  
 #include <stdarg.h>
 
 #define INCLUSION_MODE_TIME 1 // Number of minutes inclusion mode is enabled
@@ -37,7 +37,7 @@
 //Gateway gw;
 
 // To start gateway with include button and led blinking functionality enabled use this instead!
-Gateway gw(9, 10, INCLUSION_MODE_TIME, INCLUSION_MODE_PIN,  6, 5, 4);
+MyGateway gw(9, 10, INCLUSION_MODE_TIME, INCLUSION_MODE_PIN,  6, 5, 4);
 
 
 char inputString[MAX_RECEIVE_LENGTH] = "";    // A string to hold incoming commands from serial/ethernet interface
diff --git a/libraries/MySensors/MyConfig.h b/libraries/MySensors/MyConfig.h
index 5d7956f..474b705 100644
--- a/libraries/MySensors/MyConfig.h
+++ b/libraries/MySensors/MyConfig.h
@@ -12,6 +12,6 @@
 /***
  * Enable/Disable debug logging
  */
-//#define DEBUG
+#define DEBUG
 
 #endif
diff --git a/libraries/MySensors/MyGateway.cpp b/libraries/MySensors/MyGateway.cpp
index 6d8fa89..f6e56d0 100644
--- a/libraries/MySensors/MyGateway.cpp
+++ b/libraries/MySensors/MyGateway.cpp
@@ -37,8 +37,8 @@ void MyGateway::begin(rf24_pa_dbm_e paLevel, uint8_t channel, rf24_datarate_e da
 		useWriteCallback = false;
 	}
 
-	radioId = 0;
-	distance = 0;
+	s.nodeId = 0;
+	s.distance = 0;
 	inclusionMode = 0;
 	buttonTriggeredInclusion = false;
 	countRx = 0;
diff --git a/libraries/MySensors/MyMessage.cpp b/libraries/MySensors/MyMessage.cpp
index 3b31083..759d72e 100644
--- a/libraries/MySensors/MyMessage.cpp
+++ b/libraries/MySensors/MyMessage.cpp
@@ -116,7 +116,11 @@ char* MyMessage::getString(char *buffer) {
 }
 
 uint8_t MyMessage::getByte() {
-	return data[0];
+	if (getPayloadType() == P_BYTE)
+		return data[0];
+	if (getPayloadType() == P_STRING)
+		return atoi(data);
+	return 0;
 }
 bool MyMessage::getBool() {
 	return !(data[0]-'0'==0);
@@ -125,17 +129,34 @@ bool MyMessage::getBool() {
 double MyMessage::getDouble() {
 	return strtod(data, NULL);
 }
+
 long MyMessage::getLong() {
-	return lValue;
+	if (getPayloadType() == P_LONG32)
+		return lValue;
+	if (getPayloadType() == P_STRING)
+		return atol(data);
+	return 0;
 }
 unsigned long MyMessage::getULong() {
-	return ulValue;
+	if (getPayloadType() == P_ULONG32)
+		return ulValue;
+	if (getPayloadType() == P_STRING)
+		return atol(data);
+	return 0;
 }
 int MyMessage::getInt() {
-	return iValue;
+	if (getPayloadType() == P_INT16)
+		return iValue;
+	if (getPayloadType() == P_STRING)
+		return atoi(data);
+	return 0;
 }
 unsigned int MyMessage::getUInt() {
-	return uiValue;
+	if (getPayloadType() == P_UINT16)
+		return uiValue;
+	if (getPayloadType() == P_STRING)
+		return atoi(data);
+	return 0;
 }
 
 // Header setters
diff --git a/libraries/MySensors/MySensor.cpp b/libraries/MySensors/MySensor.cpp
index e4a3638..bc58710 100644
--- a/libraries/MySensors/MySensor.cpp
+++ b/libraries/MySensors/MySensor.cpp
@@ -20,13 +20,13 @@ void MySensor::setupRadio(rf24_pa_dbm_e paLevel, uint8_t channel, rf24_datarate_
 
 	// Start up the radio library
 	RF24::begin();
-	RF24::setAutoAck(1);
-	RF24::enableAckPayload();
+
 	if (!RF24::isPVariant()) {
-		debug(PSTR("Radio wiring NOT ok or not using P version of NRF24L01.\n"));
+		debug(PSTR("err=check wire\n"));
 		while(1);
 	}
-
+	RF24::setAutoAck(1);
+	RF24::enableAckPayload();
 	RF24::setChannel(channel);
 	RF24::setPALevel(paLevel);
 	RF24::setDataRate(dataRate);
@@ -34,8 +34,6 @@ void MySensor::setupRadio(rf24_pa_dbm_e paLevel, uint8_t channel, rf24_datarate_
 	RF24::setCRCLength(RF24_CRC_16);
 	RF24::enableDynamicPayloads();
 
-
-
 	// All repeater nodes and gateway listen to broadcast pipe (for PING messages)
 	if (relayMode) {
 		RF24::openReadingPipe(BROADCAST_PIPE, TO_ADDR(BROADCAST_ADDRESS));
@@ -43,138 +41,85 @@ void MySensor::setupRadio(rf24_pa_dbm_e paLevel, uint8_t channel, rf24_datarate_
 }
 
 void MySensor::setupRelayMode(){
-	childNodeTable = (uint8_t *) malloc(256);
-	// Read routing table from EEPROM
-	for (unsigned int i=0;i<sizeof(childNodeTable);i++) {
-		childNodeTable[i] = EEPROM.read(EEPROM_ROUTES_ADDRESS+i);
-	}
+	s.childNodeTable = (uint8_t *) malloc(256);
+	eeprom_read_block((void*)s.childNodeTable, (void*)EEPROM_ROUTES_ADDRESS, 256);
 }
 
-void MySensor::begin(void (*_msgCallback)(MyMessage), boolean _relayMode, uint8_t _radioId, rf24_pa_dbm_e paLevel, uint8_t channel, rf24_datarate_e dataRate) {
+void MySensor::begin(void (*_msgCallback)(MyMessage), boolean _relayMode, uint8_t _nodeId, rf24_pa_dbm_e paLevel, uint8_t channel, rf24_datarate_e dataRate) {
 	Serial.begin(BAUD_RATE);
-
 	relayMode = _relayMode;
 	msgCallback = _msgCallback;
-	radioId = _radioId;
-
-	debug(PSTR("Started %s.\n"), relayMode?"relay":"sensor");
 
 	if (relayMode) {
 		setupRelayMode();
 	}
 	setupRadio(paLevel, channel, dataRate);
 
-	// Fetch relay from EEPROM
-	relayId = EEPROM.read(EEPROM_RELAY_ID_ADDRESS);
-	// Fetch distance from eeprom
-	distance = EEPROM.read(EEPROM_DISTANCE_ADDRESS);
-
-	if (relayId == 0xff) {
-		// No relay previously fetched and stored in eeprom. Try to find one.
-		findRelay();
+	// Read settings from EEPROM
+	eeprom_read_block((void*)&s, (void*)EEPROM_START, 3*sizeof(uint8_t));
+	if (_nodeId != AUTO) {
+		// Set static id
+		s.nodeId = _nodeId;
 	}
-	debug(PSTR("Relay=%d, distance=%d\n"), relayId, distance);
 
-	initializeRadioId();
+	// If no parent was found. Try to find one.
+	if (s.parentNodeId == 0xff) {
+		findParentNode();
+	}
+
+	// Try to fetch node-id from gateway
+	if (s.nodeId == AUTO) {
+		requestNodeId();
+	}
+	debug(PSTR("%s started\n"), relayMode?"relay":"sensor");
 
 	// Open reading pipe for messages directed to this node (set write pipe to same)
-	RF24::openReadingPipe(WRITE_PIPE, TO_ADDR(radioId));
-	RF24::openReadingPipe(CURRENT_NODE_PIPE, TO_ADDR(radioId));
+	RF24::openReadingPipe(WRITE_PIPE, TO_ADDR(s.nodeId));
+	RF24::openReadingPipe(CURRENT_NODE_PIPE, TO_ADDR(s.nodeId));
 
 	// Send presentation for this radio node
 	present(NODE_CHILD_ID, relayMode? S_ARDUINO_RELAY : S_ARDUINO_NODE);
 
-	// Send relay information back to sensor net gateway.
-	sendInternal(I_RELAY_NODE, itoa(relayId, convBuffer, 10));
+	// Send parent information back to sensor net gateway.
+	sendInternal(I_RELAY_NODE, itoa(s.parentNodeId, convBuffer, 10));
 }
 
 
-uint8_t MySensor::getRadioId() {
-	return radioId;
+uint8_t MySensor::getNodeId() {
+	return s.nodeId;
 }
 
-void MySensor::initializeRadioId() {
-	if (radioId == AUTO) {
-		radioId = EEPROM.read(EEPROM_RADIO_ID_ADDRESS);
-		if (radioId == 0xFF || radioId == 0) {
-			radioId = AUTO;
-			debug(PSTR("No radio id found in EEPROM fetching one from sensor net gateway\n"));
-			// No radio id has been fetched yet ant EEPROM is unwritten.
-			// Request new id from sensor net gateway. Use radioId 4095 temporarily
-			// to be able to receive my correct nodeId.
-			RF24::openReadingPipe(CURRENT_NODE_PIPE, TO_ADDR(radioId));
-			radioId = getInternal(I_REQUEST_ID).getByte();
-			// Write id to EEPROM
-			if (radioId == AUTO) { // sensor net gateway will return max id if all sensor id are taken
-				debug(PSTR("Sensor network is full! You already have the maximum of sensors!\n"));
-				while (1) {} // Wait here. Nothing else we can do...
-			} else {
-				debug(PSTR("Radio id received: %d\n"), radioId);
-				EEPROM.write(EEPROM_RADIO_ID_ADDRESS, radioId);
-			}
-		}
-		debug(PSTR("Radio id stored in EEPROM was: %d\n"), radioId);
-	}
+void MySensor::requestNodeId() {
+	debug(PSTR("req node id\n"));
+
+	RF24::openReadingPipe(CURRENT_NODE_PIPE, TO_ADDR(s.nodeId));
+	sendRoute(msg.build(s.nodeId, GATEWAY_ADDRESS, NODE_CHILD_ID, C_INTERNAL, I_REQUEST_ID));
 }
 
 
-void MySensor::findRelay() {
-	// This can be improved by using status = read_register(OBSERVE_TX,&observe_tx,1) on the
-	// sending side.. the problem is getting this information here without too much fuss.
-	// Try to find relay 3 times before giving up
-	if (radioId == GATEWAY_ADDRESS)
+void MySensor::findParentNode() {
+	if (s.nodeId == GATEWAY_ADDRESS)
 		return; // Gateway has no business here!
 
 	failedTransmissions = 0;
 
-	// Open a reading pipe for current radioId (if it differs from broadcast)
-	if (radioId != BROADCAST_PIPE) {
-		RF24::openReadingPipe(CURRENT_NODE_PIPE, TO_ADDR(radioId));
-		debug(PSTR("Open ping reading pipe: %d\n"), radioId);
+	// Open a reading pipe for current nodeId (if it differs from broadcast)
+	if (s.nodeId != BROADCAST_PIPE) {
+		RF24::openReadingPipe(CURRENT_NODE_PIPE, TO_ADDR(s.nodeId));
 	}
-	distance = 255;
-	uint8_t oldRelayId = relayId;
-	uint8_t retries=0;
-	while (distance == 255 && retries<FIND_RELAY_RETRIES) {
-		// Send ping message to BROADCAST_ADDRESS (to which all nodes listens and should reply to)
-		msg.build(radioId, BROADCAST_ADDRESS, NODE_CHILD_ID, C_INTERNAL, I_PING);
-		sendWrite(BROADCAST_ADDRESS, msg);
-		// Wait for replies for max 10 seconds (or when buffer for all relay nodes have been filled up)
-		unsigned long enter = millis();
-		uint8_t neighborDistanceToGW;
+	// Set distance to max
+	s.distance = 255;
 
-		// Wait for ack responses 5 seconds
-		while (millis() - enter < 5000) {
-			if (process()) {
-				if (msg.getCommand() == C_INTERNAL &&
-						msg.getType() == I_PING_ACK &&
-						msg.getDestination() == radioId)  {
-					neighborDistanceToGW = msg.getByte();
-					if (neighborDistanceToGW<distance-1) {
-						// Found a neighbor closer to GW than previously found
-						distance = neighborDistanceToGW + 1;
-						relayId = msg.getSender();
-						debug(PSTR("Using relay %d. Distance is %d\n"), msg.getSender(), neighborDistanceToGW);
-						if (neighborDistanceToGW == 0) // We found gateway. Search no more.
-							break;
-					} else {
-						debug(PSTR("Discarded relay %d. Distance is %d\n"), msg.getSender(), neighborDistanceToGW);
-					}
-				}
-			}
-		}
-		if (distance == 255) {
-			debug(PSTR("No relay nodes was found. Trying again in 10 seconds.\n"));
-			delay(10000);
-		}
-		retries++;
+	// Send ping message to BROADCAST_ADDRESS (to which all relaying nodes and gateway listens and should reply to)
+	sendWrite(BROADCAST_ADDRESS, msg.build(s.nodeId, BROADCAST_ADDRESS, NODE_CHILD_ID, C_INTERNAL, I_PING));
+	// Wait for replies for max 10 seconds (or when buffer for all parent nodes have been filled up)
+
+	unsigned long enter = millis();
+	// Wait for ack responses 5 seconds
+	while (millis() - enter < 5000) {
+		process();
 	}
 
-	// Store new relay address in EEPROM
-	if (relayId != oldRelayId) {
-		EEPROM.write(EEPROM_RELAY_ID_ADDRESS, relayId);
-		EEPROM.write(EEPROM_DISTANCE_ADDRESS, distance);
-	}
 }
 
 
@@ -184,35 +129,44 @@ void MySensor::findRelay() {
 boolean MySensor::sendAck(MyMessage message) {
 	// Build ack message
 	ack = message;
-//	memcpy((void *)ack, (void *)message, sizeof(MyMessage));
-	ack.setSender(radioId);
+	ack.setCommand(C_SET_VARIABLE);
+	ack.setSender(s.nodeId);
 	ack.setDestination(message.getSender());
 	return sendRoute(ack);
 }
 
 
 boolean MySensor::sendRoute(MyMessage message) {
+	// Make sure to process any incoming messages before sending (could this end up in recursive loop?)
+	process();
+
+	// If we still don't have any node id, re-request and skip this message.
+	if (s.nodeId == AUTO && !(message.getCommand() == C_INTERNAL && message.getType() == I_REQUEST_ID)) {
+		requestNodeId();
+		return false;
+	}
+
 	if (relayMode) {
 		uint8_t dest = message.getDestination();
 		uint8_t route = getChildRoute(dest);
 		if (route>0 && route<255 && dest != GATEWAY_ADDRESS) {
-			debug(PSTR("Routing message to %d.\n"), route);
+			debug(PSTR("ro=%d.\n"), route);
 			// Message destination is not gateway and is in routing table for this node.
 			// Send it downstream
 			return sendWrite(route, message);
 		}
 	}
 
-	if (radioId != GATEWAY_ADDRESS) {
-		debug(PSTR("Relaying message back to gateway.\n"));
+	if (s.nodeId != GATEWAY_ADDRESS) {
+		debug(PSTR("ro=parent\n"));
 		// Should be routed back to gateway.
-		bool ok = sendWrite(relayId, message);
+		bool ok = sendWrite(s.parentNodeId, message);
 
 		if (!ok) {
-			// Failure when sending to relay node. The relay node might be down and we
-			// need to find another route to gateway. Max 20 retries before giving up.
+			// Failure when sending to parent node. The parent node might be down and we
+			// need to find another 	route to gateway. Max 20 retries before giving up.
 			if (failedTransmissions > FIND_RELAY_AFTER_FAILED_TRANSMISSIONS) {
-				findRelay();
+				findParentNode();
 			}
 			failedTransmissions++;
 		} else {
@@ -229,11 +183,11 @@ boolean MySensor::sendRoute(MyMessage message) {
 
 boolean MySensor::sendWrite(uint8_t next, MyMessage message) {
 	uint8_t length = message.getLength();
-	message.setLast(radioId);
+	message.setLast(s.nodeId);
 	message.setCRC(crc8Message(message));
 	// Only print payload if string type
-	debug(PSTR("Tx: fr=%d,to=%d,la=%d,ne=%d,ci=%d,mt=%d,ty=%d,cr=%d:%s\n"),
-			message.getSender(),message.getDestination(), message.getLast(), next, message.getSensor(), message.getCommand(), message.getType(),  message.getCRC(), message.getString());
+	debug(PSTR("tx: %d-%d-%d-%d s=%d,c=%d,t=%d:%s\n"),
+			message.getSender(),message.getLast(), next, message.getDestination(), message.getSensor(), message.getCommand(), message.getType(), message.getString());
 
 	bool broadcast =  message.getCommand() == C_INTERNAL &&  message.getType() == I_PING;
 
@@ -245,21 +199,21 @@ boolean MySensor::sendWrite(uint8_t next, MyMessage message) {
 	RF24::startListening();
 
 	if (ok)
-		debug(PSTR("Sent successfully\n"));
+		debug(PSTR("s=ok\n"));
 	else
-		debug(PSTR("Send failed\n"));
+		debug(PSTR("s=fail\n"));
 
 
 	return ok;
 }
 
 void MySensor::sendInternal(uint8_t variableType, const char *value) {
-	sendRoute(msg.build(radioId, GATEWAY_ADDRESS, NODE_CHILD_ID, C_INTERNAL, variableType).set(value));
+	sendRoute(msg.build(s.nodeId, GATEWAY_ADDRESS, NODE_CHILD_ID, C_INTERNAL, variableType).set(value));
 }
 
 
 bool MySensor::send(MyMessage message, bool enableAck, uint8_t retry) {
-	message.setSender(radioId);
+	message.setSender(s.nodeId);
 	if (enableAck) {
 		message.setCommand(C_SET_WITH_ACK);
 		return wait(message, C_SET_VARIABLE, retry).getCommand() != C_FAILED;
@@ -270,7 +224,7 @@ bool MySensor::send(MyMessage message, bool enableAck, uint8_t retry) {
 
 
 void MySensor::present(uint8_t childSensorId, uint8_t sensorType) {
-	sendRoute(msg.build(radioId, GATEWAY_ADDRESS, childSensorId, C_PRESENTATION, sensorType).set(LIBRARY_VERSION));
+	sendRoute(msg.build(s.nodeId, GATEWAY_ADDRESS, childSensorId, C_PRESENTATION, sensorType).set(LIBRARY_VERSION));
 }
 
 void MySensor::sketchInfo(const char *name, const char *version) {
@@ -281,13 +235,13 @@ void MySensor::sketchInfo(const char *name, const char *version) {
 }
 
 MyMessage MySensor::get( uint8_t childSensorId, uint8_t variableType, uint8_t destination, uint8_t retry) {
-	msg.build(radioId, destination, childSensorId, C_REQ_VARIABLE, variableType);
+	msg.build(s.nodeId, destination, childSensorId, C_REQ_VARIABLE, variableType);
 	return wait(msg,  C_SET_VARIABLE, retry);
 }
 
 void MySensor::request(uint8_t childSensorId, uint8_t variableType, uint8_t destination) {
 	process(); // Make sure to process any queued messages before sending new request
-	sendRoute(msg.build(radioId, destination, childSensorId, C_REQ_VARIABLE, variableType));
+	sendRoute(msg.build(s.nodeId, destination, childSensorId, C_REQ_VARIABLE, variableType));
 }
 
 
@@ -325,7 +279,7 @@ MyMessage MySensor::wait(MyMessage message, uint8_t expectedResponseCommand, uin
 
 
 MyMessage MySensor::getInternal(uint8_t variableType, uint8_t retry) {
-	msg.build(radioId, GATEWAY_ADDRESS, NODE_CHILD_ID, C_INTERNAL, variableType);
+	msg.build(s.nodeId, GATEWAY_ADDRESS, NODE_CHILD_ID, C_INTERNAL, variableType);
 	return wait(msg, C_INTERNAL, retry);
 }
 
@@ -335,7 +289,7 @@ MyMessage MySensor::getInternal(uint8_t variableType, uint8_t retry) {
 }*/
 
 void MySensor::requestTime() {
-	sendRoute(msg.build(radioId, GATEWAY_ADDRESS, NODE_CHILD_ID, C_INTERNAL, I_TIME));
+	sendRoute(msg.build(s.nodeId, GATEWAY_ADDRESS, NODE_CHILD_ID, C_INTERNAL, I_TIME));
 }
 
 
@@ -348,7 +302,7 @@ void MySensor::requestTime() {
 }*/
 
 void MySensor::requestIsMetricSystem() {
-	sendRoute(msg.build(radioId, GATEWAY_ADDRESS, NODE_CHILD_ID, C_INTERNAL, I_UNIT));
+	sendRoute(msg.build(s.nodeId, GATEWAY_ADDRESS, NODE_CHILD_ID, C_INTERNAL, I_UNIT));
 }
 
 
@@ -356,9 +310,6 @@ boolean MySensor::process() {
 	uint8_t pipe;
 	boolean available = RF24::available(&pipe);
 
-	if (available) {
-		debug(PSTR("Message available on pipe %d\n"),pipe);
-	}
 
 	if (available && pipe<7) {
 		uint8_t len = RF24::getDynamicPayloadSize();
@@ -370,8 +321,8 @@ boolean MySensor::process() {
 
 		// Make sure string gets terminated ok for full string messages.
 		msg.data[len - HEADER_SIZE ] = '\0';
-		debug(PSTR("Rx: fr=%d,to=%d,la=%d,ci=%d,mt=%d,t=%d,cr=%d(%s): %s\n"),
-					msg.getSender(),msg.getDestination(), msg.getLast(), msg.getSensor(), msg.getCommand(), msg.getType(), msg.getCRC(), valid==0?"ok":valid==1?"ec":"ev", msg.getString());
+		debug(PSTR("rx: %d-%d-%d s=%d,c=%d,t=%d,cr=%s: %s\n"),
+					msg.getSender(), msg.getLast(), msg.getDestination(),  msg.getSensor(), msg.getCommand(), msg.getType(), valid==0?"ok":valid==1?"ec":"ev", msg.getString());
 
 		if (ok) {
 			uint8_t command = msg.getCommand();
@@ -380,68 +331,85 @@ boolean MySensor::process() {
 			uint8_t last = msg.getLast();
 			uint8_t destination = msg.getDestination();
 
-			if (relayMode && command == C_INTERNAL && type == I_PING) {
-				// Relays should always answer ping messages
+			if (relayMode && command == C_INTERNAL && type == I_PING && s.nodeId != AUTO) {
+				// Relaying nodes should always answer ping messages
 				// Wait a random delay of 0-2 seconds to minimize collision
 				// between ping ack messages from other relaying nodes
 				randomSeed(millis());
 				delay(random(2000));
 				uint8_t replyTo = msg.getSender();
-				msg.build(radioId, replyTo, NODE_CHILD_ID, C_INTERNAL, I_PING_ACK).set(distance);
+				msg.build(s.nodeId, replyTo, NODE_CHILD_ID, C_INTERNAL, I_PING_ACK).set(s.distance);
 				sendWrite(replyTo, msg);
-			} else if (sender == radioId) {
+				return false;
+			} else if (destination == s.nodeId) {
 				// This message is addressed to this node
-				if (relayMode && command == C_INTERNAL) {
-					// Handle all internal messages to this node
-					if (type == I_RELAY_NODE && destination != GATEWAY_ADDRESS) {
-						// Someone at sensor net gateway side wants this node to refresh its relay node
-						findRelay();
-						// Send relay information back to sensor net gateway node device.
-						sendInternal(I_RELAY_NODE, ltoa(relayId, convBuffer, 10));
-						return false;
-					} else if (type == I_CHILDREN && destination != GATEWAY_ADDRESS) {
-						debug(PSTR("Route command received"));
-						if (msg.getByte() == 'F') {
-							// Send in as many children we can fit in a message (binary)
-							sendChildren();
-						} else if (msg.getByte() == 'C') {
-							// Clears child relay data for this node
-							clearChildRoutes();
+
+				if (relayMode && last != s.parentNodeId) {
+					// Message is from one of the child nodes. Add it to routing table.
+					addChildRoute(sender, last);
+				}
+
+				if (command == C_INTERNAL) {
+					if (type == I_PING_ACK) {
+						uint8_t distance = msg.getByte();
+						if (distance<s.distance-1) {
+							// Found a neighbor closer to GW than previously found
+							s.distance = distance + 1;
+							s.parentNodeId = msg.getSender();
+							eeprom_write_byte((uint8_t*)EEPROM_PARENT_NODE_ID_ADDRESS, s.parentNodeId);
+							eeprom_write_byte((uint8_t*)EEPROM_DISTANCE_ADDRESS, s.distance);
+							debug(PSTR("p=%d, d=%d\n"), s.parentNodeId, s.distance);
+						}
+					} else if (type == I_REQUEST_ID && sender == GATEWAY_ADDRESS) {
+						if (s.nodeId == AUTO) {
+							s.nodeId = msg.getByte();
+							// Write id to EEPROM
+							if (s.nodeId == AUTO) {
+								// sensor net gateway will return max id if all sensor id are taken
+								debug(PSTR("full or no reply\n"));
+								while (1); // Wait here. Nothing else we can do...
+							} else {
+								eeprom_write_byte((uint8_t*)EEPROM_NODE_ID_ADDRESS, s.nodeId);
+							}
+							debug(PSTR("new node=%d\n"), s.nodeId);
 						}
 						return false;
+					} else if (relayMode && sender == GATEWAY_ADDRESS) {
+						if (type == I_RELAY_NODE && destination != GATEWAY_ADDRESS) {
+							// Someone at sensor net gateway side wants this node to refresh its relay node
+							findParentNode();
+							// Send relay information back to sensor net gateway node device.
+							sendInternal(I_RELAY_NODE, ltoa(s.parentNodeId, convBuffer, 10));
+							return false;
+						} else if (type == I_CHILDREN) {
+							if (msg.getByte() == 'F') {
+								// Send in as many children we can fit in a message (binary)
+								sendChildren();
+							} else if (msg.getByte() == 'C') {
+								// Clears child relay data for this node
+								clearChildRoutes();
+							}
+							return false;
+						}
 					}
-					// Return rest of internal messages back to sketch...
-					if (last != GATEWAY_ADDRESS)
-						addChildRoute(sender, last);
-
-					return true;
-				} else {
-					// If this is variable message from sensor net gateway. Send ack back.
-					debug(PSTR("Message addressed for this node.\n"));
-
-					// Send set-message back to sender if sender wants this
-					if (command == C_SET_WITH_ACK) {
-						sendAck(msg);
-						// The library user should not need to care about this ack request. Just treat it as a normal SET.
-						msg.setCommand(C_SET_VARIABLE);
-					}
-					// Call incoming message callback if available
-					if (msgCallback != NULL) {
-						msgCallback(msg);
-					}
-
-					if (last != GATEWAY_ADDRESS)
-						addChildRoute(sender, last);
-
-					// Return true if message was addressed for this node...
-					return true;
 				}
+				// If this is variable message from sensor net gateway. Send ack back.
+				if (command == C_SET_WITH_ACK) {
+					sendAck(msg);
+					// The library user should not need to care about this ack request. Just treat it as a normal SET.
+					msg.setCommand(C_SET_VARIABLE);
+				}
+				// Call incoming message callback if available
+				if (msgCallback != NULL) {
+					msgCallback(msg);
+				}
+				// Return true if message was addressed for this node...
+				return true;
 			} else if (relayMode) {
 				// We should try to relay this message to another node
 
 				uint8_t route = getChildRoute(msg.getDestination());
 				if (route>0 && route<255) {
-					debug(PSTR("Routing message to child node.\n"));
 					// This message should be forwarded to a child node. If we send message
 					// to this nodes pipe then all children will receive it because the are
 					// all listening to this nodes pipe.
@@ -466,9 +434,8 @@ boolean MySensor::process() {
 					//
 					// Message should be passed to node A (this nodes relay)
 
-					debug(PSTR("Routing message to relay.\n"));
 					// This message should be routed back towards sensor net gateway
-					sendWrite(relayId, msg);
+					sendWrite(s.parentNodeId, msg);
 					// Add this child to our "routing table" if it not already exist
 					addChildRoute(sender, last);
 				} else {
@@ -487,14 +454,17 @@ boolean MySensor::process() {
 					// This can happen if a node never has sent any message to
 					// sensor net gateway (which will build up routing information in all nodes
 					// that gets passed).
-					if (radioId == GATEWAY_ADDRESS) {
+
+					// THIS SHOULD NOT HAPPEN ANY MORE
+
+					/*if (s.nodeId == GATEWAY_ADDRESS) {
 						// We should perhaps inform gateway about the no-route situation?!
-						debug(PSTR("Unknown route from GW\n"));
+						debug(PSTR("ro=?\n"));
 
 					} else {
-						debug(PSTR("Remove child node from routing table.\n"));
+						//debug(PSTR("Remove child node from routing table.\n"));
 						removeChildRoute(sender);
-					}
+					}*/
 				}
 
 			}
@@ -570,42 +540,42 @@ uint8_t MySensor::validate(MyMessage message) {
 
 
 void MySensor::addChildRoute(uint8_t childId, uint8_t route) {
-	if (childNodeTable[childId] != route) {
-		childNodeTable[childId] = route;
-		EEPROM.write(EEPROM_ROUTES_ADDRESS+childId, route);
+	if (s.childNodeTable[childId] != route) {
+		s.childNodeTable[childId] = route;
+		eeprom_write_byte((uint8_t*)EEPROM_ROUTES_ADDRESS+childId, route);
 	}
 }
 
 void MySensor::removeChildRoute(uint8_t childId) {
-	if (childNodeTable[childId] != 0xff) {
-		childNodeTable[childId] = 0xff;
-		EEPROM.write(EEPROM_ROUTES_ADDRESS+childId, 0xff);
+	if (s.childNodeTable[childId] != 0xff) {
+		s.childNodeTable[childId] = 0xff;
+		eeprom_write_byte((uint8_t*)EEPROM_ROUTES_ADDRESS+childId, 0xff);
 	}
 }
 
 uint8_t MySensor::getChildRoute(uint8_t childId) {
-	return childNodeTable[childId];
+	return s.childNodeTable[childId];
 }
 
 void MySensor::clearChildRoutes() {
-	debug(PSTR("Clear child routing data\n"));
-	for (unsigned int i=0;i< sizeof(childNodeTable); i++) {
+	debug(PSTR("rd=clear\n"));
+	for (unsigned int i=0;i< sizeof(s.childNodeTable); i++) {
 		removeChildRoute(i);
 	}
 	sendInternal(I_CHILDREN, "");
 }
 
 void MySensor::sendChildren() {
-	// Send in which children that is using this node as an relay.
+	// Send in which children that is using this node as parent.
 	// TODO: Fix this
-	debug(PSTR("Dump beginning of routing table\n"));
+	debug(PSTR("rd=dump\n"));
 
 	for (int i=0;i< 50; i++) {
-//		Serial.println(childNodeTable[i]);
-		debug(PSTR("rt:%d, %d\n"), i, getChildRoute(i) );
+//		Serial.println(s.childNodeTable[i]);
+		debug(PSTR("rd=%d, %d\n"), i, getChildRoute(i) );
 	}
 
-	sendInternal(I_CHILDREN, "not implemented");
+	sendInternal(I_CHILDREN, "nit impl");
 }
 
 
@@ -615,7 +585,7 @@ void MySensor::sendChildren() {
 #ifdef DEBUG
 void MySensor::debugPrint(const char *fmt, ... ) {
 	char fmtBuffer[300];
-	if (radioId == GATEWAY_ADDRESS) {
+	if (s.nodeId == GATEWAY_ADDRESS) {
 		// prepend debug message to be handled correctly by gw
 		Serial.print("0;0;4;11;");
 	}
@@ -623,7 +593,7 @@ void MySensor::debugPrint(const char *fmt, ... ) {
 	va_start (args, fmt );
 	vsnprintf_P(fmtBuffer, 300, fmt, args);
 	va_end (args);
-	if (radioId == GATEWAY_ADDRESS) {
+	if (s.nodeId == GATEWAY_ADDRESS) {
 		// Truncate message if this is gateway node
 		vsnprintf_P(fmtBuffer, 60, fmt, args);
 		fmtBuffer[59] = '\n';
@@ -633,7 +603,7 @@ void MySensor::debugPrint(const char *fmt, ... ) {
 	}
 	va_end (args);
 	Serial.print(fmtBuffer);
-
+	Serial.flush();
 
 	//Serial.print("0;0;4;11;Mem free:");
 	//Serial.println(freeRam());
diff --git a/libraries/MySensors/MySensor.h b/libraries/MySensors/MySensor.h
index 1cc3537..f4090a1 100644
--- a/libraries/MySensors/MySensor.h
+++ b/libraries/MySensors/MySensor.h
@@ -21,7 +21,7 @@
 #include <Arduino.h>
 #include <stddef.h>
 #include <SPI.h>
-#include <EEPROM.h>
+#include <avr/eeprom.h>
 #include <avr/pgmspace.h>
 #include <stdarg.h>
 
@@ -33,16 +33,18 @@
 
 #define BAUD_RATE 115200
 
-#define AUTO 0xFF // 0-254. Id 255 is reserved for auto initialization of radioId.
+#define AUTO 0xFF // 0-254. Id 255 is reserved for auto initialization of nodeId.
 #define NODE_CHILD_ID 0xFF // Node child id is always created for when a new sensor is detected
-#define EEPROM_RADIO_ID_ADDRESS 0 // Where to store radio id in EEPROM
-#define EEPROM_RELAY_ID_ADDRESS 1 // Where to store relay id in EEPROM
-#define EEPROM_DISTANCE_ADDRESS 2 // Where to store distance to gateway in EEPROM
-#define EEPROM_ROUTES_ADDRESS ((uint8_t)3) // Where to start storing routing information in EEPROM. Will allocate 256 bytes.
+
+#define EEPROM_START 0
+#define EEPROM_NODE_ID_ADDRESS EEPROM_START // Where to store radio id in EEPROM
+#define EEPROM_PARENT_NODE_ID_ADDRESS (EEPROM_START+1) // Where to store relay id in EEPROM
+#define EEPROM_DISTANCE_ADDRESS (EEPROM_PARENT_NODE_ID_ADDRESS+1) // Where to store distance to gateway in EEPROM
+#define EEPROM_ROUTES_ADDRESS (EEPROM_DISTANCE_ADDRESS+1) // Where to start storing routing information in EEPROM. Will allocate 256 bytes.
 
 
-// This is the radioId for sensor net gateway receiver sketch (where all sensors should send their data).
-// This is also act as base value for sensor radioId
+// This is the nodeId for sensor net gateway receiver sketch (where all sensors should send their data).
+// This is also act as base value for sensor nodeId
 #define BASE_RADIO_ID ((uint64_t)0xA8A8E1FC00LL)
 #define GATEWAY_ADDRESS ((uint8_t)0)
 #define BROADCAST_ADDRESS ((uint8_t)0xFF)
@@ -53,8 +55,7 @@
 #define BROADCAST_PIPE ((uint8_t)2)
 
 #define SEND_RETRIES 3
-#define FIND_RELAY_RETRIES 2
-#define FIND_RELAY_AFTER_FAILED_TRANSMISSIONS 20
+#define FIND_RELAY_AFTER_FAILED_TRANSMISSIONS 5
 
 
 class MySensor : public RF24
@@ -76,27 +77,27 @@ class MySensor : public RF24
 	* Call this in setup(), before calling any other sensor net library methods.
 	* @param incomingMessageCallback Callback function for incoming messages. Default is NULL
 	* @param relayMode Activate relay mode. This node will forward messages to other nodes in the radio network. Make sure to call process() regularly. Default in false
-	* @param radioId The unique id (1-254) for this sensor. Default is AUTO(255) which means sensor tries to fetch an id from controller.
+	* @param nodeId The unique id (1-254) for this sensor. Default is AUTO(255) which means sensor tries to fetch an id from controller.
 	* @param paLevel Radio PA Level for this sensor. Default RF24_PA_MAX
 	* @param channel Radio channel. Default is channel 76
 	* @param dataRate Radio transmission speed. Default RF24_1MBPS
 	*/
 
-	void begin(void (* msgCallback)(MyMessage)=NULL, boolean relayMode=false, uint8_t radioId=AUTO, rf24_pa_dbm_e paLevel=RF24_PA_LEVEL, uint8_t channel=RF24_CHANNEL, rf24_datarate_e dataRate=RF24_DATARATE);
+	void begin(void (* msgCallback)(MyMessage)=NULL, boolean relayMode=false, uint8_t nodeId=AUTO, rf24_pa_dbm_e paLevel=RF24_PA_LEVEL, uint8_t channel=RF24_CHANNEL, rf24_datarate_e dataRate=RF24_DATARATE);
 
 	/**
-	 * Return the nodes radioId.
+	 * Return the nodes nodeId.
 	 */
-	uint8_t getRadioId();
+	uint8_t getNodeId();
 
 	/**
 	* Each node must present all attached sensors before any values can be handled correctly by the controller.
     * It is usually good to present all attached sensors after power-up in setup().
 	*
-	* @param sensorChildId Sensor child id for the presented. Choose anything between 0-254.
+	* @param sensorId Select a unique sensor id for this sensor. Choose a number between 0-254.
 	* @param sensorType The sensor type. See sensor typedef in Message.h.
 	*/
-	void present(uint8_t sensorChildId, uint8_t sensorType);
+	void present(uint8_t sensorId, uint8_t sensorType);
 
 	/**
 	 * Sends sketch meta information to the gateway. Not mandatory but a nice thing to do.
@@ -128,7 +129,7 @@ class MySensor : public RF24
 	*
 	* @param childSensorId  The unique child id for the different sensors connected to this arduino. 0-254.
 	* @param variableType The variableType to fetch
-	* @param destination The radioId of other node in radio network. Default is gateway
+	* @param destination The nodeId of other node in radio network. Default is gateway
 	* @param number of attempts to fetch
 	* @return Message response
 	*/
@@ -141,7 +142,7 @@ class MySensor : public RF24
 	* @param childSensorId  The unique child id for the different sensors connected to this arduino. 0-254.
 	* @param variableType The variableType to fetch
 	* @param callback for result. This will be called when a message reply is received or request fails (see status of second argument).
-	* @param destination The radioId of other node in radio network. Default is gateway
+	* @param destination The nodeId of other node in radio network. Default is gateway
 	*/
 	/*void (* msgCallback)(MyMessage, bool)=NULL, */
 	void request(uint8_t childSensorId, uint8_t variableType, uint8_t destination=GATEWAY_ADDRESS);
@@ -187,19 +188,22 @@ class MySensor : public RF24
 	int freeRam();
 #endif
 
-
   protected:
 	MyMessage msg;  // Buffer for incoming messages.
 	uint8_t failedTransmissions;
+	struct settings_t
+	{
+		uint8_t nodeId; // Current node id
+		uint8_t parentNodeId; // Where this node sends its messages
+		uint8_t distance; // This nodes distance to sensor net gateway (number of hops)
+		uint8_t *childNodeTable; // In memory buffer for routing to other nodes. also stored in EEPROM
+	} s;
 	boolean relayMode;
-	uint8_t radioId;
-	uint8_t distance; // This nodes distance to sensor net gateway (number of hops)
-	uint8_t relayId;
 	char convBuffer[20];
 
 	void setupRelayMode();
 	void setupRadio(rf24_pa_dbm_e paLevel, uint8_t channel, rf24_datarate_e dataRate);
-	void findRelay();
+	void findParentNode();
 	boolean sendRoute(MyMessage message);
 	boolean sendWrite(uint8_t dest, MyMessage message);
 	void sendInternal(uint8_t variableType, const char *value);
@@ -219,11 +223,12 @@ class MySensor : public RF24
 		uint8_t type;
 		void (*msgCallback)(MyMessage, bool);
 	};*/
+
+
 	MyMessage ack;  // Buffer for ack messages.
-	uint8_t *childNodeTable; // In memory buffer for routing to other nodes. Also stored in EEPROM
     void (*msgCallback)(MyMessage); // Callback for undefined incoming messages
 
-	void initializeRadioId();
+	void requestNodeId();
 	uint8_t crc8Message(MyMessage);
 	MyMessage wait(MyMessage msg, uint8_t expectedReceiveType, uint8_t retry);
 	MyMessage getInternal(uint8_t variableType, uint8_t retry=SEND_RETRIES);