#include "bexus_gps.h"

#define PMTK_SET_NMEA_UPDATE_5HZ  "$PMTK220,200*2C"
#define PMTK_API_SET_FIX_CTL_5HZ  "$PMTK300,200,0,0,0,0*2F"
#define PMTK_SET_BAUD_57600 "$PMTK251,57600*2C"
#define PMTK_SET_NMEA_OUTPUT_RMCGGA "$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28"

const uint8_t ubxRate5Hz[] = {0x00,200,0x00,0x01,0x00,0x01};
/*
const char disableRMC[] = "PUBX,40,RMC,0,0,0,0,0,0";
const char disableGLL[] = "PUBX,40,GLL,0,0,0,0,0,0";
const char disableGSV[] = "PUBX,40,GSV,0,0,0,0,0,0";
const char disableGSA[]  = "PUBX,40,GSA,0,0,0,0,0,0";
const char disableGGA[] = "PUBX,40,GGA,0,0,0,0,0,0";
const char disableVTG[] = "PUBX,40,VTG,0,0,0,0,0,0";
const char disableZDA[] = "PUBX,40,ZDA,0,0,0,0,0,0";

const char enableRMC[] = "PUBX,40,RMC,0,1,0,0,0,0";
const char enableGLL[] = "PUBX,40,GLL,0,1,0,0,0,0";
const char enableGSV[] = "PUBX,40,GSV,0,1,0,0,0,0";
const char enableGSA[] = "PUBX,40,GSA,0,1,0,0,0,0";
const char enableGGA[] = "PUBX,40,GGA,0,1,0,0,0,0";
const char enableVTG[] = "PUBX,40,VTG,0,1,0,0,0,0";
const char enableZDA[] = "PUBX,40,ZDA,0,1,0,0,0,0";

const char baud9600  [] = "PUBX,41,1,3,3,9600,0";
const char baud38400 [] = "PUBX,41,1,3,3,38400,0";
const char baud57600 [] = "PUBX,41,1,3,3,57600,0";
*/
const char baud115200[] = "PUBX,41,1,3,3,115200,0";

uint8_t aRxBuffer[1];
volatile bool newData = false;

uint32_t GPS_RX_ISR_FLAG = 1;

unsigned long chars;
unsigned short sentences, failed;

float flat, flon;
unsigned long age;
uint32_t count = 0;
int year;
uint8_t month, day, hour, minute, second, hundredths;


extern DMA_HandleTypeDef hdma_usart2_rx;
uint8_t sendConfigToGps(uint8_t cls, uint8_t id, uint16_t len, uint8_t payload[]);

void Gps_Init() {
	//HAL_UART_Transmit(&huart2, (uint8_t *)aRxBuffer, 1,0xFFFF);
	/*
	HAL_UART_Transmit(&huart2, (uint8_t*)PMTK_SET_NMEA_UPDATE_5HZ, sizeof(PMTK_SET_NMEA_UPDATE_5HZ), 100);
	HAL_Delay(50);
	HAL_UART_Transmit(&huart2, (uint8_t*)PMTK_API_SET_FIX_CTL_5HZ, sizeof(PMTK_API_SET_FIX_CTL_5HZ), 100);
	HAL_Delay(50);
	HAL_UART_Transmit(&huart2, (uint8_t*)PMTK_SET_NMEA_OUTPUT_RMCGGA, sizeof(PMTK_SET_NMEA_OUTPUT_RMCGGA), 100);
	HAL_Delay(50);
	HAL_UART_Transmit(&huart2, (uint8_t*)PMTK_SET_BAUD_57600, sizeof(PMTK_SET_BAUD_57600), 100);
	HAL_Delay(50);
	*/
	
	//sendConfigToGps(6, 8, 6, (uint8_t*)ubxRate5Hz);
	//	while(millis()<250);
	//HAL_Delay(250);
	//HAL_UART_Transmit(&huart2, (uint8_t*)baud115200, sizeof(baud115200), 100);
	
	
	HAL_UART_DeInit(&huart2);
	huart2.Init.BaudRate = 9600; // 57600
	HAL_UART_Init(&huart2);
	HAL_UART_MspInit(&huart2);
	
	//__HAL_UART_FLUSH_DRREGISTER(&huart2);
	HAL_UART_Receive_DMA(&huart2, aRxBuffer, 1);
}



int Gps_Receive() {
	
	count++;
	
  if (newData) {
   f_get_position(&flat, &flon, &age);
		Dataset_Master[buff_sensor_data_index].latitude = flat;
		Dataset_Master[buff_sensor_data_index].longitude = flon;
		Dataset_Master[buff_sensor_data_index].hdop = _hdop;
		Dataset_Master[buff_sensor_data_index].altitude = f_altitude();

		crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
		
		Dataset_Master[buff_sensor_data_index].time = 
			hundredths + (
			second + (
			minute + (
			hour * 24UL) * 60UL) * 60UL) * 1000UL;
		
		newData = false;
  }
  
	stats(&chars, &sentences, &failed);

	return HAL_OK;
}


uint8_t sendConfigToGps(uint8_t cls, uint8_t id, uint16_t len, uint8_t payload[]) {
	
	uint8_t cfg_msg[len + 8];
	cfg_msg[0] = 0xb5;
	cfg_msg[1] = 0x62;
	cfg_msg[2] = cls;
	cfg_msg[3] = id;
	cfg_msg[4] = len;
	cfg_msg[5] = len>>8;
	
	memcpy(&cfg_msg[6], payload, len);
	
	uint8_t CK_A = len+6;
	uint8_t CK_B = len+7;
	
	for(int i=2; i<CK_A; i++) {
		cfg_msg[CK_A] = cfg_msg[CK_A] + cfg_msg[i];
		cfg_msg[CK_B] = cfg_msg[CK_B] + cfg_msg[CK_A];
	}

	HAL_UART_Transmit(&huart2, cfg_msg, len+8, 5000);
	
	uint8_t ack[4];
	/*
	do {
		HAL_UART_Receive(&huart2, ack, 1, 5000);
	} while (ack[0] != 0xb5);
	*/
	HAL_UART_Receive(&huart2, ack, 4, 5000); //ACK: B5 62 05 01 NAK: B5 62 05 00
	return ack[3];
	
}