"""Test jogging to compare UI jogging vs homing jogging."""

import time
import json
import sys
import os

sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

from test_client import NovaSimTestClient


def test_ui_jogging():
    """Test UI-style jogging (sending continuous action messages)."""
    print("\n" + "=" * 70)
    print("UI-Style Jogging Test")
    print("=" * 70)
    print("\nThis test simulates how the UI sends jogging commands.")
    print("It sends continuous 'action' messages with joint velocities.\n")

    client = NovaSimTestClient("http://localhost:3004/nova-sim/api/v1")

    try:
        client.connect()
        print("✓ Connected to Nova-Sim")

        # Get initial position
        time.sleep(0.5)
        initial_joints = client.get_joint_positions()
        print(f"\n[1] Initial joint positions:")
        print(f"    [{', '.join(f'{j:7.4f}' for j in initial_joints)}]")

        # Send continuous action messages for joint 6, like the UI does
        print(f"\n[2] Sending continuous 'action' messages (UI style)...")
        print(f"    Jogging joint 6 in negative direction for 3 seconds")

        duration = 3.0
        rate_hz = 10  # UI typically sends at ~10 Hz
        num_messages = int(duration * rate_hz)

        for i in range(num_messages):
            # Send action message with j6 velocity (like UI does)
            client.ws.send(json.dumps({
                'type': 'action',
                'data': {
                    'j6': -0.5  # Negative velocity for joint 6
                }
            }))
            time.sleep(1.0 / rate_hz)
            if (i + 1) % 10 == 0:
                print(f"    Sent {i + 1}/{num_messages} messages")

        # Stop jogging
        client.ws.send(json.dumps({
            'type': 'action',
            'data': {

            }
        }))

        time.sleep(0.5)

        # Get final position
        final_joints = client.get_joint_positions()
        print(f"\n[3] Final joint positions:")
        print(f"    [{', '.join(f'{j:7.4f}' for j in final_joints)}]")

        # Calculate movement
        movement = [abs(final - initial) for final, initial in zip(final_joints, initial_joints)]
        total_movement = sum(movement)

        print(f"\n[4] Results:")
        print(f"    Total movement: {total_movement:.4f} rad")
        print(f"    Joint 6 movement: {movement[5]:.4f} rad")
        print(f"    Joint 6 change: {initial_joints[5]:.4f} -> {final_joints[5]:.4f}")

        if movement[5] > 0.1:
            print(f"\n✓ UI jogging works - joint 6 moved {movement[5]:.4f} rad")
        else:
            print(f"\n✗ UI jogging failed - joint 6 barely moved")

        client.disconnect()
        print("✓ Disconnected")

    except Exception as e:
        print(f"\n✗ Test error: {e}")
        import traceback
        traceback.print_exc()
        try:
            client.disconnect()
        except:
            pass

if __name__ == "__main__":
    print("\n" + "=" * 70)
    print("JOGGING COMPARISON TEST")
    print("=" * 70)
    print("\nThis test compares UI jogging vs homing jogging.")
    print("Both should trigger continuous jogging, but we'll see if they behave differently.\n")

    input("Press Enter to start UI jogging test (or Ctrl+C to exit)...")
    test_ui_jogging()

    time.sleep(2)

    input("\nPress Enter to start homing jogging test (or Ctrl+C to exit)...")
    test_homing_jogging()

    print("\n" + "=" * 70)
    print("Check /tmp/nova_sim_server.log for detailed logs")
    print("Compare the 'Nova Jogger' logs from both tests")
    print("=" * 70)