#include <stdio.h>
#include <ctype.h>
#include <string.h>
//
#include <Arduino.h>
#include "GpsNEO6.h"
//
#define UartPC Serial
#define UartLRS Serial1
#define UartGPS Serial2
//
const int PIN_LEDSYSTEM = 2;
const int PIN_RXD1 = 16;
const int PIN_TXD1 = 17;
const int PIN_RXD2 = 25;
const int PIN_TXD2 = 26;
//
//
CGpsNEO6 GpsNEO6;
int GPSYear;
byte GPSMonth, GPSDay, GPSHours, GPSMinutes, GPSSeconds, GPSMillis;
unsigned long GPSFixAge;
float GPSLatitude, GPSLongitude, GPSAltitude;
unsigned short GPSCountSatellites;
unsigned long GPSCountCharacters;
unsigned short GPSCountGood, GPSCountFailed;
//
char LRSBuffer[256];
int LRSBufferIndex = 0;
int LRSBlockIndex = 0;
//
char GPSBuffer[256];
//
void setup()
{
  UartPC.begin(115200);// Error! , PIN_RXD0, PIN_TXD0);
  UartLRS.begin(115200, SERIAL_8N1, PIN_RXD1, PIN_TXD1);
  UartGPS.begin(9600, SERIAL_8N1, PIN_RXD2, PIN_TXD2);
  delay(333);
  pinMode(PIN_LEDSYSTEM, OUTPUT);
  digitalWrite(PIN_LEDSYSTEM, LOW);
  //
  UartPC.print("\r\n# **************************");
  UartPC.print("\r\n# * Esp32Esp32LRSCollector *");
  UartPC.print("\r\n# * Version: 01V04         *");
  UartPC.print("\r\n# * Date...: 211123        *");
  UartPC.print("\r\n# * Time...: 1332          *");
  UartPC.print("\r\n# * Author.: FM            *");
  UartPC.print("\r\n# **************************");
  //
  memset(LRSBuffer, 0x00, 256);
  LRSBufferIndex = 0;
  LRSBlockIndex = 0;
  //
  memset(GPSBuffer, 0x00, 256);
}

void loop()
{ //---------------------------------------------------------------
  // LRS - DecodeData
  //---------------------------------------------------------------
  if (UartLRS.available())
  {
    char C = UartLRS.read();
    // Filter for LRS-Incoming-Data
    if (isblank(C) || isalpha(C) || isdigit(C) || ('+' == C) || 
        ('-' == C) || ('.' == C) || ('\r' == C) || ('\n' == C))
    {
      switch (C)
      {
        case '\r': case '\n':
          LRSBuffer[LRSBufferIndex] = 0x00;
          if (0 < strlen(LRSBuffer))
          {
            if (isalpha(LRSBuffer[0]) && (0 == LRSBlockIndex))
            { 
              UartPC.print("\r\nLRS>HEADER[");
              UartPC.print(LRSBuffer);
              UartPC.print("]");
              LRSBlockIndex = 1;
            }
            else
            if (isdigit(LRSBuffer[0]) || ('+' == LRSBuffer[0]) || 
               ('-' == LRSBuffer[0]) || ('.' == LRSBuffer[0]))
            { 
              switch (LRSBlockIndex)
              {
                case 1:
                  UartPC.print("\r\nLRS>FI00C[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 2:
                  UartPC.print("\r\nLRS>FI08C[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 3:
                  UartPC.print("\r\nLRS>FI16C[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 4:
                  UartPC.print("\r\nLRS>FI24C[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 5:
                  UartPC.print("\r\nLRS>FI00H[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 6:
                  UartPC.print("\r\nLRS>FI08H[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 7:
                  UartPC.print("\r\nLRS>FI16H[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 8:
                  UartPC.print("\r\nLRS>FI24H[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 9:
                  UartPC.print("\r\nLRS>FI00A[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 10:
                  UartPC.print("\r\nLRS>FI08A[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 11:
                  UartPC.print("\r\nLRS>FI16A[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 12:
                  UartPC.print("\r\nLRS>FI24A[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 13:
                  UartPC.print("\r\nLRS>HK00C[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 14:
                  UartPC.print("\r\nLRS>HK00H[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex++;
                  break;
                case 15:
                  UartPC.print("\r\nLRS>HK00A[");
                  UartPC.print(LRSBuffer);
                  UartPC.print("]");
                  LRSBlockIndex = 0;
                  break;
              }
            }
          }
          memset(LRSBuffer, 0x00, 256);
          LRSBufferIndex = 0;
          break;
        default:
          LRSBuffer[LRSBufferIndex] = C;
          LRSBufferIndex++;
          if (255 <= LRSBufferIndex)
          {
            LRSBufferIndex = 254;
          }
          LRSBuffer[LRSBufferIndex] = 0x00;
          break;
      }
    }
  }
  //---------------------------------------------------------------
  //  GPS - DecodeData
  //---------------------------------------------------------------
  if (UartGPS.available())
  {
    char C = UartGPS.read();
    // debug UartPC.print(C);
    if (GpsNEO6.encode(C))
    {
      GpsNEO6.ReadDateTime(&GPSYear, &GPSMonth, &GPSDay, &GPSHours, &GPSMinutes, 
                          &GPSSeconds, &GPSMillis, &GPSFixAge);
      GpsNEO6.GetLocation(&GPSLatitude, &GPSLongitude, &GPSFixAge);
      GPSAltitude = GpsNEO6.GetAltitude();
      GpsNEO6.GetStatistics(&GPSCountCharacters, &GPSCountGood, &GPSCountFailed);
      GPSCountSatellites = GpsNEO6.GetCountSatellites();
      //
      sprintf(GPSBuffer, "\r\nGPS>DATE[%02d.%02d.%04d] TIME[%02d:%02d:%02d.%03d]",
                         GPSDay, GPSMonth, GPSYear, 
                         GPSHours, GPSMinutes, GPSSeconds, GPSMillis);
      UartPC.print(GPSBuffer);
      UartPC.print("\r\nGPS>LATITUDE[");
      UartPC.print(GPSLatitude == CGpsNEO6::GPS_INVALID_F_ANGLE ? 0.0 : GPSLatitude, 6);
      UartPC.print("]deg LONGITUDE[");
      UartPC.print(GPSLongitude == CGpsNEO6::GPS_INVALID_F_ANGLE ? 0.0 : GPSLongitude, 6);
      UartPC.print("]deg ALTITUDE[");
      UartPC.print(GPSAltitude == CGpsNEO6::GPS_INVALID_F_ALTITUDE ? 0.0 : GPSAltitude, 3);    
      UartPC.print("]m");
      //    
      UartPC.print("\r\nGPS>SATELLITES[");
      UartPC.print(GPSCountSatellites);
      UartPC.print("] PRECISION[");
      UartPC.print(GpsNEO6.hdop());
      UartPC.print("] CHARACTERS[");
      UartPC.print(GPSCountCharacters);
      UartPC.print("] GOOD[");
      UartPC.print(GPSCountGood);
      UartPC.print("] ERRORS[");
      UartPC.print(GPSCountFailed);
      UartPC.print("]");
    }  
  }
}







// { //---------------------------------------------------------------
//   // LRS - RawData
//   //---------------------------------------------------------------
//   if (UartLRS.available())
//   {
//     char C = UartLRS.read();
//     // Filter for LRS-Incoming-Data
//     if (isblank(C) || isalpha(C) || isdigit(C) || ('+' == C) || 
//         ('-' == C) || ('.' == C) || ('\r' == C) || ('\n' == C))
//     {
//       switch (C)
//       {
//         case '\r': case '\n':
//           LRSBuffer[LRSBufferIndex] = 0x00;
//           if (0 < strlen(LRSBuffer))
//           {
//             UartPC.print("\r\nLRS>DATA[");
//             UartPC.print(LRSBuffer);
//             UartPC.print("]");
//           }
//           memset(LRSBuffer, 0x00, 256);
//           LRSBufferIndex = 0;
//           break;
//         default:
//           LRSBuffer[LRSBufferIndex] = C;
//           LRSBufferIndex++;
//           if (255 <= LRSBufferIndex)
//           {
//             LRSBufferIndex = 254;
//           }
//           LRSBuffer[LRSBufferIndex] = 0x00;
//           break;
//       }
//     }
//   }




  //
  //---------------------------------------------------------------
  //  LRS - RawData
  //---------------------------------------------------------------
  // if (UartLRS.available())
  // {
  //   char C = UartLRS.read();
  //   // Filter for LRS-Incoming-Data
  //   if (isblank(C) || isalpha(C) || isdigit(C) || ('+' == C) || 
  //       ('-' == C) || ('.' == C) || ('\r' == C) || ('\n' == C))
  //   {
  //     switch (C)
  //     {
  //       case '\r': case '\n':
  //         LRSBuffer[LRSBufferIndex] = 0x00;
  //         if (0 < strlen(LRSBuffer))
  //         {
  //           UartPC.print("\r\nLRS>");
  //           UartPC.print(LRSBuffer);
  //         }
  //         memset(LRSBuffer, 0x00, 256);
  //         LRSBufferIndex = 0;
  //         break;
  //       default:
  //         LRSBuffer[LRSBufferIndex] = C;
  //         LRSBufferIndex++;
  //         if (255 <= LRSBufferIndex)
  //         {
  //           LRSBufferIndex = 254;
  //         }
  //         LRSBuffer[LRSBufferIndex] = 0x00;
  //         break;
  //     }
  //   }
  // }
  //
  // //---------------------------------------------------------------
  // //  GPS - RawData
  // //---------------------------------------------------------------
  // if (UartGPS.available())
  // {
  //   char C = UartGPS.read();
  //   // debug UartPC.print(C);
  //   // Filter for GPS-Incoming-Data
  //   if (((' ' <= C) && (C <= '~')) || ('\r' == C) || ('\n' == C))
  //   {
  //     switch (C)
  //     {
  //       case 0x0D: case 0x0A:
  //         GPSBuffer[GPSBufferIndex] = 0x00;
  //         if (0 < strlen(GPSBuffer))
  //         {
  //           //UartPC.print("\r\nGPS>");
  //           //UartPC.print(GPSBuffer);
  //         }
  //         memset(GPSBuffer, 0x00, 256);
  //         GPSBufferIndex = 0;
  //         break;
  //       default:
  //         GPSBuffer[GPSBufferIndex] = (char)C;
  //         GPSBufferIndex++;
  //         if (255 <= GPSBufferIndex)
  //         {
  //           GPSBufferIndex = 254;
  //         }
  //         GPSBuffer[GPSBufferIndex] = 0x00;
  //         break;
  //     }
  //   }
  // }
  //

    // debug Serial.write(c); 
  //   if (GpsNEO6.encode(C))
  //   {
  //     GpsNEO6.ReadDateTime(&GPSYear, &GPSMonth, &GPSDay, &GPSHours, &GPSMinutes, 
  //                         &GPSSeconds, &GPSMillis, &GPSFixAge);
  //     GpsNEO6.GetLocation(&GPSLatitude, &GPSLongitude, &GPSFixAge);
  //     GPSAltitude = GpsNEO6.GetAltitude();
  //     GpsNEO6.GetStatistics(&GPSCountCharacters, &GPSCountGood, &GPSCountFailed);
  //     GPSCountSatellites = GpsNEO6.GetCountSatellites();
  //     //
  //     Serial.print("\r\n-------------------------------------------------");
  //     sprintf(Buffer, "\r\nDATE[%02d.%02d.%04d] TIME[%02d:%02d:%02d.%03d]",
  //                     GPSDay, GPSMonth, GPSYear, 
  //                     GPSHours, GPSMinutes, GPSSeconds, GPSMillis);
  //     UartPC.print(Buffer);
  //     UartPC.print("\r\nLATITUDE[");
  //     UartPC.print(GPSLatitude == CGpsNEO6::GPS_INVALID_F_ANGLE ? 0.0 : GPSLatitude, 6);
  //     UartPC.print("]deg LONGITUDE[");
  //     UartPC.print(GPSLongitude == CGpsNEO6::GPS_INVALID_F_ANGLE ? 0.0 : GPSLongitude, 6);
  //     UartPC.print("]deg ALTITUDE[");
  //     UartPC.print(GPSAltitude == CGpsNEO6::GPS_INVALID_F_ALTITUDE ? 0.0 : GPSAltitude, 3);    
  //     UartPC.print("]m");
  //     //    
  //     UartPC.print("\r\nSATELLITES[");
  //     UartPC.print(GPSCountSatellites);
  //     UartPC.print("] PRECISION[");
  //     UartPC.print(GpsNEO6.hdop());
  //     UartPC.print("] CHARACTERS[");
  //     UartPC.print(GPSCountCharacters);
  //     UartPC.print("] GOOD[");
  //     UartPC.print(GPSCountGood);
  //     UartPC.print("] ERRORS[");
  //     UartPC.print(GPSCountFailed);
  //     UartPC.print("]");
  //   }
  //   digitalWrite(PIN_LEDSYSTEM, LOW);
  // }



  // LoopLRS = true;
  // while (LoopLRS)
  // {
  //   char C = UartLRS.read();
  //   switch (C)
  //   {
  //     case '\r': case '\n':
  //       LRSBuffer[LRSBufferIndex] = 0x00;
  //       if (0 < strlen(LRSBuffer))
  //       {
  //         UartPC.println(LRSBuffer);
  //       }
  //       memset(LRSBuffer, 0x00, 256);
  //       LRSBufferIndex = 0;
  //       LoopLRS = false;
  //       break;
  //     default:
  //       LRSBuffer[LRSBufferIndex] = C;
  //       LRSBufferIndex++;
  //       if (255 <= LRSBufferIndex)
  //       {
  //         LRSBufferIndex = 254;
  //       }
  //       LRSBuffer[LRSBufferIndex] = 0x00;
  //       break;
  //   }
  //   LoopLRS &= UartLRS.available();
  // }
  // digitalWrite(PIN_LEDSYSTEM, LOW);
  //
  // while (UartGPS.available())
  // {
  //   digitalWrite(PIN_LEDSYSTEM, HIGH);
  //   //
  //   char C = UartGPS.read();
  //   // debug Serial.write(c); 
  //   if (GpsNEO6.encode(C))
  //   {
  //     GpsNEO6.ReadDateTime(&GPSYear, &GPSMonth, &GPSDay, &GPSHours, &GPSMinutes, 
  //                         &GPSSeconds, &GPSMillis, &GPSFixAge);
  //     GpsNEO6.GetLocation(&GPSLatitude, &GPSLongitude, &GPSFixAge);
  //     GPSAltitude = GpsNEO6.GetAltitude();
  //     GpsNEO6.GetStatistics(&GPSCountCharacters, &GPSCountGood, &GPSCountFailed);
  //     GPSCountSatellites = GpsNEO6.GetCountSatellites();
  //     //
  //     Serial.print("\r\n-------------------------------------------------");
  //     sprintf(Buffer, "\r\nDATE[%02d.%02d.%04d] TIME[%02d:%02d:%02d.%03d]",
  //                     GPSDay, GPSMonth, GPSYear, 
  //                     GPSHours, GPSMinutes, GPSSeconds, GPSMillis);
  //     UartPC.print(Buffer);
  //     UartPC.print("\r\nLATITUDE[");
  //     UartPC.print(GPSLatitude == CGpsNEO6::GPS_INVALID_F_ANGLE ? 0.0 : GPSLatitude, 6);
  //     UartPC.print("]deg LONGITUDE[");
  //     UartPC.print(GPSLongitude == CGpsNEO6::GPS_INVALID_F_ANGLE ? 0.0 : GPSLongitude, 6);
  //     UartPC.print("]deg ALTITUDE[");
  //     UartPC.print(GPSAltitude == CGpsNEO6::GPS_INVALID_F_ALTITUDE ? 0.0 : GPSAltitude, 3);    
  //     UartPC.print("]m");
  //     //    
  //     UartPC.print("\r\nSATELLITES[");
  //     UartPC.print(GPSCountSatellites);
  //     UartPC.print("] PRECISION[");
  //     UartPC.print(GpsNEO6.hdop());
  //     UartPC.print("] CHARACTERS[");
  //     UartPC.print(GPSCountCharacters);
  //     UartPC.print("] GOOD[");
  //     UartPC.print(GPSCountGood);
  //     UartPC.print("] ERRORS[");
  //     UartPC.print(GPSCountFailed);
  //     UartPC.print("]");
  //   }
  //   digitalWrite(PIN_LEDSYSTEM, LOW);
  // }
//



  // digitalWrite(PIN_LEDSYSTEM, HIGH);
  // while(UartLRS.available())
  // {
  //   char C = UartLRS.read();
  //   // Filter for LRS-Incoming-Data
  //   if (isblank(C) || isalpha(C) || isdigit(C) || ('+' == C) || 
  //       ('-' == C) || ('.' == C) || ('\r' == C) || ('\n' == C))
  //   {
  //     UartPC.print(C);
  //   }
  //   else
  //   {
  //     // debug UartPC.print('?');
  //   }
  // }
  // digitalWrite(PIN_LEDSYSTEM, LOW);
