// Sdr. startlinie 57.046789, 9.846766
// Ndr. startlinie 57.049263, 9.844106
// Sejlbåd start 57.054305, 9.856980
// Kurs 225

// Koordinator for sejlbådens kurs langs Limfjord fra sejlklubben og vestpå
// Kurs/bearing regnet ud til 243.81
/*
float gpsLat1 = 57.057028;
float gpsLon1 = 9.869665;
float gpsLat2 = 57.046859;
float gpsLon2 = 9.831667;
*/
// Koordinator for sejlbådens kurs langs Limfjord fra sejlklubben og vestpå


// Koordinator for kapsejladsens startliniens fra nord mod syd
// Kurs/bearing regnet ud til 175.28 -SHOULD BE 184 deg 42 min
// Den nordligste buoy
float gpsLat1 = 57.049320;
float gpsLon1 = 9.839342;
// Den sydligste buoy
float gpsLat2 = 57.048481;
float gpsLon2 = 9.839232;
// Koordinator for kapsejladsens startliniens nord og syd markeringer


float gpsBearing;

/*
float gpsconv(char coord, char NESW);{
  // To do: find the whole degrees from the position of the decimal point
  //        find the decimal degrees from the position of the decimal point
  int posOfPoint;
  float helCoord;
  float decCoord;
  float convCoord;
  pointer = coord.indexOf(".");
  helCoord = coord.substring(0, pointer - 2).toFloat();
  decCoord = coord.substring(pointer + 1)/60;
  convCoord = helCoord + decCoord;
  Serial.print("Converted GPS = ");
  Serial.println(convCoord);
}
*/

// Found at https://gis.stackexchange.com/questions/252672/calculate-bearing-between-two-decimal-gps-coordinates-arduino-c
float bearing(float lat,float lon,float lat2,float lon2){

    // Vic
    float deltaDistX;
    float deltaDistY;
    // Vic

    float teta1 = radians(lat);
    float teta2 = radians(lat2);
    float delta1 = radians(lat2-lat);
    float delta2 = radians(lon2-lon);

    //==================Heading Formula Calculation================//

    float y = sin(delta2) * cos(teta2);
    float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
    float brng = atan2(y,x);
    brng = degrees(brng);// radians to degrees
    brng = ( ((int)brng + 360) % 360 );
        
    // Adapted from https://forum.arduino.cc/t/distance-between-multiple-gps-coordinates/545969
    // This is the haversine approximation to calculate distance
    // See also https://www.movable-type.co.uk/scripts/latlong.html
    float flat1 = lat;
    float flon1 = lon;
    float dist_calc = 0;
    float dist_calc2 = 0;
    float dist_calcKM = 0;
    float dist_calcNM = 0;
    float diflat = 0;
    float diflon = 0;
    float x2lat = lat2;
    float x2lon = lon2;
    
    Serial.print("y = ");
    Serial.println(y,38);
    Serial.print("x = ");
    Serial.println(x,38);
    Serial.print("Heading GPS: ");
    Serial.println(brng);

    Serial.print("flat1 = ");
    Serial.println(flat1,32);
    Serial.print("flon1 = ");
    Serial.println(flon1,32);
    Serial.print("x2lat = ");
    Serial.println(x2lat,32);
    Serial.print("x2lon = ");
    Serial.println(x2lon,32);
    

    diflat = radians(x2lat - flat1);
    Serial.print("diflat = ");
    Serial.println(diflat,32);
    flat1 = radians(flat1);
    Serial.print("flat1 = ");
    Serial.println(flat1,32);
    x2lat = radians(x2lat);
    Serial.print("x2lat = ");
    Serial.println(x2lat,32);
    diflon = radians((x2lon) - (flon1));
    Serial.print("diflon = ");
    Serial.println(diflon,32);
    dist_calc = (sin(diflat / 2.0) * sin(diflat / 2.0));
    Serial.print("dist_calc = ");
    Serial.println(dist_calc,32);
    dist_calc2 = cos(flat1);
    dist_calc2 *= cos(x2lat);
    dist_calc2 *= sin(diflon / 2.0);
    dist_calc2 *= sin(diflon / 2.0);
    Serial.print("dist_calc2 = ");
    Serial.println(dist_calc2,32);
    dist_calc += dist_calc2;
    Serial.print("dist_calc = ");
    Serial.println(dist_calc,32);
    dist_calc = (2 * atan2(sqrt(dist_calc), sqrt(1.0 - dist_calc)));
    Serial.print("dist_calc = ");
    Serial.println(dist_calc,32);
    dist_calc *= 6371000.0;
    Serial.print("dist_calc = ");
    Serial.println(dist_calc,32);
    dist_calcKM = dist_calc / 1000;
    dist_calcNM = dist_calc / 1852;
    /*
    Serial.print("diflat = ");
    Serial.println(diflat,32);
    Serial.print("flat1 = ");
    Serial.println(diflat,32);
    Serial.print("x2lat = ");
    Serial.println(x2lat,32);
    Serial.print("diflon = ");
    Serial.println(diflon,32);
    Serial.print("flon = ");
    Serial.println(diflat,32);
    Serial.print("diflat = ");
    Serial.println(diflat,32);
    */
    Serial.print("Distance: ");
    Serial.print(dist_calcKM, 3);
    Serial.print(" km - ");
    Serial.print(dist_calcNM, 3);
    Serial.println(" nm");
    // Vic
    Serial.println();
    Serial.print("Distance: ");
    Serial.print(dist_calc, 3);
    Serial.println(" m");
    // Serial.print("brng = ");
    // Serial.println(brng);
    deltaDistX = dist_calc * cos(radians(brng));
    deltaDistY = dist_calc * sin(radians(brng));
    // Serial.print("sin brng = ");
    // Serial.println(sin(radians(brng)));
    // Serial.print("cos brng = ");
    // Serial.println(cos(radians(brng)));
    Serial.print("deltaDistX = ");
    Serial.print(deltaDistX, 2);
    Serial.println(" m");
    Serial.print("deltaDistY = ");
    Serial.print(deltaDistY, 2);
    Serial.println(" m");
    // Vic
  
    
    return brng;
}


void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
  // Arduino UNO WiFiR4 seriel forbindelse kan være lidt sløv og skal have tid
  delay(5000);
  while(!Serial){
  }
  display_Running_Sketch();
  Serial.println("Calculate bearing and distance between 2 GPS coordinates");
  Serial.print("Starting latitude: ");
  Serial.println(gpsLat1,6);
  Serial.print("Starting longitude: ");
  Serial.println(gpsLon1,6);
  Serial.print("Ending latitude: ");
  Serial.println(gpsLat2,6);
  Serial.print("Ending longitude: ");
  Serial.println(gpsLon2,6);
  gpsBearing = bearing(gpsLat1, gpsLon1, gpsLat2, gpsLon2);
  Serial.print("Bearing: ");
  Serial.println(gpsBearing,2); 
}

void loop() {
  /*
  int count = 1;
  while (count <= 1) {
    Serial.println("Calculate bearing and distance between 2 GPS coordinates");
    Serial.print("Starting latitude: ");
    Serial.println(gpsLat1);
    Serial.print("Starting longitude: ");
    Serial.println(gpsLon1);
    Serial.print("Ending latitude: ");
    Serial.println(gpsLat2);
    Serial.print("Ending longitude: ");
    Serial.println(gpsLon2);
    Serial.print("Bearing: ");
    // gpsBearing = bearing(gpsLat1, gpsLon1, gpsLat2, gpsLon2);
    Serial.println(gpsBearing);
    count += 1;
  }
  */
}

void display_Running_Sketch (void){
  // Found at https://forum.arduino.cc/t/sketch-name-as-preprocessor-value/116152/3
  // See too https://gcc.gnu.org/onlinedocs/gcc-3.1/cpp/Standard-Predefined-Macros.html
  String the_path = __FILE__;
  // V.F.Grant
  // In Windows, the path separator is '\' and searching for it requires the '\' delimiter hence 2 x \ in the next line
  // For Mac/Linux, the path separator is '/' so it can be searched for directly

  // Was the sketch compiled on Linux or Mac ?
  // The value returned will be it's position in the string or -1 if not found
  int slash_loc = the_path.lastIndexOf('/');
  if (slash_loc = -1) {
    // The sketch was compiled on Windows
    slash_loc = the_path.lastIndexOf('\\');
  }
  String the_sketchname = the_path.substring(slash_loc+1);
  // Check whether the file name has an extension and remove it
  int dot_loc = the_sketchname.lastIndexOf('.');
  if (dot_loc != -1) {
    the_sketchname = the_sketchname.substring(0, dot_loc);
  }
  Serial.print("\nThe sketch currently running is: ");
  Serial.println(the_sketchname);
  Serial.print("Compiled on: ");
  Serial.print(__DATE__);
  Serial.print(" at ");
  Serial.print(__TIME__);
  Serial.print("\n");
}