# -*- coding: utf-8 -*-
"""
Spyder Editor

This is a temporary script file.
"""

import rospy
import serial
from std_msgs.msg import Float64
from std_msgs.msg import String, Header
import utm



def calculate_lat(lat, direction):
    latitude = float(lat[0:2]) + float(lat[2:]) / 60.0
    if direction == 'S':
        latitude = -latitude
    return latitude

def calculate_lon(lon, direction):
    longitude = float(lon[0:3]) + float(lon[3:]) / 60.0
    if direction == 'W':
        longitude = -longitude
    return longitude



if __name__ == '__main__':
    ### Start Rospy and get all the parameters.
    rospy.logdebug("Initializing Minicom")

    h = Header()
    #h.stamp = rospy.Time.now()
    rospy.init_node('serial_connection', anonymous = True)
    serial_port = rospy.get_param('~port','/dev/ttyUSB0')
    serial_baud = rospy.get_param('~baudrate',4800)

    ### Read the serial ports with above parameters
    port = serial.Serial(serial_port, serial_baud, timeout=3)

    lat_pub = rospy.Publisher("Minicom"+'/lattitude', Float64, queue_size=5)
    lon_pub = rospy.Publisher("Minicom"+'/longitude', Float64, queue_size=5)
    alt_pub = rospy.Publisher("Minicom"+'/altitude', Float64, queue_size=5)
    utm_easting_pub = rospy.Publisher("Minicom"+'/utm_easting', Float64, queue_size=5)
    utm_northing_pub = rospy.Publisher("Minicom"+'/utm_northing', Float64, queue_size=5)
    zone_pub = rospy.Publisher("Minicom"+'/zone', Float64, queue_size=5)
    letter_pub = rospy.Publisher("Minicom"+'/letter', String, queue_size=5)
	quality_pub = rospy.Publisher("Minicom"+'/quality', String, queue_size=5)


    # End the initialization Part
    rospy.logdebug("Initialization Complete")





    rospy.loginfo("Calculating lattitude, longitude and altitude")



    count = 0
    try:
        while not rospy.is_shutdown():
            data = str(port.readline())

            if data == '':
                rospy.logwarn("No data")
                print("data")
            else:

                if data.startswith('$GNGGA'):

                    datalist = data.split(',')

                    ### Saving Lattitude, longitude and Altitude in each reading in seperate lists.
                    lattitude = datalist[2]
                    latdirection = datalist[3]
                    finalLat = calculate_lat(lattitude, latdirection)

                    longitude = datalist[4]
                    londirection = datalist[5]
                    finalLon = calculate_lon(longitude, londirection)
 					quality = datalist[6]


                    altitude = float(datalist[9]) + float(datalist[11])

                    utm_values = utm.from_latlon(finalLat, finalLon)
                    utm_easting = utm_values[0]
                    utm_northing = utm_values[1]
                    utm_zone = utm_values[2]
                    utm_letter = utm_values[3]

                    count = count + 1
                    print(count)


                    lat_pub.publish(finalLat)
                    lon_pub.publish(finalLon)
                    alt_pub.publish(altitude)
                    utm_easting_pub.publish(utm_easting)
                    utm_northing_pub.publish(utm_northing)
                    zone_pub.publish(utm_zone)
                    letter_pub.publish(utm_letter)
					quality_pub.publish(quality)





    except rospy.ROSInterruptException:
        port.close()

    except serial.serialutil.SerialException:
        rospy.loginfo("Shutting down Minicom Node due to Serial Exception!!!")