using UnityEngine;
using UnityEditor;
using System.IO;
using URDF;

public class URDFImporterEditor : EditorWindow
{
    private string urdfFilePath = "";
    private Object selectedURDF;
    private bool ignoreURDFScale = true; // Default to true - assume models are already in meters
    private float jointPositionScale = 1.0f; // Scale joint positions - default 1.0 (no scaling)

    [MenuItem("Assets/Import URDF")]
    public static void ShowWindow()
    {
        URDFImporterEditor window = GetWindow<URDFImporterEditor>("URDF Importer");
        window.Show();
    }

    [MenuItem("Assets/Import URDF", true)]
    public static bool ValidateImportURDF()
    {
        // Only enable if we're in the editor
        return true;
    }

    void OnGUI()
    {
        GUILayout.Label("URDF Robot Importer", EditorStyles.boldLabel);
        EditorGUILayout.Space();

        EditorGUILayout.HelpBox(
            "Select a URDF file to import. The importer will create a GameObject hierarchy " +
            "with correct parent-child relationships and pivot points.",
            MessageType.Info);

        EditorGUILayout.Space();

        // File selection
        EditorGUILayout.BeginHorizontal();
        EditorGUILayout.LabelField("URDF File:", GUILayout.Width(100));
        
        selectedURDF = EditorGUILayout.ObjectField(
            selectedURDF,
            typeof(Object),
            false,
            GUILayout.ExpandWidth(true));

        if (selectedURDF != null)
        {
            string assetPath = AssetDatabase.GetAssetPath(selectedURDF);
            if (assetPath.EndsWith(".urdf"))
            {
                urdfFilePath = Path.Combine(Application.dataPath, assetPath.Substring("Assets/".Length));
            }
            else
            {
                EditorGUILayout.HelpBox("Please select a .urdf file", MessageType.Warning);
                selectedURDF = null;
            }
        }

        EditorGUILayout.EndHorizontal();

        EditorGUILayout.Space();

        // Scale options
        ignoreURDFScale = EditorGUILayout.Toggle(
            new GUIContent("Ignore URDF Scale", 
                "If checked, models are assumed to be already scaled to meters. " +
                "If unchecked, URDF scale values (typically 0.001 for millimeter models) will be applied."),
            ignoreURDFScale);

        if (ignoreURDFScale)
        {
            jointPositionScale = EditorGUILayout.FloatField(
                new GUIContent("Joint Position Scale",
                    "Scale factor for joint positions. Use 1000 if models were scaled from millimeters to meters. " +
                    "Use 1.0 if joint positions are already correct."),
                jointPositionScale);
        }

        EditorGUILayout.Space();

        // Import button
        GUI.enabled = !string.IsNullOrEmpty(urdfFilePath) && File.Exists(urdfFilePath);
        if (GUILayout.Button("Import URDF", GUILayout.Height(30)))
        {
            ImportURDF();
        }
        GUI.enabled = true;

        EditorGUILayout.Space();

        // Quick import from context menu
        if (Event.current.type == EventType.ContextClick)
        {
            GenericMenu menu = new GenericMenu();
            menu.AddItem(new GUIContent("Import Selected URDF"), false, () =>
            {
                if (Selection.activeObject != null)
                {
                    string path = AssetDatabase.GetAssetPath(Selection.activeObject);
                    if (path.EndsWith(".urdf"))
                    {
                        selectedURDF = Selection.activeObject;
                        urdfFilePath = Path.Combine(Application.dataPath, path.Substring("Assets/".Length));
                        ImportURDF();
                    }
                }
            });
            menu.ShowAsContext();
        }
    }

    [MenuItem("Assets/Import URDF (Selected)", true)]
    public static bool ValidateImportSelectedURDF()
    {
        if (Selection.activeObject == null)
            return false;

        string path = AssetDatabase.GetAssetPath(Selection.activeObject);
        return path.EndsWith(".urdf");
    }

    [MenuItem("Assets/Import URDF (Selected)")]
    public static void ImportSelectedURDF()
    {
        if (Selection.activeObject == null)
        {
            Debug.LogError("No URDF file selected");
            return;
        }

        string assetPath = AssetDatabase.GetAssetPath(Selection.activeObject);
        if (!assetPath.EndsWith(".urdf"))
        {
            Debug.LogError("Selected file is not a URDF file");
            return;
        }

        string fullPath = Path.Combine(Application.dataPath, assetPath.Substring("Assets/".Length));
        // Default to ignoring scale (assuming models are in meters) with no joint position scaling
        ImportURDFFile(fullPath, ignoreScale: true, jointPositionScale: 1.0f);
    }

    private void ImportURDF()
    {
        if (string.IsNullOrEmpty(urdfFilePath) || !File.Exists(urdfFilePath))
        {
            EditorUtility.DisplayDialog("Error", "Please select a valid URDF file", "OK");
            return;
        }

        ImportURDFFile(urdfFilePath, ignoreURDFScale, ignoreURDFScale ? jointPositionScale : 1.0f);
    }

    private static void ImportURDFFile(string urdfFilePath, bool ignoreScale = true, float jointPositionScale = 1.0f)
    {
        try
        {
            EditorUtility.DisplayProgressBar("Importing URDF", "Parsing URDF file...", 0f);

            GameObject robotRoot = URDFImporter.ImportURDF(urdfFilePath, null, ignoreScale, jointPositionScale);

            if (robotRoot != null)
            {
                // Select the imported robot in the hierarchy
                Selection.activeGameObject = robotRoot;
                EditorGUIUtility.PingObject(robotRoot);

                Debug.Log($"Successfully imported URDF robot: {robotRoot.name}");
            }
            else
            {
                EditorUtility.DisplayDialog("Import Failed", "Failed to import URDF file. Check console for errors.", "OK");
            }
        }
        catch (System.Exception e)
        {
            Debug.LogError($"Error importing URDF: {e.Message}\n{e.StackTrace}");
            EditorUtility.DisplayDialog("Import Error", $"Error importing URDF: {e.Message}", "OK");
        }
        finally
        {
            EditorUtility.ClearProgressBar();
        }
    }
}