/*
 * 2253P demonstration program
 * Copyright (c) 2013, Sensoray Co., Inc.
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <linux/types.h>
#include <unistd.h>
#include <time.h>

#include "driver/s2253ioctl.h"

void msleep(int ms)
{
	struct timespec ts = {ms / 1000, (ms % 1000) * 1000000};

	nanosleep(&ts, NULL);
}

int main (int argc, char * argv[])
{
	int fd, rc;
	char msg[256];
	int logioc = S2253_VIDIOC_LOG;
	char *devname = "/dev/video0";
	int i = 1;
	enum {
		nop = 0,
		get = 1,
		set = 2,
		enable = 3,
		disable = 4,
		reset = 5,
	} mode = 0;
	enum {
		none = 0,
		gpio = 1,
		enc = 2,
		xio = 3,
		gps = 4,
		enc_scaled = 5,
		enc_scale_factor = 6,
	} what = 0;

	if (argc <= 1) {
		fprintf(stderr,
			"Usage: %s [/dev/videoX]\n"
			"\t[get [enc [0][1]] [enc_scaled [0][1]] [gpio [0][1]] [xio [0..3]]]\n"
			"\t     [gps [status][lat][long][alt][time][date][gga][gsa][gsv][rmc][sats][lock] ]\n"
			"\t[set [enc [0=N][1=N]] [enc_sf [0=F][1=F]] [gpio [0={0,1}] [1={0,1}]] ]\n"
			"\t[reset [enc [0][1]] ]\n"
			"\t[echo string]\n"
			"\t[version]\n"
			"\t[comstat]\n"
			"\t[online]\n"
			"\t[enable [xio [0..3]] [gps] [enc [0][1]] [suspend] ]\n"
			"\t[disable [xio [0..3]] [gps] [enc [0][1]] [suspend] ]\n"
			"\t[pause {cap,prev,out} {0,1,2,3} {none,rise,fall,high,low} ]\n",
			argv[0]);
		return 0;
	}
	
	++argv;
	--argc;
	
	if (strncmp(*argv, "/dev/video", 10) == 0) {
		devname = *argv;
		++argv;
		--argc;
	}
	
	fd = open(devname, O_RDWR);
	if (fd < 0) {
		perror(devname);
		return 1;
	}
	
	while (argc > 0) {
		if (strcmp(*argv, "get") == 0) {
			mode = get;
		} else if (strcmp(*argv, "set") == 0) {
			mode = set;
		} else if (strcmp(*argv, "enable") == 0) {
			mode = enable;
		} else if (strcmp(*argv, "disable") == 0) {
			mode = disable;
		} else if (strcmp(*argv, "reset") == 0) {
			mode = reset;
		} else if (strcmp(*argv, "enc") == 0) {
			what = enc;
		} else if (strcmp(*argv, "enc_scaled") == 0) {
			what = enc_scaled;
		} else if (strcmp(*argv, "enc_sf") == 0) {
			what = enc_scale_factor;
		} else if (strcmp(*argv, "gpio") == 0) {
			what = gpio;
		} else if (strcmp(*argv, "xio") == 0) {
			what = xio;
		} else if (strcmp(*argv, "echo") == 0) {
			struct s2253p_data arg = {};
			++argv;
			--argc;
			while (argc > 0) {
				strcpy(arg.data + arg.size, *argv);
				arg.size += strlen(*argv);
				++argv;
				--argc;
				if (argc > 0)
					arg.data[arg.size++] = ' ';
			}
			printf("arg.size=%d\n", arg.size);
			rc = ioctl(fd, S2253P_VIDIOC_S_ECHO, &arg);
			if (rc < 0) perror("echo");
			break;
		} else if (strcmp(*argv, "pause") == 0) {
			struct s2253p_xio_pause arg;
			++argv;
			--argc;
			if (argc <= 0)
				break;
			if (strcmp(*argv, "cap") == 0) {
				arg.stream = 0;
			} else if (strcmp(*argv, "prev") == 0) {
				arg.stream = 1;
			} else if (strcmp(*argv, "out") == 0) {
				arg.stream = 2;
			} else {
				fprintf(stderr, "invalid pause stream %s\n", *argv);
				break;
			}
			++argv;
			--argc;
			if (argc <= 0)
				break;
			if (strcmp(*argv, "0") == 0 || strcmp(*argv, "B1") == 0) {
				arg.port = 0;
			} else if (strcmp(*argv, "1") == 0 || strcmp(*argv, "A1") == 0) {
				arg.port = 1;
			} else if (strcmp(*argv, "2") == 0 || strcmp(*argv, "B0") == 0) {
				arg.port = 2;
			} else if (strcmp(*argv, "3") == 0 || strcmp(*argv, "A0") == 0) {
				arg.port = 3;
			} else {
				fprintf(stderr, "invalid pause xio port %s\n", *argv);
				break;
			}
			++argv;
			--argc;
			if (argc <= 0)
				break;
			if (strcmp(*argv, "none") == 0 || strcmp(*argv, "disable") == 0) {
				arg.mode = S2253P_PAUSE_MODE_DISABLED;
			} else if (strcmp(*argv, "rise") == 0) {
				arg.mode = S2253P_PAUSE_MODE_RISING_EDGE;
			} else if (strcmp(*argv, "fall") == 0) {
				arg.mode = S2253P_PAUSE_MODE_FALLING_EDGE;
			} else if (strcmp(*argv, "high") == 0) {
				arg.mode = S2253P_PAUSE_MODE_LEVEL_HIGH;
			} else if (strcmp(*argv, "low") == 0) {
				arg.mode = S2253P_PAUSE_MODE_LEVEL_LOW;
			} else {
				fprintf(stderr, "invalid pause mode %s\n", *argv);
				break;
			}
			printf("pause: stream=%d port=%d mode=%d\n", arg.stream, arg.port, arg.mode);
			rc = ioctl(fd, S2253P_VIDIOC_S_XIO_PAUSE, &arg);
			if (rc < 0)
				perror("pause");
		} else if (strlen(*argv) == 1 && argv[0][0] >= '0' && argv[0][0] <= '9') {
			int n = argv[0][0] - '0';
			if (what == enc) {
				struct s2253p_encoder arg;
				arg.chan_mask = 1 << n;
				if (mode == get) {
					rc = ioctl(fd, S2253P_VIDIOC_S_ENCODER_READ, &arg);
					do {
						msleep(10);
						rc = ioctl(fd, S2253P_VIDIOC_G_ENCODER_READ, &arg);
					} while (rc >= 0 && arg.counts[n] == 0xffffffff);
					if (rc >= 0)
						printf("enc[%d]=%u\n", n, arg.counts[n]);
					else
						perror("encoder read");
				} else if (mode == reset) {
					rc = ioctl(fd, S2253P_VIDIOC_S_ENCODER_RESET, &arg);
				} else if (mode == enable) {
					arg.enable_mask = arg.chan_mask;
					rc = ioctl(fd, S2253P_VIDIOC_S_ENC_ASYNCEN, &arg);
				} else if (mode == disable) {
					arg.enable_mask = 0;
					rc = ioctl(fd, S2253P_VIDIOC_S_ENC_ASYNCEN, &arg);
				} else {
					printf("bad mode for enc\n");
				}
				if (rc < 0)
					perror("enc");
			} else if (what == enc_scaled && mode == get) {
				struct s2253p_encoder arg;
				arg.chan_mask = 1 << n;
				rc = ioctl(fd, S2253P_VIDIOC_S_ENCODER_READ, &arg);
				do {
					msleep(10);
					rc = ioctl(fd, S2253P_VIDIOC_G_ENCODER_READ_SCALED, &arg);
				} while (rc >= 0 && arg.scaled[n] == -1.0);
				if (rc >= 0)
					printf("enc_scaled[%d]=%f\n", n, (double)arg.scaled[n]);
				else
					perror("encoder read");
				
			} else if (what == gpio) {
				struct s2253p_gpio arg;
				arg.port_mask = 1 << n;
				if (mode == get) {
					rc = ioctl(fd, S2253P_VIDIOC_S_GPIO_READ, &arg);
					do {
						msleep(10);
						rc = ioctl(fd, S2253P_VIDIOC_G_GPIO_READ, &arg);
						if (rc < 0) break;
					} while (arg.not_ready_mask & (1 << n));
					printf("gpio[%d]=%d\n", n, (arg.value_mask >> n) & 1);
				}
			} else if (what == xio) {
				struct s2253p_xio arg;
				arg.port_mask = 1 << n;
				if (mode == get) {
					rc = ioctl(fd, S2253P_VIDIOC_S_XIO_READ, &arg);
					do {
						msleep(10);
						rc = ioctl(fd, S2253P_VIDIOC_G_XIO_READ, &arg);
						if (rc < 0) break;
					} while (arg.not_ready_mask & (1 << n));
					printf("xio[%d]=%d\n", n, (arg.value_mask >> n) & 1);
				} else if (mode == enable) {
					arg.enable_mask = arg.port_mask;
					rc = ioctl(fd, S2253P_VIDIOC_S_XIO_ENABLE, &arg);
				} else if (mode == disable) {
					arg.enable_mask = 0;
					rc = ioctl(fd, S2253P_VIDIOC_S_XIO_ENABLE, &arg);
				}
			}
		} else if (what == gpio && mode == set && strlen(*argv) == 3 && argv[0][1] == '=') {
			struct s2253p_gpio arg;
			int n = (argv[0][0]-'0');
			int val = (argv[0][2]-'0');
			arg.port_mask = 1 << n;
			arg.value_mask = val << n;
			rc = ioctl(fd, S2253P_VIDIOC_S_GPIO_WRITE, &arg);
			if (rc < 0)
				perror("gpio set");
		} else if (what == enc && mode == set && strlen(*argv) >= 3 && argv[0][1] == '=') {
			struct s2253p_encoder arg;
			int n = (argv[0][0]-'0');
			arg.chan_mask = 1 << n;
			arg.counts[n] = (__u32) atol(&argv[0][2]);
			printf("set enc %d %u\n", n, arg.counts[n]);
			rc = ioctl(fd, S2253P_VIDIOC_S_ENCODER_LOAD, &arg);
			if (rc < 0)
				perror("encoder load");

		} else if (what == enc_scale_factor && mode == set && strlen(*argv) >= 3 && argv[0][1] == '=') {
			struct s2253p_encoder arg;
			int n = (argv[0][0]-'0');
			arg.chan_mask = 1 << n;
			arg.scale_factor[n] = (__u32) atof(&argv[0][2]);
			printf("set enc scale factor %d %f\n", n, (double)arg.scale_factor[n]);
			rc = ioctl(fd, S2253P_VIDIOC_S_ENCODER_SCALE_FACTOR, &arg);
			if (rc < 0)
				perror("encoder scale factor");
			
		} else if (strcmp(*argv, "gps") == 0) {
			struct s2253p_gps arg;
			what = gps;
			if (mode == enable) {
				struct s2253p_gps arg;
				arg.enable = 1;
				rc = ioctl(fd, S2253P_VIDIOC_S_GPS_ENABLE, &arg);
			} else if (mode == disable) {
				struct s2253p_gps arg;
				arg.enable = 0;
				rc = ioctl(fd, S2253P_VIDIOC_S_GPS_ENABLE, &arg);
			} else if (mode == get) {
			} else {
				printf("unexpected gps command: %s\n", *argv);
			}
		
		} else if (strcmp(*argv, "online") == 0) {
			struct s2253p_enable arg;
			rc = ioctl(fd, S2253P_VIDIOC_G_ONLINE, &arg);
			if (rc >= 0)
				printf("online=%d\n", arg.enable);
			else
				perror("get online");

		} else if (strcmp(*argv, "suspend") == 0  && (mode == enable || mode == disable)) {
			struct s2253p_enable arg;
			arg.enable = (mode == enable);
			rc = ioctl(fd, S2253P_VIDIOC_S_SUSPEND, &arg);
			if (rc < 0)
				perror("set suspend");
			
		} else if (strcmp(*argv, "version") == 0) {
			struct s2253p_version arg;
			rc = ioctl(fd, S2253P_VIDIOC_S_VERSION_READ, NULL);
			do {
				msleep(10);
				rc = ioctl(fd, S2253P_VIDIOC_G_VERSION_READ, &arg);
			} while (rc >= 0 && arg.version == 0xffffffff);
			if (rc >= 0)
				printf("version=%x\n", arg.version);
			else
				perror("get version");
		} else if (strcmp(*argv, "comstat") == 0) {
			struct s2253p_comstat arg;
			rc = ioctl(fd, S2253P_VIDIOC_S_COMSTAT_READ, NULL);
			do {
				msleep(10);
				rc = ioctl(fd, S2253P_VIDIOC_G_COMSTAT_READ, &arg);
				if (rc < 0)
					break;
			} while (0);
			printf("comstat:\n"
				"  framing_errors=%d\n"
				"  overrun_errors=%d\n"
				"  buffer_overflow=%d\n"
				"  gps_framing_errors=%d\n"
				"  gps_overrun_errors=%d\n"
				"  gps_buffer_overflow=%d\n",
				arg.framing_errors,
				arg.overrun_errors,
				arg.buffer_overflow,
				arg.gps_framing_errors,
				arg.gps_overrun_errors,
				arg.gps_buffer_overflow);
		} else if (mode == get && what == gps) {
			if (strcmp(*argv, "status") == 0) {
				struct s2253p_gps arg;
				arg.enable = 1;
				rc = ioctl(fd, S2253P_VIDIOC_S_GPS_READSTATUS, &arg);
				do {
					msleep(10);
					rc = ioctl(fd, S2253P_VIDIOC_G_GPS_READSTATUS, &arg);
				} while (rc >= 0 && arg.status == 0xff);
				if (rc >= 0)
					printf("gps status=%x\n", arg.status);
				else
					perror("get gps status");
			} else if (strcmp(*argv, "lat") == 0) {
				struct s2253p_gps_item arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_LATITUDE, &arg);
				printf("gps latitude=%s\n", arg.str);
			} else if (strcmp(*argv, "long") == 0) {
				struct s2253p_gps_item arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_LONGITUDE, &arg);
				printf("gps longitude=%s\n", arg.str);
			} else if (strcmp(*argv, "alt") == 0) {
				struct s2253p_gps_item arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_ALTITUDE, &arg);
				printf("gps altitude=%s\n", arg.str);
			} else if (strcmp(*argv, "speed") == 0) {
				struct s2253p_gps_item arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_SPEED, &arg);
				printf("gps speed=%s\n", arg.str);
			} else if (strcmp(*argv, "course") == 0) {
				struct s2253p_gps_item arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_COURSE, &arg);
				printf("gps course=%s\n", arg.str);
			} else if (strcmp(*argv, "time") == 0) {
				struct s2253p_gps_item arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_UTC_TIME, &arg);
				printf("gps time=%s\n", arg.str);
			} else if (strcmp(*argv, "date") == 0) {
				struct s2253p_gps_item arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_UTC_DATE, &arg);
				printf("gps date=%s\n", arg.str);
			} else if (strcmp(*argv, "sats") == 0) {
				struct s2253p_gps arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_SATELLITES, &arg);
				printf("gps sats=%d\n", arg.satellites);
			} else if (strcmp(*argv, "lock") == 0) {
				struct s2253p_gps arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_LOCK, &arg);
				printf("gps lock=%d\n", arg.lock);
			} else if (strcmp(*argv, "gga") == 0) {
				struct s2253p_gps_msg arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_GGA, &arg);
				printf("gps gga=%s\n", arg.str);
			} else if (strcmp(*argv, "gsa") == 0) {
				struct s2253p_gps_msg arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_GSA, &arg);
				printf("gps gsa=%s\n", arg.str);
			} else if (strcmp(*argv, "gsv") == 0) {
				struct s2253p_gps_msg arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_GSV, &arg);
				printf("gps gsv=%s\n", arg.str);
			} else if (strcmp(*argv, "rmc") == 0) {
				struct s2253p_gps_msg arg;
				rc = ioctl(fd, S2253P_VIDIOC_G_GPS_RMC, &arg);
				printf("gps rmc=%s\n", arg.str);
			} else {
				fprintf(stderr, "%s unexpected\n", *argv);
			}
	
		} else {
			fprintf(stderr, "%s unexpected\n", *argv);
		}
		++argv;
		--argc;
	}
	
	close(fd);
	return 0;
}
