# import statements
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Point
import utm
import numpy as np

class GPS(Node):
    
    # Node initialization with the set of coordinates
    def __init__(self):
        super().__init__("gps_api")
        self.coords = [(48.75093,9.10330),
               (48.75081,9.10311),
               (48.75070,9.10323), 
               (48.75050,9.10288), 
               (48.75030,9.10299),
               (48.75014,9.10322),
               (48.74999,9.10311),
               (48.74990,9.10296),
               (48.74974,9.10320),
               (48.74967,9.10290),
               (48.74944,9.10282),
               (48.74927,9.10320),
               (48.74895,9.10295),
               (48.74886,9.10306),
               (48.74845,9.10278),
               (48.74813,9.10303),
               (48.74777,9.10258),
               (48.74717,9.10300),
               (48.74684,9.10291),
               (48.74629,9.10262),
               (48.74585,9.10244),
               (48.74534,9.10255),
               (48.74515,9.10269),
               (48.74499,9.10242),
               (48.74469,9.10260),
               (48.74462,9.10216),
               (48.74480,9.10169),
               (48.74457,9.10122),
               (48.74472,9.10061),
               (48.74454,9.10007)]
        
        self.gps_pub = self.create_publisher(Point, '/gps', 10)
        self.timer_ = self.create_timer(1, self.spinner)
        self.i = 0

    # Function used to add points between every item in the list above
    def increase_density_fixed(self, route, density=4):
        smoothed_x, smoothed_y = [], []

        for (x0, y0), (x1, y1) in zip(route, route[1:]):
            smoothed_x.extend([x0 + k * (x1 - x0) / density for k in range(1, density)])
            smoothed_y.extend([y0 + k * (y1 - y0) / density for k in range(1, density)])

        smoothed_x.append(route[-1][0])
        smoothed_y.append(route[-1][1])
        route = list(zip(smoothed_x, smoothed_y))
        
        return route
    
    # Cooridnates conversion to UTM values for better handling
    def to_utm(self, route):
        x, y = zip(*route)
        out = utm.from_latlon(np.array(x), np.array(y))
        route_conv = list(zip(out[0], out[1]))

        return route_conv
    
    # Function publishing the GPS coordinate every second, emualting the GPS receiver
    def spinner(self):
        msg = Point()
        route_new = self.to_utm(self.increase_density_fixed(self.coords))
        msg.x = route_new[self.i][0]
        msg.y = route_new[self.i][1]
        self.gps_pub.publish(msg)
        self.i += 1

def main(args=None):
    rclpy.init(args=args)
    node = GPS()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()