#include <CalcGPS.h>
#include <TinyGPS.h>
#include <NewSoftSerial.h>
#include <Servo.h>
#include <Wire.h>
 
 
NewSoftSerial nss(2,3);
TinyGPS gps;
CalcGPS calc = CalcGPS();
Servo se;
Servo fahrt;
 
float wpk1[] = {
  52.308689,10.209555};
float wpk2[] = {
  52.308369,10.210126};
float wpk3[] = {
  52.308297,10.210992};
float wpk4[] = {
  52.308651,10.210582};
int compass_address = 0x42 >> 1;
 
float flat, flon, distance, qdm;
unsigned long fix_age;
long lat,lon;
int mh;
int new_servo_pos;
unsigned long time1,time2;
 
boolean wp1=false,wp2=false,wp3=false,wp4=false;
 
void setup(){
  Serial.begin(115200);
  nss.begin(9600);
  Wire.begin();
  fahrt.attach(6);
  se.attach(4);
  fahrt.write(90);
  se.write(90);
  time1=millis();
  while(time1+3000<=millis()){
  }
  Serial.println("Ready...");
  time1 = millis();
}
 
void loop(){
  fahrt.write(120);
 
  while(!wp1){
    get_gps();
    get_mh();
    calc.get_nwp_info(flat,flon,wpk1[],wpk1[1],&distance,&qdm);
    new_servo_pos = calc.get_servo_pos((float)mh,qdm);
    se.write(/*new_servo_pos*/90);
    if(distance<1.0){
      wp1=true;
    }
    if(millis()>=time1+2000){
      Serial.print("Welcher Wegpunkt: ");Serial.println("WP1");
      Serial.print("Start Lat: ");printFloat(flat,5);Serial.println("");
      Serial.print("Ziel Lat: ");printFloat(wpk1[],5);Serial.println("");
      Serial.print("Lon: ");printFloat(flon,5);Serial.println("");
      Serial.print("Ziel Lon: ");printFloat(wpk1[1],5);Serial.println("");
      Serial.print("MH: ");Serial.println(mh);
      Serial.print("QDM: ");Serial.println(qdm);
      Serial.print("Servo: ");Serial.println(new_servo_pos);
      Serial.print("Fahrtregler: ");Serial.println(fahrt.read());
      Serial.print("Distance: ");Serial.println(distance);
      Serial.println("_______________________________________");
      time1 = millis();
    }
  }
  while(!wp2){
    get_gps();
    get_mh();
    calc.get_nwp_info(flat,flon,wpk2[],wpk2[1],&distance,&qdm);
    se.write(calc.get_servo_pos((float)mh,qdm));
    if(distance<1.0){
      wp2=true;
    }
  }
  while(!wp3){
    get_gps();
    get_mh();
    calc.get_nwp_info(flat,flon,wpk3[],wpk3[1],&distance,&qdm);
    se.write(calc.get_servo_pos((float)mh,qdm));
    if(distance<1.0){
      wp3=true;
    }
  }
  while(!wp4){
    get_gps();
    get_mh();
    calc.get_nwp_info(flat,flon,wpk4[],wpk4[1],&distance,&qdm);
    se.write(calc.get_servo_pos((float)mh,qdm));
    if(distance<1.0){
      wp4=true;
      fahrt.write(60);
      delay(3000);
      fahrt.write(90);
    }
  }
}
 
void get_gps(){
  while(nss.available()){
    int c = nss.read();
    if(gps.encode(c)){
      gps.get_position(&lat,&lon,&fix_age);
      gps.f_get_position(&flat,&flon,&fix_age);
    }
  }
}
 
void get_mh(){
  Wire.beginTransmission(compass_address);
  Wire.send('A');
  Wire.endTransmission();
  time2=millis();
  while(time2+10<=millis()){}
  Wire.requestFrom(compass_address, 2);
 
  if(2 <= Wire.available())     
  { 
    mh = Wire.receive();   
    mh = mh << 8;     
    mh += Wire.receive();  
    mh /= 10;
    mh -= 2;
    if(mh==)
      mh+=360;
  }
}
 
void printFloat(double number, int digits)
{
  // Handle negative numbers
  if (number < 0.0)
  {
     Serial.print('-');
     number = -number;
  }
 
  // Round correctly so that print(1.999, 2) prints as "2.00"
  double rounding = 0.5;
  for (uint8_t i=; i<digits; ++i)
    rounding /= 10.0;
 
  number += rounding;
 
  // Extract the integer part of the number and print it
  unsigned long int_part = (unsigned long)number;
  double remainder = number - (double)int_part;
  Serial.print(int_part);
 
  // Print the decimal point, but only if there are digits beyond
  if (digits > )
    Serial.print("."); 
 
  // Extract digits from the remainder one at a time
  while (digits-- > 0)
  {
    remainder *= 10.0;
    int toPrint = int(remainder);
    Serial.print(toPrint);
    remainder -= toPrint; 
  } 
}