#/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Sun Jan 27 12:02:28 2019

@author: hitomii23
"""

import sys
import lcm
#import time
import serial
from gps import gps_t
import utm

class GPS(object):
    def __init__(self, port_name):
        self.port = serial.Serial(port_name, 4800, timeout = 1.)  # 4800-N-8-1
        self.lcm = lcm.LCM()
        self.packet = gps_t()

    def readloop(self):
        while True:
            line = self.port.readline()
            try:
                vals = [x for x in line.split(',')]
                if vals[0] == "$GPGGA":
                    print vals
                    self.packet.timestamp = float(vals[1])
                    lat_full = vals[2]
                    lat_degrs = float(lat_full[:2])
                    lat_mins = float(lat_full[2:])
                    mins_1 = lat_mins/60
                    lat = lat_degrs + mins_1
                    lat = round(lat,5)
                    self.packet.latitude = lat

                    self.packet.lat_dir = vals[3]
                    if vals[3] == 'S':
                        self.packet.latitude = -self.packet.latitude
                    print self.packet.latitude

                    lon_full = vals[4]
                    lon_degrs = float(lon_full[:3])
                    lon_mins = float(lon_full[3:])
                    mins_2 = lon_mins/60
                    lon = lon_degrs + mins_2
                    lon = round(lon,5)
                    self.packet.longitude = lon

                    self.packet.long_dir = vals[5]
                    if vals[5] == 'W':
                        self.packet.longitude = - self.packet.longitude

                    self.packet.altitude = float(vals[9])
                    print self.packet.longitude   #print self.packet.altitude
                    lat_lon = (self.packet.latitude, self.packet.longitude)

                    UTM_latlon = utm.from_latlon(lat_lon[0], lat_lon[1])

                    self.packet.utmx = UTM_latlon[0]
                    self.packet.utmy = UTM_latlon[1]
                    self.lcm.publish("GPS", self.packet.encode())

            except :
               print 'GPS ERROR (' + line + ')'
               #print e
               #sys.exit(0)


if __name__ =="__main__":
    if len(sys.argv) != 2:
        print "Usage: %s <serial_port>\n" % sys.argv[0]
        sys.exit(0)
    myGPS = GPS(sys.argv[1])
    myGPS.readloop()