#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"

void zero(int zoom);
void write_dac(int zoom, int dac);
void single_sweep(int zoom);
void calibrate(int zoom, int gpib);
void sweep_ota(int zoom, int gpib, int ota_sweep_count);
void hold_ota(int zoom, int gpib, int ota_cmd);
void hold_ota2(int zoom, int gpib, int ota_cmd);

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

int g_quit = 0;
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_A6007wag-if00-port0");
	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 getopt_index;
	int zoom, gpib;
	int do_write = 0;
	int do_zero = 0;
	int write_cmd = 32768;
	int do_single_sweep = 0;
	int ota_opt = 0;
	int ota_sweep_count = 64;
	int ota_cmd = 32768;
	char *end;

	static struct option long_opts[] = {
		{ "zoom-device", required_argument, NULL, 'Z' },
		{ "gpib-device", required_argument, NULL, 'G' },
		{ "write-dac", required_argument, NULL, 'w' },
		{ "zero", no_argument, NULL, 'z' },
		{ "single-sweep", no_argument, NULL, 's' },
		{ "ota-sweep", required_argument, NULL, 'o' },
		{ "ota-hold", required_argument, NULL, 'l' },
		{ "help", no_argument, NULL, 'h' },
		{ 0, 0, 0, 0}
	};
	int help=0;
	char c;

	while ((c = getopt_long(argc, argv, "Z:G:szw:o:l: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 'w':
			do_write = 1;
			write_cmd = strtoul(optarg, &end, 0);
			if (*end) {
				fprintf(stderr, "bad number %s\n", optarg);
				help = 1;
			}
			break;
		case 's':
			do_single_sweep = 1;
			break;
		case 'z':
			do_zero = 1;
			break;
		case 'o':
			ota_opt = 1;
			ota_sweep_count = strtoul(optarg, &end, 0);
			if (*end) {
				fprintf(stderr, "bad number %s\n", optarg);
				help = 1;
			}
			break;
		case 'l':
			ota_opt = 2;
			ota_cmd = strtoul(optarg, &end, 0);
			if (*end) {
				fprintf(stderr, "bad number %s\n", optarg);
				help = 1;
			}
			break;
		case 'h':

		case '?':
		default:
			help = 1;
			break;
		}
	}
	
	if (help) {
		fprintf(stderr, "Zoom Nilm Calibration Tool\n");
		fprintf(stderr, "usage: %s [options]\n\n", *argv);
		fprintf(stderr, "  -Z,  --zoom-device %-14s zoom NILM serial port\n", "/dev/xxx" /*zoomdev*/);
		fprintf(stderr, "  -G,  --gpib-device %-14s GPIB serial port\n", "/dev/xxx" /*gpibdev*/);
		fprintf(stderr, "  -w,  --write-dac %-14d   write one value to the DAC constantly\n", write_cmd);
		fprintf(stderr, "  -s,  --single-sweep               do a single sweep on the PIC\n");
		fprintf(stderr, "  -z,  --zero                       set DAC value such that ADC input is centered\n");
		fprintf(stderr, "  -o,  --ota-sweep %-14d   sweep OTA\n", ota_sweep_count);
		fprintf(stderr, "  -l,  --ota-hold %-14d    hold OTA constant\n", ota_cmd);
		fprintf(stderr, "  -h,  --help                       this help\n");
		return 1;
	}

	signal(SIGINT, handle_sig);

	if ((zoom = serial_open(zoomdev, 115200)) == -1)
		err(1, "failed to open zoom device %s", zoomdev);

	if (do_write) {
		write_dac(zoom, write_cmd);
		close(zoom);
		return 0;
	}

	if (do_zero) {
		zero(zoom);
		close(zoom);
		return 0;
	}

	if (do_single_sweep) {
		single_sweep(zoom);
		close(zoom);
		return 0;
	}

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

	switch (ota_opt) {
	case 1:
		sweep_ota(zoom, gpib, ota_sweep_count); 
		break;
	case 2:
		hold_ota2(zoom, gpib, ota_cmd);
		break;
	default:
		calibrate(zoom, gpib);
		break;
	}

	close(zoom);
	close(gpib);
	return 0;
}

void calibrate(int zoom, int gpib)
{
	double idesired, iactual;
	int i;
	int zero;
	int r = 0;
	int dac[ZOOM_SWEEP_COUNT];
	int adc[ZOOM_SWEEP_COUNT];	

	info("Initializing Zoom NILM\n");
	if (zoom_init(zoom) < 0) goto fail;

	info("Zeroing\n");
	if (zoom_zero_start(zoom) < 0) goto fail;

	info("Initializing GPIB\n");
	if (gpib_init(gpib) < 0) goto fail;

	info("Initializing Keithley\n");
	if (gpib_addr(gpib, 24) < 0) goto fail;
	if (keithley_init(gpib) < 0) goto fail;
	if (keithley_current(gpib, 0) < 0) goto fail;
	if (isnan(keithley_read(gpib))) goto fail;

	info("Stop zeroing\n");
	if (zoom_zero_stop(zoom) < 0) goto fail;

	info("Sweeping\n");
	for (idesired = -1.0; idesired <= 1.0 && !g_quit; idesired += 0.10) {
//	for (idesired = -0.2; idesired <= 0.2 && !g_quit; idesired += 0.02) {
		info("Zeroing\n");
		if (zoom_zero_start(zoom) < 0) goto fail;
		
		info("Setting current: %.8f\n", idesired);
		keithley_current(gpib, idesired);
		usleep(100000);
		iactual = keithley_read(gpib);
		info("Actual current: %.8f\n", iactual);

		info("Stop zeroing\n");
		if ((zero = zoom_zero_stop(zoom)) < 0) goto fail;
		info("DAC zero point = %d\n", zero);

		info("Sweeping...\n");
		if ((r = zoom_sweep(zoom, dac, adc)) < 0) goto fail;

		info("Done\n");
		for (i = 0; i < ZOOM_SWEEP_COUNT; i++) {
			printf("%.8f %d %d\n", iactual, dac[i], adc[i]);
		}
	}

safecleanup:
	zoom_zero_start(zoom);
	keithley_off(gpib);
	usleep(50000);
	zoom_zero_stop(zoom);
	return;

fail:
	info("Failed (code %d)\n", r);
	goto safecleanup;
}
 
void write_dac(int zoom, int dac)
{
	char buf[128];

	if (zoom_init(zoom) < 0)
		errx(1, "init failed");

	if (dac < 0) 
		dac = 0;
	if (dac > 65535)
		dac = 65535;
	while (!g_quit) {
		sprintf(buf, "v%04x", dac);
		if (safewrite(zoom, buf, 5) != 5)
			errx(1, "write failed");
		if (fdgets(buf, 128, zoom, 1000) == NULL)
			errx(1, "read timeout");
		chomp(buf);
		printf("%s\n", buf);
	}
}

void single_sweep(int zoom)
{
	int i, r;
	int dac[ZOOM_SWEEP_COUNT];
	int adc[ZOOM_SWEEP_COUNT];	

	info("Initializing Zoom NILM\n");
	if (zoom_init_nozero(zoom) < 0) goto fail;

	info("Sweeping\n");
	if ((r = zoom_sweep(zoom, dac, adc)) < 0) goto fail;

	info("Done\n");
	for (i = 0; i < ZOOM_SWEEP_COUNT; i++) {
		printf("%d %d\n", dac[i], adc[i]);
	}

	return;

fail:
	info("Failed (code %d)\n", r);
	return;
}

void zero(int zoom)
{
	int r;
	int dac;

	info("Initializing Zoom NILM\n");
	if (zoom_init(zoom) < 0) goto fail;

	info("Starting zeroing, ^C to stop\n");
	if (zoom_zero_start(zoom) < 0) goto fail;

	while (!g_quit)
		usleep(100000);

	info("Stop zeroing\n");
	if ((dac = zoom_zero_stop(zoom)) < 0) goto fail;
	info("DAC zero point = 0x%04x %d\n", dac, dac);

	info("Done\n");
	return;

fail:
	info("Failed (code %d)\n", r);
	return;
}

void sweep_ota(int zoom, int gpib, int ota_sweep_count)
{
	double iactual;
	int i,j;
	int r = 0;
	char buf[128];
	char zcmd[128];
	int mycmd = 0;
	int cmd_inc = 4096;
	
	info("Initializing Zoom NILM\n");
	if (zoom_init(zoom) < 0) goto fail;

	info("Initializing GPIB\n");
	if (gpib_init(gpib) < 0) goto fail;

	info("Initializing Keithley 2002 Multimeter\n");
	if (gpib_addr(gpib, 23) < 0) goto fail;
	if (keithley2002_init(gpib) < 0) goto fail;
	if (isnan(keithley2002_read(gpib))) goto fail;

	buf[0] = '0';
	if (safewrite(zoom, buf, 1) != 1)
		errx(1, "write failed");
        if (fdgets(buf, 128, zoom, 1000) == NULL)
		errx(1, "read timeout");
	drain(zoom);



	info("Sweeping OTA\n");
	for (i = 0; i <= 16 && !g_quit; i++){

		for(j = -2; j <= 2; j++){
			if(mycmd+j > 65535 || mycmd+j < 0)
				continue;

			if(sprintf(zcmd,"v%.4x",(mycmd +j)) < 5)
				errx(1, "fail hex conversion");

			if (safewrite(zoom, zcmd, 5) != 5)
				errx(1, "write failed");
			if (fdgets(buf, 128, zoom, 1000) == NULL)
				errx(1, "read timeout");
			
			usleep(100);
			
			iactual = keithley2002_read(gpib);
		
			printf("%d %.8f\n",(int)(mycmd + j), iactual);
		}
		
		mycmd += cmd_inc;

	}

	// return to zero
	buf[0] = '0';
	if (safewrite(zoom, buf, 1) != 1)
		errx(1, "write failed");
        if (fdgets(buf, 128, zoom, 1000) == NULL)
		errx(1, "read timeout");



safecleanup:
	return;

fail:
	info("Failed (code %d)\n", r);
	goto safecleanup;
}


void hold_ota(int zoom, int gpib, int ota_cmd)
{
	double iactual;
	double tx;
	int i;
	int r = 0;
	char buf[128];
	char zcmd[128];
	
	info("Initializing Zoom NILM\n");
	if (zoom_init(zoom) < 0) goto fail;

	info("Initializing GPIB\n");
	if (gpib_init(gpib) < 0) goto fail;

	info("Initializing Keithley 2002 Multimeter\n");
	if (gpib_addr(gpib, 23) < 0) goto fail;
	if (keithley2002_init2(gpib) < 0) goto fail;
	if (isnan(keithley2002_read(gpib))) goto fail;

	buf[0] = '0';
	if (safewrite(zoom, buf, 1) != 1)
		errx(1, "write failed");
        if (fdgets(buf, 128, zoom, 1000) == NULL)
		errx(1, "read timeout");
	drain(zoom);

	if(ota_cmd > 65535 || ota_cmd < 0)
		errx(1, "ota command out-of-range");

	if(sprintf(zcmd,"v%.4x",(ota_cmd)) < 5)
		errx(1, "fail hex conversion");

	if (safewrite(zoom, zcmd, 5) != 5)
		errx(1, "write failed");
	if (fdgets(buf, 128, zoom, 1000) == NULL)
		errx(1, "read timeout");
			
	usleep(100);

	info("Holding OTA\n");
	for (i = 0; i <= 500 && !g_quit; i++){

		iactual = keithley2002_read2(gpib, &tx);
		
		printf("%.8f %.8f\n", tx, iactual);
		
	}

	// return to zero
	buf[0] = '0';
	if (safewrite(zoom, buf, 1) != 1)
		errx(1, "write failed");
        if (fdgets(buf, 128, zoom, 1000) == NULL)
		errx(1, "read timeout");



safecleanup:
	return;

fail:
	info("Failed (code %d)\n", r);
	goto safecleanup;
}

void hold_ota2(int zoom, int gpib, int ota_cmd)
{
	int i;
	int r = 0;
	char buf[128];
	char zcmd[128];
	int dac1, dac2, adc1, adc2;
	
	info("Initializing Zoom NILM\n");
	if (zoom_init(zoom) < 0) goto fail;

	//info("Initializing GPIB\n");
	//if (gpib_init(gpib) < 0) goto fail;

	//info("Initializing Keithley 2002 Multimeter\n");
	//if (gpib_addr(gpib, 23) < 0) goto fail;
	//if (keithley2002_init2(gpib) < 0) goto fail;
	//if (isnan(keithley2002_read(gpib))) goto fail;

	buf[0] = '0';
	if (safewrite(zoom, buf, 1) != 1)
		errx(1, "write failed");
        if (fdgets(buf, 128, zoom, 1000) == NULL)
		errx(1, "read timeout");
	drain(zoom);

	if(ota_cmd > 65535 || ota_cmd < 0)
		errx(1, "ota command out-of-range");

	if(sprintf(zcmd,"v%.4x",(ota_cmd)) < 5)
		errx(1, "fail hex conversion");

	if (safewrite(zoom, zcmd, 5) != 5)
		errx(1, "write failed");
  	if (fdgets(buf, 128, zoom, 1000) == NULL)
		errx(1, "read timeout");
			
	usleep(100);

	zcmd[0] = ' ';

	info("Holding OTA\n");
	for (i = 0; i <= 1000 && !g_quit; i++){

		if(safewrite(zoom,zcmd,1) != 1)
			errx(1, "write failed");

		if (fdgets(buf, 128, zoom, 1000) == NULL)
			errx(1, "read timeout");

		if (sscanf(buf,"%x %d %x %d", &dac1, &dac2, &adc1, &adc2) != 4)
			errx(1, "read error");
		
		printf("%d %d\n", dac2, adc2);
		
	}

	// return to zero
	buf[0] = '0';
	if (safewrite(zoom, buf, 1) != 1)
		errx(1, "write failed");
        if (fdgets(buf, 128, zoom, 1000) == NULL)
		errx(1, "read timeout");



safecleanup:
	return;

fail:
	info("Failed (code %d)\n", r);
	goto safecleanup;
}