1 files changed, 44 insertions, 46 deletions

diff --git a/src/device/main.ino b/src/device/main.ino
index fc307f9..49cb0e7 100644
--- a/src/device/main.ino
+++ b/src/device/main.ino
@@ -31,9 +31,16 @@
 #include "utils.h"
 #include "tlv.h"
 
+#define seconds(a) (((a) * 1000) / (period))
+
 static const unsigned int internal_led = 2;
 static unsigned int led_state = 0;
-static char *fugg = "O Fugg, ids an errror :DDDDD";
+static char fugg[] = "error :3ccc";
+static const char devstr[] = "Device IP:";
+static const char daemonstr[] = "Daemon IP:";
+static char ipstr[] = "                                ";
+static unsigned int period = 1000;
+
 SSD1306Wire display(0x3c, 4, 5, GEOMETRY_128_32);
 
 static void init_OLED(void);
@@ -47,9 +54,9 @@ static struct progstate_t {
 	struct display_status ds;
 	struct tlv field;
 	struct tlv_packet heartbeat;
-	size_t bytes_read;
 	char buf[MTU];
 	char hbcounter;
+	IPAddress *daemon_ip;
 } progstate;
 
 void setup(void)
@@ -61,93 +68,84 @@ void setup(void)
 	pinMode(internal_led, OUTPUT);
 	toggle_led(internal_led);
 	init_OLED();
+
 	display.fillCircle(32, 16, 12);
 	display.display();
+
 	wifi_connect(ssid, password, 1, internal_led);
 	udp_init(com_port);
+
 	display.fillCircle(64, 16, 12);
 	display.display();
-	discover_client(com_port);
+
+	progstate.daemon_ip = discover_client(com_port);
+
 	display.fillCircle(92, 16, 12);
 	display.display();
-	progstate.ip_print_count = 5;
+
 	tlv_init(&field, HEARTBEAT);
 	field.data = strdup(heartbeat_device);
 	field.head.size = sizeof(heartbeat_device);
+
 	tlv_packet_init(&progstate.heartbeat);
 	tlv_pack(&progstate.heartbeat, &field);
 	tlv_packet_finalize(&progstate.heartbeat);
 	tlv_destroy(&field);
-	display_status_init(&display, &progstate.ds, fugg);
+
+	progstate.ip_print_count = 5;
 }
 
 void loop(void)
 {
-	static const String devstr = "Device IP:";
-	static const String daemonstr = "Daemon IP:";
-	static String prefix;
-	static IPAddress ip_to_print;
-	static IPAddress *daemon_ip = NULL;
-#if 0
-	char *saveptr;
-#endif
-
-	static unsigned int delta = 2000; /* sleep length to use (ms) */
-
-	delay(delta);
+	static int displayed = END_OF_MESSAGE;
+	String current_ip;
+
+	delay(period);
+
 	if (!progstate.hbcounter) {
 		udp_init_packet(com_port);
 		udp_push(progstate.heartbeat.data, progstate.heartbeat.cursor + 1);
 		udp_flush();
-		progstate.hbcounter = 5;
+		progstate.hbcounter = seconds(30);
 	}
 
-	if (progstate.ip_print_count) {
-		/* Initial display of ip's. */
-		if (!daemon_ip) {
-			daemon_ip = get_daemon_address();
-		}
-
-		prefix = (progstate.ip_print_count % 2) ? devstr : daemonstr;
-		ip_to_print = (progstate.ip_print_count % 2) ? WiFi.localIP() : *daemon_ip;
-		display.clear();
-		display.drawString(0, 0, prefix);
-		display.drawString(0, 16, ip_to_print.toString());
-		display.display();
-		progstate.ip_print_count--;
-	} else {
-		progstate.bytes_read = udp_get_data(progstate.buf, sizeof(progstate.buf));
-		if (progstate.bytes_read > 0) {
+	/* Initial display of ips. */
+	if (!progstate.ip_print_count) {
+		if (udp_get_data(progstate.buf, sizeof(progstate.buf))) {
 			tlv_get(progstate.buf, &progstate.field, NULL);
 			handle_tlv(&progstate.field);
-#if 0
-			/* Dealing with tlv's one at a time. */
-			saveptr = progstate.buf;
-			while (!(tlv_get(saveptr, &progstate.field, &saveptr))) {
-				handle_tlv(&progstate.field);
-			}
-#endif
 		}
-
-		display_update_scroll(&progstate.ds);
+	} else if (displayed == END_OF_MESSAGE) {
+		current_ip = (progstate.ip_print_count % 2) ? WiFi.localIP().toString() : progstate.daemon_ip->toString();
+		memcpy(ipstr, (progstate.ip_print_count % 2) ? devstr : daemonstr, strlen(devstr));
+		memcpy(ipstr + 16, current_ip.c_str(), strlen(current_ip.c_str()));
+		display_status_init(&display, &progstate.ds, ipstr, strlen(ipstr), seconds(1));
+		progstate.ip_print_count--;
 	}
 
+	displayed = display_update_scroll(&progstate.ds);
 	--progstate.hbcounter;
 }
 
 void handle_tlv(const struct tlv *in)
 {
-	static char *fugg = "O Fugg, ids an errror :DDDDD";
 	/* Currently just dealing with text.
 	 * */
 	switch (in->head.type) {
 	case TEXT:
-		display_status_init(&display, &progstate.ds, in->data);
+		if (in->head.size) {
+			display_status_init(&display, &progstate.ds, in->data, in->head.size, seconds(3));
+		}
+	case INVALID:
+	case HEARTBEAT:
 		break;
 	default:
-		display_status_init(&display, &progstate.ds, fugg);
+		display_status_init(&display, &progstate.ds, fugg, sizeof(fugg), seconds(5));
 		break;
 	}
+
+	/* clear the tlv after handling it */
+	progstate.field = tlv_none;
 }
 
 static void init_OLED(void)