import gradio as gr
from transformers import Qwen3VLForConditionalGeneration, AutoProcessor

model = Qwen3VLForConditionalGeneration.from_pretrained(
    "UBTECH-Robotics/Thinker-4B",
    dtype="auto",
    device_map="auto"
)
processor = AutoProcessor.from_pretrained("UBTECH-Robotics/Thinker-4B")

SYSTEM_PROMPT = """You are controlling Walker S2 robot.
You must output ONLY a JSON array. No explanation, no text, only JSON.

Available commands:
1. {"skill": "A000004", "params": {"mode": "DYNAMIC"}}
2. {"skill": "A000020", "params": {}}
3. {"skill": "A000002", "params": {"pose": [x, y, yaw], "speed": [vx, vy, vyaw]}}
   - x: forward distance in METERS (not pixels!)
   - y: left/right distance in METERS (not pixels!)
   - yaw: rotation in RADIANS
   - speed: max [0.8, 0.3, 0.6]
4. {"skill": "A000026", "params": {"actionId": "qyh/handshake"}}
5. {"skill": "A000004", "params": {"mode": "STAND"}}

IMPORTANT: pose values must be in METERS, not pixels!
Example: pose [0.5, 0.0, 0.0] means move 0.5 meters forward.

Output ONLY JSON array. Nothing else."""

def control_robot(command):
    try:
        messages = [
            {
                "role": "user",
                "content": [
                    {
                        "type": "image",
                        "image": "http://images.cocodataset.org/val2017/000000039769.jpg",
                    },
                    {
                        "type": "text",
                        "text": SYSTEM_PROMPT + "\n\n" + command
                    },
                ],
            }
        ]

        inputs = processor.apply_chat_template(
            messages,
            tokenize=True,
            add_generation_prompt=True,
            return_dict=True,
            return_tensors="pt"
        )
        inputs = inputs.to(model.device)

        generated_ids = model.generate(**inputs, max_new_tokens=256)
        generated_ids_trimmed = [
            out_ids[len(in_ids):] for in_ids, out_ids
            in zip(inputs.input_ids, generated_ids)
        ]
        output_text = processor.batch_decode(
            generated_ids_trimmed,
            skip_special_tokens=True,
            clean_up_tokenization_spaces=False
        )
        return output_text[0]

    except Exception as e:
        return f"에러 발생: {str(e)}"

gr.Interface(
    fn=control_robot,
    inputs=gr.Textbox(
        label="명령 입력",
        placeholder="예: Find the remote and go to it"
    ),
    outputs=gr.Textbox(label="Walker S2 명령어 출력"),
    title="Walker S2 Robot Controller",
    description="명령을 입력하면 Thinker가 Walker S2 명령어로 변환합니다"
).launch()