#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import NavSatFix, NavSatStatus
import subprocess
import json

class GPSNode(Node):
    def __init__(self):
        super().__init__('gps_node')
        self.declare_parameter('gps_provider', 'termux')
        self.declare_parameter('update_rate', 1.0)

        self.gps_provider = self.get_parameter('gps_provider').value
        self.update_rate = self.get_parameter('update_rate').value

        self.publisher = self.create_publisher(NavSatFix, '/phone/gps', 10)
        self.timer_period = 1.0 / self.update_rate
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

    def get_location_termux(self):
        try:
            result = subprocess.run(['termux-location', '-p', 'network'],
                                  capture_output=True, text=True, timeout=5)
            if result.returncode == 0:
                data = json.loads(result.stdout)
                return {
                    'latitude': data.get('latitude'),
                    'longitude': data.get('longitude'),
                    'altitude': data.get('altitude', 0),
                    'accuracy': data.get('accuracy', 0),
                }
        except Exception as e:
            self.get_logger().debug(f"GPS error: {e}")
        return None

    def timer_callback(self):
        location = self.get_location_termux()
        if location is None:
            return

        msg = NavSatFix()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'phone_gps'
        msg.latitude = location['latitude']
        msg.longitude = location['longitude']
        msg.altitude = location['altitude']
        msg.status.status = NavSatStatus.STATUS_FIX
        msg.status.service = NavSatStatus.SERVICE_GPS

        accuracy = location['accuracy']
        msg.position_covariance = [accuracy**2, 0, 0, 0, accuracy**2, 0, 0, 0, (accuracy*2)**2]
        msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED

        self.publisher.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    node = GPSNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
