/*
 *        *** Test program for ZED-F9P module for M5Stack ***
 * 
 */
#include <M5Stack.h>
#include <WiFi.h>  
#include <Wire.h>  
//#include <SPI.h>
#include <SD.h>

#include "BluetoothSerial.h"
#include "SparkFun_Ublox_Arduino_Library.h"
#include "NTRIPClient.h"

#define TINY_GSM_MODEM_UBLOX
#include <TinyGsmClient.h>

#define E7 10000000.0
#define E3 1000.0

#define F9P_IF_UART 1
#define F9P_I2C_ADDR 0x42

#define BT_ENABLE 1

SFE_UBLOX_GPS myGPS;
BluetoothSerial bts;

int f9pI2cAddr;

#define USE_WIFI 1
#define USE_MODEM 2

//int inetAccess = USE_WIFI;
int inetAccess = USE_MODEM;

const char* ssid     = "my ssid";
const char* password = "my password";

char* host = "rtk2go.com";
int httpPort = 2101; //port 2101 is default port of NTRIP caster
char* mntpnt = "geosense_f9p";
char* user   = "";
char* passwd = "";

NTRIPClient *ntrip_c;
WiFiClient wifiClient;

TinyGsm modem(Serial2); /* 3G board modem */
TinyGsmClient ctx(modem);

#define NTRIP_BUFF_MAX 128
char ntripBuff[ NTRIP_BUFF_MAX ];
int ntripIdx;

int chipSelect = 4;
#define SAVE_BUFF_MAX 1024
char saveBuff[ SAVE_BUFF_MAX ];
int saveIndex;
char saveFileName[256];

int fileSaved;
int fileSaving;

int wifiConnected;

int ntripCount;
int ntripLastCount;
unsigned long ntripLastMillis;
unsigned long ntripLastCountMillis;

unsigned long gpsLastMillis;
File fd;
int solutionRate;
int msecSolution;

void setup() {
	// put your setup code here, to run once:
	M5.begin();
	dacWrite(25, 0);	// speaker off

	M5.Lcd.setTextSize(3);
	M5.Lcd.setCursor(0,0);

	// serial
	Serial.begin(115200);

	// ZED-F9P interface	
	if (F9P_IF_UART){	// UART
		//	Serial1.begin(38400, SERIAL_8N1, 16, 17);
		Serial1.begin(38400, SERIAL_8N1, 26, 13);

		// gps
		if ( ! myGPS.begin(Serial1) ){
			Serial.println("gps initialize failed via UART");
			M5.Lcd.println("gps initialize failed via UART");
			while(1);
		}
		myGPS.setUART1Output(COM_TYPE_UBX); //Set the UART port to output UBX only
	}
	else {	// I2C
		f9pI2cAddr = F9P_I2C_ADDR;
		//Wire.begin( f9pI2cAddr );
		Wire.begin();
		Wire.setClock( 400000 );
		delay(3000);	// Short wait time cause error
		if (myGPS.begin() == false){ //Connect to the Ublox module using Wire port
		    Serial.println("gps initialize failed via I2C");
			M5.Lcd.println("gps initialize failed via I2C");
		    while (1);
		}
	  myGPS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
	}
	myGPS.setAutoPVT( true, true, 0 );	// これを設定しないとI2Cの場合、RTKで正常に動作しない
	//myGPS.saveConfiguration(); //Save the current settings to flash and BBR

	// Bluetooth
	if ( BT_ENABLE ){
		if ( ! bts.begin( "M5Stack" ) ) Serial.println("Bluetooth init failed");
	}
	
	
	if ( inetAccess == USE_WIFI ){
		// Wifi
		delay(10);
		Serial.print("Connecting to ");
		Serial.println(ssid);

		WiFi.begin(ssid, password);
		while (WiFi.status() != WL_CONNECTED) {
			delay(500);
			Serial.print(".");
		}
		wifiConnected = 1;
  
		Serial.println("WiFi connected");
		Serial.println("IP address: ");
		Serial.println(WiFi.localIP());
		
		ntrip_c = new NTRIPClient( &wifiClient );
		
		// NTRIP
		/*
		Serial.println("Requesting SourceTable.");
		if(ntrip_c->reqSrcTbl(host,httpPort)){
			char buffer[512];
			delay(5);
			while(ntrip_c->available()){
				ntrip_c->readLine(buffer,sizeof(buffer));
				Serial.print(buffer); 
			}
		}
		else{
			Serial.println("SourceTable request error");
		}
		Serial.print("Requesting SourceTable is OK\n");
		ntrip_c->stop(); //Need to call "stop" function for next request.
    	*/
    
	}
	else if ( inetAccess == USE_MODEM ){
		Serial2.begin(115200, SERIAL_8N1, 16, 17);
		modem.restart();
		
		String modemInfo = modem.getModemInfo();
		Serial.println( modemInfo );
		
		Serial.println( "connecting soracom" );
		while (!modem.waitForNetwork()) Serial.print(".");
		modem.gprsConnect("soracom.io", "sora", "sora");
		while (!modem.isNetworkConnected()) Serial.print(".");
		IPAddress ipaddr = modem.localIP();
		Serial.println( ipaddr );
		ntrip_c = new NTRIPClient( &ctx );
	}

	Serial.println("Requesting MountPoint's Raw data");
	if(!ntrip_c->reqRaw(host,httpPort,mntpnt,user,passwd)){
		delay(15000);
		ESP.restart();
	}
	Serial.println("Requesting MountPoint is OK");
  
	strcpy( saveFileName, "" );
	fileSaved = 0;
	fileSaving = 0;
	saveIndex = 0;
	ntripCount = 0;
	ntripLastCount = 0;
	ntripLastCountMillis = millis();
	ntripIdx = 0;
	solutionRate = 1;	// number of solution per second
}

void loop() {
	// put your main code here, to run repeatedly:
	
	int nret,numBytes;
	
	M5.update();
	if(ntrip_c->available()) {
		char ch = ntrip_c->read();
		ntripBuff[ ntripIdx++ ] = ch;
		if ( ntripIdx == NTRIP_BUFF_MAX || ch == '\n' ){
			nret = f9pWrite( ntripBuff, ntripIdx );
			if ( nret > 0 ) ntripCount += nret;
			ntripIdx = 0;
		}
		ntripLastMillis = millis();
	}

	//if ( F9P_IF_UART && Serial1.available() > 0) myGPS.checkUblox();
	myGPS.checkUblox();
	msecSolution = 1000 / solutionRate - 1;
	if (millis() - gpsLastMillis > msecSolution){
		gpsLastMillis = millis();
		double lat = myGPS.getLatitude() / E7;
		double lon = myGPS.getLongitude() / E7;
		double alt = myGPS.getAltitude() / E3;
		int fix = myGPS.getCarrierSolutionType();
		
		//M5.Lcd.clear(BLACK);
		M5.Lcd.setCursor(0,0);
		 M5.Lcd.printf("LAT=%.8lf\r\n\r\n", lat);
		 M5.Lcd.printf("LON=%.8lf\r\n\r\n", lon);
		 M5.Lcd.printf("ALT=%.3lf\r\n\r\n", alt);
		 M5.Lcd.printf("FIX=%u  SPS=%d\r\n\r\n", fix, solutionRate);

		 wifiConnected =  (WiFi.status() == WL_CONNECTED) ;
		 
		 if ( fileSaving ){
			//Serial.println("file saving");
			//Serial.println(saveFileName);
			if ( strlen( saveFileName ) == 0 ){
				nret = setFilePath( myGPS.getYear(), myGPS.getMonth() , myGPS.getDay(),
							myGPS.getHour(), myGPS.getMinute(), myGPS.getSecond() );
				if ( nret < 0 ) {
					fileSaved = -1;
					fileSaving = 0;
					strcpy( saveFileName, "" );
				}
			}
			if ( strlen( saveFileName ) > 0 ){
				numBytes = setNmeaData();
				if ( numBytes > 0 ){
					nret = sdSave( saveFileName, saveBuff, numBytes );
					if ( nret == numBytes ) fileSaved = 1;
					else fileSaved = 0;
				}
			}
		}
		else fileSaved = 0;
		
		//BT
		if ( BT_ENABLE ){
			numBytes = setNmeaData();
			if ( numBytes > 0 ){
				bts.print( saveBuff );
			}
		}
		
			
		M5.Lcd.printf("SAVED=%d\r\n", fileSaved);
		 
		// NTRIP bytes per second
		int n = ntripCount - ntripLastCount;
		unsigned long milis = millis();
		unsigned long spanMillis = milis - ntripLastCountMillis;
		if ( spanMillis > 0 ){
			int bps = n * 8 * 1000 / spanMillis;
			ntripLastCountMillis = milis;
			ntripLastCount = ntripCount;
		
			 M5.Lcd.printf("NTRIP=%d bps\r\n", bps );
		}
		 
		if ( millis() - ntripLastMillis > 5000 ){	// 5sec no ntrip data
			if(!ntrip_c->reqRaw(host,httpPort,mntpnt,user,passwd)){
				Serial.println("ntrip reconnct failed");
			}
		}
	 }
	 
	 if ( M5.BtnA.wasReleased() ){
		fileSaving = ! fileSaving;
		Serial.println(fileSaving);
		if ( ! fileSaving ) strcpy( saveFileName, "" );
	}

	 if ( M5.BtnB.wasReleased() ){
		if ( solutionRate == 1 ) solutionRate = 5;
		else solutionRate = 1;
		myGPS.setNavigationFrequency( solutionRate );
	}
}

// ZED-F9Pにデータを送る
//
int f9pWrite( char *buff, int numBytes )
{
	int nret,bytesWritten;
	if (F9P_IF_UART) bytesWritten = Serial1.write( (byte *)buff, numBytes );
	else {
		Wire.beginTransmission( f9pI2cAddr );
		bytesWritten = Wire.write( (byte *)buff, numBytes );
		nret = Wire.endTransmission();
		if ( nret ) bytesWritten = -nret;
	}

	return bytesWritten;
}

// 保存用ファイル名バッファ(saveFileName)にファイル名をセットする。
//
int setFilePath( int year, int month, int day, int hour, int minute, int sec)
{
	char buff[256];

	sprintf( buff, "/gpslog/%d%02d%02d", year, month, day );
	if ( ! SD.exists( buff ) ){
		if ( ! SD.mkdir( buff ) ) return -1;
	}
	sprintf( saveFileName, "%s/gps%02d%02d%02d.log", buff, hour, minute, sec );
	return 0;
}

// NMEA RMC,GGAセンテンスを作成して保存用バッファ(saveBuff)に格納
//
// 戻り値＝バッファに保存したデータのバイト数
//
int setNmeaData( )
{
	char buff[256];
	
	int year = myGPS.getYear();
	int month = myGPS.getMonth();
	int day = myGPS.getDay();
	int hour = myGPS.getHour();
	int minute = myGPS.getMinute();
	int sec = myGPS.getSecond();
	int msec = myGPS.getMillisecond();
	
	int dmy = day * 10000 + month * 100 + ( year - 2000 );
	int hms = hour * 10000 + minute * 100 + sec;
	int csec = msec / 10;
	
	double lat = myGPS.getLatitude() / E7;
	double lon = myGPS.getLongitude() / E7;
	double alt = myGPS.getAltitude() / E3;
	double altMSL = myGPS.getAltitudeMSL() / E3;
	double geoid = alt - altMSL;
	
	int fixType = myGPS.getFixType();	 //0=no fix, 1=dead reckoning, 2=2D, 3=3D, 4=GNSS, 5=Time fix
	int rtk = myGPS.getCarrierSolutionType(); //0=No solution, 1=Float solution, 2=Fixed solution

	int fixGGA = 0;
	if ( rtk > 0 ){
		switch ( rtk ){
			case 1: fixGGA = 5; break;
			case 2: fixGGA = 4; break;
		}
	}
	else{
		switch ( fixType ){
			case 1: fixGGA = 6; break;
			case 2: fixGGA = 1; break;
			case 3: fixGGA = 1; break;
			case 4: fixGGA = 1; break;
		}
	}
	
	double speed = mmsec2knots( myGPS.getGroundSpeed() );	// knots
	double heading = myGPS.getHeading() / E7;
	int numSats = myGPS.getSIV();
	double pdop = myGPS.getPDOP();

	// RMC
	sprintf( buff, "$GPRMC,%06d.%02d,A,%.5lf,N,%.5lf,E,%.3lf,%.1lf,%06d,,",
					hms, csec, deg2degmin(lat), deg2degmin(lon), speed, heading, dmy );
	unsigned char csum = checksumOf( buff + 1, strlen( buff ) - 1 );
	sprintf( buff + strlen( buff ), "*%02x\r\n", csum );
	strcpy( saveBuff, buff );

	//GGA
	sprintf( buff, "$GPGGA,%06d.%02d,%.5lf,N,%.5lf,E,%d,%d,%.1lf,%.3lf,M,%.3lf,,",
				hms, csec, deg2degmin(lat), deg2degmin(lon),  fixGGA, numSats, pdop, altMSL, geoid );
	csum = checksumOf( buff + 1, strlen( buff ) - 1 );
	sprintf( buff + strlen( buff ), "*%02x\r\n", csum );
	strcat( saveBuff, buff );
				
	return strlen(saveBuff);					
}

double mmsec2knots( double mm )
{
	return ( mm / 514.444 );
}

double deg2degmin( double degree )
{
	int deg = (int)degree;
	double min = (degree - deg) * 60;
	double degmin = deg * 100 + min;
	return degmin;
}
	

int sdSave( char *fileName, char *buff, int numBytes )
{
   fd = SD.open(fileName, FILE_APPEND);
   if (! fd) return -1;

	int nret = fd.write( ( unsigned char *) buff, numBytes );
	if ( nret != numBytes ) nret = -2;
	fd.close();
	
	return nret;
}

// Checksum を返す。
//	
unsigned char checksumOf( char* pbuff, int NumOfBytes )
{
	int i;
	unsigned char *buff;
	
	buff = (unsigned char*) pbuff;
	
	unsigned char c = *buff++;
	for ( i = 1; i < NumOfBytes; i++ )  c = c ^ *buff++ ;
	
	return c;
}