/* (C) 2011 by Dieter Spaar <spaar@mirider.augusta.de>
 *
 * All Rights Reserved
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * 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.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */
 
/*

  The idea behind the RRLP server is as follows:
  
  - Every RRLP related response from the MS is sent to the RRLP server
  
  - If it is a position, the decoded position is returned
  
  - If there is an error, an error message is returned
  
  - If assistance data are requested, a set of PDUs with the required
    data is generated. A set of PDUs is used to not exceed the maximum
    size for a RRLP message.
    
  - If an assitance data ACK is received, the next assitance data PDU 
    is sent (no data are returned if it is the final message).    
    
 */
 
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h> 

#include <sys/ioctl.h>
#include <termios.h>
#include <fcntl.h>

#include <PDU.h>

#include "gps.h"
#include "ubx.h"
#include "ubx-parse.h"
#include "rrlp.h"

#define DEBUG 1
#if DEBUG
	#define printd(x, args ...) printf(x, ## args)
#else
	#define printd(x, args ...)
#endif

#define DEBUG_GPS 1
#if DEBUG_GPD
	#define prints(x, args ...) printf(x, ## args)
#else
	#define prints(x, args ...)
#endif

//#define PDU RRLP_Component

/* Convert "Type" defined by -DPDU into "asn_DEF_Type" */
#define	ASN_DEF_PDU(t)	asn_DEF_ ## t
#define	DEF_PDU_Type(t)	ASN_DEF_PDU(t)
#define	PDU_Type	DEF_PDU_Type(PDU)

/* TODO */

#define RRLP_SERV_PORT	7890

#define MAX_RRLP_DATA	256

/* Server cmds */

#define RRLP_CMD_MS_DATA		1 /* data from MS */
#define RRLP_CMD_MS_DATA_SLOW	2 /* data from MS, slow channel */

/* Server response */

#define RRLP_RSP_ASSIST_DATA	1 /* assitance data, send to MS */
#define RRLP_RSP_RRLP_ERROR		2 /* RRLP error */
#define RRLP_RSP_RRLP_POSITION	3 /* RRLP position */
#define RRLP_RSP_ERROR			4 /* something went wrong */


#define SUBSC_ID_NONE		0

#define N_MAX_AD_RECORDS		10
#define N_MAX_AD_RECORDS_SLOW	1  /* maximum number of messages used on slow channel */

enum rrlp_state { RRLP_NONE = 0, RRLP_ASSIST, RRLP_ERROR, RRLP_POSITION, RRLP_WAIT_POSITION };

/* record for the state of a subscriber */

struct rrlp_sub_state {
	long long unsigned int id;	/* subscriber ID from OpenBSC */
	time_t time_first;			/* time when record was used the first time */
	
	int reference_num;			/* RRLP reference number */
	
	enum rrlp_state state;
	
	int	error;					/* error */
	
	/* a large assistance data APDU is split in smaller parts */	
	int cur_assist_record;		/* current assistance data record */
	uint8_t assist_data_len[N_MAX_AD_RECORDS]; /* length of a record */
	uint8_t assist_data[N_MAX_AD_RECORDS][MAX_RRLP_DATA]; /* record */
	
	/* GPS position */
	long latitude; /* */
	long longitude; /* */
	long altitude; /* in meter */
};

void free_rrlp_subscriber(struct rrlp_sub_state *rrlp_sub_state);

/* static allocation, might change that for a large number of queries at once */

#define N_MAX_RRLP_STATES	3000

struct rrlp_sub_state g_rrlp_states[N_MAX_RRLP_STATES];

/* assistance data from the GPS receiver */

struct gps_assist_data g_gps;

/* 
	According to TS 23.032
	
	So far only "Ellipsoid point with altitude and uncertainty Ellipsoid"
	seems to be sent from the MS.
*/

int decode_pos_estimate(struct rrlp_sub_state *rrlp_sub_state, uint8_t *data, int len)
{
	long latitude;
	long longitude;
	long altitude;
	
	if(len < 1) {
		fprintf(stderr, "decode_pos_estimate: len too short (%d)\n", len);
		return -1;
	}

	/* type of shape */
	switch((data[0] & 0xF0) >> 4) {
	case 0x00:
		printd("Ellipsoid point\n");
		break;

	case 0x01:
		printd("Ellipsoid point with uncertainty Circle\n");
		break;

	case 0x03:
		printd("Ellipsoid point with uncertainty Ellipse\n");
		break;

	case 0x05:
		printd("Polygon\n");
		break;

	case 0x08:
		printd("Ellipsoid point with altitude\n");
		break;

	case 0x09:
		printd("Ellipsoid point with altitude and uncertainty Ellipsoid\n");
		if(len != 14) {
			fprintf(stderr, "decode_pos_estimate: invalid len (%d)\n", len);
			return -1;
		}
		/* Deggrees of latitude */
		latitude = ((data[1] & 0x7F) << 16) + (data[2] << 8) + data[3];
		if(data[1] & 0x80)
			latitude = -latitude;
		printd("latitude = %f\n", ((double)latitude * 90.0) / 0x800000L);
		/* Deggrees of longitude */
		longitude = (data[4] << 16) + (data[5] << 8) + data[6];
		if(data[4] & 0x80)
			longitude |= 0xFF000000;
		printd("longitude = %f\n", ((double)longitude * 360.0) / 0x1000000L);
		/* Altitude */
		altitude = ((data[7] & 0x7F) << 8) + data[8];
		if(data[7] & 0x80)
			altitude = -altitude;
		printd("altitude = %ld\n", altitude);
		/* Uncertainty semi-major */
		/* Uncertainty semi-minor */
		/* Orientation of major axis */
		/* Uncertainty Altitude */
		/* Confidence */
		
		rrlp_sub_state->latitude = latitude;
		rrlp_sub_state->longitude = longitude;
		rrlp_sub_state->altitude = altitude;
		
		return 0;
		break;

	case 0x0A:
		printd("Ellipsoid Arc\n");
		break;

	default:
		printd("Unknown shape type: %d\n", (data[0] & 0xF0) >> 4);
		break;
	}
	
	fprintf(stderr, "cannot decode shape type (%d)\n", (data[0] & 0xF0) >> 4);
	
	return -1;
}

int decode_rrlp_data(struct rrlp_sub_state *rrlp_sub_state, int max_reply_msgs,
	uint8_t *data, int len,
	uint8_t *cmd_reply, uint8_t *reply, int *len_reply)
{
    static asn_TYPE_descriptor_t *pduType = &PDU_Type;
    void *structure; /* Decoded structure */    
	asn_codec_ctx_t *opt_codec_ctx = 0;
	asn_dec_rval_t rval;
	int rc;
	
	structure = 0;
    
	rval = uper_decode(opt_codec_ctx, pduType, (void **)&structure, data, len, 0, 0);
	
	if(!structure) {
		if(errno) {
			fprintf(stderr, "Error: errno = %d\n", errno);
			return -1;
		} else {
			/* EOF */
			fprintf(stderr, "EOF\n");
			return -1;
		}
	} else {
		/* dump PDU */
		asn_fprint(stdout, pduType, structure);
		
		long l;
		PDU_t *pdu = (PDU_t*)structure;
		
		printd("\n");
		printd("-- referenceNumber = %ld\n", pdu->referenceNumber);
		
		rrlp_sub_state->reference_num = pdu->referenceNumber;
		
		/* CHOICE */
		switch(pdu->component.present) {

		case RRLP_Component_PR_msrPositionReq:
			printd("-- msrPositionReq\n");
			
			*cmd_reply = RRLP_RSP_ERROR;
			strcpy(reply, "APU unsupported: msrPositionReq");
			*len_reply = strlen(reply) + 1;
			break;
		
		case RRLP_Component_PR_msrPositionRsp:
			printd("-- msrPositionRsp\n");
			
			/* "MsrPosition-Rsp": process either "locationInfo" or "locationError" */
			
			/* SEQUENCE with OPTIONAL components */
			MsrPosition_Rsp_t *msrPositionRsp = &pdu->component.choice.msrPositionRsp;
			
			if(msrPositionRsp->multipleSets) {
				printd("---- multipleSets\n");
			}
			
			if(msrPositionRsp->referenceIdentity) {
				printd("---- referenceIdentity\n");
			}
			
			if(msrPositionRsp->otd_MeasureInfo) {
				printd("---- otd_MeasureInfo\n");
			}
			
			if(msrPositionRsp->locationInfo) {
				printd("---- locationInfo\n");
				printd("------ refFrame = %ld\n", msrPositionRsp->locationInfo->refFrame);
				if(msrPositionRsp->locationInfo->gpsTOW) {
					printd("------ gpsTOW = %ld\n", *msrPositionRsp->locationInfo->gpsTOW);
					/* in ms */
				}
				printd("---- fixType = %ld\n", msrPositionRsp->locationInfo->fixType);
				printd("---- posEstimate\n");
				int n = msrPositionRsp->locationInfo->posEstimate.size;
				uint8_t *buf = msrPositionRsp->locationInfo->posEstimate.buf;
			#if 1 /* dump data */
				int i;
				for(i = 0; i < n; i++)
					printd("%02X ", buf[i]);
				printd("\n");
			#endif
				/* TODO */
				if(decode_pos_estimate(rrlp_sub_state, buf, n) != 0) {
					fprintf(stderr, "decode_pos_estimate failed\n");
					
					*cmd_reply = RRLP_RSP_ERROR;
					strcpy(reply, "decode_pos_estimate failed");
					*len_reply = strlen(reply) + 1;
				} else {
					*cmd_reply = RRLP_RSP_RRLP_POSITION;
					
					rrlp_sub_state->state = RRLP_POSITION;
					
					reply[0] = rrlp_sub_state->latitude & 0xFF;
					reply[1] = (rrlp_sub_state->latitude >>  8) & 0xFF;
					reply[2] = (rrlp_sub_state->latitude >> 16) & 0xFF;
					reply[3] = (rrlp_sub_state->latitude >> 24) & 0xFF;
					
					reply[4] = rrlp_sub_state->longitude & 0xFF;
					reply[5] = (rrlp_sub_state->longitude >>  8) & 0xFF;
					reply[6] = (rrlp_sub_state->longitude >> 16) & 0xFF;
					reply[7] = (rrlp_sub_state->longitude >> 24) & 0xFF;
					
					reply[ 8] = rrlp_sub_state->altitude & 0xFF;
					reply[ 9] = (rrlp_sub_state->altitude >>  8) & 0xFF;
					reply[10] = (rrlp_sub_state->altitude >> 16) & 0xFF;
					reply[11] = (rrlp_sub_state->altitude >> 24) & 0xFF;
					
					*len_reply = 3 * 4;
				}
				/* we got a location, ignore anything else */
				goto done;
			}
			
			if(msrPositionRsp->gps_MeasureInfo) {
				printd("---- gps_MeasureInfo\n");
			}
			
			if(msrPositionRsp->locationError) {
				printd("---- locationError\n");
				if(asn_INTEGER2long(&msrPositionRsp->locationError->locErrorReason, &l) == 0) {
					asn_INTEGER_specifics_t *specs = (asn_INTEGER_specifics_t *)asn_DEF_LocErrorReason.specifics;
					const asn_INTEGER_enum_map_t *el;	
					el = INTEGER_map_value2enum(specs, l);
					if(el) {
						printd("---- locErrorReason = %ld (%s)\n", l, el->enum_name);
						sprintf(reply, "locationError %ld (%s)", l, el->enum_name);
					} else {
						printd("---- locErrorReason = %ld\n", l);
						sprintf(reply, "locationError %ld", l);
					}
				} else {
					printd("---- locErrorReason: decode error\n");
					strcpy(reply, "locationError ??");				
					l = 9999;
				}
				if(msrPositionRsp->locationError->additionalAssistanceData) {
					printd("------ additionalAssistanceData\n");
					if(msrPositionRsp->locationError->additionalAssistanceData->gpsAssistanceData) {
						printd("------ gpsAssistanceData\n");
						int n = msrPositionRsp->locationError->additionalAssistanceData->gpsAssistanceData->size;
						uint8_t *buf = msrPositionRsp->locationError->additionalAssistanceData->gpsAssistanceData->buf;
						struct rrlp_assist_req ar;
					#if 1 /* dump data */
						int i;
						for(i = 0; i < n; i++)
							printd("%02X ", buf[i]);
						printd("\n");
					#endif
						/* check state */
						if(rrlp_sub_state->state == RRLP_ASSIST) {
							fprintf(stderr, "assistanceData in invalid state\n");
							
							*cmd_reply = RRLP_RSP_ERROR;
							strcpy(reply, "assistanceData in invalid state");
							*len_reply = strlen(reply) + 1;
							goto done;
						}
						if(rrlp_sub_state->state == RRLP_WAIT_POSITION) {
							fprintf(stderr, "assistanceData already sent\n");
							
							*cmd_reply = RRLP_RSP_ERROR;
							strcpy(reply, "assistanceData already sent");
							*len_reply = strlen(reply) + 1;
							goto done;
						}
						/* decode the requested assistance data */
						rc = rrlp_decode_assistance_request(&ar, buf, n);
						if(rc != 0) {
							fprintf(stderr, "rrlp_decode_assistance_request failed: %d\n", rc);
							
							*cmd_reply = RRLP_RSP_ERROR;
							sprintf(reply, "rrlp_decode_assistance_request failed: %d", rc);
							*len_reply = strlen(reply) + 1;
						} else if(l == LocErrorReason_gpsAssDataMissing) {													
							void *pdus[N_MAX_AD_RECORDS];
							int lens[N_MAX_AD_RECORDS];
							int i;
							int error = 0;
							int ref_num;
							
							/* take a new reference number for the assistance data */
							ref_num = rrlp_sub_state->reference_num;
							ref_num++;
							if(ref_num > 7) /* 0..7 */
								ref_num = 7;
							if(ref_num == 0) /* 0 is special */
								ref_num = 1;
						
							/* generate PDUs */							
							rc = rrlp_gps_assist_pdus(ref_num, &g_gps, &ar, pdus, lens, N_MAX_AD_RECORDS);
							if(rc < 0) {
								fprintf(stderr, "rrlp_gps_assist_pdus: %d\n", rc);
							
								*cmd_reply = RRLP_RSP_ERROR;
								sprintf(reply, "rrlp_gps_assist_pdus failed: %d", rc);
								*len_reply = strlen(reply) + 1;
								goto done;
							}
														
							/* clear messages */
							for(i = 0; i < N_MAX_AD_RECORDS; i++) {
								rrlp_sub_state->assist_data_len[i] = 0;
								memset(rrlp_sub_state->assist_data[i], 0, MAX_RRLP_DATA);
							}
							
							/* check if maximum number of messages exceeded */
							if(rc > max_reply_msgs) {
								for(i = 0; i < rc; i++) {
									if(pdus[i])
										asn_DEF_PDU.free_struct(&asn_DEF_PDU, pdus[i], 0);
								}
								
								fprintf(stderr, "too many assitance messages: (%d > %d)\n", rc, max_reply_msgs);
							
								*cmd_reply = RRLP_RSP_ERROR;
								sprintf(reply, "too many assitance messages: (%d > %d)\n", rc, max_reply_msgs);
								*len_reply = strlen(reply) + 1;
								goto done;
							}
							
							/* copy PDUs to subscriber state */
							for(i = 0; i < rc; i++) {
								printd("%p %d\n", pdus[i], lens[i]);
								if(pdus[i] == NULL) { 
									fprintf(stderr, "pdus[%d] == NULL\n", i);
									sprintf(reply, "pdus[%d] == NULL\n", i);
									error = -1;
								}
								#if 0 /* Debug */
									{
										int rv, fd;
										char filename[50];
										sprintf(filename, "pdu%02d.uper", i);
										fd = open(filename, O_TRUNC | O_CREAT | O_RDWR, S_IREAD | S_IWRITE);
										if (fd < 0)
											printf("File error\n");
										rv = write(fd, pdus[i], lens[i]);
										if(rv != lens[i])
											printf("Write error\n");
										close(fd);
									}
								#endif
								#if 0 /* Debug */
									int j;
									for(j = 0; j < lens[i]; j++)
										printf("%02X ", ((uint8_t*)pdus[i])[j]);
									printf("\n");
								#endif
								
								if(lens[i] > MAX_RRLP_DATA) {
									fprintf(stderr, "lens[%d] > MAX_RRLP_DATA (%d)\n", i, lens[i]);
									sprintf(reply, "lens[%d] > MAX_RRLP_DATA (%d)\n", i, lens[i]);
									error = -2;
								}
																
								if(error == 0) {
									rrlp_sub_state->assist_data_len[i] = lens[i];
									memcpy(rrlp_sub_state->assist_data[i], pdus[i], lens[i]);
									
								}
								
								if(pdus[i])
									free(pdus[i]);
							}
							
							if(error) {
								*cmd_reply = RRLP_RSP_ERROR;
								*len_reply = strlen(reply) + 1;
								goto done;
							}
							
							*cmd_reply = RRLP_RSP_ASSIST_DATA;
					
							rrlp_sub_state->state = RRLP_ASSIST;							
							rrlp_sub_state->cur_assist_record = 0;
							
							/* send first assistent data block */
							i = rrlp_sub_state->cur_assist_record;
							memcpy(reply, rrlp_sub_state->assist_data[i], rrlp_sub_state->assist_data_len[i]);
							*len_reply = rrlp_sub_state->assist_data_len[i];
							/* advance if not already at the end */
							if(rrlp_sub_state->assist_data_len[i])
								rrlp_sub_state->cur_assist_record++;
							
							/* we got assistance data, ignore anything else */
							goto done;
						}
					}
				}
				rrlp_sub_state->state = RRLP_ERROR;

				*cmd_reply = RRLP_RSP_RRLP_ERROR;
				*len_reply = strlen(reply) + 1;
				/* we got an error, ignore anything else */
				goto done;
			}
			
			if(msrPositionRsp->extensionContainer) {
				printd("---- extensionContainer\n");
			}			
			
			/* no "locationInfo" or "locationError" */
			*cmd_reply = RRLP_RSP_ERROR;
			strcpy(reply, "unexpected MsrPosition-Rsp component");
			*len_reply = strlen(reply) + 1;
			break;

		case RRLP_Component_PR_assistanceData:
			printd("-- assistanceData\n");
			
			*cmd_reply = RRLP_RSP_ERROR;
			strcpy(reply, "APDU unsupported: assistanceData");
			*len_reply = strlen(reply) + 1;
			break;

		case RRLP_Component_PR_assistanceDataAck:
			printd("-- assistanceDataAck\n");
						
			/* check state */
			if(rrlp_sub_state->state != RRLP_ASSIST) {
				*cmd_reply = RRLP_RSP_ERROR;
				fprintf(stderr, "assistanceData Ack in invalid state\n");
				strcpy(reply, "assistanceData Ack in invalid state");
				*len_reply = strlen(reply) + 1;
				goto done;
			}
						
			/* send next assitance data reply or done */
			
			*cmd_reply = RRLP_RSP_ASSIST_DATA;

			int i = rrlp_sub_state->cur_assist_record;
			if(i < N_MAX_AD_RECORDS) {
				memcpy(reply, rrlp_sub_state->assist_data[i], rrlp_sub_state->assist_data_len[i]);
				*len_reply = rrlp_sub_state->assist_data_len[i];
				/* advance if not already at the end */
				if(rrlp_sub_state->assist_data_len[i]) 
					rrlp_sub_state->cur_assist_record++;				
				else {
					rrlp_sub_state->state = RRLP_NONE;
				}
			} else {
				*len_reply = 0;
				rrlp_sub_state->state = RRLP_WAIT_POSITION;
			}
			
			break;

		case RRLP_Component_PR_protocolError:
			printd("-- protocolError\n");
			ProtocolError_t *protocolError = &pdu->component.choice.protocolError;
			if(asn_INTEGER2long(&protocolError->errorCause, &l) == 0) {
				asn_INTEGER_specifics_t *specs = (asn_INTEGER_specifics_t *)asn_DEF_ErrorCodes.specifics;
				const asn_INTEGER_enum_map_t *el;	
				el = INTEGER_map_value2enum(specs, l);
				if(el) {
					printd("---- errorCause = %ld (%s)\n", l, el->enum_name);
					sprintf(reply, "protocolError %ld (%s)", l, el->enum_name);
				} else {
					printd("---- errorCause = %ld\n", l);
					sprintf(reply, "protocolError %ld", l);				
				}
			} else {
				printd("---- errorCause: decode error\n");
				strcpy(reply, "protocolError ??");				
			}
			
			rrlp_sub_state->state = RRLP_ERROR;

			*cmd_reply = RRLP_RSP_RRLP_ERROR;
			*len_reply = strlen(reply) + 1;			
			break;

		default:
			printd("-- unknown %d\n", pdu->component.present);
			
			*cmd_reply = RRLP_RSP_ERROR;
			sprintf(reply, "unknown PDU conponent %d", pdu->component.present);
			*len_reply = strlen(reply) + 1;
			break;
		}
				
done:		
		/* free structure */
		
		asn_DEF_PDU.free_struct(&asn_DEF_PDU, (void*)structure, 0);
	}	
	
	return 0;
}

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

static int ubx_parse(struct gps_assist_data *gps, uint8_t *data, int len)
{
	int rv = 0, i;

	/* Parse each message */
	for(i = 0; i < len; ) {
		rv = ubx_msg_dispatch(ubx_parse_dt, data + i, len - i, gps);
		if (rv < 0)
			i++;	/* Invalid message: try one byte later */
		else
			i += rv;
	}

	return (rv >= 0) ? 0 : rv;
}

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

/* taken from OsmocomBB */

static int serial_init(const char *serial_port, speed_t baudrate)
{
	int rc, serial_fd;
	struct termios tio;

	serial_fd = open(serial_port, O_RDWR | O_NOCTTY | O_NDELAY);
	if (serial_fd < 0) {
		perror("cannot open serial port");
		return serial_fd;
	}

	//fcntl(serial_fd, F_SETFL, 0);

	/* Configure serial interface */
	rc = tcgetattr(serial_fd, &tio);
	if (rc < 0) {
		close(serial_fd);
		perror("tcgetattr()");
		return rc;
	}
	rc = cfsetispeed(&tio, baudrate);
	if (rc < 0) {
		close(serial_fd);
		perror("cfsetispeed()");
		return rc;
	}
	rc = cfsetospeed(&tio, baudrate);
	if (rc < 0) {
		close(serial_fd);
		perror("cfsetospeed()");
		return rc;
	}
	tio.c_cflag &= ~(PARENB | CSTOPB | CSIZE | CRTSCTS);
	tio.c_cflag |=  (CREAD | CLOCAL | CS8);
	tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
	tio.c_iflag |=  (INPCK | ISTRIP);
	tio.c_iflag &= ~(ISTRIP | IXON | IXOFF | IGNBRK | INLCR | ICRNL | IGNCR);
	tio.c_oflag &= ~(OPOST | ONLCR);
	rc = tcsetattr(serial_fd, TCSANOW, &tio);
	if (rc < 0) {
		close(serial_fd);
		perror("tcsetattr()");
		return rc;
	}

	return serial_fd;
}

#define GPS_USB	1 /* set to 1 if uBlox GPS receiver is connected to USB */

#if GPS_USB
  #define GPS_BAUDRATE	B115200
#else
  #define GPS_BAUDRATE	B9600
#endif

/*

USB: switch back to NMEA output

"$PUBX,41,3,0003,0002,9600,0*17\r\n"

*/

static int set_ubx_protocol(int fd)
{
	int len;
	/* Set Protocol and baudrate (switch to UBX output, still allow NMEA input) */
#if GPS_USB /* if uBlox is connected through USB */
	char upx_chg_mode[] = "$PUBX,41,3,0003,0001,115200,0*1C\r\n";
#else /* if uBlox is connected through the serial port */
	char upx_chg_mode[] = "$PUBX,41,1,0003,0001,9600,0*16\r\n";
#endif

	len = write(fd, upx_chg_mode, sizeof(upx_chg_mode) - 1);
	if(len != sizeof(upx_chg_mode) - 1) {
		perror("write()");
		return len;
	}

	/* a short delay */

	sleep(1);

	/* flush the input buffer (there is probably are more elegant solution) */
	
	for(;;) {			
		fd_set readset;
		struct timeval tv;
		int rc;	
		int len;
		uint8_t buf[256];
		
		/* check for data */
				
		FD_ZERO(&readset);
		FD_SET(fd, &readset);
		tv.tv_sec = 0;
		tv.tv_usec = 300 * 1000;
		
		rc = select(fd + 1, &readset, NULL, NULL, &tv);
		if(rc < 0) {
			if(errno != EINTR) {
				fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno));
				break;
			}
		}
		if(!FD_ISSET(fd, &readset))
			break;	
			
		/* read data */
		
	 	len = read(fd, buf, sizeof(buf));
		if(len <= 0) {
			fprintf(stderr, "serial read() failed: (%d) %s\n", len, strerror(errno));
			break;
		}				
			
	}	
	
	return 0;
}

static int ubx_get_data(int fd)
{
	int len;	
	char upx_msg1[] = "\xb5\x62\x01\x02\x00\x00\x03\x0a"; /* NAV-POSLLH */	
	char upx_msg2[] = "\xb5\x62\x0b\x10\x00\x00\x1b\x5c"; /* AID-DATA */
	char upx_msg3[] = "\xB5\x62\x06\x01\x03\x00\x01\x20\x01\x2C\x83"; /* CFG-MSG, send NAV-TIMEGPS every second */

	len = write(fd, upx_msg1, sizeof(upx_msg1) - 1);
	if(len != sizeof(upx_msg1) - 1) {
		perror("write()");
		return len;
	}

	len = write(fd, upx_msg2, sizeof(upx_msg2) - 1);
	if(len != sizeof(upx_msg2) - 1) {
		perror("write()");
		return len;
	}

#if 1	
	len = write(fd, upx_msg3, sizeof(upx_msg3) - 1);
	if(len != sizeof(upx_msg3) - 1) {
		perror("write()");
		return len;
	}
#endif	

	return 0;
}

static int ubx_get_time(int fd)
{
	int len;	
	char upx_msg1[] = "\xb5\x62\x01\x20\x00\x00\x21\x64";  /* NAV-TIMEGPS */
	//char upx_msg1[] = "\xb5\x62\x01\x22\x00\x00\x23\x6A";  /* NAV-CLOCK */
	fd_set readset;
	struct timeval tv;
	int rc;	
	uint8_t buf[256];
	struct gps_assist_data gps;

	len = write(fd, upx_msg1, sizeof(upx_msg1) - 1);
	if(len != sizeof(upx_msg1) - 1) {
		perror("write()");
		return len;
	}

	/* wait for reply (does it really take nearly a second ?) */
	
	FD_ZERO(&readset);
	FD_SET(fd, &readset);
	tv.tv_sec = 0;
	tv.tv_usec = 1000 * 1000;
		
	rc = select(fd + 1, &readset, NULL, NULL, &tv);
	if(rc < 0) {
		if(errno != EINTR) {
			fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno));
			return -1;
		}
	}
	if(!FD_ISSET(fd, &readset)) {
		fprintf(stderr, "timeout waiting for GPS time\n");
		return -2;	
	}
			
	/* read data */
		
 	len = read(fd, buf, sizeof(buf));
	if(len <= 0) {
		fprintf(stderr, "serial read() failed: (%d) %s\n", len, strerror(errno));
		return -1;
	}				
	
	/* parse data */
	
	memset(&gps, 0, sizeof(gps));
	if(ubx_parse(&gps, buf, len) != 0) {
		fprintf(stderr, "ubx_parse failed\n");
		return -1;
	}
	
	return 0;
}

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

void init_rrlp_states(void)
{
	int i;
	memset(&g_rrlp_states, 0, sizeof(g_rrlp_states));
	
	for(i = 0; i < N_MAX_RRLP_STATES; i++) {
		g_rrlp_states[i].id = SUBSC_ID_NONE;
		g_rrlp_states[i].state = RRLP_NONE;
	}
}

/* find record with this ID or get an unused one */

struct rrlp_sub_state *get_rrlp_subscriber(long long unsigned int id)
{
	int i;
	
	/* search for this ID */
	for(i = 0; i < N_MAX_RRLP_STATES; i++) {
		if(g_rrlp_states[i].id == id)
			return &g_rrlp_states[i];
	}
	
	/* search for an unused record */
	for(i = 0; i < N_MAX_RRLP_STATES; i++) {
		if(g_rrlp_states[i].id == SUBSC_ID_NONE) {
			g_rrlp_states[i].id = id;
			g_rrlp_states[i].time_first = time(NULL);
			return &g_rrlp_states[i];
		}
	}
	
	return NULL;
}

void free_rrlp_subscriber(struct rrlp_sub_state *rrlp_sub_state)
{
	memset(rrlp_sub_state, 0, sizeof(struct rrlp_sub_state));
	rrlp_sub_state->id = SUBSC_ID_NONE;
	rrlp_sub_state->state = RRLP_NONE;
}

/* find expired records and free them */

#define SUBS_RECORD_LIFETIME 300 /* in seconds */

void free_expired_rrlp_subscribers(void)
{
	int i;
	
	/* search for this ID */
	for(i = 0; i < N_MAX_RRLP_STATES; i++) {
		if(g_rrlp_states[i].id != SUBSC_ID_NONE && time(NULL) - g_rrlp_states[i].time_first > SUBS_RECORD_LIFETIME) {
			free_rrlp_subscriber(&g_rrlp_states[i]);
		}
	}
}

#define GPS_POLL_TIME	100 /* in seconds */

#define MAX(a, b) ((a > b) ? a : b)

int rrlp_serv(int fd, int fd_serial)
{
	fd_set readset;
	struct timeval tv;
	int rc;	
	int len;
	uint8_t buf[2 + 1 + 8 + MAX_RRLP_DATA]; /* len, cmd, subscriber ID, data */
	uint8_t buf_reply[MAX_RRLP_DATA];
	uint8_t buf_serial[1024 * 8]; /* should be large enough for the complete data set */
	int len_pkt, offs, len_reply, len_data;
	uint8_t cmd, cmd_reply;
	long long unsigned int id;	
	struct sockaddr_in from;
	int from_len;
	time_t last_gps_query = 0;
	time_t last_subs_expiration = time(NULL);
	struct rrlp_sub_state *rrlp_sub_state;

	memset(&g_gps, 0, sizeof(g_gps));
	
	init_rrlp_states();	
	
	for(;;) {	
			
		/* new GPS poll ? */
		
		if(time(NULL) - last_gps_query > GPS_POLL_TIME) {
			printd("GPS poll\n");			
			ubx_get_data(fd_serial);
			last_gps_query = time(NULL);
		}

		/* any data from RRLP client or GPS Receicer ? */ 
		
		FD_ZERO(&readset);
		FD_SET(fd, &readset);
		if(fd_serial != -1)
			FD_SET(fd_serial, &readset);
		tv.tv_sec = 1;
		tv.tv_usec = 0;
		
        /* this creates another UDP socket on Cygwin !? */
#if 0
		rc = select(sizeof(readset) * 8, &readset, NULL, NULL, &tv);
#else
		rc = select(MAX(fd, fd_serial) + 1, &readset, NULL, NULL, &tv);
#endif
		if(rc < 0) {
			if(errno == EINTR)
				continue;
			fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno));
			return -1;
		}
		
		/* data from RRLP client ? */
		
		if(FD_ISSET(fd, &readset)) {
		
			/* read packet */
        	from_len = sizeof(from);
			len = recvfrom(fd, buf, sizeof(buf), 0, (struct sockaddr*)&from, &from_len);
			if(len < 0) {
				fprintf(stderr, "recvfrom() failed: (%d) %s\n", len, strerror(errno));
				return -1;
			}		
			if(len < 2) {
				fprintf(stderr, "len < 2: %d\n", len);
				return -1;
			}		
			len_pkt = buf[0] + (buf[1] << 8);
			if(len_pkt < 2 + 1 + 8) {
				fprintf(stderr, "len_pkt < 2 + 8 + 1: %d\n", len_pkt);
				return -1;
			}		
			if(len != len_pkt) {
				/* TODO: we might have received more than one packet */
				fprintf(stderr, "recvfrom: len != len_pkt: %d %d\n", len, len_pkt);
				return -1;
			}		
			len_pkt -= 2;
			offs = 2;
			
#if 0		/* dump packet */	
			{
				int i;
				for(i = 0; i < len_pkt; i++)
					printd("%02X ", buf[offs + i]);
				printd("\n");
			}
#endif			

			cmd = buf[offs + 0];
			/* get subscriber ID */
			id = buf[offs + 1];
			id += ((long long unsigned int)buf[offs + 2] << 8);
			id += ((long long unsigned int)buf[offs + 3] << 16);
			id += ((long long unsigned int)buf[offs + 4] << 24);
			id += ((long long unsigned int)buf[offs + 5] << 32);
			id += ((long long unsigned int)buf[offs + 6] << 40);
			id += ((long long unsigned int)buf[offs + 7] << 48);
			id += ((long long unsigned int)buf[offs + 8] << 56);
			
			printd("cmd = 0x%02X ID = %llu\n", cmd, id);			
			
			len_data = len_pkt - (1 + 8);
			offs += (1 + 8);
			
#if 1		/* dump data */	
			{
				int i;
				for(i = 0; i < len_data; i++)
					printd("%02X ", buf[offs + i]);
				printd("\n");
			}
#endif			
			
			/* process packet */
			
			len_reply = 0;
			cmd_reply = 0x00;
			
			/* search subscriber record with this ID or allocate a new one */
			
			rrlp_sub_state = get_rrlp_subscriber(id);
			if(rrlp_sub_state == NULL) {
				fprintf(stderr, "cannot get subscriber reccord\n");
				
				cmd_reply = RRLP_RSP_ERROR;
				strcpy(buf_reply, "cannot get subscriber reccord");
				len_reply = strlen(buf_reply) + 1;
			} else {		
				if(cmd == RRLP_CMD_MS_DATA) {
					if(decode_rrlp_data(rrlp_sub_state, N_MAX_AD_RECORDS, &buf[offs], len_data, &cmd_reply, buf_reply, &len_reply) != 0) {
						cmd_reply = RRLP_RSP_ERROR;
						strcpy(buf_reply, "decode_rrlp_data failed");
						len_reply = strlen(buf_reply) + 1;
					}
				} else if(cmd == RRLP_CMD_MS_DATA_SLOW) {
					if(decode_rrlp_data(rrlp_sub_state, N_MAX_AD_RECORDS_SLOW, &buf[offs], len_data, &cmd_reply, buf_reply, &len_reply) != 0) {
						cmd_reply = RRLP_RSP_ERROR;
						strcpy(buf_reply, "decode_rrlp_data failed");
						len_reply = strlen(buf_reply) + 1;
					}
				} else {			
					cmd_reply = RRLP_RSP_ERROR;
					strcpy(buf_reply, "invalid command");
					len_reply = strlen(buf_reply) + 1;
				}
			}
			
			/* send reply, build packet */
			len_pkt = 2 + 1 + len_reply;
			buf[0] = len_pkt & 0xFF;
			buf[1] = (len_pkt >> 8) & 0xFF;
			buf[2] = cmd_reply; /* cmd */
			/* data */
			memcpy(&buf[3], buf_reply, len_reply);
			
			len = sendto(fd, buf, len_pkt, 0, (struct sockaddr*)&from, sizeof(from));
			if(len < 0) {
				fprintf(stderr, "sendto() failed: (%d) %s\n", len, strerror(errno));
				return -1;
			}		
			if(len != len_pkt) {
				fprintf(stderr, "sendto: len != len_pkt: %d %d\n", len, len_pkt);
				return -1;
			}					
		}
		
		/* data from the GPS receiver ? */
		
		if(fd_serial != -1 && FD_ISSET(fd_serial, &readset)) {

			int buf_offset = 0;
			int total_len = 0;
			struct gps_assist_data gps;

			for(;;) {			
			 	len = read(fd_serial, buf_serial + buf_offset, sizeof(buf_serial) - buf_offset);
				if(len <= 0) {
					fprintf(stderr, "serial read() failed: (%d) %s\n", len, strerror(errno));
					break;
				}				
				prints("serial data: %d\n", len);
				
				total_len += len;
				
				buf_offset += len;
				if(buf_offset >= sizeof(buf_serial)) {
					fprintf(stderr, "serial buffer full\n");
					break;
				}
				
				/* check for more data */
				
				FD_ZERO(&readset);
				FD_SET(fd_serial, &readset);
				tv.tv_sec = 0;
				tv.tv_usec = 300 * 1000;
		
				rc = select(fd_serial + 1, &readset, NULL, NULL, &tv);
				if(rc < 0) {
					if(errno != EINTR) {
						fprintf(stderr, "select() failed: (%d) %s\n", rc, strerror(errno));
						break;
					}
				}
				if(!FD_ISSET(fd_serial, &readset))
					break;
			}

			prints("total_len: %d\n", total_len);

#if 0		/* dump hex data */	
			{
				int i;
				for(i = 0; i < total_len; i++)
					prints("%02X ", buf_serial[i]);
				prints("\n");
			}
#endif			
#if 0		/* dump ASCII data */	
			{
				int i;
				for(i = 0; i < total_len; i++)
					prints("%c", buf_serial[i]);
			}
#endif			

			memset(&gps, 0, sizeof(gps));
			if(ubx_parse(&gps, buf_serial, total_len) != 0) {
				fprintf(stderr, "ubx_parse failed\n");
			}
			else {
				if(gps.fields == GPS_FIELD_REFTIME) {
					g_gps.ref_time = gps.ref_time;
				}
				else if (gps.fields != 0) {
					g_gps = gps;
				}
			}
			
			
		#if 0 /* for time testing */
			close(fd_serial);
			fd_serial = -1;			
		#endif
		}
		
		/* expire subscribers ? */
		
		if(time(NULL) - last_subs_expiration > SUBS_RECORD_LIFETIME) {
			printd("Subscriber expiration\n");			
			free_expired_rrlp_subscribers();
			last_subs_expiration = time(NULL);
		}
		
#if 0 /* for testing only */
		ubx_get_time(fd_serial);
#endif
	}

	return 0;
}

static int fd = -1; 
static int fd_serial = -1;

static void signal_handler(int signal)
{
	int rc;
	
	fprintf(stdout, "signal %u received\n", signal);

	switch (signal) {
	case SIGINT:	
		if(fd != -1) {
			printd("close\n");
			rc = close(fd);
			if(rc != 0) {
				fprintf(stderr, "close() failed: (%d) %s\n", rc, strerror(errno));
			}
		}
		if(fd_serial != -1) {
			printd("close serial\n");
			rc = close(fd_serial);
			if(rc != 0) {
				fprintf(stderr, "close() failed: (%d) %s\n", rc, strerror(errno));
			}
		}
		exit(0);
		break;
	default:
		break;
	}
}
			
int main(int argc, char *argv[])
{
	int rc;
	struct sockaddr_in sa;
    int on;
	
	printf("RRLP Server\n");

	if(argc != 3) {
		fprintf(stderr, "usage: rrlp-serv <IP address of the network interface to use> <GPS serial device>.\n");
		return -1;
	}
		
	fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
	if(fd < 0) {
		fprintf(stderr, "socket() failed: (%d) %s\n", fd, strerror(errno));
		return -1;
	}
	
	/* This is for Cygwin/Windows, for Linux take the interface name */
	
	sa.sin_family = AF_INET;
	sa.sin_port = htons(RRLP_SERV_PORT);
	sa.sin_addr.s_addr = INADDR_ANY;
	if(inet_aton(argv[1], &sa.sin_addr) != 1) {
		fprintf(stderr, "inet_aton() failed: %s\n", strerror(errno));
		close(fd);
		return -1;
	}
	
    if(sa.sin_addr.s_addr == INADDR_NONE)
    {
        fprintf(stderr, "Invalid local address\n"); 
		close(fd);
        return -1;
    }
	
	on = 1;
	rc = setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));
	if(rc != 0) {
		fprintf(stderr, "setsockopt() failed: (%d) %s\n", rc, strerror(errno));
	}

	rc = bind(fd, (struct sockaddr *)&sa, sizeof(sa));
	if(rc < 0) {
		fprintf(stderr, "bind() failed: (%d) %s\n", rc, strerror(errno));
		close(fd);
		return -1;
	}

	/* serial port for GPS receiver */
	
	fd_serial= serial_init(argv[2], GPS_BAUDRATE);
	if(fd_serial < 0) {
		fprintf(stderr, "serial_init failed: (%d) %s\n", fd_serial, strerror(errno));
		close(fd);
		return -1;
	}

	set_ubx_protocol(fd_serial);

	signal(SIGINT, &signal_handler);
	signal(SIGABRT, &signal_handler);
	signal(SIGUSR1, &signal_handler);

	printf("Waiting for incoming data....\n");
	
	rrlp_serv(fd, fd_serial);
	
	rc = close(fd);
	if(rc != 0) {
		fprintf(stderr, "close() failed: (%d) %s\n", rc, strerror(errno));
	}
	fd = -1;
	
	rc = close(fd_serial);
	if(rc != 0) {
		fprintf(stderr, "close() serial failed: (%d) %s\n", rc, strerror(errno));
	}
	fd_serial = -1;
	
	return 0;
}