#!/usr/bin/env python3
"""Test Nova Jogger v2 with real robot movements."""

import os
import sys
import time
from pathlib import Path

# Add parent directory to path
sys.path.insert(0, str(Path(__file__).parent.parent))

# Load .env.local
def load_env_file(filepath):
    """Simple .env file loader."""
    if not filepath.exists():
        return
    with open(filepath, 'r') as f:
        for line in f:
            line = line.strip()
            if line and not line.startswith('#') and '=' in line:
                key, value = line.split('=', 1)
                os.environ[key.strip()] = value.strip()

def main():
    # Load .env.local
    env_path = Path(__file__).parent.parent / ".env.local"
    if not env_path.exists():
        print(f"Error: {env_path} not found")
        return 1

    print(f"Loading environment from {env_path}")
    load_env_file(env_path)

    # Import NovaJogger
    try:
        from robots.ur5.nova_jogger import NovaJogger
        print("✓ NovaJogger imported successfully")
    except ImportError as e:
        print(f"✗ Failed to import NovaJogger: {e}")
        return 1

    # Get config
    instance_url = os.getenv("NOVA_INSTANCE_URL")
    access_token = os.getenv("NOVA_ACCESS_TOKEN")
    cell_id = os.getenv("NOVA_CELL_ID", "cell")
    controller_id = os.getenv("NOVA_CONTROLLER_ID")
    motion_group_id = os.getenv("NOVA_MOTION_GROUP_ID")

    if not all([instance_url, access_token, controller_id, motion_group_id]):
        print("✗ Missing required environment variables")
        return 1

    print("\n" + "=" * 70)
    print("Nova Jogger v2 Integration Test")
    print("=" * 70)
    print(f"Instance: {instance_url}")
    print(f"Controller: {controller_id}")
    print(f"Motion Group: {motion_group_id}")
    print()

    # Create jogger instance
    jogger = NovaJogger(
        instance_url=instance_url,
        access_token=access_token,
        cell_id=cell_id,
        controller_id=controller_id,
        motion_group_id=motion_group_id
    )

    try:
        # Connect
        print("[1] Connecting to Nova API v2...")
        if not jogger.connect():
            print("✗ Failed to connect")
            return 1
        print("✓ Connected\n")

        # Test 1: Joint jogging
        print("[2] Testing joint jogging (joint 0, 0.05 rad/s)...")
        if not jogger.start_joint_jog(joint=0, direction="+", velocity_rads_per_sec=0.05):
            print("✗ Failed to start joint jog")
            return 1
        print("✓ Joint jog started")
        print("   Jogging for 1 second...")
        time.sleep(1.0)

        if not jogger.stop():
            print("✗ Failed to stop")
            return 1
        print("✓ Stopped\n")

        time.sleep(0.5)

        # Test 2: Cartesian translation
        print("[3] Testing cartesian translation (X axis, 5 mm/s)...")
        if not jogger.start_cartesian_translation(axis="x", direction="+", velocity_mm_per_sec=5.0):
            print("✗ Failed to start cartesian translation")
            return 1
        print("✓ Cartesian translation started")
        print("   Jogging for 1 second...")
        time.sleep(1.0)

        if not jogger.stop():
            print("✗ Failed to stop")
            return 1
        print("✓ Stopped\n")

        time.sleep(0.5)

        # Test 3: Mode switching - joint to cartesian without reconnecting
        print("[4] Testing dynamic mode switching...")
        print("   Phase 1: Joint mode (joint 1, 0.05 rad/s)")
        if not jogger.start_joint_jog(joint=1, direction="+", velocity_rads_per_sec=0.05):
            print("✗ Failed to start joint jog")
            return 1
        time.sleep(0.5)

        print("   Phase 2: Switching to cartesian (Y axis, 5 mm/s)")
        if not jogger.start_cartesian_translation(axis="y", direction="+", velocity_mm_per_sec=5.0):
            print("✗ Failed to switch to cartesian")
            return 1
        time.sleep(0.5)

        print("   Phase 3: Switching back to joint (joint 2)")
        if not jogger.start_joint_jog(joint=2, direction="+", velocity_rads_per_sec=0.05):
            print("✗ Failed to switch back to joint")
            return 1
        time.sleep(0.5)

        if not jogger.stop():
            print("✗ Failed to stop")
            return 1
        print("✓ Mode switching successful\n")

        time.sleep(0.5)

        # Test 4: Cartesian rotation
        print("[5] Testing cartesian rotation (Z axis, 0.1 rad/s)...")
        if not jogger.start_cartesian_rotation(axis="z", direction="+", velocity_rads_per_sec=0.1):
            print("✗ Failed to start cartesian rotation")
            return 1
        print("✓ Cartesian rotation started")
        print("   Jogging for 0.5 seconds...")
        time.sleep(0.5)

        if not jogger.stop():
            print("✗ Failed to stop")
            return 1
        print("✓ Stopped\n")

        # Disconnect
        print("[6] Disconnecting...")
        jogger.disconnect()
        print("✓ Disconnected")

        print("\n" + "=" * 70)
        print("✓ ALL TESTS PASSED")
        print("=" * 70)
        print("\nNova Jogger v2 API integration is working correctly!")
        return 0

    except Exception as e:
        print(f"\n✗ Test failed: {type(e).__name__}: {e}")
        import traceback
        traceback.print_exc()
        try:
            jogger.disconnect()
        except:
            pass
        return 1


if __name__ == "__main__":
    sys.exit(main())