﻿using AGXUnity.Utils;
using UnityEngine;

namespace AGXUnity
{
  /// <summary>
  /// Friction model object with friction model type and solve type.
  /// </summary>
  [HelpURL( "https://us.download.algoryx.se/AGXUnity/documentation/current/editor_interface.html#friction-model" )]
  public class FrictionModel : ScriptAsset
  {
    public enum ESolveType
    {
      Direct = 1,
      Iterative = 2,
      Split = 3,
      DirectAndIterative = 4
    }

    public enum EType
    {
      IterativeProjectedFriction = 0,
      ScaleBoxFriction,
      BoxFriction,
      ConstantNormalForceBoxFriction
    }

    public enum PrimaryDirection
    {
      X,
      Y,
      Z
    }

    /// <summary>
    /// Converts ESolveType to agx.FrictionModel.SolveType.
    /// </summary>
    /// <param name="solveType">Input ESolveType.</param>
    /// <returns>Output agx.FrictionModel.SolveType.</returns>
    public static agx.FrictionModel.SolveType Convert( ESolveType solveType )
    {
      if ( solveType == ESolveType.Direct )
        return agx.FrictionModel.SolveType.DIRECT;
      else if ( solveType == ESolveType.Iterative )
        return agx.FrictionModel.SolveType.ITERATIVE;
      else if ( solveType == ESolveType.Split )
        return agx.FrictionModel.SolveType.SPLIT;
      else if ( solveType == ESolveType.DirectAndIterative )
        return agx.FrictionModel.SolveType.DIRECT_AND_ITERATIVE;

      return agx.FrictionModel.SolveType.NOT_DEFINED;
    }

    /// <summary>
    /// Converts native solve type to ESolveType.
    /// </summary>
    /// <param name="solveType">Native solve type.</param>
    /// <returns>ESolveType given native solve type.</returns>
    public static ESolveType Convert( agx.FrictionModel.SolveType solveType )
    {
      if ( solveType == agx.FrictionModel.SolveType.DIRECT )
        return ESolveType.Direct;
      else if ( solveType == agx.FrictionModel.SolveType.ITERATIVE )
        return ESolveType.Iterative;
      else if ( solveType == agx.FrictionModel.SolveType.DIRECT_AND_ITERATIVE )
        return ESolveType.DirectAndIterative;

      return ESolveType.Split;
    }

    public static Vector3 Convert( PrimaryDirection primaryDirection )
    {
      return primaryDirection == PrimaryDirection.X ?
               Vector3.right :
             primaryDirection == PrimaryDirection.Y ?
               Vector3.up :
               Vector3.forward;
    }

    /// <summary>
    /// Finds friction model type given native instance.
    /// </summary>
    /// <param name="native">Native friction model.</param>
    /// <returns>Native friction model type.</returns>
    public static EType FindType( agx.FrictionModel native )
    {
      if ( native == null || native.asIterativeProjectedConeFriction() != null )
        return EType.IterativeProjectedFriction;
      else if ( native.asScaleBoxFrictionModel() != null )
        return EType.ScaleBoxFriction;
      else if ( native.asConstantNormalForceOrientedBoxFrictionModel() != null )
        return EType.ConstantNormalForceBoxFriction;
      else if ( native.asBoxFrictionModel() != null )
        return EType.BoxFriction;

      Debug.LogWarning( "Unknown native friction model type - returning default." );

      return EType.IterativeProjectedFriction;
    }

    /// <summary>
    /// Get native instance, if created.
    /// </summary>
    public agx.FrictionModel Native { get; private set; } = null;

    /// <summary>
    /// Solve type, paired with property SolveType.
    /// </summary>
    [SerializeField]
    private ESolveType m_solveType = ESolveType.Split;

    /// <summary>
    /// Get or set solve type of this friction model.
    /// </summary>
    [Tooltip( "Get or set solve type of this friction model." )]
    public ESolveType SolveType
    {
      get { return m_solveType; }
      set
      {
        m_solveType = value;
        if ( Native != null )
          Native.setSolveType( Convert( m_solveType ) );
      }
    }

    /// <summary>
    /// Delegate when friction model type is changed and a new native
    /// instance is created.
    /// </summary>
    /// <param name="newFrictionModel">New native instance.</param>
    public delegate void OnNativeInstanceChangedDelegate( agx.FrictionModel newFrictionModel );

    /// <summary>
    /// On native instance changed event.
    /// </summary>
    public event OnNativeInstanceChangedDelegate OnNativeInstanceChanged = delegate { };

    /// <summary>
    /// Friction model type, paired with property Type.
    /// </summary>
    [SerializeField]
    private EType m_type = EType.IterativeProjectedFriction;

    /// <summary>
    /// Specifies the friction model to use for this contact materials using this asset.
    /// * Iterative Projected Cone - The default model. Provides a good base model which handles anisotripic frictions better than the box models.
    /// * Scale Box - Will attempt to scale the friction bounds throughout the solve stage.
    /// * Box - Has fixed friction bounds throughout the solve stage, performant but low accuracy. Makes a guess at the normal force to solve for friction.
    /// * Constant Normal Force Box - Like Box, but will use a provided constant normal force magnitude.
    /// </summary>
    [Tooltip( "Specifies the friction model to use for this contact materials using this asset.\n" +
              "* Iterative Projected Cone - The default model. Provides a good base model which handles anisotripic frictions better than the box models.\n" +
              "* Scale Box - Will attempt to scale the friction bounds throughout the solve stage.\n" +
              "* Box - Has fixed friction bounds throughout the solve stage, performant but low accuracy. Makes a guess at the normal force to solve for friction.\n" +
              "* Constant Normal Force Box - Like Box, but will use a provided constant normal force magnitude." )]
    public EType Type
    {
      get { return m_type; }
      set
      {
        if ( m_type == value )
          return;

        m_type = value;

        if ( Native == null )
          return;

        Native = CreateNative( Type, SolveType );

        OnNativeInstanceChanged( Native );
      }
    }

    /// <summary>
    /// Enable to mark that this friction model will be used for ContactMaterials involving track. This allows the friction frame to be set up automatically for these materials.
    /// </summary>
    [Tooltip( "Enable to mark that this friction model will be used for ContactMaterials involving track. This allows the friction frame to be set up automatically for these materials" )]
    [field: SerializeField]
    public bool TrackFrictionModel { get; set; } = false;

    /// <summary>
    /// This field is specific for agx.ConstantNormalForceOrientedBoxFrictionModel and
    /// only used when type is EType.ConstantNormalForceBoxFriction.
    /// </summary>
    [SerializeField]
    private float m_normalForceMagnitude = 100.0f;

    private bool IsConstantNormalForceModel => Type == EType.ConstantNormalForceBoxFriction;

    /// <summary>
    /// Normal force magnitude used in ConstantNormalForceBoxFriction.
    /// </summary>
    [Tooltip( "The normal force magnitude used" )]
    [ClampAboveZeroInInspector( true )]
    [DynamicallyShowInInspector( nameof( IsConstantNormalForceModel ) )]
    public float NormalForceMagnitude
    {
      get { return m_normalForceMagnitude; }
      set
      {
        m_normalForceMagnitude = value;
        if ( Native != null ) {
          if ( Native is agxVehicle.TrackBoxFrictionModel trackFriction )
            trackFriction.setConstantNormalForceMagnitude( m_normalForceMagnitude );
          else {
            var native = Native.asConstantNormalForceOrientedBoxFrictionModel();
            if ( native != null )
              native.setNormalForceMagnitude( m_normalForceMagnitude );
          }
        }
      }
    }

    /// <summary>
    /// This field is specific for agx.ConstantNormalForceOrientedBoxFrictionModel and
    /// only used when type is EType.ConstantNormalForceBoxFriction.
    /// </summary>
    [SerializeField]
    private bool m_scaleNormalForceWithDepth = false;

    /// <summary>
    /// Enable/disable scale of the given normal force with the contact
    /// point depth resulting in a maximum friction force:
    ///   depth * primary_friction_coefficient * given_normal_force
    ///   depth * secondary_friction_coefficient * given_normal_force
    /// Default: false.
    /// </summary>
    [Tooltip( "Enable/disable scale of the given normal force with the contact " +
              "point depth resulting in a maximum friction force: \n" +
              "  depth * primary_friction_coefficient * given_normal_force\n" +
              "  depth * secondary_friction_coefficient * given_normal_force\n" )]
    [DynamicallyShowInInspector( nameof( IsConstantNormalForceModel ) )]
    public bool ScaleNormalForceWithDepth
    {
      get { return m_scaleNormalForceWithDepth; }
      set
      {
        m_scaleNormalForceWithDepth = value;
        if ( Native != null ) {
          if ( Native is agxVehicle.TrackBoxFrictionModel trackFriction )
            trackFriction.setEnableScaleWithDepth( m_scaleNormalForceWithDepth );
          else {
            var native = Native.asConstantNormalForceOrientedBoxFrictionModel();
            if ( native != null )
              native.setEnableScaleWithDepth( m_scaleNormalForceWithDepth );
          }
        }
      }
    }

    /// <summary>
    /// Create native friction model given solve type and friction model type.
    /// </summary>
    /// <param name="type">Friction model type.</param>
    /// <param name="solveType">Solve type.</param>
    /// <returns>New native instance.</returns>
    public agx.FrictionModel CreateNative( EType type,
                                           ESolveType solveType )
    {
      agx.Frame referenceFrame = null;
      if ( m_orientedFrictionReferenceObject != null ) {
        referenceFrame = m_orientedFrictionReferenceObject.GetComponent<RigidBody>() != null ?
                           m_orientedFrictionReferenceObject.GetComponent<RigidBody>().Native.getFrame() :
                         m_orientedFrictionReferenceObject.GetComponent<Collide.Shape>() != null ?
                           m_orientedFrictionReferenceObject.GetComponent<Collide.Shape>().NativeGeometry.getFrame() :
                         m_orientedFrictionReferenceObject.GetComponent<ObserverFrame>() != null ?
                           m_orientedFrictionReferenceObject.GetComponent<ObserverFrame>().Native.getFrame() :
                           null;
      }

      agx.FrictionModel frictionModel = null;

      if ( TrackFrictionModel ) {
        frictionModel = type switch
        {
          EType.IterativeProjectedFriction => new agxVehicle.TrackIterativeProjectedConeFrictionModel( Convert( solveType ) ),
          EType.ScaleBoxFriction => new agxVehicle.TrackScaleBoxFrictionModel( Convert( solveType ) ),
          EType.BoxFriction => new agxVehicle.TrackBoxFrictionModel( Convert( solveType ) ),
          EType.ConstantNormalForceBoxFriction => new agxVehicle.TrackBoxFrictionModel( Convert( solveType ), NormalForceMagnitude, ScaleNormalForceWithDepth ),
          _ => null
        };

        if ( frictionModel == null )
          Debug.LogError( $"Friction Type {System.Enum.GetName( typeof( EType ), type )} does not support a track variant" );
      }
      else {
        if ( type == EType.IterativeProjectedFriction )
          frictionModel = referenceFrame != null ?
                            new agx.OrientedIterativeProjectedConeFrictionModel( referenceFrame,
                                                                                 Convert( m_orientedFrictionPrimaryDirection ).ToHandedVec3(),
                                                                                 Convert( solveType ) ) :
                            new agx.IterativeProjectedConeFriction( Convert( solveType ) );
        else if ( type == EType.ScaleBoxFriction )
          frictionModel = referenceFrame != null ?
                            new agx.OrientedScaleBoxFrictionModel( referenceFrame,
                                                                   Convert( m_orientedFrictionPrimaryDirection ).ToHandedVec3(),
                                                                   Convert( solveType ) ) :
                            new agx.ScaleBoxFrictionModel( Convert( solveType ) );
        else if ( type == EType.BoxFriction )
          frictionModel = referenceFrame != null ?
                            new agx.OrientedBoxFrictionModel( referenceFrame,
                                                              Convert( m_orientedFrictionPrimaryDirection ).ToHandedVec3(),
                                                              Convert( solveType ) ) :
                            new agx.BoxFrictionModel( Convert( solveType ) );
        else if ( type == EType.ConstantNormalForceBoxFriction ) {
          frictionModel = new agx.ConstantNormalForceOrientedBoxFrictionModel( NormalForceMagnitude,
                                                                               referenceFrame,
                                                                               Convert( m_orientedFrictionPrimaryDirection ).ToHandedVec3(),
                                                                               Convert( solveType ),
                                                                               ScaleNormalForceWithDepth );
        }
      }

      return frictionModel;
    }

    public FrictionModel RestoreLocalDataFrom( agx.FrictionModel native )
    {
      SolveType = Convert( native.getSolveType() );
      Type      = FindType( native );

      // TODO: How do we handle oriented when restoring from file?
      if ( Type == EType.ConstantNormalForceBoxFriction ) {
        NormalForceMagnitude = System.Convert.ToSingle( native.asConstantNormalForceOrientedBoxFrictionModel().getNormalForceMagnitude() );
        ScaleNormalForceWithDepth = native.asConstantNormalForceOrientedBoxFrictionModel().getEnableScaleWithDepth();
      }

      return this;
    }

    public void InitializeOriented( ScriptComponent referenceComponent, PrimaryDirection primaryDirection )
    {
      m_orientedFrictionReferenceObject = referenceComponent;
      m_orientedFrictionPrimaryDirection = primaryDirection;
      Native = CreateNative( Type, SolveType );
      OnNativeInstanceChanged( Native );
    }

    protected override void Construct()
    {
      m_type      = EType.IterativeProjectedFriction;
      m_solveType = ESolveType.Split;
    }

    protected override bool Initialize()
    {
      if ( Native != null )
        return true;

      Native = CreateNative( Type, SolveType );

      return true;
    }

    public override void Destroy()
    {
      Native = null;
    }

    private ScriptComponent m_orientedFrictionReferenceObject = null;
    private PrimaryDirection m_orientedFrictionPrimaryDirection = PrimaryDirection.X;
  }
}
