
/**********************************************************************
 
	Copyright (C) 2003-2004
	Hirohisa MORI <joshua@nichibun.ac.jp>
	Tomoki SEKIYAMA <sekiyama@yahoo.co.jp>
 
	This program is free software; you can redistribute it 
	and/or modify it under the terms of the GLOBALBASE 
	Library General Public License (G-LGPL) as published by 

	http://www.globalbase.org/
 
	This program is distributed in the hope that it will be 
	useful, but WITHOUT ANY WARRANTY; without even the 
	implied warranty of MERCHANTABILITY or FITNESS FOR A 
	PARTICULAR PURPOSE.

**********************************************************************/


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

#include "locator.h"
#include "gps.h"
#include "memory_debug.h"

GPS_GET_SENTENCE *
gps_get_sentence_start(STREAM *s)
{
	GPS_GET_SENTENCE * ret = d_alloc(sizeof(GPS_GET_SENTENCE));
	ret->s = s;
	ret->cur = 0;
	ret->start = 0;
	ret->end = 0;
	return ret;
}

void
gps_get_sentence_end(GPS_GET_SENTENCE *ggs)
{
	s_close(ggs->s);
	d_f_ree(ggs);
}

int
gps_check_sentence(const char *sentence)
{
	/* return value = 0 : correct, 1 : no check sum : -1 : error, -2 : imcomplete sentence */
unsigned sum = 0, sum2;
int i;
	
	if ( sentence[0] != '$' )
		return -2;
	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");
		return 1;
	}
	sscanf(sentence+i+1, "%x", &sum2);
	if ( sum != sum2 ) {
		fprintf(stderr, "ERROR - CHECK SUM NOT MATCH [%x] [%x]\n", sum, sum2);
		return -1;
	}
	return 0;
}

const char *
gps_get_sentence(GPS_GET_SENTENCE *ggs)
{
	int i, j, cur = ggs->cur, start = ggs->start, end = ggs->end;
	char *buf = ggs->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;
				ggs->cur = cur = cur+1;
				ggs->start = start = cur;
				if ( gps_check_sentence(ret) >= 0 )
					return ret;
				continue;
			}
		}
		
		// センテンスの途中まで読み込んでいた
		if ( start != 0 ) {
			// センテンスをbufの頭に移動
			ggs->end = end -= start;
			ggs->cur = cur = end;
			for ( i = 0 ; i < end ; i++ ) {
				buf[i] = buf[start+i];
			}
			start = ggs->start = 0;
		}
	
		j = s_read(ggs->s, buf+end, sizeof(ggs->buf)-end);
		if ( j < 0 ) {
			perror("read");
			exit(1);
		}
		else if ( j == 0 ) {
			return 0;
		}
		else {
			ggs->end = end += j;
		}
	}
}



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

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

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

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

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

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

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

	return 0;
}

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

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

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

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

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