#include "gps.h"

#include <math.h>
#include <stdlib.h>
#include <stdlibm.h>
//#include <string.h>

#define  LCD_RS         PIN_B1      // rizeni registru LCD displeje
#define  LCD_E          PIN_B0      // enable LCD displeje
#define  LCD_DATA_LSB   PIN_B4      // pripojeni LSB bitu datoveho portu LCD displeje
#include "MYLCD.C"

// NMEA
#define  TIME        6
#define  TIME_LEN    10
#define  LATDEG      19
#define  LATMIN      21
#define  LONDEG      32
#define  LONMIN      34
#define  DEG_LEN     2
#define  MIN_LEN     7
#define  AZIMUTH     46

float lat,lon;
int16 az;

inline void read_NMEA()
{
//      auto char line[70];
      auto char item[16];
      char *ptr,*line;
      auto int8  n;
      auto char  c;

      line=malloc(72);
      if (line==NULL) {printf(lcd_putc,"Error"); sleep();};
      line[71]=0;

      while(fgetc(NMEA)!='$');
      for(n=0;n<71;n++)
      {
         c=fgetc(NMEA);
         if(c=='*')
         {
            line[n]=0;
            break;
         };
         line[n]=c;
      };

      lcd_gotoxy(12,4);
      printf(lcd_putc,"%c %06.0g ",line[17],strtod(&line[TIME],&ptr));

      lcd_gotoxy(1,3);
      strncpy(item,&line[LATDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0;
      printf(lcd_putc,"%s*",item);
      strncpy(item,&line[LONDEG],DEG_LEN+MIN_LEN); item[DEG_LEN+MIN_LEN]=0;
      printf(lcd_putc,"%s*",item);
      lcd_gotoxy(1,4);
//      strncpy(item,&line[AZIMUTH],6); item[6]=0;
//      printf(lcd_putc,"%s*",item);

      lcd_gotoxy(1,2);
      lat=strtod(&line[LATMIN],&ptr);
      line[LATMIN]=0;
      lat+=60*strtod(&line[LATDEG],&ptr);
      lon=strtod(&line[LONMIN],&ptr);
      line[LONMIN]=0;
      lon+=60*strtod(&line[LONDEG],&ptr);
      strtod(&line[AZIMUTH],&ptr);
      az=strtoul(ptr+1,&ptr,10);
      printf(lcd_putc,"%.3g*%.3g*%03Lu",lat,lon,az);
      free(line);
}

void main()
{

   setup_adc_ports(NO_ANALOGS);
   setup_adc(ADC_OFF);
   setup_spi(FALSE);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DISABLED,0,1);
   setup_comparator(NC_NC_NC_NC);
   setup_vref(VREF_LOW|-2);
   lcd_init();

//   gets(line,NMEA); // Dummy read form the GPS


   while(TRUE)
   {
      const float gc_lat=49*60+15.574,gc_lon=14*60+42.331; // Cisticka

      read_NMEA();

      lon=(gc_lon-lon)*1214;
      lat=(gc_lat-lat)*1854;

lcd_gotoxy(1,4);
      printf(lcd_putc,"%.0g* ",sqrt((lon*lon) + (lat*lat)));
lcd_gotoxy(1,1);
      if (lat==0) lat=0.001;
      lon=3-(6/PI*atan2(lat,lon));
      if (lon<0) lon+=12;
      if (lon>12) lon-=12;
      printf(lcd_putc,"BE%2.0g*",lon);
      lat=12.0/360*az;
      lon=lon-lat;
      if (lon<0) lon+=12;
      if (lon>12) lon-=12;
      printf(lcd_putc,"HE%2.0g*AZ%2.0g* ",lon,lat);
   }

}