#include <gps.h>

byte CFG_RST[12] = {0xb5, 0x62, 0x06, 0x04, 0x04, 0x00, 0x00, 0x00, 0x01,0x00, 0x0F, 0x66};

void Gps::init()
{
  Serial.begin(9600);
  Serial.setTimeout(2);
  Serial.write(CFG_RST, sizeof(CFG_RST)); // Soft Reset GPS on Start
  satsInView.begin(tGps, "GPGSV", 3); // NMEA Sentence GPGSV, Element 3 (Sat in View)
}

void Gps::encode()
{       
    int data;
    unsigned long previousMillis = millis();

    while((previousMillis + 100) > millis())
    {
        while (Serial.available() )
        {
            char data = Serial.read();
            tGps.encode(data);
        }
    }
     //Serial.println("");
}

bool Gps::buildPacket(uint8_t txBuffer[9])
{
  LatitudeBinary = ((tGps.location.lat() + 90) / 180.0) * 16777215;
  LongitudeBinary = ((tGps.location.lng() + 180) / 360.0) * 16777215;
  
  txBuffer[0] = ( LatitudeBinary >> 16 ) & 0xFF;
  txBuffer[1] = ( LatitudeBinary >> 8 ) & 0xFF;
  txBuffer[2] = LatitudeBinary & 0xFF;

  txBuffer[3] = ( LongitudeBinary >> 16 ) & 0xFF;
  txBuffer[4] = ( LongitudeBinary >> 8 ) & 0xFF;
  txBuffer[5] = LongitudeBinary & 0xFF;

  altitudeGps = tGps.altitude.meters();
  txBuffer[6] = ( altitudeGps >> 8 ) & 0xFF;
  txBuffer[7] = altitudeGps & 0xFF;

  hdopGps = tGps.hdop.value()/10;
  txBuffer[8] = hdopGps & 0xFF;

  return true;
}

void Gps::gdisplay(uint16_t dispBuffer[])
{
  dispBuffer[0] = tGps.hdop.value()/10;
  dispBuffer[1] = TinyGPSPlus::parseDecimal(satsInView.value());
  dispBuffer[2] = tGps.satellites.value();
  dispBuffer[3] = tGps.altitude.meters();
  dispBuffer[4] = tGps.speed.kmph();
}

bool Gps::checkGpsFix()
{
  encode();
  if (tGps.location.isValid() && 
    tGps.location.age() < 4000 &&
    tGps.hdop.isValid() &&
    tGps.hdop.value() <= 600 &&
    tGps.hdop.age() < 4000 &&
    tGps.altitude.isValid() && 
    tGps.altitude.age() < 4000 )
  {
    return true;
  }
  else
  {
    return false;
  }
}