#include <stdio.h>
#include <sys/types.h>
#include <termios.h>
#include <stdlib.h>

void save_cbreak (FILE* fp, struct termios *termattr) {
	int fd = fileno(fp);
	int rc = tcgetattr(fd, termattr);
	if (rc < 0)
		fprintf(stderr,"Error save_cbreak\n");
}

void set_cbreak (FILE* fp) {
    int rc = 0;
    int fd = fileno(fp);
    struct termios termattr;

    rc = tcgetattr(fd, &termattr);
    if (rc < 0) {
	/* Error handler here!! */
    }
    termattr.c_lflag &= ~(ICANON | ECHO);
    termattr.c_cc[VMIN]  = 1;
    termattr.c_cc[VTIME] = 0;

    rc = tcsetattr(fd, TCSANOW, &termattr);
    if (rc < 0) {
	/* Error handler here!! */
    }
}

void dosomething( struct timeval *tv ) {
	printf("\r%d.%02d ", tv->tv_sec, tv->tv_usec);
	fflush(stdout);
}

#define INC_SEC 0
#define INC_USEC 100000

struct timeval timeinc = {
	INC_SEC,
	INC_USEC
};

void timevaladd(struct timeval *result, struct timeval *add) {
	result->tv_sec += add->tv_sec;
	result->tv_usec += add->tv_usec;
	if (result->tv_usec > 999999) {
		result->tv_usec -= 1000000;
		result->tv_sec++;
	}
}

void timevaldiff(struct timeval *diff, struct timeval *now, struct timeval *next) {
	diff->tv_sec = next->tv_sec - now->tv_sec;
	diff->tv_usec = next->tv_usec - now->tv_usec;
	if (diff->tv_usec < 0) {
		diff->tv_usec += 1000000;
		diff->tv_sec--;
	}
	if (diff->tv_sec < 0) {
		diff->tv_sec = 0;
		diff->tv_usec = 1;
	}
}

int main( void ) {
	struct timeval tv, tv_next, tv_now, tv_base;
	int rc;
	fd_set read_set;
	struct termios save_termios;
	int fd_keyboard = fileno(stdin);
	char run = 0;

	printf("Press 'q' to quit program.\n");

	save_cbreak(stdin,&save_termios);
	set_cbreak(stdin);

	FD_ZERO(&read_set);

	gettimeofday(&tv_next, NULL);
	memcpy(&tv_base, &tv_next, sizeof(tv_base));
	while (run!='q') {
		FD_SET(fd_keyboard, &read_set);
		gettimeofday(&tv_now, NULL);
		while (tv_now.tv_sec >= tv_next.tv_sec && tv_now.tv_usec > tv_next.tv_usec)
			timevaladd(&tv_next, &(struct timeval){INC_SEC,INC_USEC} );
		timevaldiff(&tv, &tv_now, &tv_next);
		rc = select( 2, &read_set, NULL, NULL, &tv);
		gettimeofday(&tv_now, NULL);
		if (rc == 0 || (tv_now.tv_sec >= tv_next.tv_sec && tv_now.tv_usec > tv_next.tv_usec) ) { /* time out */
			timevaldiff(&tv, &tv_base, &tv_next);
			dosomething(&tv);
			timevaladd(&tv_next, &timeinc );
		}
		if (FD_ISSET(fd_keyboard, &read_set)) {
			printf(" %c", run = fgetc(stdin) );
			fflush(stdout);
		}
	}
	tcsetattr(fd_keyboard, TCSANOW, &save_termios);
	printf("\n");

	return EXIT_SUCCESS;
}