#include <Wire.h>
#include <TinyGPSPlus.h>

//02-11-2025 Latest
//////////////////////
/*
    get_gps_data();
    sendSMSLocation();
*/
//////////////////////
////////////////////////////////////////////////
String s = "http://www.google.com/maps/place/";//"www.google.com/maps/dir/";
static const uint32_t GPSBaud = 9600;
String Lat = "";
String Lng = "";
int SendSMS_Now = 0;
const size_t BUFSIZE = 300;
char f_buffer[BUFSIZE];
float *f_buf = (float*)f_buffer;
TinyGPSPlus gps;
////////////////////////////////////////////////

////////////////////////////////////////////////
const int MPU_ADDR = 0x68;
// MPU6050 register addresses
const int PWR_MGMT_1 = 0x6B;
const int ACCEL_XOUT_H = 0x3B;
const int GYRO_XOUT_H = 0x43;
const int TEMP_OUT_H = 0x41;
// Calibration offsets
float accelOffsetX = 0;
float accelOffsetY = 0;
float accelOffsetZ = 0;
float gyroOffsetX = 0;
float gyroOffsetY = 0;
float gyroOffsetZ = 0;
// Adaptive thresholds - will adjust based on normal motion
float adaptiveAccelThreshold = 3.0;    // Start with reasonable value
float adaptiveGyroThreshold = 50.0;    // Start with reasonable value
// Impact detection
const unsigned long IMPACT_WINDOW = 100;    // Time to analyze impact (ms)
const unsigned long HIT_COOLDOWN = 2000;    // Prevent multiple detections
const float SUDDEN_CHANGE_FACTOR = 3.0;     // Must be 3x normal motion
// Fall detection parameters
const float FREE_FALL_THRESHOLD = 0.5;      // Low G-force for free fall
const float FALL_IMPACT_THRESHOLD = 4.0;    // G-force for fall impact
const unsigned long FALL_TIME_WINDOW = 3000; // Max time for fall sequence
const unsigned long FALL_COOLDOWN = 5000;   // Cooldown between fall detections
// State variables
bool hitDetected = false;
bool fallDetected = false;
unsigned long lastHitTime = 0;
unsigned long lastFallTime = 0;
unsigned long impactStartTime = 0;
bool analyzingImpact = false;
// Fall detection states
bool inFreeFall = false;
unsigned long fallStartTime = 0;
bool waitingForFallImpact = false;
// For adaptive threshold calculation
float normalAccelMax = 0;
float normalGyroMax = 0;
unsigned long lastNormalUpdate = 0;
const unsigned long NORMAL_LEARNING_TIME = 5000; // Learn normal motion for 5 sec
// Impact analysis
float impactAccelPeaks[10] = {0};
float impactGyroPeaks[10] = {0};
int peakCount = 0;
// Filtering
const int MOVING_AVERAGE_SAMPLES = 5;
float accelBuffer[MOVING_AVERAGE_SAMPLES] = {0};
float gyroBuffer[MOVING_AVERAGE_SAMPLES] = {0};
int bufferIndex = 0;
bool calibrated = false;
const int CALIBRATION_SAMPLES = 100;
bool learningPhase = true;
////////////////////////////////////////////////
#define LED_PIN 18


void setup() {
  Serial.begin(115200);
  Wire.begin();
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, 0);
  // Wake up MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(PWR_MGMT_1);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial.println("HEAD IMPACT & FALL DETECTION SYSTEM");
  Serial.println("Learning normal motion for 5 seconds...");
  Serial.println("Please walk around normally");
  calibrateSensor();
  Serial.println("Calibration complete!");
  Serial.println("======================================");
  lastNormalUpdate = millis();

  setupA9G();
  Serial.println("Setup Executed");
}

void send_SMS_NOW() {
  SendSMS_Now = 1;
}

void calibrateSensor() {
  float accelXSum = 0, accelYSum = 0, accelZSum = 0;
  float gyroXSum = 0, gyroYSum = 0, gyroZSum = 0;

  for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
    int16_t accelX = readSensor(ACCEL_XOUT_H);
    int16_t accelY = readSensor(ACCEL_XOUT_H + 2);
    int16_t accelZ = readSensor(ACCEL_XOUT_H + 4);
    int16_t gyroX = readSensor(GYRO_XOUT_H);
    int16_t gyroY = readSensor(GYRO_XOUT_H + 2);
    int16_t gyroZ = readSensor(GYRO_XOUT_H + 4);

    accelXSum += accelX / 16384.0;
    accelYSum += accelY / 16384.0;
    accelZSum += accelZ / 16384.0;
    gyroXSum += gyroX / 131.0;
    gyroYSum += gyroY / 131.0;
    gyroZSum += gyroZ / 131.0;

    delay(10);
  }

  accelOffsetX = accelXSum / CALIBRATION_SAMPLES;
  accelOffsetY = accelYSum / CALIBRATION_SAMPLES;
  accelOffsetZ = accelZSum / CALIBRATION_SAMPLES - 1.0;

  gyroOffsetX = gyroXSum / CALIBRATION_SAMPLES;
  gyroOffsetY = gyroYSum / CALIBRATION_SAMPLES;
  gyroOffsetZ = gyroZSum / CALIBRATION_SAMPLES;

  calibrated = true;
}

float movingAverage(float buffer[], float newValue) {
  buffer[bufferIndex] = newValue;
  bufferIndex = (bufferIndex + 1) % MOVING_AVERAGE_SAMPLES;

  float sum = 0;
  for (int i = 0; i < MOVING_AVERAGE_SAMPLES; i++) {
    sum += buffer[i];
  }
  return sum / MOVING_AVERAGE_SAMPLES;
}


//////////////////////////////////////////////////////////
///////////////////     LOOP    //////////////////////////
//////////////////////////////////////////////////////////
void loop() {
  digitalWrite(LED_PIN, 1);
  fullGPSSerial();
  readGPS_Data();
  showLATLON();

  if (SendSMS_Now == 1) {
    digitalWrite(LED_PIN, 1);
    delay(50);
    digitalWrite(LED_PIN, 0);
    delay(50);
    digitalWrite(LED_PIN, 1);
    delay(50);
    digitalWrite(LED_PIN, 0);
    delay(50);
    digitalWrite(LED_PIN, 1);
    delay(50);
    digitalWrite(LED_PIN, 0);
    delay(50);
    digitalWrite(LED_PIN, 1);
    delay(50);
    digitalWrite(LED_PIN, 0);
    delay(50);
    get_gps_data();
    sendSMSLocation();
    delay(2000);
    s = "http://www.google.com/maps/place/";//"www.google.com/maps/dir/";
    SendSMS_Now = 0;
  }
  //digitalWrite(LED_PIN, 0);
  // Read sensor data
  int16_t rawAccelX = readSensor(ACCEL_XOUT_H);
  int16_t rawAccelY = readSensor(ACCEL_XOUT_H + 2);
  int16_t rawAccelZ = readSensor(ACCEL_XOUT_H + 4);

  int16_t rawGyroX = readSensor(GYRO_XOUT_H);
  int16_t rawGyroY = readSensor(GYRO_XOUT_H + 2);
  int16_t rawGyroZ = readSensor(GYRO_XOUT_H + 4);

  // Convert and apply calibration
  float accelX_g = rawAccelX / 16384.0 - accelOffsetX;
  float accelY_g = rawAccelY / 16384.0 - accelOffsetY;
  float accelZ_g = rawAccelZ / 16384.0 - accelOffsetZ;

  float gyroX_deg = rawGyroX / 131.0 - gyroOffsetX;
  float gyroY_deg = rawGyroY / 131.0 - gyroOffsetY;
  float gyroZ_deg = rawGyroZ / 131.0 - gyroOffsetZ;

  // Calculate magnitudes with moving average filtering
  float accelMagnitude = sqrt(accelX_g * accelX_g + accelY_g * accelY_g + accelZ_g * accelZ_g);
  float gyroMagnitude = sqrt(gyroX_deg * gyroX_deg + gyroY_deg * gyroY_deg + gyroZ_deg * gyroZ_deg);

  float filteredAccel = movingAverage(accelBuffer, accelMagnitude);
  float filteredGyro = movingAverage(gyroBuffer, gyroMagnitude);

  unsigned long currentTime = millis();

  // Learning phase - understand normal motion
  if (learningPhase) {
    updateNormalMotionLevels(filteredAccel, filteredGyro, currentTime);
  }

  // Run both detection systems
  checkForImpact(filteredAccel, filteredGyro, currentTime);
  checkForFall(filteredAccel, currentTime);

  // Print status
  static unsigned long lastPrint = 0;
  if (currentTime - lastPrint > 1000) {
    printStatus(accelX_g, accelY_g, accelZ_g, gyroX_deg, gyroY_deg, gyroZ_deg,
                filteredAccel, filteredGyro, currentTime);
    lastPrint = currentTime;
  }

  delay(20);

}// end of LOOP
///////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////
void sendSMSLocation() {
  Serial.println("WARNING! Helmit Impact Detected");

  /*
    /////
    Serial2.println("AT+CMGF=1\r");
    delay(1000);

    Serial2.println("AT+CNMI=2,2,0,0,0\r");
    delay(1000);

    Serial2.print("AT+CMGS=\"+966543522266\"\r");//Replace this with your mobile number
    delay(1000);
    Serial2.print(s);
    Serial2.write(0x1A);
    delay(3000);
  */
  /////

  Serial2.println("AT+CMGF=1\r");
  delay(1000);
  Serial2.println("AT+CNMI=2,2,0,0,0\r");
  delay(1000);
  Serial2.print("AT+CMGS=\"+966543522266\"\r");//Replace this with your mobile number
  delay(1000);
  Serial2.print("WARNING! Helmit Impact Detected\n");
  Serial2.print(s);
  Serial2.write(0x1A);

  Serial.println("Sending SMS is Done check your Phone!!!!!!!!!!!!!!!");
  delay(1000);
  s = "http://www.google.com/maps/place/";//"www.google.com/maps/dir/";
}
////////////////////////////////////////////////////////////

void updateNormalMotionLevels(float accel, float gyro, unsigned long currentTime) {
  // Update maximum normal motion levels during learning phase
  if (accel > normalAccelMax) normalAccelMax = accel;
  if (gyro > normalGyroMax) normalGyroMax = gyro;

  // End learning phase after 5 seconds
  if (currentTime - lastNormalUpdate > NORMAL_LEARNING_TIME) {
    learningPhase = false;

    // Set adaptive thresholds based on learned normal motion
    adaptiveAccelThreshold = normalAccelMax * SUDDEN_CHANGE_FACTOR;
    adaptiveGyroThreshold = normalGyroMax * SUDDEN_CHANGE_FACTOR;

    // Ensure minimum thresholds
    if (adaptiveAccelThreshold < 2.0) adaptiveAccelThreshold = 2.0;
    if (adaptiveGyroThreshold < 30.0) adaptiveGyroThreshold = 30.0;

    Serial.println("\n*** LEARNING PHASE COMPLETE ***");
    Serial.print("Normal motion - Max Accel: "); Serial.print(normalAccelMax, 2); Serial.println("g");
    Serial.print("Normal motion - Max Gyro: "); Serial.print(normalGyroMax, 1); Serial.println("°/s");
    Serial.print("Adaptive thresholds - Accel: "); Serial.print(adaptiveAccelThreshold, 2); Serial.println("g");
    Serial.print("Adaptive thresholds - Gyro: "); Serial.print(adaptiveGyroThreshold, 1); Serial.println("°/s");
    Serial.println("Now monitoring for impacts and falls...");
  }
}

void checkForImpact(float accel, float gyro, unsigned long currentTime) {
  // Check if we have a sudden change that might be an impact
  bool suddenChange = (accel > adaptiveAccelThreshold) || (gyro > adaptiveGyroThreshold);

  if (suddenChange && !analyzingImpact) {
    // Start analyzing potential impact
    analyzingImpact = true;
    impactStartTime = currentTime;
    peakCount = 0;
    impactAccelPeaks[0] = accel;
    impactGyroPeaks[0] = gyro;
    Serial.println("💥 Potential impact detected - analyzing...");
  }

  if (analyzingImpact) {
    // Track peaks during impact window
    if (accel > impactAccelPeaks[peakCount]) impactAccelPeaks[peakCount] = accel;
    if (gyro > impactGyroPeaks[peakCount]) impactGyroPeaks[peakCount] = gyro;

    // Check if impact analysis period is over
    if (currentTime - impactStartTime > IMPACT_WINDOW) {
      analyzeImpactPattern(currentTime);
      analyzingImpact = false;
    }

    // Move to next peak every 20ms
    if (currentTime - impactStartTime > (peakCount + 1) * 20 && peakCount < 9) {
      peakCount++;
      impactAccelPeaks[peakCount] = accel;
      impactGyroPeaks[peakCount] = gyro;
    }
  }

  // Reset hit detection after cooldown
  if (currentTime - lastHitTime > HIT_COOLDOWN) {
    hitDetected = false;
  }
}

void analyzeImpactPattern(unsigned long currentTime) {
  // Find maximum values during impact window
  float maxAccel = 0;
  float maxGyro = 0;

  for (int i = 0; i <= peakCount; i++) {
    if (impactAccelPeaks[i] > maxAccel) maxAccel = impactAccelPeaks[i];
    if (impactGyroPeaks[i] > maxGyro) maxGyro = impactGyroPeaks[i];
  }

  // Determine if this is a real head impact
  bool isHeadImpact = verifyHeadImpact(maxAccel, maxGyro);

  if (isHeadImpact && !hitDetected) {
    hitDetected = true;
    lastHitTime = currentTime;


    Serial.println("🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨");
    Serial.println("🚨         HEAD IMPACT DETECTED!        🚨");
    Serial.println("🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨");
    Serial.print("Impact Force: "); Serial.print(maxAccel, 1); Serial.println("g");

    if (maxAccel > 1) {//2.1
      SendSMS_Now = 1;
      Serial.println("Sending SMS - Please Wait ... ");
      Serial.println("Sending SMS - Please Wait ... ");
      Serial.println("Sending SMS - Please Wait ... ");
      delay(5000);
    }
    /*
      Serial.print("Rotational Force: "); Serial.print(maxGyro, 1); Serial.println("°/s");
      Serial.print("Thresholds: "); Serial.print(adaptiveAccelThreshold, 1);
      Serial.print("g / "); Serial.print(adaptiveGyroThreshold, 1); Serial.println("°/s");
      Serial.println("⚠️  CHECK USER FOR CONCUSSION SYMPTOMS!");
      Serial.println();
    */
  } else if (!isHeadImpact) {

    Serial.print("❌ False alarm: ");
    Serial.print(maxAccel, 1); Serial.print("g, ");
    Serial.print(maxGyro, 1); Serial.println("°/s - Normal motion pattern");
    /*
        if (maxAccel > 1) {//2.1
          SendSMS_Now = 1;
          Serial.println("Sending SMS - Please Wait ... ");
          Serial.println("Sending SMS - Please Wait ... ");
          Serial.println("Sending SMS - Please Wait ... ");
          delay(5000);
        }*/
  }
}

bool verifyHeadImpact(float maxAccel, float maxGyro) {
  // Real head impacts typically have:
  // 1. Very high acceleration OR very high rotation
  // 2. Both values significantly above normal motion
  // 3. Specific pattern characteristics

  bool highAccel = maxAccel > (normalAccelMax * 4.0);  // 4x normal motion
  bool highGyro = maxGyro > (normalGyroMax * 4.0);     // 4x normal motion

  // Head impact usually has both linear and rotational components
  bool hasBothComponents = (maxAccel > adaptiveAccelThreshold * 1.2) &&
                           (maxGyro > adaptiveGyroThreshold * 1.2);

  // Or extremely high values in one component
  bool extremeSingleComponent = (maxAccel > adaptiveAccelThreshold * 2.0) ||
                                (maxGyro > adaptiveGyroThreshold * 2.0);

  return (hasBothComponents || extremeSingleComponent) && (highAccel || highGyro);
}

void checkForFall(float accel, unsigned long currentTime) {
  // Fall detection logic: Free fall followed by impact

  // Step 1: Detect free fall (low acceleration)
  if (accel < FREE_FALL_THRESHOLD && !inFreeFall && !waitingForFallImpact) {
    inFreeFall = true;
    fallStartTime = currentTime;
    Serial.println("⚠️  Free fall detected! Monitoring for impact...");
  }

  // Step 2: If we're in free fall, wait for impact
  if (inFreeFall) {
    // Check if free fall period is too long (might not be a real fall)
    if (currentTime - fallStartTime > FALL_TIME_WINDOW) {
      inFreeFall = false;
      Serial.println("❌ Free fall too long - probably not a real fall");
    }

    // Check for impact after free fall
    if (accel > FALL_IMPACT_THRESHOLD) {
      // This is the impact after a fall
      if (!fallDetected && (currentTime - lastFallTime > FALL_COOLDOWN)) {
        fallDetected = true;
        lastFallTime = currentTime;
        inFreeFall = false;
        waitingForFallImpact = false;


        Serial.println("🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨");
        Serial.println("🚨           FALL DETECTED!           🚨");
        Serial.println("🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨 🚨");
        Serial.print("Fall Impact: "); Serial.print(accel, 1); Serial.println("g");
        Serial.print("Free fall duration: ");
        Serial.print(currentTime - fallStartTime);
        Serial.println("ms");
        Serial.println("⚠️  USER MAY HAVE FALLEN - CHECK IMMEDIATELY!");
        Serial.println();

      }
    }
  }

  // Reset fall detection after cooldown
  if (currentTime - lastFallTime > FALL_COOLDOWN) {
    fallDetected = false;
  }

  // Reset free fall state if acceleration returns to normal
  if (inFreeFall && accel > FREE_FALL_THRESHOLD * 2.0) {
    inFreeFall = false;
    waitingForFallImpact = true;
    // Wait a bit to see if impact comes
    if (currentTime - fallStartTime > 1000) {
      waitingForFallImpact = false;
    }
  }
}

void printStatus(float accelX, float accelY, float accelZ, float gyroX, float gyroY, float gyroZ,
                 float accelMag, float gyroMag, unsigned long currentTime) {
  /*
    Serial.print("Accel: ");
    Serial.print(accelX, 2); Serial.print("g, ");
    Serial.print(accelY, 2); Serial.print("g, ");
    Serial.print(accelZ, 2); Serial.print("g");
    Serial.print(" | Total: "); Serial.print(accelMag, 2); Serial.println("g");

    Serial.print("Gyro:  ");
    Serial.print(gyroX, 1); Serial.print("°/s, ");
    Serial.print(gyroY, 1); Serial.print("°/s, ");
    Serial.print(gyroZ, 1); Serial.print("°/s");
    Serial.print(" | Total: "); Serial.print(gyroMag, 1); Serial.println("°/s");
  */
  if (learningPhase) {
    float remaining = (NORMAL_LEARNING_TIME - (currentTime - lastNormalUpdate)) / 1000.0;
    /*
      Serial.print("📚 Learning normal motion: ");
      Serial.print(remaining, 0); Serial.println("s remaining");
      Serial.print("Current max - Accel: "); Serial.print(normalAccelMax, 2);
      Serial.print("g, Gyro: "); Serial.print(normalGyroMax, 1); Serial.println("°/s");
    */
  } else {
    /*
      Serial.print("🎯 Thresholds: "); Serial.print(adaptiveAccelThreshold, 1);
      Serial.print("g / "); Serial.print(adaptiveGyroThreshold, 1); Serial.println("°/s");
    */
    // Status display for both systems
    if (hitDetected) {
      //Serial.println("🚨 HEAD IMPACT DETECTED - CHECK USER!");
    } else if (fallDetected) {
      //Serial.println("🚨 FALL DETECTED - USER MAY HAVE FALLEN!");
    } else if (inFreeFall) {
      //Serial.println("⚠️  Free fall detected - watching for impact...");
    } else if (analyzingImpact) {
      //Serial.println("💥 Analyzing potential impact...");
    } else {
      //Serial.println("✅ Normal - Monitoring impacts & falls");
    }
  }

  Serial.println("-----------------------");
}

int16_t readSensor(uint8_t reg) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 2, true);
  return (Wire.read() << 8) | Wire.read();
}


void get_gps_data() {
  if (gps.location.lat() == 0 || gps.location.lng() == 0) {
    Serial.println("Return Executed");
    s = "http://www.google.com/maps/place/21.413024,39.355396";//"http://www.google.com/maps/dir/21.413024,39.355396";
    Serial.println(s);
    return;
  }
  Serial.print("Latitude (deg): ");
  Lat = String(gps.location.lat(), 6);
  Serial.println(Lat);
  Serial.print("Longitude (deg): ");
  Lng = String(gps.location.lng(), 6);
  Serial.println(Lng);
  Serial.println();
  s += String(gps.location.lat(), 6);
  s += ",";
  s += String(gps.location.lng(), 6);
  s += "/";
  Serial.println(s);
}

void setupA9G() {
  Serial2.begin(GPSBaud);
  Serial.println("Starting...");
  Serial2.println("\r");
  Serial2.println("AT\r");
  delay(10);
  Serial2.println("\r");
  Serial2.println("AT+GPS=1\r");
  delay(100);
  Serial2.println("AT+CREG=2\r");
  delay(6000);
  //ss.print("AT+CREG?\r");
  Serial2.println("AT+CGATT=1\r");
  delay(6000);
  Serial2.println("AT+CGDCONT=1,\"IP\",\"WWW\"\r");
  delay(6000);
  // ss.println("AT+LOCATION=1\r");
  Serial2.println("AT+CGACT=1,1\r");
  delay(6000);
  //Initialize ends
  //Initialize GPS
  Serial2.println("\r");
  Serial2.println("AT+GPS=1\r");
  delay(1000);
  //ss.println("AT+GPSMD=1\r");   // Change to only GPS mode from GPS+BDS, set to 2 to revert to default.
  Serial2.println("AT+GPSRD=10\r");
  delay(100);
  // set SMS mode to text mode
  Serial2.println("AT+CMGF=1\r");
  delay(1000);
  //ss.println("AT+LOCATION=2\r");
  Serial.println(F("FullExample.ino"));
  Serial.println(F("An extensive example of many interesting TinyGPSPlus features"));
  Serial.print(F("Testing TinyGPSPlus library v. ")); Serial.println(TinyGPSPlus::libraryVersion());
  Serial.println(F("by Mikal Hart"));
  Serial.println();
  Serial.println(F("Sats HDOP  Latitude   Longitude   Fix  Date       Time     Date Alt    Course Speed Card  Distance Course Card  Chars Sentences Checksum"));
  Serial.println(F("           (deg)      (deg)       Age                      Age  (m)    --- from GPS ----  ---- to London  ----  RX    RX        Fail"));
  Serial.println(F("----------------------------------------------------------------------------------------------------------------------------------------"));
}

void readGPS_Data() {
  smartDelay(200);
  if (millis() > 5000 && gps.charsProcessed() < 10)
    Serial.println(F("No GPS data received: check wiring"));
}

void showLATLON() {
  Serial.print("Latitude (deg): ");
  Lat = String(gps.location.lat(), 6);
  Serial.println(Lat);
  Serial.print("Longitude (deg): ");
  Lng = String(gps.location.lng(), 6);
  Serial.println(Lng);
  Serial.println();
}


void fullGPSSerial() {
  static const double LONDON_LAT = 51.508131, LONDON_LON = -0.128002;
  printInt(gps.satellites.value(), gps.satellites.isValid(), 5);
  printFloat(gps.hdop.hdop(), gps.hdop.isValid(), 6, 1);
  printFloat(gps.location.lat(), gps.location.isValid(), 11, 6);
  printFloat(gps.location.lng(), gps.location.isValid(), 12, 6);
  printInt(gps.location.age(), gps.location.isValid(), 5);
  printDateTime(gps.date, gps.time);
  printFloat(gps.altitude.meters(), gps.altitude.isValid(), 7, 2);
  printFloat(gps.course.deg(), gps.course.isValid(), 7, 2);
  printFloat(gps.speed.kmph(), gps.speed.isValid(), 6, 2);
  printStr(gps.course.isValid() ? TinyGPSPlus::cardinal(gps.course.deg()) : "*** ", 6);

  unsigned long distanceKmToLondon =
    (unsigned long)TinyGPSPlus::distanceBetween(
      gps.location.lat(),
      gps.location.lng(),
      LONDON_LAT,
      LONDON_LON) / 1000;
  printInt(distanceKmToLondon, gps.location.isValid(), 9);

  double courseToLondon =
    TinyGPSPlus::courseTo(
      gps.location.lat(),
      gps.location.lng(),
      LONDON_LAT,
      LONDON_LON);

  printFloat(courseToLondon, gps.location.isValid(), 7, 2);

  const char *cardinalToLondon = TinyGPSPlus::cardinal(courseToLondon);

  printStr(gps.location.isValid() ? cardinalToLondon : "*** ", 6);

  printInt(gps.charsProcessed(), true, 6);
  printInt(gps.sentencesWithFix(), true, 10);
  printInt(gps.failedChecksum(), true, 9);
  Serial.println();

  smartDelay(100);
  if (gps.charsProcessed() > 63) {
  }
  if (millis() > 5000 && gps.charsProcessed() < 10)
    Serial.println(F("No GPS data received: check wiring"));
}


// This custom version of delay() ensures that the gps object
// is being "fed".
static void smartDelay(unsigned long ms)
{
  unsigned long start = millis();
  do
  {
    while (Serial2.available())
      gps.encode(Serial2.read());
  } while (millis() - start < ms);
}

static void printFloat(float val, bool valid, int len, int prec)
{
  if (!valid)
  {
    while (len-- > 1) {}
    Serial.print('*');
    Serial.print(' ');
  }
  else
  {
    Serial.print(val, prec);
    int vi = abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1); // . and -
    flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
    for (int i = flen; i < len; ++i) {}
    Serial.print(' ');
  }
  smartDelay(0);
}

static void printInt(unsigned long val, bool valid, int len)
{
  char sz[32] = "*****************";
  if (valid)
    sprintf(sz, "%ld", val);
  sz[len] = 0;
  for (int i = strlen(sz); i < len; ++i)
    sz[i] = ' ';
  if (len > 0)
    sz[len - 1] = ' ';
  Serial.print(sz);
  smartDelay(0);
}

static void printDateTime(TinyGPSDate &d, TinyGPSTime &t)
{
  if (!d.isValid())
  {
    Serial.print(F("********** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year());
    Serial.print(sz);
  }

  if (!t.isValid())
  {
    Serial.print(F("******** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d:%02d:%02d ", t.hour(), t.minute(), t.second());
    Serial.print(sz);
  }

  printInt(d.age(), d.isValid(), 5);
  smartDelay(0);
}

static void printStr(const char *str, int len)
{
  int slen = strlen(str);
  for (int i = 0; i < len; ++i)
    Serial.print(i < slen ? str[i] : ' ');
  smartDelay(0);
}