#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import asyncio
import websockets
import threading

class WSBridge(Node):
    def __init__(self):
        super().__init__('phone_ws_bridge')
        self.declare_parameter('bridge_url', 'ws://saltylab-orin:9090')
        self.declare_parameter('reconnect_interval', 5.0)

        self.bridge_url = self.get_parameter('bridge_url').value
        self.reconnect_interval = self.get_parameter('reconnect_interval').value

        self.create_subscription(String, '/saltybot/speech_text', self.forward_msg, 10)
        self.create_subscription(String, '/saltybot/chat_response', self.forward_msg, 10)

        self.ws_connected = False
        self.ws_thread = threading.Thread(target=self.ws_loop, daemon=True)
        self.ws_thread.start()

    def forward_msg(self, msg):
        pass

    def ws_loop(self):
        try:
            loop = asyncio.new_event_loop()
            asyncio.set_event_loop(loop)
            loop.run_until_complete(self.ws_connect())
        except Exception as e:
            self.get_logger().error(f"WebSocket error: {e}")

    async def ws_connect(self):
        while True:
            try:
                async with websockets.connect(self.bridge_url, ping_interval=10, ping_timeout=5) as ws:
                    self.ws_connected = True
                    self.get_logger().info("Connected to Jetson")
                    while True:
                        try:
                            msg = await asyncio.wait_for(ws.recv(), timeout=10)
                        except asyncio.TimeoutError:
                            await ws.ping()
            except Exception as e:
                self.ws_connected = False
                self.get_logger().warn(f"Disconnected: {e}")
                await asyncio.sleep(self.reconnect_interval)

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

if __name__ == '__main__':
    main()
