#!/usr/bin/python

# median_track.py
#       --copyright--                   Copyright 2008 (C) Tranzoa, Co. All rights reserved.    Warranty: You're free and on your own here. This code is not necessarily up-to-date or of public quality.
#       --url--                         http://www.tranzoa.net/tzpython/
#       --email--                       pycode is the name to send to. tranzoa.com is the place to send to.
#       --bodstamps--
#       November 24, 2008       bar
#       November 27, 2008       bar     put file names in median point's description
#                                       HIDE_COUNT
#       November 12, 2011       bar     correct the url
#       November 29, 2011       bar     pyflake cleanup
#       --eodstamps--
#
#
#       Create a median GPS track file.
#
#       Who knows of what use this is. It's just a couriosity to me.
#       In effect, it's kinda like a bendable least-squares-ish line through the points given to it. Sorta.
#
#
#       TODO:
#
#               Need to look in southern hemisphere, equitorial, etc. points. Does this logic work for them?
#
#

import  sys

import  latlon
import  tzlib
import  tz_gps




HIDE_COUNT  = 5                             # hide the ends of the median path since they tend to wander off (see 2 points in 2 dimensions how the points, x1:y1/x1:y2/x2:y1/x2:y2, are not in a line 'tween x1:y1/x2:y2)
                                            # !!!! what we should do is to eliminate the end points that are outside the boundary (convex hull?) of the tracks' points but this code isn't important enough to spend time on that


def get_xyzs(tracks) :
    tracks  = tz_gps.make_array_of_tracks(tracks)

    xyz     = []
    for t in tracks :
        for p in t.points :
            xyz.append(latlon.convert_lat_lon_to_xyz_point(p.lat, p.lon))
        pass

    return(xyz)



def sort_xyz(xyz, rtn) :
    ln  = float(min(101, len(xyz)))

    xyz.sort(rtn)
    a   = [ xyz[int(min((i * len(xyz)) / ln, len(xyz) - 1))] for i in xrange(0, int(ln)) ]

    return(a)



def make_median_points(tracks) :
    tracks  = tz_gps.make_all_big_points(tracks, 1)
    tracks  = tz_gps.remove_redundant_points_from_tracks(tracks)            # note: Sparsify fixes real weirdness on ends of *compton*.
    tpoints = tz_gps.points_from_all_tracks(tracks)

    xyz     = get_xyzs(tracks)

    xa      = sort_xyz(xyz, lambda a, b : cmp(a.x, b.x) or cmp(a.y, b.y) or cmp(a.z, b.z))
    ya      = sort_xyz(xyz, lambda a, b : cmp(a.y, b.y) or cmp(a.x, b.x) or cmp(a.z, b.z))
    za      = sort_xyz(xyz, lambda a, b : cmp(a.z, b.z) or cmp(a.x, b.x) or cmp(a.y, b.y))

    bi      = 0
    bdist   = 1000000000000000000.0

    for i in xrange(4) :
        if  i & 1 :
            xa.reverse()
        if  i & 2 :
            ya.reverse()
        if  i & 4 :
            za.reverse()

        ( lat, lon )    = latlon.convert_x_y_z_to_lat_lon(xa[0].x, ya[0].y, za[0].z)
        pf              = tz_gps.a_point(lat = lat, lon = lon, speed = tz_gps.TINY_SPEED)
        df              = tz_gps.find_closest_point(pf, tpoints).flat_distance_from(pf)

        ( lat, lon )    = latlon.convert_x_y_z_to_lat_lon(xa[-1].x, ya[-1].y, za[-1].z)
        pl              = tz_gps.a_point(lat = lat, lon = lon, speed = tz_gps.TINY_SPEED)
        dl              = tz_gps.find_closest_point(pl, tpoints).flat_distance_from(pl)

        db              = pf.flat_distance_from(pl)
        if  db == 0.0 :
            db  = 1.0

        d               = (df + dl) / db
        # print i, df, dl, db, d
        if  bdist   >= d :
            bdist   = d
            bi      = i

        if  i & 1 :
            xa.reverse()
        if  i & 2 :
            ya.reverse()
        if  i & 4 :
            za.reverse()

        pass

    i   = bi
    if  i & 1 :
        xa.reverse()
    if  i & 2 :
        ya.reverse()
    if  i & 4 :
        za.reverse()

    points  = []
    for  pi in xrange(len(xa)) :
        ( lat, lon )    = latlon.convert_x_y_z_to_lat_lon(xa[pi].x, ya[pi].y, za[pi].z)
        points.append(tz_gps.a_point(lat = lat, lon = lon, speed = tz_gps.TINY_SPEED))

    if  False :
        pts = []
        for a1 in [ xa, ya, za ] :
            for a2 in [ xa, ya, za ] :
                for a3 in [ xa, ya, za ] :
                    ( lat, lon )    = latlon.convert_x_y_z_to_lat_lon(a1[0].x, a2[0].y, a3[0].z)
                    pts.append(tz_gps.a_point(lat = lat, lon = lon, description = "%u" % i))                # these jump around because they are from sampled points which can be from all over the place
        x               = (xa[0].x + ya[0].x + za[0].x) / 3.0
        y               = (xa[0].y + ya[0].y + za[0].y) / 3.0
        z               = (xa[0].z + ya[0].z + za[0].z) / 3.0
        ( lat, lon )    = latlon.convert_x_y_z_to_lat_lon(x, y, z)
        pts.append(tz_gps.a_point(lat = lat, lon = lon, description = "average"))
        tz_gps.write_gps_file("x.kml", program_name = "median_track.py", waypoints = pts, tracks = [])

    return(points)



def write_median_points(ofile_name, points, from_nms = None) :
    fs      = ""
    if  from_nms :
        fs  = ":\r\n" + "\r\n".join(from_nms) + "\r\n"
    p       = points[len(points) / 2]
    wp      = tz_gps.a_point(copied_point = p, description = "Median point" + fs)

    tracks  = tz_gps.make_array_of_tracks(points[HIDE_COUNT : -HIDE_COUNT])

    tz_gps.write_gps_file(ofile_name, program_name = "median_track.py", waypoints = [ wp ], tracks = tracks)



if  __name__ == '__main__' :
    import  glob
    import  os

    import  TZCommandLineAtFile


    program_name    = os.path.split(sys.argv.pop(0))[1]
    TZCommandLineAtFile.expand_at_sign_command_line_files(sys.argv)


    ofile_name      = None


    while True :
        oi  = tzlib.array_find(sys.argv, [ "--output", "-o" ] )
        if  oi < 0 :    break
        del sys.argv[oi]
        ofile_name  = sys.argv.pop(oi)

    if  not len(sys.argv) :
        print "Tell me GPS track file(s) to analyze!"
        sys.exit(104)


    fnames  = {}
    while len(sys.argv) :
        fn  = sys.argv.pop(0)
        fns = tzlib.make_dictionary(glob.glob(fn))
        if  not fns :
            print "Cannot find file(s):", fn
            sys.exit(105)
        fnames.update(fns)

    fnames  = fnames.keys()
    fnames.sort()

    tracks  = []
    for fn in fnames :
        trks        = tz_gps.tracks_from_file(fn)
        if  not trks :
            print "Cannot read GPS track from %s!" % fn
            sys.exit(106)

        tracks += trks

    points      = make_median_points(tracks)

    if  ofile_name :
        write_median_points(ofile_name, points, fnames)
    else :
        print points[0]
        print points[len(points) / 2]
        print points[-1]

    pass

#
#
#
# eof