#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(9600);
  Serial.print(1);
  byte updateRateCmd[] = {0xA0, 0xA1, 0x00, 0x03, 0x0E, 0x14, 0x00, 0x1A, 0x0D, 0x0A}; //  for 10 Hz
  byte baudrateCmd[] = {0xA0, 0xA1, 0x00, 0x04, 0x05, 0x00, 0x05, 0x00, 0x00, 0x0D, 0x0A}; //  for 115200
  byte NMEAmessage[] = {0xA0, 0xA1, 0x00, 0x09, 0x08, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x0D, 0x0A};// for altitude
    HWSerial.write(baudrateCmd, sizeof(baudrateCmd));
    HWSerial.flush();
    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';
}