// Copyright 2019 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

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

namespace Mujoco {
// API for importing Mujoco XML files into Unity scenes.
public class MjcfImporter {
  // Any materials created at runtime will only be automatically cleaned up if no other objects
  // reference it AND Unity changes scenes. In long-running worlds with no scene resets, this means
  // those materials are never cleaned even after their MjBodies are destroyed, so we cache a
  // single shared material to use for all generated meshes.
  public static Material DefaultMujocoMaterial {
    get {
      if (_DefaultMujocoMaterial == null) {
        _DefaultMujocoMaterial = new Material(Shader.Find("Standard"));
      }

      return _DefaultMujocoMaterial;
    }
    set {
      _DefaultMujocoMaterial = value;
    }
  }

  // Modifiers that change the settings of parsed nodes. They're limited to the scope of ParseRoot
  // method.
  protected MjXmlModifiers _modifiers;

  private static Material _DefaultMujocoMaterial;

  private const string _angleTypeDegree = "degree";

  private int _numGeneratedNames = 0;

  // A list of callbacks on how to handle custom node types.
  private IDictionary<string, Action<XmlElement, GameObject>> _customNodeHandlers =
      new Dictionary<string, Action<XmlElement, GameObject>>();

  public void AddNodeHandler(string name, Action<XmlElement, GameObject> handler) {
    _customNodeHandlers[name] = handler;
  }

  public MjcfImporter() {}

  public GameObject ImportString(string mjcfString, string name = null) {
    var mjcfXml = new XmlDocument();
    mjcfXml.LoadXml(mjcfString);
    return ImportXml(mjcfXml, name);
  }

  // A generic version of CreateGameObjectWithUniqueName.
  public GameObject CreateGameObjectWithUniqueName<T>(
      GameObject parentObject, XmlElement parentNode) where T : MjComponent {
    return CreateGameObjectWithUniqueName(parentObject, parentNode, typeof(T));
  }

  public string GenerateUniqueName(XmlElement parentNode) {
    var name = string.Empty;
    if (parentNode.HasAttribute("name")) {
      name = parentNode.GetAttribute("name");
    }
    if (string.IsNullOrEmpty(name)) {
      name = parentNode.Name + "_" + _numGeneratedNames++;
    }
    return name;
  }

  public unsafe GameObject ImportXml(XmlDocument mjcfXml, string name = null) {
    // Reset the numbering of anonymous nodes to ensure deterministic naming of loaded scenes.
    _numGeneratedNames = 0;

    _modifiers = new MjXmlModifiers(mjcfXml);

    ConfigureAngleType(mjcfXml);

    // We're adopting the approach of building the scene hierarchy as we parse the Mjcf.
    // Should we encounter any error, we'll terminate and clean it immediately.
    // Having a single root object for that will be very helpful, since we only need to delete that
    // object.
    var sceneRoot = new GameObject(name ?? "default");

    // We'll use Exceptions as a way to communicate errors encountered during parsing.
    try {
      ParseRoot(sceneRoot, mjcfXml.SelectSingleNode("/mujoco") as XmlElement);
    } catch (Exception ex) {
      // We consider any error as critical, and end the import process immediately, cleaning up
      // what we have already built.
      Debug.LogException(ex);
      GameObject.DestroyImmediate(sceneRoot);
      throw ex;
    } finally {
      _modifiers = null;
    }

    return sceneRoot;
  }

  // Creates a new GameObject, assigns it a unique name, and attaches a component of the specified
  // type.  If the requested component type is abstract, the method will attempt to instantiate one
  // of its implementations that passes the parser test.
  protected GameObject CreateGameObjectWithUniqueName(
      GameObject parentObject, XmlElement parentNode, Type componentType) {
    var name = GenerateUniqueName(parentNode);
    var gameObject = new GameObject(name);
    gameObject.transform.parent = parentObject.transform;
    var component = gameObject.AddComponent(componentType) as MjComponent;
    component.ParseMjcf(parentNode);
    return gameObject;
  }

  protected void ConfigureAngleType(XmlDocument mjcfXml) {
    var compilerNode = mjcfXml.SelectSingleNode("/mujoco/compiler") as XmlElement;
    if (compilerNode != null) {
      // Parse the angle type.
      var angleSetting = compilerNode.GetStringAttribute("angle", defaultValue: _angleTypeDegree);
      MjSceneImportSettings.AnglesInDegrees = angleSetting == _angleTypeDegree;
    }
  }

  protected virtual void ParseRoot(GameObject parentObject, XmlElement parentNode) {

    // This makes no references nor being referred into, so it can be parsed whenever.
    var optionNode = parentNode.SelectSingleNode("option") as XmlElement;
    var sizeNode = parentNode.SelectSingleNode("size") as XmlElement;
    if (optionNode != null || sizeNode != null) {
      var globalsObject = CreateGameObjectInParent("Global Settings", parentObject);
      var settingsComponent = globalsObject.AddComponent<MjGlobalSettings>();
      settingsComponent.ParseOptionSizeMjcf(optionNode, sizeNode);
    }

    // This makes references to assets.
    var worldBodyNode = parentNode.SelectSingleNode("worldbody") as XmlElement;
    ParseBodyChildren(parentObject, worldBodyNode);

    // This section references bodies, must be parsed after worldbody.
    var excludeNode = parentNode.SelectSingleNode("contact") as XmlElement;
    if (excludeNode != null) {
      var excludesParentObject = CreateGameObjectInParent("excludes", parentObject);
      foreach (var child in excludeNode.OfType<XmlElement>()) {
        if (child.Name != "exclude") {
          Debug.LogWarning(
              $"Only 'exclude' is supported - {child.Name} isn't supported yet.",
              parentObject);
        } else {
          _modifiers.ApplyModifiersToElement(child);
          CreateGameObjectWithUniqueName<MjExclude>(excludesParentObject, child);
        }
      }
    }

    // This section references joints/sites/geoms, must be parsed after worldbody.
    var tendonNode = parentNode.SelectSingleNode("tendon") as XmlElement;
    if (tendonNode != null) {
      var tendonsParentObject = CreateGameObjectInParent("tendons", parentObject);
      foreach (var child in tendonNode.OfType<XmlElement>()) {
        if (child.Name == "fixed") {
          CreateGameObjectWithUniqueName<MjFixedTendon>(tendonsParentObject, child);
        } else if (child.Name == "spatial") {
          CreateGameObjectWithUniqueName<MjSpatialTendon>(tendonsParentObject, child);
        } else {
          Debug.Log($"Tendon type {child.Name} not supported.");
        }
      }
    }

    // This section references worldbody elements + tendons, must be parsed after them.
    var equalityNode = parentNode.SelectSingleNode("equality") as XmlElement;
    if (equalityNode != null) {
      var equalitiesParentObject = CreateGameObjectInParent("equality constraints", parentObject);
      foreach (var child in equalityNode.OfType<XmlElement>()) {
        var equalityType = ParseEqualityType(child);
        _modifiers.ApplyModifiersToElement(child);
        CreateGameObjectWithUniqueName(equalitiesParentObject, child, equalityType);
      }
    }

    // This section references joints and tendons, must be parsed after worldbody and tendon.
    var actuatorNode = parentNode.SelectSingleNode("actuator") as XmlElement;
    if (actuatorNode != null) {
      var actuatorsParentObject = CreateGameObjectInParent("actuators", parentObject);
      foreach (var child in actuatorNode.OfType<XmlElement>()) {
        _modifiers.ApplyModifiersToElement(child);
        CreateGameObjectWithUniqueName<MjActuator>(actuatorsParentObject, child);
      }
    }

    // This section references tendons, actuators and worldbody elements, must be parsed last.
    var sensorNode = parentNode.SelectSingleNode("sensor") as XmlElement;
    if (sensorNode != null) {
      var sensorParentObject = CreateGameObjectInParent("sensors", parentObject);
      foreach (var child in sensorNode.OfType<XmlElement>()) {
        _modifiers.ApplyModifiersToElement(child);
        var sensorType = ParseSensorType(child);
        if (sensorType != null) {
          CreateGameObjectWithUniqueName(sensorParentObject, child, sensorType);
        }
      }
    }
  }

  protected virtual void ParseGeom(GameObject parentObject, XmlElement child) {
    var gameObject = CreateGameObjectWithUniqueName<MjGeom>(parentObject, child);
    // Add the visuals.
    gameObject.AddComponent<MjMeshFilter>();
    var renderer = gameObject.AddComponent<MeshRenderer>();
    renderer.sharedMaterial = DefaultMujocoMaterial;
  }

  private GameObject CreateGameObjectInParent(string name, GameObject parentObject) {
    var createdObject = new GameObject(name);
    createdObject.transform.parent = parentObject.transform;
    return createdObject;
  }

  private void ParseBodyChildren(GameObject parentObject, XmlElement parentNode) {
    foreach (var child in parentNode.Cast<XmlNode>().OfType<XmlElement>()) {
      _modifiers.ApplyModifiersToElement(child);

      if (_customNodeHandlers.TryGetValue(child.Name, out var handler)) {
        handler?.Invoke(child, parentObject);
      } else {
        ParseBodyChild(child, parentObject);
      }
    }
  }

  // Called by ParseBodyChildren for each XML node, overridable by inheriting claases.
  private void ParseBodyChild(XmlElement child, GameObject parentObject) {
    switch (child.Name) {
      case "geom": {
        // this one's different because it's overwritten in MjImporterWithAssets.
        ParseGeom(parentObject, child);
        break;
      }
      case "inertial": {
        CreateGameObjectWithUniqueName<MjInertial>(parentObject, child);
        break;
      }
      case "body": {
        var childObject = CreateGameObjectWithUniqueName<MjBody>(parentObject, child);
        ParseBodyChildren(childObject, child);
        break;
      }
      case "joint": {
        var jointType = ParseJointType(child);
        CreateGameObjectWithUniqueName(parentObject, child, jointType);
        break;
      }
      case "freejoint": {
        CreateGameObjectWithUniqueName<MjFreeJoint>(parentObject, child);
        break;
      }
      case "site": {
        CreateGameObjectWithUniqueName<MjSite>(parentObject, child);
        break;
      }
      case "camera": {
        CreateGameObjectWithCamera(parentObject, child);
        break;
      }
      default: {
        Debug.Log($"The importer does not yet support tags <{child.Name}>.");
        break;
      }
    }
  }

  private static Type ParseEqualityType(XmlElement node) {
    Type equalityType = null;
    switch (node.Name) {
      case "connect":
        equalityType = typeof(MjConnect);
        break;
      case "weld":
        equalityType = typeof(MjWeld);
        break;
      case "joint":
        equalityType = typeof(MjJointConstraint);
        break;
      case "tendon":
        equalityType = typeof(MjTendonConstraint);
        break;
      default:
        Debug.Log($"The importer does not yet support equality <{node.Name}>.");
        break;
    }
    return equalityType;
  }

  private static Type ParseSensorType(XmlElement node) {
    Type sensorType = null;
    switch (node.Name) {
      case "actuatorpos":
      case "actuatorvel":
      case "actuatorfrc":
        sensorType = typeof(MjActuatorScalarSensor);
        break;
      case "subtreecom":
      case "subtreelinvel":
      case "subtreeangmom":
        sensorType = typeof(MjBodyVectorSensor);
        break;
      case "jointpos":
      case "jointvel":
      case "jointlimitpos":
      case "jointlimitvel":
      case "jointlimitfrc":
        sensorType = typeof(MjJointScalarSensor);
        break;
      case "touch":
      case "rangefinder":
        sensorType = typeof(MjSiteScalarSensor);
        break;
      case "accelerometer":
      case "velocimeter":
      case "force":
      case "torque":
      case "gyro":
      case "magnetometer":
        sensorType = typeof(MjSiteVectorSensor);
        break;
      case "framequat":
        switch (node.GetAttribute("objtype")) {
          case "body":
          case "xbody":
            sensorType = typeof(MjBodyQuaternionSensor);
            break;
          case "geom":
            sensorType = typeof(MjGeomQuaternionSensor);
            break;
          case "site":
            sensorType = typeof(MjSiteQuaternionSensor);
            break;
          default:
            Debug.Log($"camera sensors unsupported <{node.Name}>.");
            break;
        }
        break;
      case "framepos":
      case "framexaxis":
      case "frameyaxis":
      case "framezaxis":
      case "framelinvel":
      case "frameangvel":
      case "framelinacc":
      case "frameangacc":
        switch (node.GetAttribute("objtype")) {
          case "body":
          case "xbody":
            sensorType = typeof(MjBodyVectorSensor);
            break;
          case "geom":
            sensorType = typeof(MjGeomVectorSensor);
            break;
          case "site":
            sensorType = typeof(MjSiteVectorSensor);
            break;
          default:
            Debug.Log($"camera sensors unsupported <{node.Name}>.");
            break;
        }
        break;
      default:
        Debug.Log($"The importer does not yet support sensor <{node.Name}>.");
        break;
    }
    return sensorType;
  }

  private Type ParseJointType(XmlElement parentNode) {
    Type jointType = null;
    switch (parentNode.GetStringAttribute("type", defaultValue: string.Empty)) {
      case "hinge":
        jointType = typeof(MjHingeJoint);
        break;
      case "ball":
        jointType = typeof(MjBallJoint);
        break;
      case "slide":
        jointType = typeof(MjSlideJoint);
        break;
      case "free":
        jointType = typeof(MjFreeJoint);
        break;
      default:
        // According to http://mujoco.org/book/XMLreference.html#joint, the default type for a joint
        // is Hinge.
        jointType = typeof(MjHingeJoint);
        break;
    }
    return jointType;
  }

  private GameObject CreateGameObjectWithCamera(
      GameObject parentObject, XmlElement parentNode) {
    var name = GenerateUniqueName(parentNode);
    var gameObject = new GameObject(name);
    gameObject.transform.parent = parentObject.transform;
    MjEngineTool.ParseTransformMjcf(parentNode, gameObject.transform);
    if (parentNode.GetStringAttribute("mode") == "targetbody") {
      // Look at the parent body:
      gameObject.transform.localRotation = Quaternion.LookRotation(
          gameObject.transform.InverseTransformPoint(parentObject.transform.position));
    } else {
      // MuJoCo's camera convention is to look down the negative z, which post-swizzle became
      // negative y, while Unity looks down positive z:
      gameObject.transform.localRotation *=
        Quaternion.FromToRotation(Vector3.forward, -Vector3.up);
    }
    var camera = gameObject.AddComponent<Camera>();
    camera.fieldOfView = parentNode.GetFloatAttribute("fovy", defaultValue: 45.0f);
    camera.nearClipPlane = 0.01f;  // MuJoCo default, TODO(etom): get from visual/map/znear
    return gameObject;
  }
}
}