using System;

using UnityEngine;

using Framework.Layers;
using Framework.Services;

namespace Framework.Actors {

  /// <summary>Manages the physical behavior of an actor</summary>
  public class ActorPhysics : ScriptComponent, IVelocityProvider {

    /// <summary>Smallest velocity change the recorded velocity can be updated by</summary>
    private const float VelocityEpsilon = 1e-4f;

    /// <summary>How far off the ground the character still counts as grounded</summary>
    private const float GroundCheckExtraDistance = 0.03f;

    /// <summary>Whether a custom layer mask should be used for collisions by this actor</summary>
    /// <remarks>
    ///   If this is set to 'false', the layer mask will be determined from Unity's
    ///   physics settings for the layer the player character is on.
    /// </remarks>
    public bool UseCustomLayerMask;

    /// <summary>Things that will block movement of the actor</summary>
    /// <remarks>
    ///   This is ignored (or rather, overwritten) unless <see cref="UseCustomLayerMask" />
    ///   is set at the same time.
    /// </remarks>
    public LayerMask SolidLayerMask = Physics.DefaultRaycastLayers;

    /// <summary>Whether the actor is currently on solid ground</summary>
    /// <remarks>
    ///   This can be used to determine when to reset the jump counter or whether
    ///   the actor is able to jump off from the ground at all. It is not very reliable,
    ///   though, and the actor often needs to be driven into the ground at a significant
    ///   velocity to even register as grounded. Consider using
    ///   the <see cref="GroundChecker" /> for a more reliable alternative.    
    /// </remarks>
    public bool UnreliableIsGrounded;
    
    /// <summary>Whether the actor is being affected by gravity</summary>
    /// <remarks>
    ///   If set, the gravity from Unity's physics settings is applied as a force
    ///   automatically before each update. Only set this to false if you do fancy
    ///   things with gravity.
    /// </remarks>
    public bool IsAffectedByGravity = true;

    /// <summary>How much the actor is affected by gravity</summary>
    /// <remarks>
    ///   You may want to increase this for fast platformers since a realistic amount
    ///   of gravity makes for very boring movements when combined with unrealistic
    ///   jump heights (of multiple times the actor's own body height).
    /// </remarks>
    public float GravityScale = 1.0f;

    /// <summary>Mass of the actor</summary>
    /// <remarks>
    ///   This should include the equipment carried by the actor.
    ///   <list>
    ///     <item>
    ///       <term>Human</term>
    ///       <description>75 kilograms</description>
    ///     </item>
    ///     <item>
    ///       <term>Dog</term>
    ///       <description>35 kilograms</description>
    ///     </item>
    ///     <item>
    ///       <term>Horse</term>
    ///       <description>450 kilograms</description>
    ///     </item>
    ///     <item>
    ///       <term>Car</term>
    ///       <description>1500 kilograms</description>
    ///     </item>
    ///   </list>
    /// </remarks>
    public float Mass = 85.0f; // Default weight = athletic "hero" human + stuff
    
    /// <summary>Maximum step height the character can traverse without jumping</summary>
    /// <remarks>
    ///   The character controller used by the actor physics component moves the actor
    ///   the full horizontal distance desired and adjusts the height as needed
    ///   (up to this height). The actor will move on top of the step without gaining
    ///   any upward velocity.  
    /// </remarks>
    public float MaximumStepHeight = 0.25f;
    
    /// <summary>Current velocity of the actor</summary>
    public Vector3 Velocity;

    /// <summary>Called when the component is loaded into a game object</summary>
    protected override void Awake() {
      base.Awake();
      
      // Unless the actor wants to use a custom layer mask, look up what layers
      // the actor (or rather the layer the actor is on) should collide against 
      if(!this.UseCustomLayerMask) {
        this.SolidLayerMask = LayerMaskHelper.GetLayerCollisionMaskFor(gameObject.layer);
      }

#if UNITY_EDITOR
      // Make sure the actor can actually detect ground contact and won't collide
      // with itself, since otherwise ground detection will not work at all.
      int ownLayer = gameObject.layer;
      if((LayerMaskHelper.GetLayerCollisionMaskFor(ownLayer) & (1 << ownLayer)) != 0) {
        string ownLayerName = LayerMask.LayerToName(ownLayer);
        Debug.LogWarning(
          "Actor '" + gameObject.name + "' is on layer '" + ownLayerName + "' and " +
          "would collide with itself. Ground detection will not work. Either change " +
          "the 'SolidLayerMask' field of its ActorPhysics component or adjust " +
          "the collision mask in Unity's physics settings."
        );
      }
#endif // UNITY_EDITOR
      
      // We rely on the built-in CharacterController component to do the movement.
      // An earlier attempt with hand-rolled movement logic was thwarted but Unity
      // or PhysX returning bad intersection points and normals for standing contacts.
      this.characterController = GetComponent<CharacterController>();
      if(this.characterController == null) {
        this.characterController = gameObject.AddComponent<CharacterController>();
        this.characterController.minMoveDistance = VelocityEpsilon;
        fitCharacterControllerToCollider();
      }

      this.characterController.stepOffset = this.MaximumStepHeight;
    }

    /// <summary>Character controller that is used so move the actor around</summary>
    public CharacterController CharacterController {
      get { return this.characterController; }
    }

    /// <summary>Collisions the actor encountered in the last physics frame</summary>
    public CollisionFlags Collisions {
      get { return this.collisions; }
    }

    /// <summary>Queues a direct movement for the actor</summary>
    /// <param name="movement">Movement that will be queued</param>
    /// <remarks>
    ///   This will bypass acceleration/deceleration and attempt to move the actor
    ///   directly by the specified amount during the next physics update. It is
    ///   useful if you want to combine physics with animation-driven root motion.
    /// </remarks>
    public void QueueMovement(Vector3 movement) {
      this.queuedMovement += movement;
    }
    
    /// <summary>Queues a force to affect an actor's velocity</summary>
    /// <param name="force">Force that will be queued for the actor's velocity</param>
    public void QueueForce(Vector3 force) {
      this.queuedForces += force;
    }

    /// <summary>Queues an impulse to affect an actor's velocity</summary>
    /// <param name="impulse">Impulse that will be queued for the actor's velocity</param>
    public void QueueImpulse(Vector3 impulse) {
      this.queuedImpulses += impulse;
    }

#if false // untested, may be fine or complete garbage
    /// <summary>Applies drag to the object</summary>
    /// <param name="dragCoefficient">Drag coefficient of the actor</param>
    public void ApplyDrag(float dragCoefficient = 1.0f) {
      const float Density = 0.233f;
      const float IncidentalArea = 0.1f;

      float speed = this.Velocity.magnitude;
      float drag = speed * speed * 0.5f * Density * dragCoefficient * IncidentalArea;
      
      QueueForce(this.Velocity * -drag);
    }
#endif
  
    /// <summary>Applies the force of gravity to the actor</summary>
    public void ApplyGravity(Vector3 gravity) {

      // Times mass b/c w/o friction, a feather falls as fast as a lead weight!
      QueueForce(gravity * this.Mass * this.GravityScale);

    }

    /// <summary>Called once per physics frame</summary>
    protected virtual void FixedUpdate() {
      float deltaTime = Time.fixedDeltaTime;

      // Auto-Apply if enabled
      if(this.IsAffectedByGravity) {
        ApplyGravity(Gravity);
      }

      // Determine the translation the actor should attempt this physics frame
      // according to its velocity, acceleration and forces.
      Vector3 translation = integrateViaEulerMethod(deltaTime);

      // Now do the movement. This requires special tricks because the CharacterController
      // component has several issues.
      Vector3 reportedVelocity;
      float velocityAdjustment = 0.0f;
      {
        moveActor(translation, out reportedVelocity);

        // As the actor travels horizontally, recharge the step climb budget
        // by the amount the charactor controller's slope limit would allow
        // the character to climb vertically.
        rechargeStepClimbBudget(translation);

        // Calculate the amount the character controller has adjusted the height
        // of the actor over our intended trajectory to compensate for higher floors. 
        float deltaHeightAdjustment;
        {
          float heightAdjustment = (reportedVelocity.y - this.Velocity.y) * deltaTime;
          deltaHeightAdjustment = heightAdjustment - this.previousHeightAdjustment;
          this.previousHeightAdjustment = heightAdjustment;
        }
        
        // If the actor went upwards by less than the step climb budget, we assume
        // that the character controller used its step offset logic. 
        bool hasTraversedStep =
          (deltaHeightAdjustment >= VelocityEpsilon) &&
          (deltaHeightAdjustment < this.characterController.stepOffset);
        if(hasTraversedStep) {
          reportedVelocity.y -= deltaHeightAdjustment / deltaTime;
          this.characterController.stepOffset -= deltaHeightAdjustment;
        }
      }
      
      // Update the velocity. This is filtered so that small errors will not accumulate,
      // like when moving at a speed of 5.0 up a slope and the movement logic says that
      // the character only moved 4.99 units, getting slower every cycle.
      updateVelocity(reportedVelocity, updateHorizontalVelocity: true);

      this.Velocity.y += velocityAdjustment;
    }

    /// <summary>Integrates acceleration and velocity using the Euler method</summary>
    /// <param name="deltaTime">Time by which the simulation will be advanced</param>
    /// <returns>The translation by which the actor should be moved</returns>
    private Vector3 integrateViaEulerMethod(float deltaTime) {
      Vector3 translation;
      {
        // Apply the other half of the acceleration from the previous update cycle.
        // This is delayed into now in order to integrate velocity at the midpoint.
        this.Velocity += this.acceleration * deltaTime;

        // Calculate the new acceleration
        this.acceleration = this.queuedForces / this.Mass;

        // Add impulses 
        Vector3 impulses = this.queuedImpulses / this.Mass;
        //this.acceleration += impulses / delta; // turn impulse into force over 1 delta tick
        this.Velocity += impulses; // add impulses directly

        // Halve the acceleration so that one half can be applied now,
        // the other half at the beginning of the next update cycle.
        this.acceleration /= 2.0f;

        // Apply half of the force scaled by time
        this.Velocity += this.acceleration * deltaTime;

        // Integrate into position
        translation = this.Velocity * deltaTime;

        // Finally, queued movements (root motion, etc.) go directly into translation
        translation += this.queuedMovement;
      }

      // Reset the queues
      this.queuedForces = Vector3.zero;
      this.queuedImpulses = Vector3.zero;
      this.queuedMovement = Vector3.zero;

      return translation;
    }

    /// <summary>
    ///   Recharge the step climb budget relative from the actor's horizontal movement
    /// </summary>
    /// <param name="translation">Amount the actor has moved</param>
    /// <remarks>
    ///   Realistically, this budget would also recover by time: an actor could walk
    ///   up very steep stairs by moving only upwards, but the character controller
    ///   performs horizontal movement in full and only then adjusts height based on
    ///   obstacles, so it's either this or unlimited stair steepness. 
    /// </remarks>
    private void rechargeStepClimbBudget(Vector3 translation) {

      // Some silly micro-optimization: we don't want to recalculate the slope units
      // Y per X each physics frames, so we cache it as per the last slope limit value. 
      if(this.lastSlopeLimit != this.characterController.slopeLimit) {
        this.lastSlopeLimit = this.characterController.slopeLimit;
        this.lastSlopeYperX = Mathf.Sin(this.lastSlopeLimit * Mathf.Deg2Rad);
      }

      // Recharge the step climb budget by the amount of horizontal movement
      // the actor has done and converting this to maximum Y via the slope limit,
      // so the actor can climb up a staircase without bouncing on the steps
      // if they are at a lesser incline than the slope limit.
      float horizontalMovement = Mathf.Sqrt(
        (translation.x * translation.x) + (translation.z * translation.z)
      );
      this.characterController.stepOffset = Mathf.Min(
        this.characterController.stepOffset + Mathf.Abs(horizontalMovement * this.lastSlopeYperX),
        this.MaximumStepHeight
      );

    }

    /// <summary>Gravity the actor is affected by</summary>
    /// <remarks>
    ///   For very special cases, like when the actor can go into space, float on water
    ///   or you do crazy things with gravity, you can override this in a derived class
    ///   and alter it as needed.      
    /// </remarks>
    protected virtual Vector3 Gravity {
      get { return Physics.gravity; }
    }

    /// <summary>Current velocity of the game object</summary>
    /// <remarks>
    ///   Implementation of the <see cref="IVelocityProvider" /> interface in case
    ///   you want to stack one actor on top of another.
    /// </remarks>
    Vector3 IVelocityProvider.Velocity {
      get { return this.Velocity;  }
    }

    /// <summary>
    ///   Moves the actor by the specified amount (unless blocked by colliders) and
    ///   provides the new grounded state and actual velocity 
    /// </summary>
    /// <param name="translation">Amount by which the actor will be moved</param>
    /// <param name="actualVelocity">Actual velocity the actor is moving with</param>
    /// <remarks>
    ///   If the actor hits a wall, the reported velocity will change. 
    /// </remarks>
    private void moveActor(Vector3 translation, out Vector3 actualVelocity) {

      // Here's the annoying part: in Unity 5, the CharacterController does not reliably
      // report being grounded even if it is driven downward relative to the ground angle.
      //
      // For example, moving /into/ a slope (velocity = -9, 2.61) while then reporting
      // being deflected by the slope (velocity = -8.78, 3.19) it still claims to not
      // be grounded

      // We first separate the horizontal and vertical movement
      Vector3 horizontalTranslation = new Vector3(translation.x, 0.0f, translation.z);
      Vector3 verticalTranslation = new Vector3(0.0f, translation.y, 0.0f);

      // Then, for positive Y velocities, we first move up and then sideways so that
      // the move can not be interpreted as "hovering slightly above the ground" and
      // for negative Y velocities, we first move sideways and then down so the final
      // bit of movement will be "into the ground" with a definite collision.
      //
      // If the ground is touched at any of the two movements, we consider the actor
      // to be grounded. Still not 100% reliable, but a significant improvement.
      if(translation.y >= 0.0f) {
        this.characterController.Move(verticalTranslation);
        actualVelocity = new Vector3(0.0f, this.characterController.velocity.y, 0.0f);
        this.UnreliableIsGrounded = this.characterController.isGrounded;
        this.collisions = this.characterController.collisionFlags;

        this.characterController.Move(horizontalTranslation);
        actualVelocity += this.characterController.velocity;
      } else {
        this.characterController.Move(horizontalTranslation);
        actualVelocity = this.characterController.velocity;
        this.UnreliableIsGrounded = this.characterController.isGrounded;
        this.collisions = this.characterController.collisionFlags;
        
        this.characterController.Move(verticalTranslation);
        actualVelocity.y += this.characterController.velocity.y;
      }
      this.collisions |= this.characterController.collisionFlags;
      this.UnreliableIsGrounded |= this.characterController.isGrounded;

    }

    /// <summary>Updates the recorded velocity of the actor</summary>
    /// <param name="newVelocity">New velocity the actor will report</param>
    /// <param name="updateHorizontalVelocity">
    ///   Whether the horizontal velocity should be update (if no collisions happen
    ///   it's often a good idea to keep the horizontal velocity untouched) 
    /// </param>
    private void updateVelocity(Vector3 newVelocity, bool updateHorizontalVelocity) {
      
      // Update the velocity only if it has changed more than the epsilon value.
      // Since the velocity is scaled by delta time, then descaled and scaled again,
      // blindly updating the recorded velocity would over time accumulate floating
      // point inaccuracies.
      if(Vector3.Distance(this.Velocity, newVelocity) > VelocityEpsilon) {
        if(updateHorizontalVelocity) {
          this.Velocity = newVelocity;
        } else {
          this.Velocity.y = newVelocity.y;
        }
      }

      // If the velocity has been set to zero on any axis, apply this in any
      // case (since otherwise, a tiny drift might never be cleared from our
      // velocity vector, very slowly moving the actor around against an obstacle).
      {
        if(updateHorizontalVelocity) {
          if(Math.Abs(newVelocity.x) < VelocityEpsilon) {
            this.Velocity.x = 0.0f;
          }
          if(Math.Abs(newVelocity.z) < VelocityEpsilon) {
            this.Velocity.z = 0.0f;
          }
        }
        if(Math.Abs(newVelocity.y) < VelocityEpsilon) {
          this.Velocity.y = 0.0f;
        }
      }

    }

    /// <summary>Fits the character controller to the collider component, if present</summary>
    private void fitCharacterControllerToCollider() {
      Collider collider = GetComponent<Collider>();

      var sphereCollider = collider as SphereCollider;
      if(sphereCollider != null) {
        this.characterController.center = sphereCollider.center;
        this.characterController.radius = sphereCollider.radius;
        this.characterController.height = sphereCollider.radius * 2.0f;
      }

      var capsuleCollider = collider as CapsuleCollider;
      if(capsuleCollider != null) {
        this.characterController.center = capsuleCollider.center;
        this.characterController.radius = capsuleCollider.radius;
        this.characterController.height = capsuleCollider.height;
      }

      var boxCollider = collider as BoxCollider;
      if(boxCollider != null) {
        float averageBoxExtents = boxCollider.size.magnitude * 0.6035533905f;
        this.characterController.center = boxCollider.center;
        this.characterController.radius = averageBoxExtents;
        this.characterController.height = averageBoxExtents * 2.0f;
      }

      this.characterController.radius -= this.characterController.skinWidth;
      this.characterController.height -= this.characterController.skinWidth * 2.0f;
    }

    /// <summary>Force that has been queued for the actor's velocity</summary>
    private Vector3 queuedForces;
    /// <summary>Impulses that have been queued for the actor's velocity</summary>    
    private Vector3 queuedImpulses;
    /// <summary>Movements that have been queued for the actor</summary>
    private Vector3 queuedMovement;

    /// <summary>Stores half of the acceleration from the last physics update</summary>
    /// <remarks>
    ///   This is used when doing Euler integration using the Midpoint Method,
    ///   where half of the acceleration is integrated into velocity before updating
    ///   the actor's position and half of the acceleration is integrated after.
    /// </remarks>
    private Vector3 acceleration;

    /// <summary>Last slope limit set on the character controlled</summary>
    private float lastSlopeLimit = 45.0f;
    /// <summary>Calculated height the character may climb per horizontal distance</summary>
    /// <remarks>
    ///   This is calculated from the slopeLimit value of the character controller
    ///   and cached here until the character controller's slope limit changes. 
    /// </remarks>
    private float lastSlopeYperX = 0.70710678118f;
    /// <summary>Height adjustment the character controller did in the last frame</summary>
    /// <remarks>
    ///   Used to filter out blocking obstacles (such as the ground countering the force
    ///   of gravity) from having an effect on the step climbing budget.
    /// </remarks>
    private float previousHeightAdjustment;
    
    /// <summary>Collisions the actor encountered in the last physics frame</summary>
    private CollisionFlags collisions;
    /// <summary>Character controller that is used so move the actor around</summary>
    private CharacterController characterController;

  }

} // namespace Framework.Actors