/**** GPS skrysohledac ****/
#define ID "$Id: gps.c 984 2008-01-06 23:41:31Z kakl $"

#include "gps.h"

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

#define  LCD_RS         PIN_B1      // LCD control
#define  LCD_E          PIN_B2      // LCD enable
#define  LCD_DATA_LSB   PIN_B4      // LSB data bit LCD
#include "MYLCD.C"

// NMEA
#define  TIME           6           // For NMEA parsing
#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
#define  NMEA_LINESIZE  60

#define  MAXGC          4
struct geocache
{
   float lat;
   float lon;
   char  name[8];
};

struct geocache const gc[MAXGC] =
{
  {49*60+15.5740,14*60+42.3310,"DOMA"},  // Testovaci souradnice: cisticka
  {48*60+57.966,14*60+28.655,"GC16JMH"},
  {48*60+57.837,14*60+28.599,"GC16F8Y"},
  {49*60+9.542,14*60+56.417,"GC-----"}
};

float lat,lon;    // Latitude, Longitude, Azimuth
int16 az;
int8 gcnum;       // Selection of the GC point

//$GPRMC,105815.503,V,4915.5877,N,01442.3896,E,0.0,0.00,060108,,,N*44
inline void read_NMEA()       // NMEA parsing
{
      auto char line[NMEA_LINESIZE];
      auto char item[16];
      auto char *ptr;
//      auto char *line;
      auto int8  n;
      auto char  c;

//      line=malloc(NMEA_LINESIZE);        // Space for one line
//      if (line==NULL) {printf(lcd_putc,"Error"); sleep();};
      line[NMEA_LINESIZE-1]=0;

      while(fgetc(NMEA)!='$');            // Waiting for start character
      for(n=0;n<(NMEA_LINESIZE-1);n++)    // Read line up to checksum
      {
         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,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();

   gcnum++;                            // Next GC
   if (gcnum>(MAXGC-1)) gcnum=0;

   delay_ms(200);
 lcd_gotoxy(1,2);
   printf(lcd_putc,"%s",gc[gcnum].name);  // Print GC name

   while(TRUE)
   {
      read_NMEA();   // Read one NMEA line and parse

      lon=(gc[gcnum].lon-lon)*1214;     // Kakona's projection :)
      lat=(gc[gcnum].lat-lat)*1854;

    lcd_gotoxy(1,4);
      printf(lcd_putc,"%.0gm ",sqrt((lon*lon) + (lat*lat))); // Distance
    lcd_gotoxy(1,1);
      if (lon==0) lon=0.001;           // Divided by zero cure
      lon=3-(6/PI*atan2(lat,lon));     // Bearing
      if (lon<0) lon+=12;              // overflow cure
      if (lon>12) lon-=12;
      printf(lcd_putc,"BE%2.0gh*",lon);
      lat=12.0/360*az;                 // Azimuth of the movement
      lon=lon-lat;                     // Heading
      if (lon<0) lon+=12;              // overflow cure
      if (lon>12) lon-=12;
      printf(lcd_putc,"HE%2.0gh*AZ%2.0gh*",lon,lat);
   }
}