/* gpxlength -- calculate total length of a track
 *
 * Parse a GPX file, calculate the total length of the track(s), and
 * print information about the track.
 *
 * The calculation assumes the points along the track are close enough
 * together that the curvature of the earth can be ignored. On the
 * other hand, the elevation information in the track points *is*
 * used.
 *
 * Todo: heuristics to compensate noise, such as large movements with
 * low precision, random movements around a point...
 *
 * Todo: handle <trkpt> without a <time>.
 *
 * Author; Bert Bos <bert@w3.org>
 * Created: 9 Feb 2010
 */

#if HAVE_CONFIG_H
# include <config.h>
#endif
#include <time.h>
#include <assert.h>
#include <sys/errno.h>
#include <float.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#ifdef HAVE_STRING_H
# include <string.h>
#elif HAVE_STRINGS_H
# include <strings.h>
#endif
#include "heap.e"
#include "types.e"
#include "scan.e"
#include "html.e"
#include "errexit.e"

#define RADIUS_POLES   6356.752314	/* in km (WGS-84) */
#define RADIUS_EQUATOR 6378.1370	/* in km (WGS-84) */
#define INCR 1000			/* track array increase step */

typedef struct {double lat, lon, elevation; time_t sec;} TrackPoint;
typedef TrackPoint *Track;
typedef enum {INIT, GPX, TRK, TRKSEG, TRKPT, ELE, TIME} ParseState;

/* Globals */
static Boolean has_error = False;	/* Parse error */
static ParseState state = INIT;		/* Parsing level */
static int nesting = 0;			/* For skipping unknown elements */
static string contents = NULL;		/* Text content of current element */
static Track points = NULL;		/* All the tracks points in the file */
static int npoints = 0;			/* Length of points array */
static int *segmentstart = NULL;	/* Index into points array */
static int nsegments = 0;		/* Length of segmentstart array */


/* iso_to_sec -- parse an ISO date and return seconds since Unix epoch */
static time_t iso_to_sec(conststring datetime)
{
  /* Todo: check for errors (i.e., strptime returns NULL) */
  struct tm tm;
  int offset;
  string s;

  s = strptime(datetime, " %Y-%m-%dT%H:%M:%S", &tm);
  if (!s) return 0;
  offset = strtol(s, NULL, 10);	/* Can be "Z" or "+hhmm" or "-hhmm" */
  tm.tm_hour -= offset / 100;
  tm.tm_min -= offset % 100;
  return timegm(&tm);
}


/* --------------- implements interface html.e -------------------------- */


/* handle_error -- called when a parse error occurred */
static void handle_error(void *clientdata, const string s, int lineno)
{
  fprintf(stderr, "%d: %s\n", lineno, s);
  has_error = True;
}


/* start -- called before the first event is reported */
static void* start(void)
{
  return NULL;
}


/* end -- called after the last event is reported */
static void end(void *clientdata)
{
  /* skip */
}


/* handle_comment -- called after a comment is parsed */
static void handle_comment(void *clientdata, string commenttext)
{
  /* skip */
}


/* handle_text -- called after a text chunk is parsed */
static void handle_text(void *clientdata, string text)
{
  /* Todo: handle entities (numeric, lt, gt, amp, quot) */
  if (!nesting)
    if (state == ELE || state == TIME)
      strapp(&contents, text, NULL);
}


/* handle_decl -- called after a declaration is parsed */
static void handle_decl(void *clientdata, string gi, string fpi, string url)
{
  /* skip */
}


/* handle_pi -- called after a PI is parsed */
static void handle_pi(void *clientdata, string pi_text)
{
  /* skip */
}


/* handle_starttag -- called after a start tag is parsed */
static void handle_starttag(void *clientdata, string name, pairlist attribs)
{
  /* Ignores namespaces */
  conststring lat, lon;

  if (nesting) {		/* We're inside an unknown element */
    nesting++;
    return;
  }
  switch (state) {
  case TIME:
    nesting = 1;
    break;
  case ELE:
    nesting = 1;
    break;
  case TRKPT:
    if (eq(name, "ele") || hasaffix(name, ":ele")) {
      dispose(contents);
      state = ELE;
    } else if (eq(name, "time") || hasaffix(name, ":time")) {
      dispose(contents);
      state = TIME;
    } else
      nesting = 1;
    break;
  case TRKSEG:
    if (!eq(name, "trkpt") && !hasaffix(name, ":trkpt"))
      nesting = 1;
    else {
      lat = pairlist_get(attribs, "lat");
      lon = pairlist_get(attribs, "lon");
      if (!lat || !lon) {
	fprintf(stderr, "Missing lat or lon attribute on a <trkpt>\n");
	has_error = True;
      } else {
	assert(npoints >= 0);
	renewarray(points, INCR * ((npoints + INCR) / INCR));
	points[npoints].lat = atof(lat); /* Todo: use strtod(), check errors */
	points[npoints].lon = atof(lon); /* Todo: use strtod(), check errors */
	points[npoints].elevation = DBL_MIN; /* Hopefully filled in later */
	points[npoints].sec = 0; /* Hopefully filled in later */
	npoints++;
      }
      state = TRKPT;
    }
    break;
  case TRK:
    if (!eq(name, "trkseg") && !hasaffix(name, ":trkseg"))
      nesting = 1;
    else {			/* Start a new track segment */
      renewarray(segmentstart, INCR * ((nsegments + INCR) / INCR));
      segmentstart[nsegments] = npoints;
      nsegments++;
      state = TRKSEG;
    }
    break;
  case GPX:
    if (eq(name, "trk") || hasaffix(name, ":trk")) state = TRK;
    else nesting = 1;		/* Unknown element */
    break;
  case INIT:
    if (eq(name, "gpx") || hasaffix(name, ":gpx")) state = GPX;
    else nesting = 1;		/* Unknown element */
    break;
  default:
    assert(!"Cannot happen");
  }
}


/* handle_endtag -- called after an endtag is parsed (name may be "") */
static void handle_endtag(void *clientdata, string name)
{
  if (nesting) {
    nesting--;
    return;
  }
  switch (state) {
  case TIME:			/* name assumed to be "time" */
    assert(points != NULL && npoints > 0);
    points[npoints-1].sec = iso_to_sec(contents);
    state = TRKPT;
    break;
  case ELE:			/* name assumed to be "ele" */
    assert(points != NULL && npoints > 0);
    points[npoints-1].elevation = atof(contents) / 1000.0; /* Todo: errors */
    state = TRKPT;
    break;
  case TRKPT:			/* name assumed to be "trkpt" */
    state = TRKSEG;
    break;
  case TRKSEG:			/* name assumed to be "trkseg" */
    state = TRK;
    break;
  case TRK:			/* name assumed to be "trk" */
    state = GPX;
    break;
  case GPX:			/* name assumed to be "gpx" */
    state = INIT;
    break;
  case INIT:
    fprintf(stderr, "Incorrect nesting of elements in GPX file\n");
    break;
  default:
    assert(!"Cannot happen!");
  }
}


/* handle_emptytag -- called after an empty tag is parsed */
static void handle_emptytag(void *clientdata, string name, pairlist attribs)
{
  handle_starttag(clientdata, name, attribs);
  handle_endtag(clientdata, name);
}


/* --------------------------------------------------------------------- */


/* distance -- return distance in meters between two points */
static double distance(const TrackPoint p1, const TrackPoint p2)
{
#define METHOD 1

  /*
   * If only one of the points has an elevation, assume the other has
   * the same elevation. If neither has an elevation, assume both are
   * at elevation 0.
   */
  double elev1 = p1.elevation != DBL_MIN ? p1.elevation :
    (p2.elevation != DBL_MIN ? p2.elevation : 0.0);
  double elev2 = p2.elevation != DBL_MIN ? p2.elevation :
    (p1.elevation != DBL_MIN ? p1.elevation : 0.0);

#if METHOD == 1
  /*
   * This function calculates the linear distance between the two
   * points with Pythagoras, i.e., assuming that the points are
   * sufficiently near each other that the curvature of the earth can
   * be ignored. The distance from the center of the earth is
   * calculated by adding the elevation to the radius of the earth at
   * the equator (RADIUS_EQUATOR) or the radius at the poles
   * (RADIUS_POLES) or something in between.
   *
   * When the points are far apart, a better approximation is given by
   * the haversine formula (calculating the distance along the surface
   * of a sphere) or Vincenty's algorithm (calculating the distance
   * along the surface of an ellipsoid).
   */
  double theta1 = p1.lat * M_PI / 180.0;
  double phi1 = p1.lon * M_PI / 180.0;
  double h1 = abs(p1.lat) / 90.0;
  double r1 = h1 * RADIUS_POLES + (1.0 - h1) * RADIUS_EQUATOR + elev1;
  double x1 = r1 * sin(theta1) * cos(phi1);
  double y1 = r1 * sin(theta1) * sin(phi1);
  double z1 = r1 * cos(theta1);
 
  double theta2 = p2.lat * M_PI / 180.0;
  double phi2 = p2.lon * M_PI / 180.0;
  double h2 = abs(p2.lat) / 90.0;
  double r2 = h2 * RADIUS_POLES + (1.0 - h2) * RADIUS_EQUATOR + elev2;
  double x2 = r2 * sin(theta2) * cos(phi2);
  double y2 = r2 * sin(theta2) * sin(phi2);
  double z2 = r2 * cos(theta2);

  double dx = x1 - x2;
  double dy = y1 - y2;
  double dz = z1 - z2;
  double result = sqrt(dx * dx + dy * dy + dz * dz);
#elif METHOD == 2
  /*
   * This function uses the haversine formula to find the distance
   * over the surface of the earth, assuming a perfect sphere with the
   * radius that the earth has at point p1. Then use Pythagoras to add
   * the difference in height, assuming the points are close enough
   * together.
   */
  double lat1 = p1.lat * M_PI / 180.0;
  double lon1 = p1.lon * M_PI / 180.0;
  double lat2 = p2.lat * M_PI / 180.0;
  double lon2 = p2.lon * M_PI / 180.0;
  double dlon = lon2 - lon1;
  double dlat = lat2 - lat1;
  double a = pow(sin(dlat/2), 2) + cos(lat1) * cos(lat2) * pow(sin(dlon/2), 2);
  double c = 2 * atan2(sqrt(a), sqrt(1 - a));

  double h1 = abs(p1.lat) / 90.0;
  double r1 = h1 * RADIUS_POLES + (1.0 - h1) * RADIUS_EQUATOR + elev1;

  double dist = r1 * c;		/* distance at equal elevation */
  double height = elev2 - elev1;
  double result = sqrt(dist * dist + height * height);
#else
  /*
   * This function uses the haversine formula to find the distance
   * over the surface of the earth, assuming a perfect sphere with the
   * average radius of the two points.
   */
  double lat1 = p1.lat * M_PI / 180.0;
  double lon1 = p1.lon * M_PI / 180.0;
  double lat2 = p2.lat * M_PI / 180.0;
  double lon2 = p2.lon * M_PI / 180.0;
  double dlon = lon2 - lon1;
  double dlat = lat2 - lat1;
  double a = pow(sin(dlat/2), 2) + cos(lat1) * cos(lat2) * pow(sin(dlon/2), 2);
  double c = 2 * atan2(sqrt(a), sqrt(1 - a));

  double h1 = abs(p1.lat) / 90.0;
  double r1 = h1 * RADIUS_POLES + (1.0 - h1) * RADIUS_EQUATOR + elev1;
  double h2 = abs(p2.lat) / 90.0;
  double r2 = h2 * RADIUS_POLES + (1.0 - h2) * RADIUS_EQUATOR + elev2;

  double result = c * (r1 + r2) / 2.0;
#endif

  /* fprintf(stderr, "%g ", result); */
  return result;
}


/* tracklength -- calculate length of track t[a..b] */
static double tracklength(const Track t, const int a, const int b,
			  double *maxspeed)
{
  /* To avoid rounding errors when adding small numbers to large ones,
   * the function works recursively: divide the track in two halves,
   * calculate the length of each half and sum the two numbers (which
   * are hopefully of similar magnitude).
   */
  int m;
  double d, s, v;

  assert(a <= b);
  if (a >= b) {
    return 0.0;
  } else if (a + 1 != b) {
    m = (a + b)/2;
    return tracklength(t, a, m, maxspeed) + tracklength(t, m, b, maxspeed);
  } else {
    d = distance(t[a], t[b]);
    s = difftime(t[b].sec, t[a].sec) / 3600;
    v = s == 0.0 ? 0.0 : d / s;
    if (v > *maxspeed) *maxspeed = v;
    return d;
  }
}


/* sum_segments -- calculate total length of segments a..b */
static double sum_segments(Track t, int segment[],
			   const int a, const int b, double *maxspeed)
{
  assert(b <= nsegments);
  if (a > b) return 0.0;
  assert(segment[a] < npoints);
  assert(segment[b+1] <= npoints);
  if (a == b) return tracklength(t, segment[a], segment[b+1] - 1, maxspeed);
  int m = (a + b) / 2;
  return sum_segments(t, segment, a, m, maxspeed) +
    sum_segments(t, segment, m + 1, b, maxspeed);
}


/* minmax_elevation -- find minimum and maximum elevation */
static void minmax_elevation(const Track t, const int len,
			     double *min_elevation, double *max_elevation)
{
  int i;

  for (i = 0; i < len; i++)
    if (t[i].elevation != DBL_MIN) {
      if (t[i].elevation < *min_elevation) *min_elevation = t[i].elevation;
      if (t[i].elevation > *max_elevation) *max_elevation = t[i].elevation;
    }
}


/* main -- main body */
int main(int argc, char *argv[])
{
  double min_elevation = DBL_MAX, max_elevation = 0.0;
  double length, hours, speed, maxspeed = 0.0;
  char starttime[25], endtime[25];

  set_error_handler(handle_error);
  set_start_handler(start);
  set_end_handler(end);
  set_comment_handler(handle_comment);
  set_text_handler(handle_text);
  set_decl_handler(handle_decl);
  set_pi_handler(handle_pi);
  set_starttag_handler(handle_starttag);
  set_emptytag_handler(handle_emptytag);
  set_endtag_handler(handle_endtag);

  if (argc == 1) yyin = stdin;
  else if (argc > 2) errexit("Usage: %s [GPX-file]\n", argv[0]);
  else if (!(yyin = fopen(argv[1], "r"))) errexit("%s: %s\n", argv[0], strerror(errno));

  if (yyparse() != 0) return 1;

  /* Store end of last segment */
  renewarray(segmentstart, INCR * ((nsegments + INCR) / INCR));
  segmentstart[nsegments] = npoints;

  length = sum_segments(points, segmentstart, 0, nsegments - 1, &maxspeed);
  minmax_elevation(points, npoints, &min_elevation, &max_elevation);
  if (npoints > 0) {
    hours = difftime(points[npoints-1].sec, points[0].sec) / 3600;
    speed =  length / hours;
    (void) strftime(starttime, sizeof(starttime), "%Y-%m-%dT%H:%M:%SZ",
		    gmtime(&points[0].sec));
    (void) strftime(endtime, sizeof(endtime), "%Y-%m-%dT%H:%M:%SZ",
		    gmtime(&points[npoints-1].sec));
  } else {
    hours = 0.0;
    speed = 0.0;
    strcpy(starttime, "-");
    strcpy(endtime, "-");
  }
  printf("distance       : %g km\n", length);
  printf("time           : %g h\n", hours);
  printf("avg. speed     : %g km/h\n", speed);
  printf("max. speed     : %g km/h\n", maxspeed);
  printf("min. elevation : %g m\n", 1000.0 * min_elevation);
  printf("max. elevation : %g m\n", 1000.0 * max_elevation);
  printf("start time     : %s\n", starttime);
  printf("end time       : %s\n", endtime);
  printf("points         : %d\n", npoints);
  printf("segments       : %d\n", nsegments);
  return 0;
}