/* Sense/Net Bus Interface Library for Raspberry Pi.
 * Sensiplicity Systems
 * 
 *  Copyright 2015
 * 
 *  Department of Botany and Plant Pathology
 *  Center for Genome Research and Biocomputing
 *  Oregon State University
 *  Corvallis, OR 97331
 * 
 * This program is not free software; you can not redistribute it and/or
 * modify it at all.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. */
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <string.h>
#include <errno.h>
#include <poll.h>
#include <wiringPi.h>
#include "sense-net.h"

/* Serial port file descriptor. */
static int SerialFD = -1;

#define MAX_PACKET_SIZE 141
#define START_BYTE 0x01
#define ESCAPE_BYTE 0x1B
#define END_BYTE 0x03

/* Print an error message to stderr. */
#define ERR_PROGNAME "sn-util-rpi"
static void error_func(const char *prefix, int line, const char *fmt, ...){
	va_list args;
    va_start(args, fmt);
	fprintf(stderr, "%s %d: ", prefix, line);
	vfprintf(stderr, fmt, args);
	fprintf(stderr, "\n");
	va_end(args);
}
#define sn_error(...) error_func(ERR_PROGNAME ": error in "  __FILE__, __LINE__, __VA_ARGS__)
/* If the condition is true, print the error message and return value. */
#define fail(condition, return_value, message_fmt, ...) do {if(condition){sn_error(message_fmt, __VA_ARGS__); return return_value;}}while(0)

// /* Print an error message to stderr, appended with "sn-lib: error: ..."
//  * and ending with a newline. Arguments work the same way as printf. */
// static void sn_error(const char *format, ...){
// 	va_list args;
// 	fprintf(stderr, "sn-lib: error: ");
// 	va_start(args, format);
// 	vfprintf(stderr, format, args);
// 	fprintf(stderr, "\n");
// 	va_end(args);
// }

/* Bus control pin definitions.
 * These are WiringPi pin numbers, which are different from
 * the common rpi numbering scheme, for no goddamn reason. */
#define B1REn 0
#define B1TE 3
#define B2REn 4
#define B2TE 5
#define B3REn 12
#define B3TE 13

/* Set the RS-485 transceiver mode and bus. */
enum rxtx_mode_t {MODE_RX, MODE_TX, MODE_SHUTDOWN};
static void sn_mode(enum rxtx_mode_t mode, int bus){
	if(MODE_SHUTDOWN == mode){
		digitalWrite (B2REn, HIGH);
		digitalWrite (B2TE, LOW);
		digitalWrite (B3REn, HIGH);
		digitalWrite (B3TE, LOW);
		digitalWrite (B1REn, HIGH);
		digitalWrite (B1TE, LOW);
	} else if(1 == bus){
		digitalWrite (B2REn, HIGH);
		digitalWrite (B2TE, LOW);
		digitalWrite (B3REn, HIGH);
		digitalWrite (B3TE, LOW);
		if(MODE_RX == mode){
			digitalWrite (B1REn, LOW);
			digitalWrite (B1TE, LOW);
		} else {
			digitalWrite (B1REn, HIGH);
			digitalWrite (B1TE, HIGH);
		}
	} else if (2 == bus) {
		digitalWrite (B1REn, HIGH);
		digitalWrite (B1TE, LOW);
		digitalWrite (B3REn, HIGH);
		digitalWrite (B3TE, LOW);
		if(MODE_RX == mode){
			digitalWrite (B2REn, LOW);
			digitalWrite (B2TE, LOW);
		} else {
			digitalWrite (B2REn, HIGH);
			digitalWrite (B2TE, HIGH);
		}	
	} else if (3 == bus) {
		digitalWrite (B2REn, HIGH);
		digitalWrite (B2TE, LOW);
		digitalWrite (B1REn, HIGH);
		digitalWrite (B1TE, LOW);
		if(MODE_RX == mode){
			digitalWrite (B3REn, LOW);
			digitalWrite (B3TE, LOW);
		} else {
			digitalWrite (B3REn, HIGH);
			digitalWrite (B3TE, HIGH);
		}
	} else {
		fprintf(stderr, "sn_mode: internal error: invalid bus.\n");
	}
}

/* Calculate CRC of a Sense/Net packet. packet[0] is the header.
 * If this function is computed over a whole packet, including the
 * CRC at the end, a return value of 0 indicates a valid packet. */
uint16_t calc_crc(uint8_t *packet, int packet_length){
	uint16_t remainder = 0xFFFF;
	
	for (int i = 0; i < packet_length; i++){
		remainder ^= packet[i] << 8;
		for (int bit = 8; bit > 0; bit--){
			if (remainder & 0x8000){
				remainder = (remainder << 1) ^ 0x1021;
			} else {
				remainder = (remainder << 1);
			}
		}
	}
	return remainder;
}

/* Open the serial port on the raspberry pi bridge. 
 * Returns 0 on success, -1 on error. Errors messages are printed to stdout. */
int sn_init(){
	/* Setup bus transceiver control lines. */
	wiringPiSetup() ;
	pinMode (B1REn, OUTPUT) ;
	pinMode (B1TE, OUTPUT) ;
	pinMode (B2REn, OUTPUT) ;
	pinMode (B2TE, OUTPUT) ;
	pinMode (B3REn, OUTPUT) ;
	pinMode (B3TE, OUTPUT) ;
	
	/* Open serial port. */
	struct termios options; /* Serial port options */
	SerialFD = open("/dev/ttyAMA0", O_RDWR | O_NOCTTY | O_NONBLOCK);
	if(-1 == SerialFD){
		fprintf(stderr, "sn_init: could not open serial port");
		perror(NULL);
		return -1;
	}
	
	/* Setup the serial port to transmit at 19200 baud 8N1 */
	if(tcgetattr(SerialFD, &options)){
		fprintf(stderr, "sn_init: could not get serial port options");
		perror(NULL);
		close(SerialFD);
		SerialFD = -1;
		return -1;
	}
	cfsetospeed(&options, B19200);
	options.c_cflag &= ~CSIZE;
	options.c_cflag |= CS8;
	options.c_cflag &= ~PARENB;
	options.c_cflag &= ~CSTOPB;
	options.c_cflag |= CS8;
	options.c_oflag = 0;
	options.c_cflag |= CREAD;
	options.c_lflag = 0;
	options.c_iflag = IGNBRK;

	options.c_cc[VMIN] = 0; //Minimum number of characters for noncanonical read (MIN)
	options.c_cc[VTIME] = 0; //Timeout in deciseconds for noncanonical read (TIME).

	memcpy(&(options.c_cc), "\x03\x1c\x7f\x15\x04\x00\x00\x00\x11\x13\x1a\x00\x12\x0f\x17\x16\x00\x00\x00", sizeof(cc_t) * NCCS);
	if(tcsetattr(SerialFD, TCSANOW, &options)){
		fprintf(stderr, "sn_init: could not set serial port options");
		perror(NULL);
		close(SerialFD);
		SerialFD = -1;
		return -1;
	}
	return 0;
}

/* Get sensor data from a node.
 * The structure of the data is defined by the node type.
 *    bus is the bus the node is located on.
 *    *node is the node to get data from.
 *    *data will be filled with the data from the node. It should be able to contain
 *          128 bytes.
 *    *size will be set to the number of bytes in data.
 * If an error occurs, *data is not set and *size is set to 0.
 * Returns 0 on success, -1 on bus error, -2 on node error. */
int sn_get_data(int bus, node_t node, char *data, int *size){
	int status;
	int r;
	
	r = sn_send_packet(bus, 0, node, NULL, 0, &status, (char *) data, size);
	
	if(0 == r)return -1;
	if(1 == r)return 0;
	return r;
}

/* Send a packet to a node. If the node replies with a packet, it will
 * be placed into *reply_data.
 *   bus is the bus the node is located on.
 *   cmd is the header command number. Allowed range is 0-7.
 *   *node is the node to send the packet to.
 *   *data is the data to be sent.
 *   data_size is the size of the data to be sent (max. 128).
 *   *status will be set to the status code received from the packet, if any.
 *   *reply_data will be filled with data from the reply packet. It must
 *               be able to hold 128 bytes.
 *   *reply_size will be set to the size of the reply.
 * *status, *reply_data, and *reply size may be NULL. If *reply_size is NULL,
 * *reply_data will not be set. *data may be null if data_size is 0.
 * Returns 0 on success (no reply), 1 on success (reply), 
 * -1 on I/O error, -2 on corrupted reply */
int sn_send_packet(int bus, int cmd, node_t node, const char *data, int data_size, int *status, char *reply_data, int *reply_size){
	if(NULL == node || (NULL == data && data_size != 0)
	   || data_size < 0 || data_size > 128){
		return 0;
	}
	
	/* Construct the packet. */
	uint8_t packet[284];
	packet[0] = 0xA0 | (cmd & 0x07);
	memcpy(packet + 1, node, 9);
	packet[10] = data_size;
	if(NULL != data)memcpy(packet + 11, data, data_size);
	uint16_t packet_crc = calc_crc((uint8_t *)packet, 11 + data_size);
	packet[11 + data_size] = packet_crc & 0x00FF;
	packet[12 + data_size] = packet_crc >> 8;
	
	/* Send the packet. */
	//tcflush(SerialFD, TCIOFLUSH); /* Flush buffer. */
	sn_mode(MODE_TX, bus);
	usleep(50);
	
	uint8_t *escaped_buf = malloc(3*MAX_PACKET_SIZE); /* More than large enough. */
	if(NULL == escaped_buf){
		fprintf(stderr, "sn-lib: malloc failure trying to get %d bytes", 3*MAX_PACKET_SIZE);
		return -1;
	}
	escaped_buf[0] = START_BYTE;
	int d,s;
	for(s=0,d=1; s < 13 + data_size; s++,d++){
		if(packet[s] == START_BYTE || packet[s] == ESCAPE_BYTE || packet[s] == END_BYTE){
			escaped_buf[d] = ESCAPE_BYTE;
			d++;
		}
		escaped_buf[d] = packet[s];
	}
	escaped_buf[d] = END_BYTE;
	d++;
	
	int r = write(SerialFD, escaped_buf, d);
	free(escaped_buf);
	if(r != d){
		perror("sn-lib");
		return -1;
	}
	
	/* Listen for a reply. */
	usleep(300 + 525  * d);
	sn_mode(MODE_RX, bus);
	//tcflush(SerialFD, TCIOFLUSH); /* Flush buffer. */
	
	usleep(80000); /* This isn't ideal, but it'll do for a first attempt. */
	int reply_packet_size = read(SerialFD, packet, 284);
	if(reply_packet_size < 0){
		if(errno == EAGAIN || errno == EWOULDBLOCK){
			return 0;
		} else {
			perror("sn-lib");
			return -1;
		}
	}
	
	if(0 == reply_packet_size){
		return 0;
	}
	
	/* Iterate through the packet, collapsing backward to remove special characters. */
	int escaped = 0;
	for(int i=0; i<reply_packet_size; i++){
		if(!escaped){
			if(packet[i] == START_BYTE){
				/* Collapse */
				memmove(&(packet[i]), &(packet[i+1]), reply_packet_size - i - 1);
				reply_packet_size--;
				i--;
			} else if(packet[i] == ESCAPE_BYTE){
				/* Collapse */
				memmove(&(packet[i]), &(packet[i+1]), reply_packet_size - i - 1);
				reply_packet_size--;
				i--;
				escaped = 1;
			} else if(packet[i] == END_BYTE){
				reply_packet_size = i;
				break;
			}
		} else {
			escaped = 0;
		}
	} 
	
	uint16_t reply_crc;
	reply_crc = (packet[reply_packet_size - 1] << 8) + packet[reply_packet_size - 2];
	
	if(reply_crc == calc_crc((uint8_t *) packet, reply_packet_size - 2)){ /* Valid packet. */
		if(reply_packet_size < 13){
			return -2; /* Too small. */
		}
		if(reply_size != NULL){
			*reply_size = reply_packet_size - 13;
			if(reply_data != NULL){
				memcpy(reply_data, packet + 11, *reply_size);
			}	
		}
		if(status != NULL){
			*status = packet[0] & 0x0F;
		}
		return 1;
	} else {
		sn_error("Invalid CRC");
		return -2;
	}
	/* We should never reach this point. */
 	return -1;
}

int sn_send_packet_setnode(int bus, int cmd, node_t node, const char *data, int data_size, int *status, char *reply_data, int *reply_size){
	if(NULL == node || (NULL == data && data_size != 0)
	   || data_size < 0 || data_size > 128){
		return 0;
	}
	
	/* Construct the packet. */
	uint8_t packet[284];
	packet[0] = 0xA0 | (cmd & 0x07);
	memcpy(packet + 1, node, 9);
	packet[10] = data_size;
	if(NULL != data)memcpy(packet + 11, data, data_size);
	uint16_t packet_crc = calc_crc((uint8_t *)packet, 11 + data_size);
	packet[11 + data_size] = packet_crc & 0x00FF;
	packet[12 + data_size] = packet_crc >> 8;
	
	/* Send the packet. */
	//tcflush(SerialFD, TCIOFLUSH); /* Flush buffer. */
	sn_mode(MODE_TX, bus);
	usleep(50);
	
	uint8_t *escaped_buf = malloc(3*MAX_PACKET_SIZE); /* More than large enough. */
	if(NULL == escaped_buf){
		fprintf(stderr, "sn-lib: malloc failure trying to get %d bytes", 3*MAX_PACKET_SIZE);
		return -1;
	}
	escaped_buf[0] = START_BYTE;
	int d,s;
	for(s=0,d=1; s < 13 + data_size; s++,d++){
		if(packet[s] == START_BYTE || packet[s] == ESCAPE_BYTE || packet[s] == END_BYTE){
			escaped_buf[d] = ESCAPE_BYTE;
			d++;
		}
		escaped_buf[d] = packet[s];
	}
	escaped_buf[d] = END_BYTE;
	d++;
				
	int r = write(SerialFD, escaped_buf, d);
	free(escaped_buf);
	if(r != d){
		perror("sn-lib");
		return -1;
	}
	
	/* Listen for a reply. */
	usleep(1000 + 521  * (13 + data_size));
	sn_mode(MODE_RX, bus);
	//tcflush(SerialFD, TCIOFLUSH); /* Flush buffer. */
	
	usleep(80000); /* This isn't ideal, but it'll do for a first attempt. */
	int reply_packet_size = read(SerialFD, packet, 284);
	if(reply_packet_size < 0){
		if(errno == EAGAIN || errno == EWOULDBLOCK){
			return 0;
		} else {
			perror("sn-lib");
			return -1;
		}
	}
	
	if(0 == reply_packet_size){
		return 0;
	}
	
	/* Iterate through the packet, collapsing backward to remove special characters. */
	int escaped = 0;
	for(int i=0; i<reply_packet_size; i++){
		if(!escaped){
			if(packet[i] == START_BYTE){
				/* Collapse */
				memmove(&(packet[i]), &(packet[i+1]), reply_packet_size - i - 1);
				reply_packet_size--;
				i--;
			} else if(packet[i] == ESCAPE_BYTE){
				/* Collapse */
				memmove(&(packet[i]), &(packet[i+1]), reply_packet_size - i - 1);
				reply_packet_size--;
				i--;
				escaped = 1;
			} else if(packet[i] == END_BYTE){
				reply_packet_size = i;
				break;
			}
		} else {
			escaped = 0;
		}
	}
	
	uint16_t reply_crc;
	reply_crc = (packet[reply_packet_size - 1] << 8) + packet[reply_packet_size - 2];
	
	if(reply_crc == calc_crc((uint8_t *) packet, reply_packet_size - 2)){ /* Valid packet. */
		if(reply_packet_size < 13){
			return -2; /* Too small. */
		}
		if(reply_size != NULL){
			*reply_size = reply_packet_size - 13;
			if(reply_data != NULL){
				memcpy(reply_data, packet + 11, *reply_size);
			}
			memcpy(node, packet + 1, 9);
		}
		if(status != NULL){
			*status = packet[0] & 0x0F;
		}
		return 1;
	} else {
		sn_error("Invalid CRC");
		return -2;
	}
	/* We should never reach this point. */
 	return -1;
}

/* Send a packet to a node. If the node replies with a packet, it will
 * be placed into *reply_data. If a reply is not received, the request will be re-tried up to
 * two more times.
 *   bus is the bus the node is located on.
 *   cmd is the header command number. Allowed range is 0-7.
 *   *node is the node to send the packet to.
 *   *data is the data to be sent.
 *   data_size is the size of the data to be sent (max. 128).
 *   *status will be set to the status code received from the packet, if any.
 *   *reply_data will be filled with data from the reply packet. It must
 *               be able to hold 128 bytes.
 *   *reply_size will be set to the size of the reply.
 * *status, *reply_data, and *reply size may be NULL. If *reply_size is NULL,
 * *reply_data will not be set. *data may be null if data_size is 0.
 * Returns 0 on success (no reply), 1 on success (reply), 
 * -1 on I/O error, -2 on corrupted reply */
int sn_send_packet_r(int bus, int cmd, node_t node, const char *data, int data_size, int *status, char *reply_data, int *reply_size){
	int r = sn_send_packet(bus, cmd, node, data, data_size, status, reply_data, reply_size);
	if(r == 0 || r == -2){
		r = sn_send_packet(bus, cmd, node, data, data_size, status, reply_data, reply_size);
		if(r == 0 || r == -2){
			r = sn_send_packet(bus, cmd, node, data, data_size, status, reply_data, reply_size);
		}
		return r;
	} else {
		return r;
	}
}

/* Enumeration reply states. */
enum {ENUM_NO_NODE, ENUM_ONE, ENUM_ZERO, ENUM_CONFLICT} reply_state_t;

/* Send a single enumeration packet.
 * The reply state received from the nodes is returned.
 * Address should be a nine-byte array.
 * sig_bits ranges from 0-71. */
static enum reply_state_t send_single_enum(int bus, const uint8_t *address, uint8_t sig_bits);

/* Send an enumeration packet, retrying several times if no reply is received. */
static enum reply_state_t send_enum(int bus, const uint8_t *address, uint8_t sig_bits);

/* Receive a fast enumeration packet from a fully-qualified address.
 * Address should be a nine-byte array.
 * On success, the typecode is written to typecode.
 * Returns 0 on success, -1 on error. */
static int get_typecode(int bus, const uint8_t *address, uint16_t *typecode);

/* Perform an enumeration command and return the result.
 *   bus   is the bus to send the query on
 *   bits  is the number of significant bits in the address,
 *         from LSB of the first byte to MSB of the last byte.
 *         bits must be less than 72.
 *   address  is a 9-byte value containing significant address bits.
 *            Non-significant bits can be any value.
 *  Returns 0 if the next bit is 0, 1 if the next bit is 1,
 *  2 if there's a conflict, or -1 on no reply or error. */
static int enum_query(int bus, const uint8_t *address, uint8_t bits){
	fail(bus < 1 || bus > 3, -1, "invalid bus value %d", bus);
 	fail(address == NULL, -1, "NULL address.");
 	fail(bits > 71, -1, "bits = %d, which is greater than 71.", bits);
	
	/* TODO */
	
	
}

/* Simple stack data structure. */
typedef struct sstack_t {
	size_t esize; /* Size of each item. */
	int capacity; /* Number of items that can be stored. */
	int size; /* Number of items currently in stack. */
	uint8_t *data; /* Data for items. */
} sstack_t;

/* Create a new stack. When done, the returned pointer should be free'd. */
static sstack_t *sstack_new(size_t esize, int capacity){
	sstack_t *s = malloc(sizeof(sstack_t) + esize * capacity);
	if(NULL == s)return NULL;
	s->esize = esize;
	s->capacity = capacity;
	s->size = 0;
	s->data = ((uint8_t *)s) + sizeof(sstack_t);
	return s;
}

/* Push an item onto the stack. The item will be copied into the stack.
 * Returns 0 on success, -1 on error (overflow). */
static int sstack_push(sstack_t *stack, void *item){
	if(stack == NULL || item == NULL)return -1;
	if(stack->size >= stack->capacity)return -1;
	memmove(stack->data + stack->size * stack->esize, item, stack->esize);
	stack->size++;
	return 0;
}

/* Pop an item from the stack, copying it into the provided buffer.
 * Returns 0 on success, -1 on error (underflow). 
 * On error, the buffer is not modified. */
static int sstack_pop(sstack_t *stack, void *item){
	if(stack == NULL || item == NULL)return -1;
	if(stack->size == 0)return -1;
	memmove(item, stack->data + (stack->size-1) * stack->esize, stack->esize);
	stack->size--;
	return 0;
}

typedef struct query_t {
	uint8_t addr[9];
	uint8_t bits;
} query_t;

/* Set a given bit in a query struct address to a given value.
 * The number of bits will be updated if the given bit
 * is beyond the current number of significant bits.
 *   bit should range from 0-71.
 *   value should be 1 or 0. */
static void set_query_bit(query_t *q, int bitnum, int value){
	/* The bit numbering counts from LSB to MSB within each byte
	 * and first to last byte within the address. */
	int byte = bit / 8;
	int bbit %= 8;
	if(bitnum + 1 > q->bits){
		q->bits = bitnum + 1;
	}
	if(value){
		/* Set to 1. */
		q->addr[byte] |= (1 << bbit);
	} else {
		/* Set to 0. */	
		q->addr[byte] &= ~(1 << bbit);
	}
}

/* Discover the nodes on the bus.
 *   bus is the bus the node is located on.
 *   **nodes will be set to point to an array of node_enum_ts.
 *           Free() this array when you are done with it.
 *    *num_nodes will be set to the number of node IDs in the array.
 * Returns 0 on success, -1 on error (nodes and num_nodes will not be set). */
int new_enumerate(int bus, node_enum_t **nodes, int *num_nodes){
	/* Push a 0-bit query onto the query stack,
	 * typecode stack is empty.
	 * while query stack is not empty:
	 *   perform query
	 *     if complete, push result(s) onto typecode stack
	 *     else push onto query stack
	 * when done, return typecode stack */
	sstack_t *qs, *ts;
	const int maxnodes = 500;
	qs = sstack_new(sizeof(query_t), 2*maxnodes);
	if(NULL == qs){
		sn_error("malloc failure");
		return -1;
	}
	ts = sstack_new(sizeof(node_enum_t), maxnodes);
	if(NULL == ts){
		sn_error("malloc failure");
		free(qs);
		return -1;
	}
	
	query_t q, qn;
	node_enum_t t;
	q.bits = 0;
	sstack.push(qs, &q);
	while(0 == sstack_pop(qs, &q)){
		if(q.bits < 71){
			int r = enum_query(bus, q.addr, q.bits);
			if(r == 0 || r == 2){
				memcpy(qn, q, sizeof(query_t));
				set_query_bit(&qn, qn.bits, 0);
				if(sstack_push(qs, &qn)){
					sn_error("stack push failure; too many nodes?");
					free(qs);
					free(ts);
				}
			} else if(r == 1 || r == 2){
				memcpy(qn, q, sizeof(query_t));
				set_query_bit(&qn, qn.bits, 1);
				if(sstack_push(qs, &qn)){
					sn_error("stack push failure; too many nodes?");
					free(qs);
					free(ts);
				}
			}
		} else {
			/* Almost complete.
			 * Do a complete enum query on both remaining bits. */
			/* Set bit one, copy to t, do query, if successful push to ts. */
			/* Set bit zero, copy to t, do query, if successful push to ts. */
		}
	}
	
	node_enum_t *n = malloc(sizeof(node_enum_t) * ts->size);
	if(NULL = n){
		sn_error("malloc failure");
		free(qs);
		free(ts);
		return -1;
	}
	int i=0;
	while(0 == sstack_pop(ts, &t)){
		memcpy(n + i, t)
		n[i].bus = bus;
		i++;
	}
	*nodes = n;
	*num_nodes = ts->size;
	free(qs);
	free(ts);
	return 0;
}

/* Discover the nodes on the bus.
 *   bus is the bus the node is located on.
 *   **nodes will be set to point to an array of node_enum_ts.
 *           Free() this array when you are done with it.
 *    *num_nodes will be set to the number of node IDs in the array. */
void sn_enumerate(int bus, node_enum_t **nodes, int *num_nodes){
	if(NULL == nodes || NULL == num_nodes)return;
	
	/* Construct the packet. */
	uint8_t packet[141];
	packet[0] = 0xA1;
	packet[10] = 0;
	uint16_t packet_crc = calc_crc((uint8_t *)packet, 11);
	packet[11] = packet_crc & 0x00FF;
	packet[12] = packet_crc >> 8;
	
	/* Send the packet. */
	//tcflush(SerialFD, TCIOFLUSH); /* Flush buffer. */
	sn_mode(MODE_TX, bus);
	int r = write(SerialFD, packet, 13);
	if(r != 13){
		perror("sn-lib");
		*nodes = NULL;
		*num_nodes = 0;
		return;
	}
	
	/* Listen for a reply. */
	usleep(1000 + 521  * 13);
	sn_mode(MODE_RX, bus);
	//tcflush(SerialFD, TCIOFLUSH); /* Flush buffer. */
	
	uint8_t *buf = malloc(1000000); /* Allocate more space than will ever be necessary. */
	int buf_size = 0;
	
	struct pollfd pfd;
	pfd.fd = SerialFD;
	pfd.events = POLLIN;
	
	
	struct timeval curtime, endtime;
	gettimeofday(&curtime, NULL);
	endtime = curtime;
	endtime.tv_sec += 5;
	
	while(1){
		gettimeofday(&curtime, NULL);
		poll(&pfd, 1, (endtime.tv_sec - curtime.tv_sec) * 1000);
		if(pfd.revents == POLLIN){
			if(buf_size < 999990){
				r = read(SerialFD, buf + buf_size, 10);
			}
			if(r > 0){
				buf_size += r;
			} else if(r < 0){
				perror("sn-util: error:");
				free(buf);
				return;
			}
		} else {
			break;
		}
	}
	
	/* As we don't have timing information, we'll have to go through the buffer byte-by-byte
	 * to find the reply packets. */
	int i = 0;
	uint16_t reply_crc;
	
	int list_size = 0; /* Number of nodes in list. */
	int list_cap = 100; /* List capacity. */
	node_enum_t *list = malloc(sizeof(node_enum_t) * list_cap);
	if(NULL == list){
		fprintf(stderr, "sn-util: error: malloc failure when requesting %zd bytes.", sizeof(node_enum_t) * list_cap);
		free(buf);
		*nodes = NULL;
		*num_nodes = 0;
	}
	
	while(i <= buf_size - 15){
		if(buf[i] == 0xD1){
			/* This looks like the header of a reply packet. */
			reply_crc = calc_crc(buf + i, 13);
			if(0 == memcmp(&reply_crc, buf + i + 13, 2)){
				/* This is a valid reply packet. */
				
				/* Add this node's ID and type code to the list. */
				memcpy(&(list[list_size].id), buf + i + 1, 9);
				list[list_size].type = buf[i + 11];
				list[list_size].type |= buf[i + 12] << 8;
				list_size++;
				if(list_size == list_cap){
					/* List is full. Resize the list. */
					list = realloc(list, list_cap * 2);
					if(NULL == list){
						fprintf(stderr, "sn-util: error: malloc failure when requesting %zd bytes.", sizeof(node_enum_t) * list_cap);
						free(buf);
						*nodes = NULL;
						*num_nodes = 0;
					}
					list_cap *= 2;
				}
				
				i += 14; /* This should be 15. The extra increment happens after exiting this if-else. */
			}
		}
		
		i++;
	}
	
	*nodes = list;
	*num_nodes = list_size;
	free(buf);
}

/* Shutdown the library and close the serial port. */
void sn_shutdown(void){
	sn_mode(MODE_SHUTDOWN, 0);
	close(SerialFD);
	SerialFD = -1;
}

/* Return a human-readable description of a node typecode. */
const char *sn_type_str(uint16_t typecode){
	switch(typecode){
		case 0x0001:
			return "\"Soil Sensor, 1-zone\"";
		case 0x0002:
			return "\"Soil Sensor, 2-zone\"";
		case 0x0004:
			return "\"Soil Sensor, 4-zone\"";
		default:
			return "\"Unknown Node Type\"";
	}
}

/* Extract a uint16_t from a little-endian data packet.
 * Index is the index of the first byte of the field in the packet data. */
uint16_t sn_uint16(uint8_t *packet_data, uint8_t index){
	return (packet_data[index + 1] << 8) | packet_data[index];
}

/* Extract a uint32_t from a little-endian data packet.
 * Index is the index of the first byte of the field in the packet data. */
uint32_t sn_uint32(uint8_t *packet_data, uint8_t index){
	return (packet_data[index + 3] << 24)| (packet_data[index + 2] << 16) | (packet_data[index + 1] << 8) | packet_data[index];
}