#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import subprocess
import threading
from queue import Queue

class OpenClawChatNode(Node):
    def __init__(self):
        super().__init__('openclaw_chat_node')
        self.declare_parameter('model_path', '/data/data/com.termux/files/home/.cache/openclaw/model.gguf')
        self.declare_parameter('context_size', 256)
        self.declare_parameter('top_p', 0.95)
        self.declare_parameter('temperature', 0.7)

        self.model_path = self.get_parameter('model_path').value
        self.context_size = self.get_parameter('context_size').value
        self.top_p = self.get_parameter('top_p').value
        self.temperature = self.get_parameter('temperature').value

        self.subscription = self.create_subscription(String, '/saltybot/speech_text', self.speech_callback, 10)
        self.response_pub = self.create_publisher(String, '/saltybot/chat_response', 10)

        self.request_queue = Queue()
        self.llm_thread = threading.Thread(target=self.llm_worker, daemon=True)
        self.llm_thread.start()

    def speech_callback(self, msg):
        self.get_logger().info(f"Received: {msg.data}")
        self.request_queue.put(msg.data)

    def llm_worker(self):
        while True:
            try:
                prompt = self.request_queue.get(timeout=1)
                response = self.infer(prompt)
                if response:
                    msg = String()
                    msg.data = response
                    self.response_pub.publish(msg)
            except:
                pass

    def infer(self, prompt):
        try:
            cmd = ['llama-cli', '-m', self.model_path, '-n', str(self.context_size),
                   '--top-p', str(self.top_p), '--temperature', str(self.temperature),
                   '-p', prompt, '--no-display-prompt']
            result = subprocess.run(cmd, capture_output=True, text=True, timeout=30)
            if result.returncode == 0:
                response = result.stdout.strip()
                if prompt in response:
                    response = response.split(prompt, 1)[1].strip()
                return response[:200]
        except Exception as e:
            self.get_logger().error(f"LLM error: {e}")
        return None

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

if __name__ == '__main__':
    main()
