#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#define TS 1.0
#define KP 0.7
#define KI 0.3

static double kp = KP;
static double ki = KI;

static double pi_control(double offset)
{
	static double drift = 0.0;
	double ki_term, ppb = 0.0;
	ki_term = ki * offset;
	ppb = kp * offset + drift + ki_term;
	drift += ki_term;
	return ppb;
}

struct clk {
	double t;
	double r;
};
static struct clk tref = {
	0.0001, 1.0
};
static struct clk tclk = {
	0.0, 1.0
};

static void age(struct clk *c, double s)
{
	c->t += s * c->r;
}

static void usage(char *progname)
{
	fprintf(stderr,
		"\n"
		"usage: %s [options]\n\n"
		" -a        adjust the PI constants to the sync period\n"
		" -s [num]  sync period in seconds (default 1.0)\n"
		" -p [kp]   proportional constant  (default 0.7)\n"
		" -i [ki]   integration constant   (default 0.3)\n"
		" -l [num]  length of simulation in seconds (default 15.0)\n"
		" -h        prints this message and exits\n"
		"\n",
		progname);
}

int main(int argc, char * argv[])
{
	char *progname;
	int adjust = 0, c, i, nsamples;
	double ts = TS, err, limit = 15.0, ppb;

	/* Process the command line arguments. */
	progname = strrchr(argv[0], '/');
	progname = progname ? 1+progname : argv[0];
	while (EOF != (c = getopt(argc, argv, "as:p:i:l:h"))) {
		switch (c) {
		case 'a':
			adjust = 1;
			break;
		case 's':
			ts = atof(optarg);
			break;
		case 'p':
			kp = atof(optarg);
			break;
		case 'i':
			ki = atof(optarg);
			break;
		case 'l':
			limit = atof(optarg);
			break;
		case 'h':
			usage(progname);
			return 0;
		default:
			usage(progname);
			return -1;
		}
	}

	if (adjust) {
		kp = kp / ts;
		ki = ki / ts;
	}
	nsamples = 1 + limit / ts;

	printf("#sec us_err ppm\n");

	for (i = 0; i < nsamples; i++) {
		err = tref.t - tclk.t;
		ppb = pi_control(err * 1e9);
		tclk.r = 1.0 + ppb / 1e9;
		age(&tref, ts);
		age(&tclk, ts);
		printf("%f %f %.9f\n", i*ts, err*1e6, ppb/1e3);
	}

	return 0;
}