/* 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 <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

/* 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[141];
	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);
	int r = write(SerialFD, packet, 13 + data_size);
	if(r != 13 + data_size){
		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, 141);
	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;
	}
	
	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 {
		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;
	}
}

/* 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];
}