/*  2018.1. 25  mtakapii
  GPS_Direction_Finder.ino

  GPS Radar ==> LCD & Servo Motor (R/C)
  EWireless module ... MONO WIRELESS TWE-Lite-RED,RED
        'D'mode, Packet, LF Trigger, 10mW,
  EA3 tact SW ON ==> Disp Antenna MAX heght

  E"Binboudeiino" by aitendo
  ELCD 1602D1 by Strawberry-Linux or
        AQM1602Y-NLW-BBW  by Akizukidensi
  ENMEA Sample
    $GPGGA,093053.000,3539.****,N,13932.****,E,2,7,1.54,
        47.1,M,39.3,M,0000,0000*6D   <== Chofu City
    $GPGGA,121914.000,3537.****,N,13918.****,E,1,10,0.84,
        137.1,M,39.2,M,,*67           <== Hachioji City

    A...target position,   B...reference position

  ref:http://keisan.casio.jp/exec/system/1257670779
      https://blogs.yahoo.co.jp/nobita_rx7/27644291.html
*/

//#define SERIAL_OUT
#include <Wire.h>
#include <ST7032.h>
//#include <TinyGPS++.h>

#define LED    13
ST7032 lcd;

char  Ant_char[5];
float Distance = 0,  Direction = 0;   // ‹——£,•ϋŠp
int   int_Distance = 0, int_Direction = 0;
float Antenna_A = 46, Antenna = 46, Antenna_MAX = 0;
float North = 35, East = 139;
int   SATE;

void setup() {
  pinMode(LED, OUTPUT);
  pinMode(A3, INPUT);
  Serial.begin(38400);
  delay(500);

#ifdef SERIAL_OUT
  Serial.println("GPS Radar V13 2018.1.24  ");
  Serial.println("=> AQM1602Y-NLW-BBW");
  Serial.println("=> Direction by Servo Motor");
#endif

  lcd.begin(16, 2);
  delay(40);
  lcd.setContrast(40);
  lcd.setCursor(0, 0);
  lcd.print("GPS radar V14   ");
  lcd.setCursor(0, 1);
  lcd.print("AQM1602Y-NLW-BBW");

  term();
  S_out('A',  0); TGL_LED();
  delay(2000);
}

void loop() {
  /*
    float LatA  =  35.****;   //A Ό‚W‘O ˆά“xN, –kˆά
    float LongA = 139.****;   //A Ό‚W‘O Œo“xE@“ŒŒo
    float LatB  =  35.****;   //B Œπ”Τ‘O ˆά“xN@–kˆά
    float LongB = 139.****;   //B Œπ”Τ‘O Œo“xE@“ŒŒo
  */
  /******  A...Base,   B...Target Constant table ***************/
  float c_LatA  =  35.39****;   // R/C Airport in Fuchu
  float c_LongA = 139.*******;   //
  float c_LatB  =  35.******;   // Traffic Safety Building
  float c_LongB = 139.******;   //
  /******  A...Base,   B...Target Variable table ***************/
  float LatA  ;   // R/C Airport in Fuchu
  float LongA ;   //
  float LatB  ;   // Traffic Safety Building
  float LongB ;   //
  /************************************************************/
  //Calculate from AirPort to Traffic Building Direction and Distance
  Direction =
    atan2( (c_LongA - c_LongB) * 1.23, (c_LatA - c_LatB)) * 57.3 + 180;
  Distance  =
    sqrt(pow(c_LongA - c_LongB, 2) + pow(c_LatA - c_LatB, 2)) * 99096.44, 0;

  lcd_clear();
  lcd.setCursor(0, 0);
  lcd.print("Traffic Building");
  delay(2000);
  lcd_clear();
  lcd.setCursor(0, 0);
  if ( Direction > 90 && Direction <= 270) {
    Direction -= 90;
    lcd.print(Direction, 2);
    lcd.print(" Southern");
  }
  int_Direction = Direction;
  lcd.setCursor(0, 1);
  lcd.print(Distance, 1);
  lcd.print(" m");
  term();
  S_out('A', int_Direction);
  delay(3000);

#ifdef SERIAL_OUT
  Serial.println("");
  Serial.println("Sample measuring");
  Serial.print("Direction = ");    //–Ϊ“I’nB‚Μ•ϋŠp(‹j
  Serial.print(Direction, 6);
  Serial.println(" [deg.]");
  Serial.print("Distance = ");     //–Ϊ“I’nB–˜‚Μ‹——£(m)
  Serial.print(Distance, 6);
  Serial.println(" [m]");
  Serial.println("");
#endif

  average();

  LatA  = ( North / 100 );
  LongA = ( East  / 100 );
  Antenna_A = Antenna;      // Šξ€ƒAƒ“ƒeƒi‚

#ifdef SERIAL_OUT
  Serial.print("LatA = ");
  Serial.print(LatA, 6);
  Serial.print(" [deg.]");
  Serial.print("  :  LongA =");
  Serial.print(LongA, 6);
  Serial.println(" [m]");
  Serial.println("");
#endif

  lcd_clear();
  lcd.setCursor(0, 0);
  lcd.print("LatA  = ");
  //lcd.setCursor(7, 0);
  lcd.println(LatA);
  lcd.setCursor(0, 1);
  lcd.print("LongA = ");
  //lcd.setCursor(8, 1);
  lcd.print(LongA);

  delay(2000);
  lcd_clear();
  lcd.setCursor(0, 0);
  lcd.print("Antenna = ");
  lcd.print(Antenna_A, 1);
  delay(2000);

  while (1) {
    char D1, D2, D3;

    get_N_E();            // Get Target Position
    LatB  = North / 100.;
    LongB = East / 100;

    Direction =
      atan2( (LongA - LongB) * 1.23, (LatA - LatB)) * 57.3 + 180;
    Distance  =
      sqrt(pow(LongA - LongB, 2) + pow(LatA - LatB, 2)) * 99096.44, 0;

    if ( Distance < 480 ) {             // =480 m/s  -> 800km/h

#ifdef SERIAL_OUT
      Serial.print("Direction = ");    //–Ϊ“I’nA‚Μ•ϋŠp(‹j
      Serial.print(Direction, 6);
      Serial.println(" [deg.]");
      Serial.print("Distance = ");     //–Ϊ“I’nA–˜‚Μ‹——£(m)
      Serial.print(Distance, 6);
      Serial.println(" [m]");
#endif

      lcd_clear();
      /******************** Direction *********************/
      int_Direction = Direction;
      lcd.setCursor(0, 0);
      if ( int_Direction >= 0 && int_Direction <= 90 ) {
        int_Direction += 90;
        dtostrf(int_Direction, 3, 0, Ant_char);
        lcd.print(Ant_char);
        lcd.setCursor(4, 0);
        lcd.print("N");
      }
      else if ( int_Direction > 90 && int_Direction <= 270) {
        int_Direction -= 90;
        dtostrf(int_Direction, 3, 0, Ant_char);
        lcd.print(Ant_char);
        lcd.setCursor(4, 0);
        lcd.print("S");
      }
      else if ( int_Direction > 270 && int_Direction <= 361) {
        int_Direction -= 270;
        dtostrf(int_Direction, 3, 0, Ant_char);
        lcd.print(Ant_char);
        lcd.setCursor(4, 0);
        lcd.print("N");
      }
      if ( int_Direction >= 0. && int_Direction <= 180.) {
        S_out('A', int_Direction);    // 'A' ... Servo Name
        /****************** Distance **********************/
        lcd.setCursor(6, 0);
        dtostrf(Distance, 7, 1, Ant_char);
        lcd.print(Ant_char);
        lcd.setCursor(13, 0);
        lcd.print(" Dm");
        /**************** Antenna height ******************/
        Antenna -= Antenna_A;   // Relative Alititude
        lcd.setCursor(0, 1);
        if (Antenna < 0) {
          lcd.print("-");
        }
        else {
          lcd.print(" ");
        }

        Antenna = abs(Antenna);
        if ( Antenna > Antenna_MAX) {
          Antenna_MAX = Antenna;
        }

        dtostrf(Antenna, 5, 1, Ant_char);
        lcd.print(Ant_char);
        lcd.setCursor(7, 1);
        lcd.print("Am");
        /***************  Satellite  ********************/
        lcd.setCursor(11, 1);
        if ( SATE < 10) {
          lcd.print(" ");
        }
        lcd.print(SATE);
        lcd.setCursor(13, 1);
        lcd.print(" Sa");
        /***********************************************/
        TGL_LED();

        if ( analogRead(A3) < 300 ) {
          lcd.setCursor(0, 0);        // Disp MAX height
          lcd.print("Antenna MAX ... ");
          lcd.setCursor(0, 1);
          dtostrf(Antenna_MAX, 5, 1, Ant_char);
          lcd.print(Ant_char);
          lcd.setCursor(6, 1);
          lcd.print(" m        ");
        }

      }
    }
  }
}

void lcd_clear() {
  lcd.setCursor(0, 0);
  lcd.print("                ");
  lcd.setCursor(0, 1);
  lcd.print("                ");
}

/**************************************/
void S_out(char No, int ang) {
  if ( ang > 0 && ang < 180) {
    Serial.print(No);
    Serial.println(ang);
    //delay(30);
  }
}

/**************************************/
void term() {
  Serial.println("");
}

/*************************************/
int tglsw = 0;

void TGL_LED() {
  tglsw++;
  lcd.setCursor(10, 1);
  switch (tglsw) {
    case 0:
      digitalWrite(LED, LOW);
      lcd.print("-");  break;
    case 1:
      digitalWrite(LED, HIGH);
      lcd.print("/");  break;
    case 2:
      digitalWrite(LED, LOW);
      lcd.print("+");  break;
    case 3:
      digitalWrite(LED, HIGH);
      lcd.print("*");
      tglsw = 0;        break;
  }
}

#define MAX_line 200
char line_buf[MAX_line];
char line_tmp[MAX_line];
/***** Get EAST, WEST, and Satellite Number *********/
void get_N_E() {
  char *A;
  int  cnt;
  // Serial.println("get_N_E");
  // clear buf area

  while ( 1 ) {         //
    for ( cnt = 0; cnt < MAX_line - 1; cnt++) {
      line_buf[cnt] = line_tmp[cnt] = 0x20;
    }
    for ( cnt = 0; cnt < MAX_line; cnt++) {
      line_buf[cnt] = line_tmp[cnt] = Read_ZigBee();
      if ( line_buf[cnt] == 0x0d ) {
        //if ( line_buf[cnt] == 0x0a ) {
        break;
      }
      if ( cnt > MAX_line ) {     // 2018.1.24
        break;
      }
    }
    A = strtok(line_buf, ",");  // Separate with ',' =>token pointer

    if ( line_buf[1] == '$' &&
         line_buf[2] == 'G' &&
         line_buf[3] == 'P' &&
         line_buf[4] == 'G' &&
         line_buf[5] == 'G' &&
         line_buf[6] == 'A'  ) {
      A = strtok(NULL, ",");    // Time position
      A = strtok(NULL, ",");    // N position
      North = strtod(A, '.');
      A = strtok(NULL, ",");    // 'N'
      A = strtok(NULL, ",");    // E position
      East = strtod(A, '.');
      A = strtok(NULL, ",");    // 'E'
      A = strtok(NULL, ",");    // differential GPS mode position
      A = strtok(NULL, ",");    // Satellite position
      SATE = strtod(A, '.');
      A = strtok(NULL, ",");    // '.'
      A = strtok(NULL, ",");    // Antenna height
      Antenna = strtod(A, '.');

      if ((North > 3000) && North < 4500 &&
          East > 12800  && East < 14600 ) { // Japan Area
        return;
      }
    }
  }
}

/*************************************/
char Read_ZigBee() {
  char zigbee;
  while (1) {
    if ( Serial.available() > 0 ) {
      zigbee = Serial.read();
      return zigbee;
    }
  }
}

/*************************************/
void average() {
  int i;
  float tmp1, tmp2, tmp3;
  char ave_num = 5;

  lcd.clear();
  delay(100);
  lcd.setCursor(0, 0);
  lcd.print("Now averaging !!");

  tmp1 = 0, tmp2 = 0, tmp3 = 0;
  for ( i = 0; i < ave_num ; i++ ) {
    get_N_E();
    tmp1 += North;
    tmp2 += East;
    tmp3 += Antenna;
    TGL_LED();
    lcd.setCursor(0, 1);
    lcd.print(i);
  }
  North   = tmp1 / ave_num;
  East    = tmp2 / ave_num;
  Antenna = tmp3 / ave_num;
}
/*****************************************/