     theta = PIDController.compute_theta(currentGpsData, nextGpsData, heading);
     rudder = PIDController.control_rudder(theta);
     rudderServo.setAngle(rudder);
-    
-    cycleDataOnLCD();
+    Serial.println(getDistanceBetweenGPSPoints(currentGpsData, nextGpsData));
   }
   
   if (millis() - timer2 > 5000) {

  uint8_t count = 0;
  double theta = 0.0;
  double rudder = 0.0;
  uint32_t timer1;
  uint32_t timer2;
  double distanceToTarget = 0.00;
#endif /* __BOAT__ */

NMEAData currentGpsData;
/********************************************/



void setup() {

  /*
   * Sensor initialization
   */
  gps.enable();
  
#if __BOAT__>0
  Serial.begin(115200);
  gyroscope.enable();
  moistureSensor.enable();
  rudderServo.enable();
  temperatureSensor.enable();
  proximitySensorArray.enable();
  /*  Uncomment when xbee is integrated
  basecampGPSData = xbee.getGPSDataFromBasecamp();
  gpsError = getGPSError(basecamp GPSData);
  */
  lcd.begin(16, 2);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Oktopus");
  lcd.setCursor(0, 1);
  lcd.print("autopilot");
  delay(TEST_DELAY);
  timer1 = millis();
  timer2 = millis();

  moisture = moistureSensor.getData();
  temperature = temperatureSensor.getData();

  currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
  
#endif /* __BOAT__ */
 /********************************************/
}

void loop() {
  currentGpsData = gps.getData();

#if __BOAT__>0
  
  currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
  while (currentEmergencyState != 0) {
    emergencyHandler.handleEmergency(currentEmergencyState);
    Serial.println("In emergency state loop.");
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("In emergency state loop.");
    currentEmergencyState = emergencyHandler.testConditions(moisture, temperature, sonar, proximitySensorData);
  }
  
  esc.setSpeed(MOTOR_SPEED, 0, 100);

  nextGpsData.longitude = currentGpsData.longitude + 5;
  nextGpsData.latitude = currentGpsData.latitude + 5;
  /*  Uncomment when xbee is integrated
  basecampGPSData = xbee.getGPSDataFromBasecamp();
  gpsError = getGPSError(basecamp GPSData);
  */
  if (timer1 > millis())  timer1 = millis();
  if (timer2 > millis())  timer2 = millis();
  

#define SECONDS 1000
#define TEST_DELAY 15*SECONDS
#define __BOAT__ 1
#define MOTOR_SPEED 22
#define TEMPERATURE_THRESHOLD 50
#define SONAR_THRESHOLD 1
#define PROXIMITY_THRESHOLD 1
#define STATIONNARY_DELAY 5*SECONDS
/********************************************/


