#include <xenomai/init.h>
#include <semaphore.h>
#include <pthread.h>
#include <signal.h>
#include <rtdm/gpiopwm.h>
#include <stdlib.h>
#include <getopt.h>
#include <errno.h>
#include <error.h>
#include <stdio.h>
#include <time.h>

#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <netdb.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <linux/netdevice.h>

#define MIN_DUTY_CYCLE				(0)
#define MAX_DUTY_CYCLE				(100)

typedef void *(*gpiopwm_control_thread)(void *cookie);
#define DEVICE_NAME "/dev/rtdm/gpiopwm"
char *device_name;
int dev;

static sem_t synch;
static sem_t setup;
static int stop;
static int step = 1;
static int port = 66666;

#define MAX_IP_INTERFACES 			(9)
static char *ip_str[MAX_IP_INTERFACES + 1];
static int last_ip;


#define GPIO_PWM_SERVO_CONFIG			\
{						\
	.duty_cycle	=	50,		\
	.range_min	=	950,		\
	.range_max	=	2050,		\
	.period		=	20000000,	\
	.gpio		=	1,		\
}

static struct gpiopwm config = GPIO_PWM_SERVO_CONFIG;

static void fail(const char *reason)
{
	perror(reason);
	exit(EXIT_FAILURE);
}

static void sem_sync(sem_t *sem)
{
	int ret;

	for (;;) {
		ret = sem_wait(sem);
		if (ret == 0)
			return;
		if (errno != EINTR)
			fail("sem_wait");
	}
}

static inline void clear_screen(void)
{
	const char* cmd = "\e[1;1H\e[2J";
	int ret;

	ret = write(2, cmd, strlen(cmd));
	if (!ret)
		error(1, ret, "clear screen error");
}

static inline void print_config(char *str)
{
	int i;

	printf("Config: %s\n", str);
	for (i = 0; i < last_ip ; i++)
		printf("%s", ip_str[i]);
	printf(" device     : %s\n", device_name);
	printf(" range      : [%d, %d]\n", config.range_min, config.range_max);
	printf(" period     : %d nsec\n", config.period);
	printf(" gpio pin   : %d\n", config.gpio);
	printf(" duty cycle : %d\n", config.duty_cycle);
}

static inline void input_message(void)
{
	print_config("");
	printf("\n GPIO PWM Control\n");
	printf( "  Enter duty_cycle [0-100] : ");
}

static void get_ip_addresses(void)
{
	char ip[INET_ADDRSTRLEN];
	struct sockaddr_in *s_in;
	struct ifconf ifconf;
	struct ifreq ifr[10];
	int ret;
	int ifs;
	int i;
	int s;

	s = socket(AF_INET, SOCK_STREAM, 0);
	if (s < 0)
		return;

	ifconf.ifc_buf = (char *) ifr;
	ifconf.ifc_len = sizeof(ifr);

	if (ioctl(s, SIOCGIFCONF, &ifconf) == -1)
		return;

	ifs = ifconf.ifc_len / sizeof(ifr[0]);

	/* we wont _display_ more than MAX_IP_INTERFACES */
	if (ifs > MAX_IP_INTERFACES)
		ifs = MAX_IP_INTERFACES;

	last_ip = ifs + 1;

	for (i = 0; i < ifs; i++) {
		s_in = (struct sockaddr_in *) &ifr[i].ifr_addr;
		if (!inet_ntop(AF_INET, &s_in->sin_addr, ip, sizeof(ip)))
			return;
		ret = asprintf(&ip_str[i]," ip      : %s\n", ip);
		if (ret)
			perror("asprintf");
	}

	ret = asprintf(&ip_str[i]," port    : %d\n\n", port);
	if (ret)
		perror("asprintf");

	close(s);
}

static void setup_sched_parameters(pthread_attr_t *attr, int prio)
{
	struct sched_param p;
	int ret;

	ret = pthread_attr_init(attr);
	if (ret)
		error(1, ret, "pthread_attr_init()");

	ret = pthread_attr_setinheritsched(attr, PTHREAD_EXPLICIT_SCHED);
	if (ret)
		error(1, ret, "pthread_attr_setinheritsched()");

	ret = pthread_attr_setschedpolicy(attr, prio ? SCHED_FIFO : SCHED_OTHER);
	if (ret)
		error(1, ret, "pthread_attr_setschedpolicy()");

	p.sched_priority = prio;
	ret = pthread_attr_setschedparam(attr, &p);
	if (ret)
		error(1, ret, "pthread_attr_setschedparam()");
}

static void *gpiopwm_init_thread(void *cookie)
{
	int ret;

	pthread_setname_np(pthread_self(), "gpio-pwm-handler");
	ret = ioctl(dev, GPIOPWM_RTIOC_SET_CONFIG, config);
	if (ret)
		error(1, ret, "failed to set config");

	ioctl(dev, GPIOPWM_RTIOC_START);

	/* setup completed: allow handler to run */
	sem_post(&setup);

	/* wait for completion */
	sem_sync(&synch);
	ioctl(dev, GPIOPWM_RTIOC_STOP);

	return NULL;
}

/*
 * Controls the motor receving the duty cycle sent over UDP
 * ie: echo -n <duty_cycle> | nc  -w1 -u <ipaddr> <port>
 */
static void *gpiopwm_udp_ctrl_thread(void *cookie)
{
	struct sockaddr_in saddr;
	struct sockaddr_in caddr;
	unsigned int duty_cycle;
	const int blen = 4;
	int optval = 1;
	socklen_t clen;
	char buf[blen];
	int sockfd;
	int ret;

	pthread_setname_np(pthread_self(), "gpio-pwm.netcat");

	sockfd = socket(AF_INET, SOCK_DGRAM, 0);
	if (sockfd < 0)
		perror("socket");

	setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(int));

	bzero((char *) &saddr, sizeof(saddr));
	saddr.sin_addr.s_addr = htonl(INADDR_ANY);
	saddr.sin_port = htons(port);
	saddr.sin_family = AF_INET;

	if (bind(sockfd, (struct sockaddr *)&saddr, sizeof(saddr)) < 0)
		perror("bind");

	clen = sizeof(caddr);
	sem_sync(&setup);
	for (;;) {

		clear_screen();
		print_config("UDP Server\n");

		memset(buf,'\0', blen);
		ret = recvfrom(sockfd, buf, blen - 1, 0, (struct sockaddr *)&caddr, &clen);
		if (ret < 0)
			perror("recvfrom");

		duty_cycle = strtol(buf, NULL, 10);
		if (duty_cycle < MIN_DUTY_CYCLE || duty_cycle > MAX_DUTY_CYCLE)
			continue;

		ret = ioctl(dev, GPIOPWM_RTIOC_CHANGE_DUTY_CYCLE, duty_cycle);
		if (ret)
			break;

		config.duty_cycle = duty_cycle;
	}

	return NULL;
}

/*
 * Manual control of the pwm duty cycle.
 */
static void *gpiopwm_manual_ctrl_thread(void *cookie)
{
	unsigned int duty_cycle;
	size_t len = 4;
	char *in;
	int ret;

	pthread_setname_np(pthread_self(), "gpio-pwm.manual");

	in = malloc(len * sizeof(*in));
	if (!in)
		goto err;

	sem_sync(&setup);
	for (;;) {
		clear_screen();
		input_message();

		len = getline(&in, &len, stdin);
		if (len == -1 || len == 1)
			break;

		duty_cycle = atoi(in);
		if (!duty_cycle && strncmp(in, "000", len - 1) != 0)
			break;

		ret = ioctl(dev, GPIOPWM_RTIOC_CHANGE_DUTY_CYCLE, duty_cycle);
		if (ret) {
			fprintf(stderr, "invalid duty cycle %d\n", duty_cycle);
			break;
		}

		config.duty_cycle = duty_cycle;
	}

	free(in);
err:
	sem_post(&synch);

	return NULL;
}

/*
 * Continuously sweep all duty cycles 0..100 and 100..0.
 * No mode switches should occur.
 */
static void *gpiopwm_sweep_ctrl_thread(void *cookie)
{
	struct timespec delay;
	struct duty_values {
		enum { fwd, bck} direction;
		int x;
	} values;
	int ret;

	pthread_setname_np(pthread_self(), "gpio-pwm.sweep");

	delay = (struct timespec) {.tv_sec = 0, .tv_nsec = 10 * config.period};
	values = (struct duty_values) {.direction = fwd, .x = MIN_DUTY_CYCLE};

	sem_sync(&setup);
	for (;;) {
		if (stop)
			break;

		ret = ioctl(dev, GPIOPWM_RTIOC_CHANGE_DUTY_CYCLE, values.x);
		if (ret) {
			fprintf(stderr, "invalid duty cycle %d\n", values.x);
			break;
		}

		nanosleep(&delay, NULL);

		if (values.direction == bck) {
			if (values.x - (step - 1) > MIN_DUTY_CYCLE)
				values.x -= step;
			else {
				values.direction = fwd;
				values.x = MIN_DUTY_CYCLE;
				continue;
			}
		}

		if (values.direction == fwd) {
			if (values.x + (step - 1) < MAX_DUTY_CYCLE)
				values.x += step;
			else {
				values.direction = bck;
				values.x = MAX_DUTY_CYCLE;
			}
		}
	}
	sem_post(&synch);

	return NULL;
}

static void gpiopwm_sweep_sig_handler(int sig)
{
	stop = 1;
}

static const struct option options[] = {
	{
#define help_opt		0
		.name = "help",
		.has_arg = 0,
		.flag = NULL,
	},
	{
#define sweep_range_opt		1
		.name = "sweep",
		.has_arg = 1,
		.flag = NULL,
	},
	{
#define manual_opt		2
		.name = "manual",
		.has_arg = 0,
		.flag = NULL,
	},
	{
#define config_opt		3
		.name = "config",
		.has_arg = 1,
		.flag = NULL,
	},
	{
#define udp_opt			4
		.name = "udp",
		.has_arg = 1,
		.flag = NULL,
	},
	{
		.name = NULL,
	}
};

static void usage(void)
{
	fprintf(stderr, "Usage:\n"
		"gpiopwm --config=dev:min:max:period:gpio:duty [--sweep=<step> | --udp=<port> | --manual]\n\n"
		"--config=<..>\n"
		"	dev:		/dev/rtdm/gpio-pwm id [0..7]\n"
		"	min:		min active period in usec\n"
		"	max:		max active period in usec\n"
		"	period:		base signal period in nsec\n"
		"	gpio:		gpio pin number\n"
		"	duty:		default duty cycle [0..100]\n"
		"--sweep=<step>\n"
		"			sweep all duty cycle ranges in a loop\n"
		"			in step increments [default 1]\n"
		"--manual		input duty cycle from the command line\n"
		"--udp=<port>		receive duty cycle from the network\n"
		"			ie: echo -n <duty_cycle> | nc  -w1 -u <ipaddr> <port>\n"
		);
}

int main(int argc, char *argv[])
{
	gpiopwm_control_thread handler = NULL;
	pthread_t pwm_task, ctrl_task;
	int opt, lindex, device = 0;
	pthread_attr_t tattr;
	char *p;
	int ret;

	for (;;) {
		lindex = -1;
		opt = getopt_long_only(argc, argv, "", options, &lindex);
		if (opt == EOF)
			break;

		switch (lindex) {
		case sweep_range_opt:
			handler = gpiopwm_sweep_ctrl_thread;
			signal(SIGINT, gpiopwm_sweep_sig_handler);
			step = atoi(optarg);
			step = step < 1 ? 1 : step;
			break;
		case manual_opt:
			handler = gpiopwm_manual_ctrl_thread;
			signal(SIGINT, SIG_IGN);
			break;
		case udp_opt:
			handler = gpiopwm_udp_ctrl_thread;
			port = atoi(optarg);
			get_ip_addresses();
			break;
		case config_opt:
			p = strtok(optarg,":");
			device = p ? atoi(p): -1;
			p = strtok(NULL,":");
			config.range_min = p ? atoi(p): -1;
			p = strtok(NULL,":");
			config.range_max = p ? atoi(p): -1;
			p = strtok(NULL,":");
			config.period = p ? atoi(p): -1;
			p = strtok(NULL,":");
			config.gpio = p ? atoi(p): -1;
			p = strtok(NULL,"");
			config.duty_cycle = p ? atoi(p): -1;
			break;
		case help_opt:
		default:
			usage();
			exit(1);
		}
	}

	if (handler == NULL) {
		usage();
		exit(1);
	}

	ret = sem_init(&synch, 0, 0);
	if (ret < 0)
		error(1, errno, "can't create synch semaphore");

	ret = sem_init(&setup, 0, 0);
	if (ret < 0)
		error(1, errno, "can't create setup semaphore");

	ret = asprintf(&device_name, "%s%d", DEVICE_NAME, device);
	if (ret < 0)
		error(1, EINVAL, "can't create device name");

	dev = open(device_name, O_RDWR);
	if (dev < 0)
		error(1, EINVAL, "can't open %s", device_name);

	setup_sched_parameters(&tattr, 99);
	ret = pthread_create(&ctrl_task, &tattr, handler, NULL);
	if (ret)
		error(1, ret, "pthread_create(ctrl_handler)");

	setup_sched_parameters(&tattr, 98);
	ret = pthread_create(&pwm_task, &tattr, gpiopwm_init_thread, NULL);
	if (ret)
		error(1, ret, "pthread_create(init thread)");

	pthread_join(pwm_task, NULL);
	pthread_join(ctrl_task, NULL);

	pthread_attr_destroy(&tattr);

	ret = close(dev);
	if (ret < 0)
		error(1, EINVAL, "can't close");

	return 0;
}