 int averageDataSamples(int dataSamples[], int numberOfSamples);
 
 LiquidCrystal lcd(8, 9, 4, 5, 6, 7);
+PIDController PIDController;
+ESC esc(1500, 2000, 3);
 GPS gps;
 Gyroscope gyroscope;
 Moisture moistureSensor;
 RudderServo rudderServo;
 Temperature temperatureSensor;
 
-int heading[15];
-NMEAData gpsData;
+double heading;
+NMEAData currentGpsData;
+NMEAData nextGpsData;
 String moisture = "Unknown";
 short temperature = 0;
 int angle = 0;

  lcd.print("Oktopus");
  lcd.setCursor(0, 1);
  lcd.print("autopilot");
}

uint32_t timer1 = millis();
uint32_t timer2 = millis();
uint8_t count = 0;

void loop() {
  
  gpsData = gps.getData();
  
  // if millis() or timer wraps around, we'll just reset it
  if (timer1 > millis())  timer1 = millis();
  if (timer2 > millis())  timer2 = millis();

  
  if (millis() - timer1 > 100) {
    timer1 = millis(); // reset the time
    heading[count] = gyroscope.getData();
    if (count < 14) {
      count++;
    } else {
      count = 0;
    }
  }
  if (millis() - timer2 > 2500) {
    gps.printData(gpsData);
    rudderServo.setAngle(-40);
    delay(500);
    rudderServo.setAngle(0);
    delay(500);
    rudderServo.setAngle(40);
    delay(500);
  }
  if (millis() - timer2 > 5000) {
      timer2 = millis(); // reset the time
      
      angle = rudderServo.getData();
      moisture = moistureSensor.getData();
      temperature = temperatureSensor.getData();
      
      gyroscope.printData(averageDataSamples(heading, 15));
      moistureSensor.printData(moisture);
      rudderServo.printData(angle);
      temperatureSensor.printData(temperature);
      cycleDataOnLCD();
  }
}


  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("GYR:");
  lcd.print((int)heading);
  lcd.print(" SAF:");
  lcd.print((int)rudder);
  lcd.setCursor(0, 1);
  lcd.print("TTA:");
  lcd.print((int)theta);
  /*
  lcd.print(" LAT:");
  lcd.print((int)currentGpsData.latitude);
  */
  /*
  lcd.print(" LON:");
  lcd.print((int)currentGpsData.longitude);
  */
  /*
  lcd.print(" HUM:");
  lcd.print(moisture);
  */
  /*
  lcd.print(" TMP: ");
  lcd.print(temperature);
  */
}

int averageDataSamples(int dataSamples[], int numberOfSamples){
