using System;
using System.Collections.Generic;
using System.IO;
using System.Xml;
using UnityEngine;

namespace URDF
{
    /// <summary>
    /// Parser for URDF XML files
    /// </summary>
    public static class URDFParser
    {
        /// <summary>
        /// Parse a URDF file and return a URDFRobot model
        /// </summary>
        public static URDFRobot ParseURDF(string filePath)
        {
            if (!File.Exists(filePath))
            {
                Debug.LogError($"URDF file not found: {filePath}");
                return null;
            }

            XmlDocument doc = new XmlDocument();
            try
            {
                doc.Load(filePath);
            }
            catch (Exception e)
            {
                Debug.LogError($"Failed to load URDF XML: {e.Message}");
                return null;
            }

            XmlNode robotNode = doc.SelectSingleNode("robot");
            if (robotNode == null)
            {
                Debug.LogError("URDF file does not contain a <robot> root element");
                return null;
            }

            string robotName = GetAttribute(robotNode, "name", "robot");
            URDFRobot robot = new URDFRobot(robotName);

            // Parse all links
            XmlNodeList linkNodes = robotNode.SelectNodes("link");
            foreach (XmlNode linkNode in linkNodes)
            {
                string linkName = GetAttribute(linkNode, "name");
                if (string.IsNullOrEmpty(linkName))
                {
                    Debug.LogWarning("Found link without name attribute, skipping");
                    continue;
                }

                URDFLink link = ParseLink(linkNode, linkName);
                robot.links[linkName] = link;
            }

            // Parse all joints
            XmlNodeList jointNodes = robotNode.SelectNodes("joint");
            foreach (XmlNode jointNode in jointNodes)
            {
                string jointName = GetAttribute(jointNode, "name");
                if (string.IsNullOrEmpty(jointName))
                {
                    Debug.LogWarning("Found joint without name attribute, skipping");
                    continue;
                }

                URDFJoint joint = ParseJoint(jointNode, jointName);
                robot.joints[jointName] = joint;
            }

            Debug.Log($"Parsed URDF: {robot.links.Count} links, {robot.joints.Count} joints");
            return robot;
        }

        private static URDFLink ParseLink(XmlNode linkNode, string linkName)
        {
            URDFLink link = new URDFLink(linkName);

            // Parse visual elements
            XmlNodeList visualNodes = linkNode.SelectNodes("visual");
            foreach (XmlNode visualNode in visualNodes)
            {
                URDFVisual visual = ParseVisual(visualNode);
                if (visual != null)
                {
                    link.visuals.Add(visual);
                }
            }

            // Parse inertial properties
            XmlNode inertialNode = linkNode.SelectSingleNode("inertial");
            if (inertialNode != null)
            {
                XmlNode originNode = inertialNode.SelectSingleNode("origin");
                if (originNode != null)
                {
                    link.inertialOrigin = ParseOrigin(originNode);
                }

                XmlNode massNode = inertialNode.SelectSingleNode("mass");
                if (massNode != null)
                {
                    link.mass = ParseFloat(GetAttribute(massNode, "value"), 0f);
                }

                XmlNode inertiaNode = inertialNode.SelectSingleNode("inertia");
                if (inertiaNode != null)
                {
                    link.inertia = new Vector3(
                        ParseFloat(GetAttribute(inertiaNode, "ixx"), 0f),
                        ParseFloat(GetAttribute(inertiaNode, "iyy"), 0f),
                        ParseFloat(GetAttribute(inertiaNode, "izz"), 0f)
                    );
                }
            }

            return link;
        }

        private static URDFVisual ParseVisual(XmlNode visualNode)
        {
            URDFVisual visual = new URDFVisual();

            // Parse origin
            XmlNode originNode = visualNode.SelectSingleNode("origin");
            if (originNode != null)
            {
                visual.origin = ParseOrigin(originNode);
            }
            else
            {
                visual.origin = new URDFOrigin();
            }

            // Parse geometry
            XmlNode geometryNode = visualNode.SelectSingleNode("geometry");
            if (geometryNode != null)
            {
                visual.geometry = ParseGeometry(geometryNode);
            }

            // Parse material
            XmlNode materialNode = visualNode.SelectSingleNode("material");
            if (materialNode != null)
            {
                visual.materialName = GetAttribute(materialNode, "name");
                XmlNode colorNode = materialNode.SelectSingleNode("color");
                if (colorNode != null)
                {
                    visual.color = ParseColor(GetAttribute(colorNode, "rgba"));
                }
            }

            return visual;
        }

        private static URDFGeometry ParseGeometry(XmlNode geometryNode)
        {
            URDFGeometry geometry = new URDFGeometry();

            // Check for mesh
            XmlNode meshNode = geometryNode.SelectSingleNode("mesh");
            if (meshNode != null)
            {
                geometry.type = URDFGeometry.GeometryType.Mesh;
                geometry.meshFilename = GetAttribute(meshNode, "filename");
                geometry.scale = ParseVector3(GetAttribute(meshNode, "scale"), Vector3.one);
            }
            // Check for box
            else if (geometryNode.SelectSingleNode("box") != null)
            {
                geometry.type = URDFGeometry.GeometryType.Box;
                XmlNode boxNode = geometryNode.SelectSingleNode("box");
                geometry.size = ParseVector3(GetAttribute(boxNode, "size"), Vector3.one);
            }
            // Check for sphere
            else if (geometryNode.SelectSingleNode("sphere") != null)
            {
                geometry.type = URDFGeometry.GeometryType.Sphere;
                XmlNode sphereNode = geometryNode.SelectSingleNode("sphere");
                geometry.radius = ParseFloat(GetAttribute(sphereNode, "radius"), 0.5f);
            }
            // Check for cylinder
            else if (geometryNode.SelectSingleNode("cylinder") != null)
            {
                geometry.type = URDFGeometry.GeometryType.Cylinder;
                XmlNode cylinderNode = geometryNode.SelectSingleNode("cylinder");
                geometry.radius = ParseFloat(GetAttribute(cylinderNode, "radius"), 0.5f);
                geometry.length = ParseFloat(GetAttribute(cylinderNode, "length"), 1f);
            }

            return geometry;
        }

        private static URDFJoint ParseJoint(XmlNode jointNode, string jointName)
        {
            URDFJoint joint = new URDFJoint(jointName);

            // Parse joint type
            string typeStr = GetAttribute(jointNode, "type", "fixed").ToLower();
            switch (typeStr)
            {
                case "revolute":
                    joint.type = URDFJoint.JointType.Revolute;
                    break;
                case "continuous":
                    joint.type = URDFJoint.JointType.Continuous;
                    break;
                case "prismatic":
                    joint.type = URDFJoint.JointType.Prismatic;
                    break;
                case "fixed":
                    joint.type = URDFJoint.JointType.Fixed;
                    break;
                case "floating":
                    joint.type = URDFJoint.JointType.Floating;
                    break;
                case "planar":
                    joint.type = URDFJoint.JointType.Planar;
                    break;
                default:
                    joint.type = URDFJoint.JointType.Fixed;
                    break;
            }

            // Parse parent and child links
            XmlNode parentNode = jointNode.SelectSingleNode("parent");
            if (parentNode != null)
            {
                joint.parentLink = GetAttribute(parentNode, "link");
            }

            XmlNode childNode = jointNode.SelectSingleNode("child");
            if (childNode != null)
            {
                joint.childLink = GetAttribute(childNode, "link");
            }

            // Parse origin
            XmlNode originNode = jointNode.SelectSingleNode("origin");
            if (originNode != null)
            {
                joint.origin = ParseOrigin(originNode);
            }

            // Parse axis
            XmlNode axisNode = jointNode.SelectSingleNode("axis");
            if (axisNode != null)
            {
                joint.axis = ParseVector3(GetAttribute(axisNode, "xyz"), Vector3.up);
            }

            // Parse limit
            XmlNode limitNode = jointNode.SelectSingleNode("limit");
            if (limitNode != null)
            {
                joint.limit = new URDFJointLimit
                {
                    lower = ParseFloat(GetAttribute(limitNode, "lower"), -Mathf.PI),
                    upper = ParseFloat(GetAttribute(limitNode, "upper"), Mathf.PI),
                    effort = ParseFloat(GetAttribute(limitNode, "effort"), 1000f),
                    velocity = ParseFloat(GetAttribute(limitNode, "velocity"), 1f)
                };
            }

            return joint;
        }

        private static URDFOrigin ParseOrigin(XmlNode originNode)
        {
            URDFOrigin origin = new URDFOrigin();
            origin.xyz = ParseVector3(GetAttribute(originNode, "xyz"), Vector3.zero);
            origin.rpy = ParseVector3(GetAttribute(originNode, "rpy"), Vector3.zero);
            return origin;
        }

        private static Vector3 ParseVector3(string str, Vector3 defaultValue)
        {
            if (string.IsNullOrEmpty(str))
                return defaultValue;

            string[] parts = str.Trim().Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
            if (parts.Length >= 3)
            {
                return new Vector3(
                    ParseFloat(parts[0], defaultValue.x),
                    ParseFloat(parts[1], defaultValue.y),
                    ParseFloat(parts[2], defaultValue.z)
                );
            }
            return defaultValue;
        }

        private static Color ParseColor(string rgbaStr)
        {
            if (string.IsNullOrEmpty(rgbaStr))
                return Color.white;

            string[] parts = rgbaStr.Trim().Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
            if (parts.Length >= 4)
            {
                return new Color(
                    ParseFloat(parts[0], 1f),
                    ParseFloat(parts[1], 1f),
                    ParseFloat(parts[2], 1f),
                    ParseFloat(parts[3], 1f)
                );
            }
            return Color.white;
        }

        private static float ParseFloat(string str, float defaultValue)
        {
            if (string.IsNullOrEmpty(str))
                return defaultValue;

            if (float.TryParse(str, out float result))
                return result;

            return defaultValue;
        }

        private static string GetAttribute(XmlNode node, string attributeName, string defaultValue = "")
        {
            if (node?.Attributes?[attributeName] != null)
                return node.Attributes[attributeName].Value;
            return defaultValue;
        }
    }
}