//*****************************************************************************
//***   GPS CZ,no terminal,no time,no position,no math atan2,no save
//***          yes - arrow, distanc, load GCPOINT from PC

#include "GPS_LCD_876_A2.h"
#include <math.h>
#include <stdlib.h>

//*****************************************************************************
//***   defines   ***

#define LCD_CS    PIN_B0
#define LCD_DATA  PIN_B1
#define LCD_CLK   PIN_B2
#define LCD_A0    PIN_B3
#define LCD_RESET PIN_B4
#include "LCD_ALCATEL.C"

#define ARROW_UR  0x1
#define ARROW_UL  0x0
#define ARROWR "\xf\x3\x5\x9\x10\x10\x10"
#define ARROWL "\x1e\x18\x14\x12\x1\x1\x1"

#define TAN22  0.4142
#define TAN67  2.4241

#define MAX_GC_POINT 2

//*****************************************************************************
//***   functions   ***

void NMEA_part_scan(int8 *point);
void NMEA_GPRMC(void);
void read_ee_gc(int8 numgc);
int8 atan2_my(float x,float y);
void calc_distanc_azimut(void);
void PC_part_scan(int8 *point);
void terminal(void);
void main(void);

//*****************************************************************************
//***   global variables   ***

int8 sel;
int8 azimut;
float lon,lat;
float gc_lon,gc_lat;
int8 la[10];
int8 lo[11];

//*****************************************************************************

void NMEA_part_scan(int8 *point)
{
   int8 a;
   a=0;
   while (a!=',') {a=fgetc(GPS);*(point++)=a;}
   *(--point)=0x00;
}

//$GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,,*10
void NMEA_GPRMC(void)
{
   int8 status;
   int8 n_s;
   int8 e_w;
   int8 course[7];

   while (fgetc(GPS)!='$');                                  //$
   while (fgetc(GPS)!=',');                                  //GPRMC,
   while (fgetc(GPS)!=',');//NMEA_part_scan(&time[0]);       //UTC,
   status=fgetc(GPS);fgetc(GPS);                             //status,
   NMEA_part_scan(&la[0]);                                   //latitude,
   n_s=fgetc(GPS);fgetc(GPS);                                //N/S,
   NMEA_part_scan(&lo[0]);                                   //longtitude,
   e_w=fgetc(GPS);fgetc(GPS);                                //E/W,
   while (fgetc(GPS)!=',');   //NMEA_part_scan(&speed[0]);   //speed,
   NMEA_part_scan(&course[0]);//while (fgetc(GPS)!=',');     //course,
   //NMEA_part_scan(&date[0]);                               //date,

   LCD_gotoxy(8,1);
   LCD_putc(status);

   lat=600.0*(la[0]-0x30)+60.0*(la[1]-0x30)+atof(&la[2]);
   lon=600.0+60.0*(lo[2]-0x30)+atof(&lo[3]);
   azimut=atol(course)/360*8;
}

void read_ee_gc(int8 numgc)
{
   int8 a,b;
   int8 *point;

   a=(13*numgc) + 1;          //5*char + 4byte float LAT + 4byte float LON
   LCD_gotoxy(1,1);
   LCD_putc('G');LCD_putc('P');
   for(b=0;b<5;b++)LCD_putc(read_eeprom(a++));
   point=&gc_lat;
   for (b=0;b<4;b++) *(point++)=read_eeprom(a++);
   point=&gc_lon;
   for (b=0;b<4;b++) *(point++)=read_eeprom(a++);
}

int8 atan2_my(float x,float y)
{
   float z;
   int8 arg;
   if (y==0) z=abs(x)/0.0001;
   else z=abs(x)/abs(y);
   if (z < TAN22) arg=0;
   else
   {
      if (z < TAN67) arg=2;
      else arg=3;
   }
   if (x>=0 && y<=0) return (4-arg);
   if (x<0 && y<=0) return (4+arg);
   if (x<0 && y>0) return (8-arg);
   return arg;
}

void calc_distanc_azimut(void)
{
   lon=(gc_lon-lon)*1214;     // metry (uhly v minutach)
   lat=(gc_lat-lat)*1854;     // metry (uhly v minutach)

   lcd_gotoxy(3,2);
   printf(LCD_putc,"%5Lum",(int32) sqrt((lon*lon) + (lat*lat)));

   azimut=atan2_my(lon,lat)-azimut;
   if (azimut<0) azimut+=8;
   if (azimut>8) azimut-=8;

   lcd_gotoxy(1,2);
   switch (azimut)
   {
    case 0: LCD_send_data(ASCII_ARROW_UP);break;
    case 1: LCD_send_data(ARROW_UR);break;
    case 2: LCD_send_data(ASCII_ARROW_R);break;
    case 7: LCD_send_data(ARROW_UL);break;
    case 6: LCD_send_data(ASCII_ARROW_L);break;
    default :LCD_send_data(ASCII_ARROW_DOWN);
   }
}

void PC_part_scan(int8 *point)
{
   int8 a;
   a=0;
   while (a!=',') {a=fgetc(PC); *(point++)=a;}
   *(--point)=0x00;
}

// line format:  name,float,float,
#INT_RDA
void terminal(void)
{
   int8 a,b;
   int8 *point;
   char name[6];
   restart_wdt();
   PC_part_scan(name);
   PC_part_scan(la);
   PC_part_scan(lo);
      lat=60*atof(la);
      lon=60*atof(lo);
      a=read_eeprom(0);
      if (a > MAX_GC_POINT) a=0;
      write_eeprom(0,a+1);
      sel=a-1;
      a=(13*a)+1;
      for (b=0;b<5;b++) write_eeprom(a++,name[b]);
      point=&lat;
      for (b=0;b<4;b++) write_eeprom(a++,*(point+b));
      point=&lon;
      for (b=0;b<4;b++) write_eeprom(a++,*(point+b));
}

void main(void)
{
   setup_adc_ports(NO_ANALOGS);
   setup_adc(ADC_OFF);
   setup_spi(SPI_MASTER|SPI_L_TO_H|SPI_CLK_DIV_4);
   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(FALSE);
   setup_wdt(WDT_2304MS);

   enable_interrupts(INT_RDA);
   enable_interrupts(GLOBAL);

      output_low(LCD_RESET);
      output_high(LCD_CS);
      delay_ms(5);
      output_high(LCD_RESET);
      delay_ms(5);
      LCD_send_command(0x80);
      printf(LCD_send_data,ARROWL);
      LCD_send_command(0x88);
      printf(LCD_send_data,ARROWR);
      
   LCD_init();
   LCD_cursor_off();

   sel++;
   if (sel > MAX_GC_POINT) sel=0;
   read_ee_gc(sel);

   while (TRUE)
   {
      restart_wdt();
      NMEA_GPRMC();
      calc_distanc_azimut();
   }
}