#!/usr/bin/env_python

import rospy
import math
from std_msgs.msg import Float32





def lat_lon_to_global_xy(lat, lon):
    x = radius * lon * math.cos((ref_lat0 + ref_lat1) / 2)
    y = radius * lat
    return x, y



def lat_lon_to_local_xy(lat, lon):
    pos_x, pos_y = lat_lon_to_global_xy(lat, lon)

    per_x = ((pos_x - ref_x0) / (ref_x1 - ref_x0))
    per_y = ((pos_y - ref_y0) / (ref_y1 - ref_y0))
    return per_x, per_y
    
def callback_lat(latitude):
    #per_x, per_y = lat_lon_to_local_xy(latitude.data, ref_lon0)
    y = radius * (latitude.data - ((ref_lat0 + ref_lat1) / 2))
    rospy.loginfo("Y = %f ||| Lat = %f", y, latitude.data)


def callback_lon(longitude):
    #per_x, per_y = lat_lon_to_local_xy(longitude.data, ref_lon0)
    x = radius * (longitude.data - ((ref_lon0 + ref_lon1) / 2)) * math.cos((ref_lat0 + ref_lat1) / 2)
    rospy.loginfo("X = %f ||| Long = %f", x, longitude.data)


    

def listener():
    rospy.init_node('conversion_node', anonymous = True)

    rospy.Subscriber("/serial_node/Longitude", Float32, callback_lon)
    rospy.Subscriber("/serial_node/Latitude", Float32, callback_lat)

    rospy.spin()

if __name__ == '__main__':
    radius = 6371
    ref_lon0 = 17.282852
    ref_lat0 = 62.393667
    ref_lon1 = 17.283008
    ref_lat1 = 62.393279
    ref_x0, ref_y0 = lat_lon_to_global_xy(ref_lat0, ref_lon0)
    ref_x1, ref_y1 = lat_lon_to_global_xy(ref_lat1, ref_lon1)
    listener()