#define HWSerial Serial1
const int sentenceSize = 80;

char sentence[sentenceSize];
int incomingByte;

void setup() {
  // put your setup code here, to run once:
  delay(1000);
  Serial.begin(9600);
  HWSerial.begin(115200);
  Serial.print(1);
//  byte softwareVersion[]={0xA0, 0xA1, 0x00, 0x02, 0x03, 0x00, 0x03, 0x0D, 0x0A};//
//  byte updateRateCmd[] = {0xA0, 0xA1, 0x00, 0x03, 0x0E, 0x14, 0x00, 0x1A, 0x0D, 0x0A}; //
//  byte baudrateCmd[] = {0xA0, 0xA1, 0x00, 0x04, 0x05, 0x00, 0x05, 0x00, 0x00, 0x0D, 0x0A}; //
//  byte NMEAmessage[] = {0xA0, 0xA1, 0x00, 0x09, 0x08, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x0D, 0x0A};//
    byte NMEAmessage1[] = {0xA0, 0xA1, 0x00, 0x09, 0x08, 0x01, 0x00, 0x00};//
    byte NMEAmessage2[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x0D, 0x0A};//
//    HWSerial.write(NMEAmessage1, sizeof(NMEAmessage1));
////    HWSerial.flush();
//    Serial.print(2);
//    HWSerial.write(NMEAmessage2, sizeof(NMEAmessage2));
//    HWSerial.flush();
//    Serial.print(3);
//  delay(10);
//    HWSerial.write(softwareVersion, sizeof(softwareVersion));
//    HWSerial.write(baudrateCmd, sizeof(baudrateCmd));
//  HWSerial.flush();
//  Serial.print(1);
//  delay(10);
//  HWSerial.begin(115200);
//  HWSerial.write(updateRateCmd, sizeof(updateRateCmd));
//  HWSerial.flush();
//   HWSerial.write(NMEAmessage, sizeof(NMEAmessage));
//   HWSerial.flush();
}

void loop() {
  // put your main code here, to run repeatedly:
  static int i = 0;
  if (HWSerial.available()>0)
    {
      char ch = HWSerial.read();
    if (ch != '\n' && i < sentenceSize)
    {
      sentence[i] = ch;
      i++;
    }
    else
    {
     sentence[i] = '\0';
     i = 0;
     displayGPS();
    }
  }
}

void displayGPS()
{
  char field[20];
  getField(field, 0);
//  if (strcmp(field, "$GPRMC") == 0)
  if (strcmp(field, "$GPGGA") ==0)
  {
  //first two
    Serial.print("one two ");
    getField(field, 1);  // number
    Serial.print(field);
    getField(field, 2); // N/S
    Serial.print(field);

    Serial.print("Lat: ");
    getField(field, 3);  // number
    Serial.print(field);
    getField(field, 4); // N/S
    Serial.print(field);

    Serial.print(" Long: ");
    getField(field, 5);  // number
    Serial.print(field);
    getField(field, 6);  // E/W
    Serial.print(field);


    Serial.print("six seven: ");
    getField(field, 6);  // number
    Serial.print(field);
    getField(field, 7); // N/S
    Serial.print(field);

    Serial.print(" Altitude: ");
    getField(field, 9);
    Serial.println(field);
  }
}

void getField(char* buffer, int index)
{
  int sentencePos = 0;
  int fieldPos = 0;
  int commaCount = 0;
  while (sentencePos < sentenceSize)
  {
    if (sentence[sentencePos] == ',')
    {
      commaCount ++;
      sentencePos ++;
    }
    if (commaCount == index)
    {
      buffer[fieldPos] = sentence[sentencePos];
      fieldPos ++;
    }
    sentencePos ++;
  }
  buffer[fieldPos] = '\0';
}