#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/time.h>
#include <sys/types.h>
#include <termios.h>
#include <unistd.h>
#include <netdb.h>
#include <sys/socket.h>
#include <netinet/in.h>

#include <pthread.h>

#define PORTNAME "/dev/tty.USA19H3b1P1.1"
#ifdef USE_HTTP_SERVER
#define DGPS_ADDR "http://www.alpsmap.co.jp//cgi-bin/dgps.cgi"
#else
#define DGPS_ADDR "bre.soc.i.kyoto-u.ac.jp"
#define DGPS_PORT 2101
#endif

pthread_t w_thread;

void moveline(int line)
{
	printf("\033[%d;1H\033[K\033[%d;1H", line+1, line+1);
	fflush(stdout);
}


int open_port(const char *name)
{
	struct termios ios;
	int port;
	
	// ポートをノンブロッキングモードで開く
	port = open(name, O_RDWR | O_NONBLOCK);
	if ( port < 0 ) {
		perror("open");
		exit(1);
	}
	
	// ポートの通信速度を設定
	tcgetattr(port, &ios);
	cfsetspeed(&ios, B4800);
	ios.c_cflag |= CLOCAL;
	tcsetattr(port, TCSAFLUSH, &ios);
	return port;
}


int read_port(int port, char buf[], int size)
{
	fd_set rfds;
	struct timeval timeout;
	int j;
	// 読み書き可能になるまで待つ
	FD_ZERO(&rfds);
	FD_SET(port, &rfds);
	timeout.tv_sec = 0;
	timeout.tv_usec = 500000;
	j = select(port+1, &rfds, NULL, NULL, &timeout);
	if ( j < 0 ) {
		perror("select");
		exit(1);
	}
	else if ( j == 0 ) {
		return 0;
	}
	
	if ( FD_ISSET(port, &rfds) ) {
		// ポートから読み込む
	  retry:
		j = read(port, buf, size);
		if ( j < 0 ) {
			if ( errno == EAGAIN )
				goto retry;
		}
		return j;
	}
	return 0;
}

int write_port(int port, char buf[], int size)
{
	struct timeval timeout;
	fd_set wfds;
	int j;
	
	// ポートが書き込み可能になるまで待つ
	FD_ZERO(&wfds);
	FD_SET(port, &wfds);
	timeout.tv_sec = 1;
	timeout.tv_usec = 0;
	j = select(port+1, NULL, &wfds, NULL, &timeout);
	if ( j < 0 ) {
		perror("select");
		exit(1);
	}
	else if ( j == 0 ) {
		return 0;
	}
	if ( FD_ISSET(port, &wfds) ) {
		// ポートに書き込む
	  retry:
		j = write(port, buf, size);
		if ( j < 0 ) {
			if ( errno == EAGAIN )
				goto retry;
		}
		return j;
	}
	return 0;
}


void w_routine(int port)
{
	char wbuf[1024];
	int wlen = 0;
	int s, j;
	
#ifndef USE_HTTP_SERVER
	// サーバに接続
	struct sockaddr_in sadd;
	struct hostent* host = gethostbyname(DGPS_ADDR);
	if ( host == NULL ) {
		perror("gethostbyname");
		exit(1);
	}
	sadd.sin_family = AF_INET;
	sadd.sin_port = htons(DGPS_PORT);
	sadd.sin_addr = *(struct in_addr*)host->h_addr;
	s = socket( AF_INET, SOCK_STREAM, 0 );
	if ( connect(s, (struct sockaddr*)&sadd, sizeof(struct sockaddr_in)) < 0 ) {
		perror("connect");
		exit(1);
	}
#endif
	
	while ( 1 ) {
		
#ifdef USE_HTTP_SERVER
		FILE * p;
		// 待機
		struct timeval timeout;
		timeout.tv_sec = 1;
		timeout.tv_usec = 0;
		select(0, NULL, NULL, NULL, &timeout);
		
		// DGPS情報取得プロセス起動
		p = popen("curl -s " DGPS_ADDR, "r");
		if ( ! p ) {
			perror("popen");
			exit(1);
		}
		if ( fgets(wbuf, sizeof(wbuf), p) )
			wlen = strlen(wbuf);
		else
			wlen = 0;
		pclose(p);
		moveline(24);
		printf("HTTP SERVER GET - OK [%d]\n", wlen);
		
		if ( wlen == 0 )
			continue;
		if ( wbuf[wlen-1] == '\n' )
			wlen--;
		if ( wbuf[wlen-1] == '\r' )
			wlen--;
#else
		// DGPS情報取得
		wlen = recv(s, wbuf, sizeof(wbuf), 0);
		fflush(stdout);
		if ( wlen < 0 ) {
			perror("read");
			exit(1);
		}
		if ( wlen == 0 )
			continue;
#endif
	
	  retry:
		j = write_port(port, wbuf, wlen);
		if ( j == 0 ) {
			goto retry;
			continue;
		}
		wlen = 0;
	}
	
#ifndef USE_HTTP_SERVER
	// 接続を切る
	shutdown( s, 2 );
#endif
}

void init_w_thread(int port)
{
	pthread_create(&w_thread, NULL, w_routine, (void*)port);
}


typedef struct PORT_INFO
{
	int fd;
	int cur, start, end;
	char buf[4096];
} PORT_INFO;

PORT_INFO * get_sentence_start(const char *name)
{
	PORT_INFO * ret = malloc(sizeof(PORT_INFO));
	ret->fd = open_port(name);
	ret->cur = 0;
	ret->start = 0;
	ret->end = 0;
	return ret;
}

void get_sentence_end(PORT_INFO *port)
{
	close(port->fd);
	free(port);
}

const char * get_sentence(PORT_INFO *port)
{
	int i, j, cur = port->cur, start = port->start, end = port->end;
	char *buf = port->buf, *ret;
	
	while ( 1 ) {
		for ( ; cur < end ; cur++ ) {
			if ( buf[cur] == '\r' || buf[cur] == '\n' ) {
				// すでに1センテンス読み込んでいた
				buf[cur] = 0;
				if ( buf[cur+1] == '\n' )
					cur++;
				ret = buf+start;
				port->cur = cur+1;
				port->start = cur+1;
				return ret;
			}
		}
		
		// センテンスの途中まで読み込んでいた
		if ( start != 0 ) {
			// センテンスをbufの頭に移動
			port->end = end -= start;
			port->cur = cur = end;
			for ( i = 0 ; i < end ; i++ ) {
				buf[i] = buf[start+i];
			}
			start = port->start = 0;
		}
	
		j = read_port(port->fd, buf+end, sizeof(port->buf)-end);
		if ( j < 0 ) {
			perror("read");
			exit(1);
		}
		else if ( j == 0 ) {
			return 0;
		}
		else {
			port->end = end += j;
		}
	}
}

const char* lookup_comma(const char *s)
{
	for ( ; *s && *s != ',' ; s++ ) { }
	if ( *s )
		return s;
	return 0;
}

const char* skip_space(const char *s)
{
	for ( ; *s && *s == ' ' ; s++ ) { }
	if ( *s )
		return s;
	return 0;
}

typedef struct GPGGA {
	int hour, min, ndeg, edeg, quality, sats, adgps, dgps_st;
	float sec, nmin, emin, elev;
} GPGGA;

int read_sentence_gga(const char *s, GPGGA *gga)
{
	const char *next;
	s = skip_space(lookup_comma(s+6)+1);
	
	// hhmmss (UTC time)
	next = lookup_comma(s);
	if ( next >= s+6 ) {
		sscanf(s, "%2d%2d%f", &gga->hour, &gga->min, &gga->sec);
	}
	s = skip_space(next+1);
	
	// ddmm.mmmm (緯度)
	next = lookup_comma(s);
	if ( next >= s+6 ) {
		sscanf(s, "%2d%f", &gga->ndeg, &gga->nmin);
	}
	s = skip_space(next+1);
	
	// N/S
	next = lookup_comma(s);
	if ( next > s && *s == 'S' )
		gga->ndeg = - gga->ndeg;
	s = skip_space(next+1);
	
	// dddmm.mmmm (経度)
	next = lookup_comma(s);
	if ( next >= s+6 ) {
		sscanf(s, "%3d%f", &gga->edeg, &gga->emin);
	}
	s = skip_space(next+1);
	
	// E/W
	next = lookup_comma(s);
	if ( next > s && *s == 'W' )
		gga->ndeg = - gga->ndeg;
	s = skip_space(next+1);
	
	// n (測位状態)
	next = lookup_comma(s);
	sscanf(s, "%d", &gga->quality);
	s = skip_space(next+1);
	
	// nn (使用衛星数)
	next = lookup_comma(s);
	sscanf(s, "%d", &gga->sats);
	s = skip_space(next+1);
	
	// h.h (HDOP数)
	next = lookup_comma(s);
	//sscanf(s, "%f", &hdop);
	s = skip_space(next+1);

	// 海抜高度 (高さ)
	next = lookup_comma(s);
	sscanf(s, "%f", &gga->elev);
	s = skip_space(next+1);

	// M
	next = lookup_comma(s);
	s = skip_space(next+1);
	
	// xm.m (ジオイド高さ)
	next = lookup_comma(s);
	//sscanf(s, "%f", &gh);
	s = skip_space(next+1);

	// M
	next = lookup_comma(s);
	s = skip_space(next+1);
	
	// ss (DGPS取得後の経過時間)
	next = lookup_comma(s);
	sscanf(s, "%d", &gga->adgps);
	s = skip_space(next+1);

	// nnnn (DGPS station number)
	next = lookup_comma(s);
	sscanf(s, "%d", &gga->dgps_st);

	return 0;
}

typedef struct GPRMC {
	int year, month, day, hour, min, ndeg, edeg;
	float sec, nmin, emin, speed, direction;
	char state;
} GPRMC;

int read_sentence_rmc(const char *s, GPRMC *rmc)
{
	const char *next;
	s = skip_space(lookup_comma(s+6)+1);
	
	// hhmmss (UTC time)
	next = lookup_comma(s);
	if ( next >= s+6 ) {
		sscanf(s, "%2d%2d%f", &rmc->hour, &rmc->min, &rmc->sec);
	}
	s = skip_space(next+1);
	
	// x (測位状態)
	next = lookup_comma(s);
	//sscanf(s, "%c", &stat);
	s = skip_space(next+1);
	
	// ddmm.mmmm (緯度)
	next = lookup_comma(s);
	if ( next >= s+6 ) {
		sscanf(s, "%2d%f", &rmc->ndeg, &rmc->nmin);
	}
	s = skip_space(next+1);
	
	// N/S
	next = lookup_comma(s);
	if ( next > s && *s == 'S' )
		rmc->ndeg = - rmc->ndeg;
	s = skip_space(next+1);
	
	// dddmm.mmmm (経度)
	next = lookup_comma(s);
	if ( next >= s+6 ) {
		sscanf(s, "%3d%f", &rmc->edeg, &rmc->emin);
	}
	s = skip_space(next+1);
	
	// E/W
	next = lookup_comma(s);
	if ( next > s && *s == 'W' )
		rmc->ndeg = - rmc->ndeg;
	s = skip_space(next+1);
	
	// k.k (速度；ノット)
	next = lookup_comma(s);
	sscanf(s, "%f", &rmc->speed);
	s = skip_space(next+1);
	
	// ddd.d (方位)
	next = lookup_comma(s);
	sscanf(s, "%f", &rmc->direction);
	s = skip_space(next+1);

	// ddmmyy (日付)
	next = lookup_comma(s);
	sscanf(s, "%2d%2d%2d", &rmc->day, &rmc->month, &rmc->year);
	s = skip_space(next+1);
	
	// d.d (磁気偏差)
	next = lookup_comma(s);
	s = skip_space(next+1);
	
	// x (E/W)
	next = lookup_comma(s);
	s = skip_space(next+1);

	// a (測位モード)
	next = lookup_comma(s);
	sscanf(s, "%c", &rmc->state);
	
	return 0;
}

typedef struct GPGSA {
	char mode;
	int state;
} GPGSA;

int read_sentence_gsa(const char *s, GPGSA *gsa)
{
	const char *next;
	s = skip_space(lookup_comma(s+6)+1);
	
	// n (測位モード)
	next = lookup_comma(s);
	sscanf(s, "%c", &gsa->mode);
	s = skip_space(next+1);
	
	// n (測位状態)
	next = lookup_comma(s);
	sscanf(s, "%d", &gsa->state);
	
	return 0;
}


typedef struct PGRME {
	float hpe, vpe, epe;
} PGRME;

int read_sentence_pgrme(const char *s, PGRME *pgrme)
{
	const char *next;
	s = skip_space(lookup_comma(s+6)+1);
	
	// h.h (水平方向の推定誤差)
	next = lookup_comma(s);
	sscanf(s, "%f", &pgrme->hpe);
	s = skip_space(next+1);
	
	// M
	next = lookup_comma(s);
	s = skip_space(next+1);
	
	// v.v (水平方向の推定誤差)
	next = lookup_comma(s);
	sscanf(s, "%f", &pgrme->vpe);
	s = skip_space(next+1);
	
	// M
	next = lookup_comma(s);
	s = skip_space(next+1);
	
	// e.e (推定誤差)
	next = lookup_comma(s);
	sscanf(s, "%f", &pgrme->epe);
	s = skip_space(next+1);
	
	// M
	next = lookup_comma(s);
	
	return 0;
}


int main(int argc, char *argv[])
{
	PORT_INFO *port;
	const char *sentence;
	GPGGA gga;
	GPGSA gsa;
	GPRMC rmc;
	PGRME pgrme;
	int i, line = 0;
	unsigned sum, sum2;
	
	printf("\033[2J");
	
	port = get_sentence_start(argc == 2 ? argv[1] : PORTNAME);
	
	// 補正データ取得スレッドを立ち上げる
	init_w_thread(port->fd);
	
	while ( 1 ) {
		sentence = get_sentence(port);
		if ( sentence == 0 ) {
			line = 0;
			continue;
		}
		moveline(line++);
		puts(sentence);
		line %= 16;
		
		if ( sentence[0] == '$' ) {
			// チェックサム検証
			sum = 0;
			for ( i = 1 ; sentence[i] && sentence[i] != '*' ; i++ )
				sum ^= (unsigned)sentence[i];
			if ( sentence[i] != '*' || !sentence[i+1] || !sentence[i+2] )
				fprintf(stderr, "WARNING - NO CHECK SUM\n");
			else {
				sscanf(sentence+i+1, "%x", &sum2);
				if ( sum != sum2 ) {
					fprintf(stderr, "ERROR - CHECK SUM NOT MATCH [%x] [%x]\n", sum, sum2);
					continue;
				}
			}
			
			// $GPRMCセンテンス認識
			if ( memcmp(sentence+1, "GPRMC", 5) == 0 ) {
				read_sentence_rmc(sentence, &rmc);
			}
			
			// $GPGGAセンテンス認識
			if ( memcmp(sentence+1, "GPGGA", 5) == 0 ) {
				read_sentence_gga(sentence, &gga);
			}
			
			// $PGRMEセンテンス認識
			if ( memcmp(sentence+1, "PGRME", 5) == 0 ) {
				read_sentence_pgrme(sentence, &pgrme);
			}
			
			// $GPGSAセンテンス認識
			if ( memcmp(sentence+1, "GPGSA", 5) == 0 ) {
				read_sentence_gsa(sentence, &gsa);
				
				moveline(20);
				printf("\033[0J");
				printf("mode - %s %s %s\n", 
					gsa.mode=='A' ? "Automatic" : "Manual",
					gsa.state==1 ? "No fix" : gsa.state==2 ? "2D" : "3D",
					rmc.state=='A' ? "Alone" : rmc.state=='D' ? "DGPS" : 
					rmc.state=='S' ? "Simulation" : rmc.state=='N' ? "" : "Unknown");
				printf("N:%d'%07.4f  E:%d'%07.4f  elev:%5.2f m   %d:%02d:%05.2f\n", 
					gga.ndeg, gga.nmin, gga.edeg, gga.emin, gga.elev, gga.hour, gga.min, gga.sec);
				printf("speed:%3.1f km/s dir:%5.2f\n", 
					rmc.speed*1.85200, rmc.direction);
				printf("lock:%d sats  after dgps %d  station %d  epe %.1f / %.1f / %.1f m\n", 
					gga.sats, gga.adgps, gga.dgps_st, pgrme.hpe, pgrme.vpe, pgrme.epe);
			}
		}
	}
	
	// ポートを閉じる
	get_sentence_end(port);
	return 0;
}