//Navitra
//
//2004 SEP
//
//JK1MLY
//
//CPU Nios4.1 32bit + Ext memory
//use 2serial(1:debug,2:GPS),3timer(1:TX,2:1200bps,3:1s),
//    3pio(LED1:HB,2:GPS,3:TX)

#include "excalibur.h"
#include <string.h>
#include <stdio.h>

//GPS data
	char dat[6+1] = "000000\0";
	char tim[6+1] = "000000\0";
	char lat[8+1] = "3600.000\0";
	char lon[9+1] = "13900.000\0";
	char dir[3+1] = "000\0";
	char alt[4+1] = "0000\0";
	char spd[3+1] = "000\0";
	char msg[16+1] = "<TEST> Hino-city\0";

	volatile int l_cnt = 0;


// port2
int nr_rxchar2(void)
	{
	return nm_printf_rxchar(na_uart2);
	}

// get GPS reciver's message
int r_get_line(char *string_out){
	int len = 0;
	int done = 0;
	int c;

	while(!done) {
// Wait for a character
		if((c = nr_rxchar2()) >= 0){

			na_adpLedDS2->np_piodata = 0x01;	//LED2 GREEN

			// See if it is special
			if(c == 27) {			// ESC escape
				len = -1; // error return: length of -1
				done = 1;
			}
			else if(c == '$') {		// Start
				len = 0;
				string_out[len] = c;
				len++;
			}
			else if (( c == '*' ) || ( c == 13 ) || ( c == 10 )) {
				string_out[len] = 0;
				done = 1;
			}
			else if(c >= 0x20 && c <= 0x7e) {		// regular character
				if(len < 256) {
					string_out[len] = c;
					len++;
				}
				else{
					len = -1; // error return: length of -1
					done = 1;
				}
			}
			na_adpLedDS2->np_piodata = 0x03;	//LED2 OFF
		}
	}
	return len;
}


// Interprint recive data
int int_data(char *string_out){
	int len = 0;
	int cmd = -1;
	int loop;
	int let =0;
	int sen = 0;
	char *str_p;
	char stat[4];

	str_p = &string_out[0];
	len = strlen(string_out);

	if(len < 0) 
		return cmd;

	na_adpLedDS2->np_piodata = 0x00;	//LED2 ORANGE

//GGA - Global Positioning System Fix Data
	if(strncmp(string_out, "$GPGGA", 6) == 0) {
		cmd = 1;
		for(loop=0; loop<len; loop++){
			if(*str_p == ','){
				sen++;
				let = 0;
			}
			else{
				switch(sen){
					case 1:
						if(let < 6)
							tim[let] = *str_p;
						break;
					case 2:
						if(let < 8)
							lat[let] = *str_p;
						break;
					case 4:
						if(let < 9)
							lon[let] = *str_p;
						break;
					case 9:
						if((let == 0) && (*str_p == '0')){
							alt[let] = '0';
							alt[let+1] = '\0';
							let = -1;
						}
						else if((let == 0) && (*str_p == '+')){
							alt[let] = '0';
							alt[let+1] = '\0';
							let = -1;
						}
						else if(let < 4){
							if(*str_p == '.'){
								if(let != 0)
									alt[let] = '\0';
								else{
									alt[let] = '0';
									alt[let+1] = '\0';
								}
								let = 3;
							}
							else{
								alt[let] = *str_p;
								alt[let+1] = '\0';
							}
						}
						break;
				}
				let++;
			}
			str_p++;
		}
	}

//GSA - GNSS DOP and Active Satellites
	else if(strncmp(string_out, "$GPGSA", 6) == 0) {
		cmd = 2;
		//NOP
	}
//GSV - Satellites in View
	else if(strncmp(string_out, "$GPGSV", 6) == 0) {
		cmd = 3;
		//NOP
	}
//RMC - Recommended Minimum Specific GNSS Data
	else if(strncmp(string_out, "$GPRMC", 6) == 0) {
		cmd = 4;
		for(loop=0; loop<len; loop++){
			if(*str_p == ','){
				sen++;
				let = 0;
			}
			else{
				switch(sen){
					case 1:
						if(let < 6)
							tim[let] = *str_p;
						break;
					case 3:
						if(let < 9)
							lat[let] = *str_p;
						break;
					case 5:
						if(let < 10)
							lon[let] = *str_p;
						break;
					case 7:
						if((let == 0) && (*str_p == '0')){
							spd[let] = '0';
							spd[let+1] = '\0';
							let = -1;
						}
						else if(let < 3){
							if(*str_p == '.'){
								if(let != 0)
									spd[let] = '\0';
								else{
									alt[let] = '0';
									alt[let+1] = '\0';
								}
								let = 2;
							}
							else
								spd[let] = *str_p;
						}
						break;
					case 8:
						if(let < 3)
							dir[let] = *str_p;
						break;
					case 9:
						if(let < 6)
							dat[let] = *str_p;
						break;
				}
				let++;
			}
			str_p++;
		}
	}
//RMZ - same RMC eTrex only
	else if(strncmp(string_out, "$GPRMZ", 6) == 0) {
		cmd = 5;
		//NOP
	}
//GLL - Geographic Position, Latitude and Longitude
	else if(strncmp(string_out, "$GPGLL", 6) == 0) {
		cmd = 6;
		for(loop=0; loop<len; loop++){
			if(*str_p == ','){
				sen++;
				let = 0;
			}
			else{
				switch(sen){
					case 1:
						if(let < 9)
							lat[let] = *str_p;
						break;
					case 3:
						if(let < 10)
							lon[let] = *str_p;
						break;
					case 5:
						if(let < 6)
							tim[let] = *str_p;
						break;
				}
				let++;
			}
			str_p++;
		}
	}
//VTG - Course Over Ground and Ground Speed
	else if(strncmp(string_out, "$GPVTG", 6) == 0) {
		cmd = 7;
		//NOP
	}
//ZDA - Time & Date
	else if(strncmp(string_out, "$GPZDA", 6) == 0) {
		cmd = 8;
		//NOP
	}
	else{
		cmd = 0;
	}

	na_adpLedDS2->np_piodata = 0x03;	//LED2 OFF

	return cmd;
}


void cng_led(int context){

	l_cnt++;
	na_timer3->np_timercontrol = 0x08 ;

	switch(l_cnt & 0x03){
		case 0:	na_adpLedDS1->np_piodata = 0x01;break;
		case 1:	na_adpLedDS1->np_piodata = 0x02;break;
		case 2:	na_adpLedDS1->np_piodata = 0x00;break;
		case 3:	na_adpLedDS1->np_piodata = 0x03;break;
	}

	na_timer3->np_timercontrol = 0x07 ;
	na_timer3->np_timerstatus = 0;
}


//==============================================================================
//
//	AX25 Frame (http://www.tapr.org/tapr/html/Fax25.html)
//
//==============================================================================

//int ax25_snd(void){
void ax25_snd(char *outa_p){
	char sum_i = 0;		//checksum
	int wave = 0;		//TX FSK wave counter
	int outb = 0;		//NRZI data

	char sum[4] = "1234";
	char crc[4] = "1234";
	char dirn[3] = "32\0";
	int dirt;
	char tmp[9];
	char *tmp_p;
	int len;
	int loop;

//dir		360/64
	dirt = 0;
	tmp_p = &dir[0];
	while(*tmp_p != '\0'){
		dirt = dirt * 10;
		dirt = (int)*tmp_p - (int)'0';
		tmp_p++;
	}
	dirt = dirt / 6;
	if(dirt < 1){
		dirn[0] = '0';
		dirn[1] = '\0';
	}
	else if(dirt < 10){
		dirn[0] = '0' + (dirt % 10);
		dirn[1] = '\0';
	}
	else{
		dirn[0] = '0' + (dirt / 10);
		dirn[1] = '0' + (dirt % 10);
		dirn[2] = '\0';
	}

//Header

//ADDRESS
//    +---+---+---+---+---+---+---+---+
//    |C/R| 6 | 5 |      SSID     | E |
//    +---+---+---+---+---+---+---+---+
//
//     Command/Respons , Extension bit

	strcpy(tmp,"GPS   \0");
	tmp_p = &tmp[0];
	while(*tmp_p != '\0'){
		*outa_p = ((*tmp_p) << 1);
		outa_p++;
		tmp_p++;
	}

	*outa_p = 0x60;		//0b01100000 SSID=0 E=0(cont)
	outa_p++;

	strcpy(tmp,"JK1MLY\0");
	tmp_p = &tmp[0];
	while(*tmp_p != '\0'){
		*outa_p = ((*tmp_p) << 1);
		outa_p++;
		tmp_p++;
	}

	*outa_p = 0xE0;		//0b11100000 SSID=0 E=0(cont)
	outa_p++;

	strcpy(tmp,"RELAY \0");
	tmp_p = &tmp[0];
	while(*tmp_p != '\0'){
		*outa_p = ((*tmp_p) << 1);
		outa_p++;
		tmp_p++;
	}

	*outa_p = 0x60;		//0b01100000 SSID=0 E=0(cont)
	outa_p++;

	strcpy(tmp,"WIDE  \0");
	tmp_p = &tmp[0];
	while(*tmp_p != '\0'){
		*outa_p = ((*tmp_p) << 1);
		outa_p++;
		tmp_p++;
	}

	*outa_p = 0x61;		//0b01100001 SSID=0 E=1(end)
	outa_p++;


//Control(UI)
//    +---+---+---+---+---+---+---+---+
//    | M | M | M |P/F| M | M | 1 | 1 |    (U-frame)
//    +---+---+---+---+---+---+---+---+
//
//     Porl/Final

	*outa_p = 0x03;		//0b00000011
	outa_p++;


//PID(Protocol Identifier)
	*outa_p = 0xF0;		//0b11110000 (No layer 3 protocol implemented)
	outa_p++;

// sum caliculate $PNTS..*
//not include $ and *
	sum_i = '$';

//
//Info(NaviTra)
//
	strcpy(tmp,"$PNTS\0");
	tmp_p = &tmp[0];
	while(*tmp_p != '\0'){
		*outa_p = *tmp_p;
		sum_i = *outa_p ^ sum_i;
		outa_p++;
		tmp_p++;
	}

	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//Ver
	*outa_p = '1';
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//Type
	*outa_p = '0';
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//DATE
	*outa_p = dat[0];		//Date
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = dat[1];
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = dat[2];		//Month
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = dat[3];
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = '2';			//Year
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = '0';
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = dat[4];
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = dat[5];
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//TIME
	tmp_p = &tim[0];
	for(loop = 0; loop < 6; loop++){
		*outa_p = *tmp_p;
		sum_i = *outa_p ^ sum_i;
		outa_p++;
		tmp_p++;
	}
	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//latitude
	tmp_p = &lat[0];
	while(*tmp_p != '\0'){
		*outa_p = *tmp_p;
		sum_i = *outa_p ^ sum_i;
		outa_p++;
		tmp_p++;
	}

	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = 'N';
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//longitude
	tmp_p = &lon[0];
	while(*tmp_p != '\0'){
		*outa_p = *tmp_p;
		sum_i = *outa_p ^ sum_i;
		outa_p++;
		tmp_p++;
	}

	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = 'E';
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//dir		360/64
	tmp_p = &dirn[0];
	while(*tmp_p != '\0'){
		*outa_p = *tmp_p;
		sum_i = *outa_p ^ sum_i;
		outa_p++;
		tmp_p++;
	}

	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//speed
	tmp_p = &spd[0];
	while(*tmp_p != '\0'){
		*outa_p = *tmp_p;
		sum_i = *outa_p ^ sum_i;
		outa_p++;
		tmp_p++;
	}

	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//mark
	*outa_p = '0';			//Icon
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//message
#ifdef DEBUG_Z
	printf("TST: %s\n",msg);
	*outa_p = 0xff;
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = 0xff;
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = 0x7e;
	sum_i = *outa_p ^ sum_i;
	outa_p++;
	*outa_p = 0xff;
	sum_i = *outa_p ^ sum_i;
	outa_p++;
#else
	tmp_p = &msg[0];
	while(*tmp_p != '\0'){
		*outa_p = *tmp_p;
		sum_i = *outa_p ^ sum_i;
		outa_p++;
		tmp_p++;
	}
#endif

	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//group
	strcpy(tmp,"000\0");
	tmp_p = &tmp[0];
	while(*tmp_p != '\0'){
		*outa_p = *tmp_p;
		sum_i = *outa_p ^ sum_i;
		outa_p++;
		tmp_p++;
	}

	*outa_p = ',';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//status
	*outa_p = '1';
	sum_i = *outa_p ^ sum_i;
	outa_p++;

//sentence end
	*outa_p = '*';
	outa_p++;

//checksum		xor \ - *(byte)
	sum[0] = ((sum_i & 0xf0) >> 4) + 0x30;
	if(sum[0] > '9')
		sum[0] = sum[0] + 7;

	sum[1] = (sum_i & 0x0f) + 0x30;
	if(sum[1] > '9')
		sum[1] = sum[1] + 7;

	*outa_p = sum[0];
	outa_p++;
	*outa_p = sum[1];
	outa_p++;

//CR
	*outa_p = 0x0d;
	outa_p++;

//End
	*outa_p = '\0';

}

//==============================================================================
//
// CRC caliculate
//
//==============================================================================

int crc_calc(char *outa_p){
	unsigned short crc = 0xffff;
	int bit;
	char crch;
	char crcl;
	char outa;

	while((outa = *outa_p) != '\0'){
		for(bit=0; bit<8; bit++){
			if((outa & 0x01) == 1)
				crc = (crc ^ 0x8000);
			if((crc & 0x8000) != 0){
				crc = crc << 1;
				crc = crc ^ 0x1021;
			}
			else{
				crc = crc << 1;
			}
			outa = outa >> 1;
		}
		outa_p++;
	}

//change MSB <-> LSB(next stage LSB farst send)
//CRC data send invert

	crch = (char)((crc >> 8) & 0xff);
	crch = (char)(((crch & 0x80) >> 7) + ((crch & 0x40) >> 5) + ((crch & 0x20) >> 3) + ((crch & 0x10) >> 1)
					 + ((crch & 0x08) <<1 ) + ((crch & 0x04) << 3) + ((crch & 0x02) << 5) + ((crch & 0x01) << 7));
	crch = (char)(crch ^ 0xff);

	crcl = (char)(crc & 0xff); 
	crcl = (char)(((crcl & 0x80) >> 7) + ((crcl & 0x40) >> 5) + ((crcl & 0x20) >> 3) + ((crcl & 0x10) >> 1)
					 + ((crcl & 0x08) <<1 ) + ((crcl & 0x04) << 3) + ((crcl & 0x02) << 5) + ((crcl & 0x01) << 7));
	crcl = (char)(crcl ^ 0xff);

	*outa_p = crch;
	outa_p++;
	*outa_p = crcl;
	outa_p++;
	*outa_p = '\0';

	return(crc);
}

//==============================================================================
//
// HDLC Frame
//
//==============================================================================

void hdlc_snd(char *outa_p){
//Mark 1200Hz--0x01 Space 2200Hz--0x02

	volatile int bout=0;
	volatile char outa;
	volatile int bsft;
	int cnt1=0;
	volatile int txdly;
	int timer=0;

	na_pio_out->np_piodata = 0x0C;	//NRZI TMG SND DAT

//Flag
	for(txdly=100; txdly>0; txdly--){
		outa = 0x7e;		//01111110

		for(bsft=8; bsft>0; bsft--){
			bout = (outa & 0x80);
			if(bout != 0){
				na_adpLedDS3->np_piodata = 0x02;	//mark
				na_pio_out->np_piodata = 0x0B;	//NRZI TMG SND DAT
			}
			else{
				na_adpLedDS3->np_piodata = 0x01;	//space
				na_pio_out->np_piodata = 0x0A;	//NRZI TMG SND DAT
			}
			outa = outa << 1;

			if((na_pio_in->np_piodata & 0x0001) != 0)
				while((na_pio_in->np_piodata & 0x0001) != 0);		//wait
			while((na_pio_in->np_piodata & 0x0001) == 0);		//wait

		}
	}

//zero insert counter
	cnt1 = 0;


//data
	while((outa = *outa_p) != '\0'){

		for(bsft=0; bsft<8; bsft++){
			bout = (outa & 0x01);

//data 1
			if(bout != 0){
				cnt1++;
				na_adpLedDS3->np_piodata = 0x02;	//mark
				na_pio_out->np_piodata = 0x0B;	//NRZI TMG SND DAT

				if((na_pio_in->np_piodata & 0x0001) != 0)
					while((na_pio_in->np_piodata & 0x0001) != 0);		//wait
				while((na_pio_in->np_piodata & 0x0001) == 0);		//wait

//data 1 zero ins
				if(cnt1 > 4){
					cnt1 = 0;
					na_adpLedDS3->np_piodata = 0x01;	//space
					na_pio_out->np_piodata = 0x0A;	//NRZI TMG SND DAT

					if((na_pio_in->np_piodata & 0x0001) != 0)
						while((na_pio_in->np_piodata & 0x0001) != 0);		//wait
					while((na_pio_in->np_piodata & 0x0001) == 0);		//wait

				}
			}
//data 0
			else{
				cnt1 = 0;
				na_adpLedDS3->np_piodata = 0x01;	//space
					na_pio_out->np_piodata = 0x0A;	//NRZI TMG SND DAT

				if((na_pio_in->np_piodata & 0x0001) != 0)
					while((na_pio_in->np_piodata & 0x0001) != 0);		//wait
				while((na_pio_in->np_piodata & 0x0001) == 0);		//wait
			}

			outa = outa >> 1;	//Next bit

		}
		outa_p++;				//Next byte
	}

//Flag
	for(txdly=20; txdly>0; txdly--){
		outa = 0x7e;		//01111110
		for(bsft=8; bsft>0; bsft--){
			bout = (outa & 0x80);
			if(bout != 0){
				na_adpLedDS3->np_piodata = 0x02;	//mark
				na_pio_out->np_piodata = 0x0B;	//NRZI TMG SND DAT
			}
			else{
				na_adpLedDS3->np_piodata = 0x01;	//space
				na_pio_out->np_piodata = 0x0A;	//NRZI TMG SND DAT
			}
			outa = outa << 1;

			if((na_pio_in->np_piodata & 0x0001) != 0)
				while((na_pio_in->np_piodata & 0x0001) != 0);		//wait
			while((na_pio_in->np_piodata & 0x0001) == 0);		//wait
		}
	}

	na_adpLedDS3->np_piodata = 0x03;	//off
	na_pio_out->np_piodata = 0x08;	//NRZI TMG SND DAT

}

void snd_navi(int context){

	char outa[255] ;	//NRZ data
	char *outa_p;		//pointer

	outa_p = &outa[0];

	na_timer1->np_timercontrol = 0x08 ;
	na_adpLedDS3->np_piodata = 0x00;	//LED3 ORANGE

//create AX.25 frame
	ax25_snd(outa_p);

//add CRC data
	crc_calc(outa_p);

//TX on
	na_adpLedDS3->np_piodata = 0x02;	//LED3 RED

//send serial data
	hdlc_snd(outa_p);

//TX off
	na_adpLedDS3->np_piodata = 0x03;	//LED3 OFF

	printf("SND :%s\n",outa_p);

//INT clear
	na_timer1->np_timercontrol = 0x07 ;
	na_timer1->np_timerstatus = 0;
}


int main(void){
	char rcvg[255] ;	//GPS data
	char *rcvg_p;		//pointer

	int len;
	int cmd = 0;
	int cnt = 0;

	rcvg_p = &rcvg[0];

	na_pio_out->np_piodata = 0x00;	//NRZI TMG SND DAT

	na_adpLedDS1->np_piodata = 0x03;
	na_adpLedDS2->np_piodata = 0x03;
	na_adpLedDS3->np_piodata = 0x03;

	nr_installuserisr(na_timer1_irq, snd_navi, 0);	//TX interval
	na_timer1->np_timerperiodl = 0x0000;	//16bits
	na_timer1->np_timerperiodh = 0x2000;	//16bits
	na_timer1->np_timercontrol = 0x07 ;	//stop start cont enable

	nr_installuserisr(na_timer3_irq, cng_led, 0);	//1sec
	na_timer3->np_timercontrol = 0x07 ;	//stop start cont enable

	while(1){
		len = r_get_line(rcvg_p);

		if(len > 0)
			cmd = int_data(rcvg_p);

		if(cmd > 0)
			printf("GPS :%s\n",rcvg_p);
	}
}