#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include <stdint.h>
#include <gpiod.h>

/* How many seconds until we give up on the response */
#define HART_RX_TIMEOUT 5

/* Require this many bytes of preamble before parsing a packet */
#define HART_RX_REQUIRED_PREAMBLE 3

/* How many preamble bytes we send */
#define HART_PREAMBLE_BYTES	20

struct hart_msg
{
	uint8_t preamble;
	uint8_t delimiter;
	uint8_t address;
	uint8_t command;
	uint8_t datasz;
	uint8_t payloadsz;
	uint16_t status;
	uint8_t data[256];
	uint8_t dataitr;
	uint8_t csum;
	uint8_t state;
};

struct hart_cmd0_response
{
	uint8_t mfg_id;
	uint8_t devtype;
	uint8_t min_req_preamble;
	uint8_t universal_command_rev;
	uint8_t device_rev;
	uint8_t software_rev;
	uint8_t hardware_rev;
	uint8_t flags;
	uint32_t dev_id;
	uint8_t num_preambles;
	uint8_t max_num_device_vars;
	uint16_t conf_change_counter;
	uint8_t ext_status;
};

void hart_enable_tx(int uartfd, int enable)
{
	uint32_t status;

	if (ioctl(uartfd, TIOCMGET, &status) == -1) {
		perror("TIOCMGET");
		return;
	}

	if (enable)
		status |= TIOCM_RTS;
	else
		status &= ~TIOCM_RTS;

	if (ioctl(uartfd, TIOCMSET, &status) == -1) {
		perror("TIOCMSET");
		return;
	}
	return;
}

void hart_enable_rx(int uartfd, int enable)
{
	uint32_t status;
	if (ioctl(uartfd, TIOCMGET, &status) == -1) {
		perror("TIOCMGET");
		return;
	}

	if (enable)
		status |= TIOCM_DTR;
	else
		status &= ~TIOCM_DTR;

	if (ioctl(uartfd, TIOCMSET, &status) == -1) {
		perror("TIOCMSET");
		return;
	}
	return;
}

int get_cd(int uartfd)
{
	uint32_t status;

	if (ioctl(uartfd, TIOCMGET, &status) == -1) {
		perror("TIOCMGET");
		return -1;
	}

	return !!(status & TIOCM_CD);
}

uint8_t hart_calculate_checksum(uint8_t *data, uint8_t size)
{
	uint8_t csum = 0;
	int i;

	for (i = 0; i < size; i++)
		csum ^= data[i];

	return csum;
}

/* A hart command is formatted with:
 *
 * 5-20 bytes of preamble (0xff).  Ours will default to 20 bytes
 * 1 byte start delimiter - for a master 0x82, or 0x6 for a slave
 * 1 byte of address
 * 1 byte of command number
 * 1 byte specifying size of data array
 * variable size of data array
 * 1 byte xor checksum initialized to 0 
 */
int hart_master_send_command(int uartfd, uint8_t address, uint8_t command, uint8_t *data, uint8_t datasz)
{
	uint8_t buf[512];
	int i = HART_PREAMBLE_BYTES;
	int ret;

	if(datasz) assert(data);

	memset(buf, 0xff, HART_PREAMBLE_BYTES);

	/* Start delimiter for a master */
	buf[i++] = 0x02;
	/* Only supporting 4-bit addresses */
	buf[i++] = address;
	buf[i++] = command;
	buf[i++] = datasz;
	if(data) memcpy(&buf[i], data, datasz);
	i += datasz;

	/* Checksum includes everything but preamble */
	buf[i] = hart_calculate_checksum(&buf[HART_PREAMBLE_BYTES], i - HART_PREAMBLE_BYTES);
	i++;

	hart_enable_rx(uartfd, 0);
	hart_enable_tx(uartfd, 1);

	ret = write(uartfd, buf, i);
	if(ret != i){
		perror("Hart command failed: ");
		return -1; 
	}

	/* 8.3ms per 1200 8e1 symbol */
	usleep(8333 * ret);
	tcdrain(uartfd);

	hart_enable_tx(uartfd, 0);
	hart_enable_rx(uartfd, 1);

	return ret;
}

#define HART_STATE_PREAMBLE		0
#define HART_STATE_ADDRESS		1
#define HART_STATE_COMMAND		2
#define HART_STATE_DATASZ		3
#define HART_STATE_STATUS_H		4
#define HART_STATE_STATUS_L		5
#define HART_STATE_DATA			6
#define HART_STATE_CSUM			7
#define HART_STATE_SUCCESS		8
#define HART_STATE_FAILED		9
void hart_parse(struct hart_msg *msg, uint8_t dat)
{
	if(msg->state != HART_STATE_CSUM)
		msg->csum ^= dat;

	/*printf("Parsing(%d) 0x%X, csum 0x%X\n", msg->state, dat, msg->csum);*/

	switch(msg->state)
	{
	case HART_STATE_PREAMBLE:
		if(dat == 0xff) {
			msg->preamble++;
			msg->csum = 0;
		} else {
			if(msg->preamble >= HART_RX_REQUIRED_PREAMBLE) {
				msg->delimiter = dat;
				msg->state = HART_STATE_ADDRESS;
			}
		}
		break;
	case HART_STATE_ADDRESS:
		msg->address = dat;
		msg->state = HART_STATE_COMMAND;
		break;
	case HART_STATE_COMMAND:
		msg->command = dat;
		msg->state = HART_STATE_DATASZ;
		break;
	case HART_STATE_DATASZ:
		msg->datasz = dat;
		msg->payloadsz = msg->datasz;
		/* If delimiter == 0x6, it is a client packet.  This
		 * has a 2 byte status variable, while a master packet
		 * does not. */
		if(msg->delimiter == 0x6) {
			msg->state = HART_STATE_STATUS_H;
			if(msg->datasz - 2 > 0)
				msg->payloadsz = msg->datasz - 2;
		} else {
			if(msg->datasz != 0)
				msg->state = HART_STATE_DATA;
			else
				msg->state = HART_STATE_CSUM;
		}
		break;
	case HART_STATE_STATUS_H:
		msg->status = dat << 8;
		msg->state = HART_STATE_STATUS_L;
		break;
	case HART_STATE_STATUS_L:
		msg->status |= dat;
		if(msg->datasz != 0)
			msg->state = HART_STATE_DATA;
		else
			msg->state = HART_STATE_CSUM;
		break;
	case HART_STATE_DATA:
		msg->data[msg->dataitr] = dat;
		msg->dataitr++;
		if(msg->dataitr == msg->payloadsz)
			msg->state = HART_STATE_CSUM;
		break;
	case HART_STATE_CSUM:
		if(msg->csum == dat)
			msg->state = HART_STATE_SUCCESS;
		else
			msg->state = HART_STATE_FAILED;
		break;
	}
}

/* https://www.knick-international.com/export/media/13565.pdf page 5 */
int hart_parse_command0(struct hart_msg *msg, struct hart_cmd0_response *resp)
{
	if(msg->data[0] != 254)
		return -1;

	if(msg->payloadsz < 16)
		return -2;

	resp->mfg_id = msg->data[1];
	resp->devtype = msg->data[2];
	resp->min_req_preamble = msg->data[3];
	resp->universal_command_rev = msg->data[4];
	resp->device_rev = msg->data[5];
	resp->software_rev = msg->data[6];
	resp->hardware_rev = msg->data[7];
	resp->flags = msg->data[8];
	resp->dev_id = ((uint32_t)msg->data[9] << 16) |
		       ((uint32_t)msg->data[10] << 8) |
		       msg->data[11];
	resp->num_preambles = msg->data[12];
	resp->max_num_device_vars = msg->data[13];
	resp->conf_change_counter = (uint16_t)msg->data[14] |
				    msg->data[15];
	resp->ext_status = msg->data[16];

	return 0;
}

int hart_receive(int uartfd, struct hart_msg *msg)
{
	int timeout = HART_RX_TIMEOUT;
	uint8_t buf[256];
	fd_set rfds;
	struct timeval tval;
	int rd;
	int i;

	while(timeout) {
		tval.tv_sec = 1;
		tval.tv_usec = 0;
		FD_ZERO(&rfds);
		FD_SET(uartfd, &rfds);
		select(uartfd + 1, &rfds, NULL, NULL, &tval);
		if(!FD_ISSET(uartfd, &rfds))
		{
			timeout--;
			continue;
		}
		rd = read(uartfd, buf, 256);
		if(rd < 0){
			perror("serial port read");
			continue;
		}

		for (i = 0; i < rd; i++){
			hart_parse(msg, buf[i]);
			if(msg->state == HART_STATE_SUCCESS)
				return 0;
			if(msg->state == HART_STATE_FAILED)
				return 1;
		}
	}
	return -1;
}

/* Returns 0 on success */
int hart_cmd0(int uartfd, struct hart_cmd0_response *resp)
{
	int ret;
	struct hart_msg msg;
	memset(&msg, 0, sizeof(struct hart_msg));

	hart_master_send_command(uartfd, 0x80, 0, NULL, 0);
	ret = hart_receive(uartfd, &msg);
	if(ret == -1) {
		fprintf(stderr, "Timed out waiting for packet\n");
		return -1;
	}

	return hart_parse_command0(&msg, resp);
}

int main(int argc, char **argv)
{
	int uartfd, ret;
	struct termios tio;
	struct hart_cmd0_response cmd0;
	memset(&cmd0, 0, sizeof(struct hart_cmd0_response));

	if(argc != 2) {
		printf("Usage: %s /dev/hartuart\n", argv[0]);
		return 1;
	}

	uartfd = open(argv[1], O_RDWR | O_SYNC ); 
	if (uartfd <0) { 
		perror(argv[1]);
		return -1;
	}

	tcgetattr(uartfd, &tio);
	cfmakeraw(&tio);
	tio.c_iflag |= INPCK;
	tio.c_cflag |= CS8 | CREAD | CLOCAL | PARENB | PARODD;
	tio.c_cc[VTIME] = 1;
	tio.c_cc[VMIN] = 1;
	cfsetspeed(&tio, B1200);
	tcsetattr(uartfd, TCSANOW, &tio);
	tcflush(uartfd, TCIOFLUSH);

	ret = hart_cmd0(uartfd, &cmd0);
	if(ret == 0) {
		printf("mfg_id=%d\n", cmd0.mfg_id);
		printf("devtype=%d\n", cmd0.devtype);
		printf("min_req_preamble=%d\n", cmd0.min_req_preamble);
		printf("universal_command_rev=%d\n", cmd0.universal_command_rev);
		printf("device_rev=%d\n", cmd0.device_rev);
		printf("software_rev=%d\n", cmd0.software_rev);
		printf("hardware_rev=%d\n", cmd0.hardware_rev);
		printf("flags=%d\n", cmd0.flags);
		printf("dev_id_num=%d\n", cmd0.dev_id);
		printf("num_preambles=%d\n", cmd0.num_preambles);
		printf("max_num_device_vars=%d\n", cmd0.max_num_device_vars);
		printf("conf_change_counter=%d\n", cmd0.conf_change_counter);
		printf("ext_status=%d\n", cmd0.ext_status);
	} else if (ret == 1){
		fprintf(stderr, "Received incomplete packet\n");
		return 1;
	} else {
		fprintf(stderr, "No response\n");
	}
		
	return 0;
}