// see http://www.espruino.com/GPS

// contains last 10 positions for smoothing
let storedData = [];

let targetLatLon = [53.912106729530376, 7.534487789079335];


/*
 data like: 
 {
  "time": "10:07:48",
  "lat": 53.92253946666, "lon": 7.4547485, "fix": 1, "satellites": 6, "altitude": 71.5 
  }
*/
function handleGpsData(data){
  if (data.fix){
    if (storedData.length >= 10){
      storedData.shift();
    }
    storedData.push(data);
    let latLon = calculateMean();
    
    let dist = Math.round(calculateDistanceKm(latLon, targetLatLon) * 1000);
    let angle = calculteAngle(latLon, targetLatLon) ;
    g.clear();
    g.setFont("Vector", 20);
    g.drawString("E: " + dist, 10, 10);
    g.drawString("R: " + Math.round(angle), 10, 30);
    g.flip();
  } else {
    g.clear();
    g.setFont("Vector", 10);
    g.drawString("NFX, #sat: "+data.satellites, 10, 10);
    g.flip();
  }
}

/*
 * Calclate mean from stored data
 * returns array with [lat,lon]
 */
function calculateMean(){
  let len = storedData.length;
  let latLonSum = storedData.reduce( (a,b) => [a[0]+b.lat, a[1]+b.lon], [0,0]);
  return [latLonSum[0] / len, latLonSum[1] / len];
}

function calculateDistanceKm(latLon1, latLon2){
  // see https://stackoverflow.com/questions/365826/calculate-distance-between-2-gps-coordinates
  return distanceInKmBetweenEarthCoordinates(latLon1[0], latLon1[1], latLon2[0], latLon2[1]);
}

function degreesToRadians(degrees) {
  // see https://stackoverflow.com/questions/365826/calculate-distance-between-2-gps-coordinates
  return degrees * Math.PI / 180;
}

function distanceInKmBetweenEarthCoordinates(lat1, lon1, lat2, lon2) {
  // see https://stackoverflow.com/questions/365826/calculate-distance-between-2-gps-coordinates
  var earthRadiusKm = 6371;

  var dLat = degreesToRadians(lat2-lat1);
  var dLon = degreesToRadians(lon2-lon1);

  lat1 = degreesToRadians(lat1);
  lat2 = degreesToRadians(lat2);

  var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
          Math.sin(dLon/2) * Math.sin(dLon/2) * Math.cos(lat1) * Math.cos(lat2); 
  var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a)); 
  return earthRadiusKm * c;
}

function calculteAngle(latLon1, latLon2){
  // see https://stackoverflow.com/questions/9566069/how-to-calculate-angle-between-two-geographical-gps-coordinates
  let dy = latLon2[0] - latLon1[0];
  let dx = Math.cos(Math.PI/180*latLon1[0])*(latLon2[1] - latLon1[1]);
  let angle = Math.atan2(dy, dx);
  return angle * 180/Math.PI; // convert to deg
}

// for use indoors.
function simulate(){
  handleGpsData({
  "time": "10:07:48",
  "lat": 51.92103966666, "lon": 7.5147485, "fix": 1, "satellites": 6, "altitude": 71.5 
  });
}

// uncomment to simulate data 
// setInterval(simulate, 1000);

Serial1.setup(9600,{tx:D10,rx:D11});
var gps = require("GPS").connect(Serial1, function(data) {
  //console.log(data);
  handleGpsData(data);
});

// Lights on
LED1.set(1)