#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <errno.h>
#include <unistd.h>
#include <getopt.h>
#include <stdint.h>
#include <string.h>
#include <syslog.h>
#include <err.h>
#include <linux/serial.h>
#include <sys/signal.h>
#include "serial-util.h"
#include "gpib.h"
#include "zoom.h"
#include "math.h"
#include <pthread.h>
#include <ctype.h>
#include "mt19937ar.h"

#define info(x...) fprintf(stderr,x)

static void dctest(int zoom, int gpib);

int g_quit = 0;
static void handle_sig(int sig) { g_quit = 1; }

int main(int argc, char *argv[])
{
	char *zoomdev=strdup("/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A6007wc5-if00-port0");
	char *gpibdev=strdup("/dev/serial/by-id/usb-Prologix_Prologix_GPIB-USB_Controller_PXQQY20G-if00-port0");
	int rate=500000;
	unsigned long seed = 1337;
	int getopt_index;
	int zoom, gpib;

	static struct option long_opts[] = {
		{ "zoom-device", required_argument, NULL, 'Z' },
		{ "gpib-device", required_argument, NULL, 'G' },
		{ "rate", required_argument, NULL, 'r' },
		{ "seed", required_argument, NULL, 's' },
		{ "help", no_argument, NULL, 'h' },
		{ 0, 0, 0, 0}
	};
	int help=0;
	char c;

	while ((c = getopt_long(argc, argv, "Z:G:r:s:h?",
				long_opts, &getopt_index)) != -1) {
		switch(c) 
		{
		case 'Z':
			free(zoomdev);
			zoomdev = strdup(optarg);
			break;
		case 'G':
			free(gpibdev);
			gpibdev = strdup(optarg);
			break;
		case 'r':
			rate = atoi(optarg);
			if(rate == 0)
				errx(1, "invalid rate: %s", optarg);
			break;
		case 's':
			seed = atol(optarg);
			if(seed == 0)
				errx(1, "invalid seed: %s", optarg);
			break;
		case 'h':
		case '?':
		default:
			help = 1;
			break;
		}
	}
	
	if (help) {
		fprintf(stderr, "Zoom Nilm DC Test Tool\n");
		fprintf(stderr, "usage: %s [options]\n\n", *argv);
		fprintf(stderr, "  -Z,  --zoom-device %-9s zoom NILM serial port\n", "/dev/xxx");
		fprintf(stderr, "  -G,  --gpib-device %-9s GPIB serial port\n", "/dev/xxx");
		fprintf(stderr, "  -r,  --rate %-16d baud rate\n", rate);
		fprintf(stderr, "  -s,  --seed %-16ld random seed\n", seed);
		fprintf(stderr, "  -h,  --help                       this help\n");
		return 1;
	}

	signal(SIGINT, handle_sig);

	info("Initializing twister with seed %ld\n", seed);
	init_genrand(seed);

	/* open devices */
	info("Opening Zoom NILM device %s\n", zoomdev);
	if ((zoom = serial_open(zoomdev, rate)) == -1)
		err(1, "failed to open zoom device %s", zoomdev);

	info("Opening GPIB device %s\n", gpibdev);
	if ((gpib = serial_open(gpibdev, 9600)) == -1)
		err(1, "failed to open gpib device %s", gpibdev);

	/* do the dc test */
	dctest(zoom, gpib);
	
	close(zoom);
	close(gpib);
	return 0;
}

struct keithley_t {
	double desired;
	double actual;
	int stable;
};

struct threadinfo_t {
	int quit_flag;
	int fd;
	pthread_mutex_t mutex;
	float calibration;
	struct keithley_t k;
};

int process_adc_dac(const uint8_t *buf, struct threadinfo_t *ti)
{
	uint16_t dac, tmp;
	int16_t adc;
	int overflow;
	double idesired, iactual;
	double calib;
	int stable;

	/* data OK? */
	if ((buf[0] & 0xC0) != 0) return 0;

	/* extract */
	overflow = (buf[0] & 0x10) ? 1 : 0;
	tmp = ((buf[0] & 0x0F) << 8) | buf[1];

	/* sign-extend ADC value */
	if (tmp & 0x0800) 
		tmp |= 0xF000;
	else
		tmp &= ~0xF000;
	adc = (int16_t)tmp;

	dac = (buf[2] << 8) | buf[3];

	/* get locked data */
	pthread_mutex_lock(&ti->mutex);
	idesired = ti->k.desired;
	iactual = ti->k.actual;
	stable = ti->k.stable;
	calib = ti->calibration;
	pthread_mutex_unlock(&ti->mutex);

	/* write it out */
	printf("%d %.12f %.12f %.8f %5d %5d %d\n", 
	       stable,
	       idesired,
	       iactual,
	       calib,
	       dac,
	       adc,
	       overflow);

	return 1;
}

static int process_calibration(const uint8_t *buf, struct threadinfo_t *ti)
{
	float f = *(float *)buf;
	pthread_mutex_lock(&ti->mutex);
	ti->calibration = f;
	pthread_mutex_unlock(&ti->mutex);
	info("New calibration value: %.8f\n", f);
	return 1;
}

static int process(const uint8_t *buf, int len, struct threadinfo_t *ti)
{
	int n = 0;

	/* Process blocks */
 retry:
	for (; (n + 5) <= len; buf += 5, n += 5) {
		int ok = 0;
		switch (buf[0]) {
		case 0xA0:
			if (process_adc_dac(buf + 1, ti)) ok = 1;
			break;
		case 0xA1:
			if (process_calibration(buf + 1, ti)) ok = 1;
			break;
		default:
			break;
		}
		if (!ok) {
			/* badly formed data; eat one byte and retry */
			info("throwing away 0x%02x '%c'\n", buf[0], isprint(buf[0]) ? buf[0] : '.');
			buf++;
			n++;
			goto retry;
		}
	}

	return n;
}

static void *read_data(void *arg)
{
	struct threadinfo_t *ti = (struct threadinfo_t *)arg;
	char buf[1024];
	int len;

	/* read data in a loop.  Use saferead_timeout here so we can
	   notice quit_flag before too long. */
	len = 0;
	while (!ti->quit_flag) {
		int processed, n;
		n = saferead_timeout(ti->fd, 
				     buf + len,
				     sizeof(buf) - len,
				     1000);
		if (n < 0) 
			err(1, "read");
		if (n == 0)
			continue;
		len += n;
		processed = process((uint8_t *) buf, len, ti);
		memmove(buf, buf + processed, len - processed);
		len -= processed;
	}
	info("read thread quitting\n");
	return NULL;
}

/* change keithley and update keithley_t structure with locking */
static int keithley_change(int gpib, double desired, struct threadinfo_t *ti)
{
	double actual;

	pthread_mutex_lock(&ti->mutex);
	ti->k.stable = 0;
	ti->k.desired = desired;
	pthread_mutex_unlock(&ti->mutex);

	if (keithley_current(gpib, desired) < 0)
		return -1;

	actual = keithley_read(gpib);
	if (isnan(actual))
		return -1;

	pthread_mutex_lock(&ti->mutex);
	ti->k.actual = actual;
	ti->k.stable = 1;
	pthread_mutex_unlock(&ti->mutex);
	return 0;
}

static double genrand(double min, double max)
{
	double x;
	x = genrand_real1();	/* [0, 1] */
	x *= (max - min);	/* [0, max-min] */
	x += min;		/* [min, max] */
	return x;
}

static void dctest(int zoom, int gpib)
{
	pthread_t thread;
	struct threadinfo_t ti;
	double tmp;
	int i;

	/* Do a calibration with Keithley off */
	info("Triggering calibration\n");
	zoomrun_trigger_calibrate(zoom);
	usleep(500000);

	/* Init Keithley */
	info("Initializing GPIB\n");
	if (gpib_init(gpib) < 0) { info("failed\n"); goto out1; }
	
	info("Initializing Keithley\n");
	if (gpib_addr(gpib, 24) < 0) { info("failed\n"); goto out1; }
	if (keithley_init(gpib) < 0) { info("failed\n"); goto out2; }
	if (keithley_current(gpib, 0) < 0) { info("failed\n"); goto out2; }
	if (isnan(keithley_read(gpib))) { info("failed\n"); goto out2; }

	/* Start the thread that reads and dumps data */
	info("Spawning thread\n");
	if (pthread_mutex_init(&ti.mutex, NULL) != 0) {
		info("failed\n");
		goto out2;
	}
	ti.calibration = 0.0;
	ti.k.desired = 0;
	ti.k.actual = 0;
	ti.k.stable = 0;
	ti.quit_flag = 0;
	ti.fd = zoom;
	drain_timeout(zoom, 0);
	if (pthread_create(&thread, NULL, read_data, &ti) != 0) {
		info("failed\n");
		goto out2;
	}

	/* Do another calibration now, and verify it works */
	info("Triggering calibration\n");
	pthread_mutex_lock(&ti.mutex);
	ti.calibration = 0.0;
	pthread_mutex_unlock(&ti.mutex);
	if (keithley_change(gpib, 0.0, &ti) < 0) { info("failed\n"); goto out3; }
	zoomrun_trigger_calibrate(zoom);
	for (i = 0; i < 100; i++) {
		usleep(50000);
		pthread_mutex_lock(&ti.mutex);
		tmp = ti.calibration;
		pthread_mutex_unlock(&ti.mutex);
		if (tmp != 0.0)
			break;
	}
	if (tmp == 0.0) {
		info("Invalid calibration data: is zoom NILM working?\n");
		goto out3;
	}

	info("Running\n");
	/* Change Keithley values */
	while (!g_quit) {

#define DELAY 100000	/* after keithley settles, in us */
#define STEPS 10	/* how many small steps to take */

#define K_MIN -0.5	/* keithley min, amps */
#define K_MAX 0.5	/* keithley max, amps */
#define STEPSIZE 0.03	/* max size of small step, in amps */
		
		/* Choose any value within the Keithley range */
		double desired = genrand(K_MIN, K_MAX);
		info("Big step: %.12f\n", desired);
		if (keithley_change(gpib, desired, &ti) < 0) {
			info("error setting keithley\n");
			break;
		}
		usleep(DELAY);

		/* Choose a few more values nearby */
		for (i = 0; i < STEPS && !g_quit; i++) {
			desired += genrand(-STEPSIZE/2, STEPSIZE/2);
			if (desired < -1.0) 
				desired = -1.0;
			if (desired > 1.0)
				desired = 1.0;
			if (keithley_change(gpib, desired, &ti) < 0) {
				info("error setting keithley\n");
				break;
			}
			usleep(DELAY);
		}

		/* one extra delay just to separate things a bit */
		usleep(DELAY);
	}
	info("main thread quitting\n");

out3:
	ti.quit_flag = 1;
	pthread_join(thread, NULL);
out2:
	keithley_off(gpib);
out1:
	return;
}