using System;
using System.Collections.Generic;
using UnityEngine;

namespace URDF
{
    /// <summary>
    /// Represents a URDF origin with position (xyz) and rotation (rpy - roll, pitch, yaw)
    /// </summary>
    [Serializable]
    public class URDFOrigin
    {
        public Vector3 xyz;
        public Vector3 rpy; // Roll, Pitch, Yaw in radians

        public URDFOrigin()
        {
            xyz = Vector3.zero;
            rpy = Vector3.zero;
        }

        public URDFOrigin(Vector3 xyz, Vector3 rpy)
        {
            this.xyz = xyz;
            this.rpy = rpy;
        }
    }

    /// <summary>
    /// Represents URDF geometry (mesh, box, sphere, cylinder)
    /// </summary>
    [Serializable]
    public class URDFGeometry
    {
        public enum GeometryType
        {
            Mesh,
            Box,
            Sphere,
            Cylinder
        }

        public GeometryType type;
        public string meshFilename; // For mesh type
        public Vector3 scale; // For mesh type
        public Vector3 size; // For box type
        public float radius; // For sphere/cylinder type
        public float length; // For cylinder type
    }

    /// <summary>
    /// Represents a URDF visual element
    /// </summary>
    [Serializable]
    public class URDFVisual
    {
        public URDFOrigin origin;
        public URDFGeometry geometry;
        public string materialName;
        public Color color;
    }

    /// <summary>
    /// Represents a URDF link
    /// </summary>
    [Serializable]
    public class URDFLink
    {
        public string name;
        public List<URDFVisual> visuals;
        public URDFOrigin inertialOrigin;
        public float mass;
        public Vector3 inertia; // ixx, iyy, izz

        public URDFLink(string name)
        {
            this.name = name;
            this.visuals = new List<URDFVisual>();
            this.inertialOrigin = new URDFOrigin();
            this.mass = 0f;
            this.inertia = Vector3.zero;
        }
    }

    /// <summary>
    /// Represents a URDF joint limit
    /// </summary>
    [Serializable]
    public class URDFJointLimit
    {
        public float lower;
        public float upper;
        public float effort;
        public float velocity;
    }

    /// <summary>
    /// Represents a URDF joint
    /// </summary>
    [Serializable]
    public class URDFJoint
    {
        public enum JointType
        {
            Revolute,
            Continuous,
            Prismatic,
            Fixed,
            Floating,
            Planar
        }

        public string name;
        public JointType type;
        public string parentLink;
        public string childLink;
        public URDFOrigin origin;
        public Vector3 axis; // Rotation/translation axis
        public URDFJointLimit limit;

        public URDFJoint(string name)
        {
            this.name = name;
            this.type = JointType.Fixed;
            this.parentLink = "";
            this.childLink = "";
            this.origin = new URDFOrigin();
            this.axis = Vector3.up;
            this.limit = new URDFJointLimit();
        }
    }

    /// <summary>
    /// Represents a complete URDF robot model
    /// </summary>
    [Serializable]
    public class URDFRobot
    {
        public string name;
        public Dictionary<string, URDFLink> links;
        public Dictionary<string, URDFJoint> joints;

        public URDFRobot(string name)
        {
            this.name = name;
            this.links = new Dictionary<string, URDFLink>();
            this.joints = new Dictionary<string, URDFJoint>();
        }
    }
}