#include <avr/io.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "gps.h"
#include "uart.h"
#include "math.h"

void uart1_put_vario();
void beep_GPS_connected();
void beep_GPS_disconnected();

uint8_t RXi;
uint8_t RXj;
char    RXc;
uint8_t RX_BUF_R=0; // Buf Read
uint8_t RX_BUF_P=0; // Buf Parse
//uint8_t RX_reading=0;

#define EATH_RADIUS 6372795

#define RX_BUF_SIZE 100
#define RX_BUF_X 3
//static char RXBuffer[RX_BUF_SIZE];
static char RXBuffer[RX_BUF_X][RX_BUF_SIZE];

void gps_clear_RXBuffer(uint8_t buf_x)
{
  
  for (RXi=0;RXi<RX_BUF_SIZE;RXi++)
    RXBuffer[buf_x][RXi] = 0;
  RXi = 0;
 
}

void gps_getc()
{
	RXc = uart_getc();
	uart1_putc(RXc);

	if (GPS.reading < 5)
		GPS.reading++;

	if ((RXc != 255) && (RXc != 0) && (RXc != 10)) {
		if (RXc != 13) {  // Не конец строки
			RXBuffer[RX_BUF_R][RXi] = RXc;
			RXi++;
		}
		else {
			if (GPS.plugged < 2)
				GPS.plugged ++;

			// Если одна из нужных строк, переходим на следующую. Иначе использовать текущую строку буфера
			if((!strncmp(RXBuffer[RX_BUF_R], "$GPGGA", 6)) | (!strncmp(RXBuffer[RX_BUF_R], "$GPZDA", 6)) | (!strncmp(RXBuffer[RX_BUF_R], "$GPVTG", 6))) {
				RX_BUF_R++;
				if (RX_BUF_R > RX_BUF_X - 1) {
					RX_BUF_R = 0;
				}
				RXi = 0;

			} else {
				gps_clear_RXBuffer(RX_BUF_R);
			}
		}
	}

	if (RXi > RX_BUF_SIZE-1) {
		gps_clear_RXBuffer(RX_BUF_R);
	}
}

void gps_init()
{
	/*
	uart_puts(GPS_CMD0);
	uart_putc('\r');
	uart_putc('\n');

	uart_puts(GPS_CMD1);
	uart_putc('\r');
	uart_putc('\n');
	*/
}

void gps_go_next()
{
	while(RXBuffer[RX_BUF_P][RXj++] != ',');
}

double gps_get_value()
{
  return strtod(&RXBuffer[RX_BUF_P][RXj], NULL);
}


long gps_get_long_value()
{
	long result = 0;
	int counter = 0;
	
	while ( ((RXBuffer[RX_BUF_P][RXj] > 45) && (RXBuffer[RX_BUF_P][RXj] < 58) ) && (counter < 8) )
	{
		if (RXBuffer[RX_BUF_P][RXj] != '.') {
			if ( (RXBuffer[RX_BUF_P][RXj] =='0') && (counter == 0) ) {

			}
			else {
			  result = result * 10;
			  result += RXBuffer[RX_BUF_P][RXj] - 48;
			  counter++;
			}
		}
		RXj++;

	}
	return result;
}

void gps_connected() {
	beep_GPS_connected();
}

void gps_disconnected() {
	beep_GPS_disconnected();
}

void gps_parse(void)
{
	RXj = 0;
	RX_BUF_P = 0;
	if(!strncmp(RXBuffer[RX_BUF_P], "$GPGGA", 6)) {
		//uart1_putc('!');
		gps_go_next(); //Время
	
		gps_go_next(); //Широта
		GPS.latitude = gps_get_long_value();
	
		gps_go_next(); //N
		GPS.latitude_c = RXBuffer[RX_BUF_P][RXj];

		gps_go_next(); //Долгота
		GPS.longitude = gps_get_long_value();

		gps_go_next(); //E
		GPS.longitude_c = RXBuffer[RX_BUF_P][RXj];

		gps_go_next(); //Коол.во спутников
		GPS.satels = gps_get_value();

		gps_go_next(); //GPS fix

		gps_go_next(); //HDOP, горизонтальная точность 
				
		gps_go_next(); // Высота
		GPS.altitude = gps_get_value();

		gps_go_next(); // Геоидальное различие
		GPS.geodeff = gps_get_value();

		if (GPS.latitude != 0) {
			if (GPS.actual == 0)
				gps_connected();
			GPS.actual = 1;
		}
		else {
			if (GPS.actual == 1)
				gps_disconnected();
			GPS.actual = 0;
		}
	}

	RXj = 0;
	RX_BUF_P = 1;
	if(!strncmp(RXBuffer[RX_BUF_P], "$GPVTG", 6)) {
		//uart1_putc('@');
		gps_go_next();	//Направление курса в градусах
		GPS.course = gps_get_value();

		gps_go_next();  //T
				
		gps_go_next();  //Магнитное склонение в градусах
		gps_go_next();  //M

		gps_go_next();  //скорость (узлов в час)
		gps_go_next();  //N

		gps_go_next();  //скорость (км в час)
		GPS.speed = gps_get_value();
	}

	RXj = 0;
	RX_BUF_P = 2;
	if(!strncmp(RXBuffer[RX_BUF_P], "$GPZDA", 6)) {
		//uart1_putc('#');
		gps_go_next();	//время
		GPS.Time = gps_get_value();

		gps_go_next();  //число
		GPS.Day = gps_get_value();

		gps_go_next();  //месяц
		GPS.Month = gps_get_value();

		gps_go_next();  //год
		GPS.Year = gps_get_value();

		// Выводим данные Варио
		uart1_put_vario();
	}
	
gps_clear_RXBuffer(0);
gps_clear_RXBuffer(1);
gps_clear_RXBuffer(2);
RX_BUF_R = 0;

	/*
	gps_clear_RXBuffer(RX_BUF_P);
	
	
	RX_BUF_P++;
	if (RX_BUF_P > RX_BUF_X - 1)
		RX_BUF_P = 0;	
	FLAG_GPS_PARSE = 0;
	*/
}

//-----------------------------------------
/*
double gps_convert_to_grad (unsigned long int GPS_DATA) {
double grad, min;
	grad= floor(GPS_DATA / 1000000);
	min=((double)GPS_DATA - grad*1000000)/10000;
	grad = grad + (min/60);
	//grad = (grad * M_PI)/180;
	return grad;
}
*/

unsigned long int gps_convert_to_grad (unsigned long int GPS_DATA) {
unsigned long int grad, min;
	grad= (GPS_DATA / 1000000)*1000000;
	min=((GPS_DATA - grad)*100)/60;
	return grad + min;
}

//---------------------------------------

//-----------------------------------------
double gps_convert_to_rad (unsigned long int GPS_DATA, char c) {
double rad;
	rad = (double)gps_convert_to_grad(GPS_DATA)* M_PI/1000000/180;
	if ((c=='S') | (c=='W')) 
		rad = -1*rad;
	return rad;
}
//-----------------------------------------


double gps_distance() {
double lat1_cos, lat2_cos, lat1_sin, lat2_sin;
double lat1, long1, lat2, long2;
double sin_delta_long, cos_delta_long;
double y, x;

	//lat1 = gps_convert(49034724);
	//long1 = gps_convert(33242705);
	lat1 = gps_convert_to_rad(GPS.latitude, GPS.latitude_c);
	long1 = gps_convert_to_rad(GPS.longitude, GPS.longitude_c);

	//lat2 = gps_convert_to_rad(49034568); // Координаты Басейна
	//long2 = gps_convert_to_rad(33243322);
	lat2 = gps_convert_to_rad(GPS_Current_Point.latitude, GPS_Current_Point.latitude_c); // Координаты точки
	long2 = gps_convert_to_rad(GPS_Current_Point.longitude, GPS_Current_Point.longitude_c);


	lat1_cos = cos(lat1);
	lat2_cos = cos(lat2);
	lat1_sin = sin(lat1);
	lat2_sin = sin(lat2);

	sin_delta_long = sin(long2-long1);
	cos_delta_long = cos(long2-long1);

	y=sqrt(pow(lat2_cos*sin_delta_long, 2)+pow(lat1_cos*lat2_sin-lat1_sin*lat2_cos*cos_delta_long,2));
	x=lat1_sin*lat2_sin+lat1_cos*lat2_cos*cos_delta_long;

	return (atan2(y,x) * EATH_RADIUS)/1000;
}

double gps_angle() {
double lat1, long1, lat2, long2;
double dlon_W, dlon_E, dphi, atn2, dlon, tc;
int sign;

	//lat1 = gps_convert(49034724);
	//long1 = gps_convert(33242705);
	lat1 = gps_convert_to_rad(GPS.latitude, GPS.latitude_c);
	long1 = gps_convert_to_rad(GPS.longitude, GPS.longitude_c);

	//lat2 = gps_convert_to_rad(49034568); // Координаты Басейна
	//long2 = gps_convert_to_rad(33243322);
	lat2 = gps_convert_to_rad(GPS_Current_Point.latitude, GPS_Current_Point.latitude_c); // Координаты точки
	long2 = gps_convert_to_rad(GPS_Current_Point.longitude, GPS_Current_Point.longitude_c);

	dlon_W = (long2 - long1) - (2*M_PI*(floor((long2 - long1)/(2*M_PI))));
	dlon_E = (long1 - long2) - (2*M_PI*(floor((long1 - long2)/(2*M_PI))));
	dphi = log((tan((lat2/2) + (M_PI/4)))/(tan((lat1/2) + (M_PI/4))));

	if (dlon_W < dlon_E){
		dlon_W = -1*dlon_W;
		//get sign
		if (dlon_W >= 0) {
			sign = 1;                         // Если не в ту сторонк заменить на -1
		}
		else {
			sign = -1;                        // Если не в ту сторонк заменить на 1
		}
  
		if (abs(dlon_W) >= abs(dphi)) {
			atn2 = (sign * M_PI/2) - atan(dphi / dlon_W);
		}
		else {
			if (dphi > 0) {
				atn2 = atan(dlon_W / dphi);
			}
			else {
				if ((-1*dlon_W) >= 0) {
					atn2 = M_PI + atan(dlon_W / dphi);
				}
				else {
					atn2 = (-1*M_PI) + atan(dlon_W / dphi);
				}
			}
		}
	}
	else {
		//get sign
		if (dlon_W >= 0) {
			sign = 1 ;
		}
		else {
			sign = -1;
		}
		if (abs(dlon_E) >= abs(dphi)) {
			if (dlon_E > 0)
				atn2 = sign * M_PI/2 - atan(dphi / (dlon_E));
			else
				atn2 = 0;
		}
		else {
			if (dphi > 0) {
				atn2 = atan((dlon_E) / dphi);
			}
			else {
				if ((dlon_E) >= 0) {
					atn2 = M_PI + atan((dlon_E) / dphi);
				}
				else {
					atn2 = (-1*M_PI) + atan((dlon_E) / dphi);
				}
			}
		}
		dlon = dlon_E;
	}

	tc = atn2 - (2*M_PI*(floor((atn2)/(2*M_PI))));
	return 360-((tc*180)/M_PI);

}
