using System;
using System.Diagnostics;

using UnityEngine;

using Framework.Geometry.Lines;
using Framework.Support;

namespace Framework.Navigation.Platformer {

  /// <summary>Stretch of ground that navigation agents can walk upon</summary>
  public class Ground : MonoBehaviour {

    /// <summary>Heights at different points on the ground</summary>
    /// <remarks>
    ///   The heights are connected to form a polyline which defines the ground heights
    /// </remarks>
    public Vector2[] Heights;

    /// <summary>Other ground that can be reached from here</summary>
    /// <remarks>
    ///   These are other locations that doesn't necessarily have to be reachable
    ///   by all agents (path searches could put limits on jump height or ground
    ///   type, for example).
    /// </remarks>
    public Ground[] JumpTargets;

    /// <summary>Whether actors can jump and down through this floor</summary>
    /// <remarks>
    ///   For 2D platformers, this is simply a convention, for 2.5D platformers,
    ///   the floor is usually placed a bit further back, so jumping up or down
    ///   means going on step back or forth in depth.
    /// </remarks>
    public bool CanPassThroughFloor;

    /// <summary>X coordinate of the left end of the ground</summary>
    public float Left {
      get {
        enforceAtLeastTwoHeightsAssigned();
        return transform.position.x + this.Heights[0].x;
      }
    }

    /// <summary>X coordinate of the right end of the ground</summary>
    public float Right {
      get {
        enforceAtLeastTwoHeightsAssigned();
        return transform.position.x + this.Heights[this.Heights.Length - 1].x;
      }
    }

    /// <summary>Gets the X coordinate of the ground's center</summary>
    public float Center {
      get {
        enforceAtLeastTwoHeightsAssigned();
        float left = this.Heights[0].x;
        float right = this.Heights[this.Heights.Length - 1].x;
        return (left + right) / 2.0f + transform.position.x;
      }
    }

    /// <summary>Determines the height of the ground at the specific X position</summary>
    /// <param name="x">Position at which the height of the ground will be determined</param>
    /// <returns>The ground height at the specified position</returns>
    /// <remarks>
    ///   Querying heights outside of the ground will clamp to the left or right sample
    ///   respectively. This is done for reasons of robustness, the navigation agent
    ///   should take care not to leave the limits of the ground.
    /// </remarks>
    public float GetGroundHeightAt(float x) {
      enforceAtLeastTwoHeightsAssigned();

      // Special case if there is only one height. Zero heights will be caught by the guard
      // in debug builds and cause an IndexOutOfRangeException here if a release build is
      // compiled containing misbehaving code (fix it at the source, not here!)
      int length = this.Heights.Length;
      if(length <= 1) {
        return this.Heights[0].y;
      }

      int index = getIndexByX(x);
      if(index == -1) { // -1 means that no support point was left of X
        return this.Heights[0].y + transform.position.y;
      } else if(index >= length - 1) { // means X is right of all support points
        return this.Heights[index].y + transform.position.y;
      } else {
        Vector2 left = this.Heights[index];
        Vector2 right = this.Heights[index + 1];
        x -= transform.position.x;
        return Mathf.Lerp(
          left.y, right.y, (x - left.x) / (right.x - left.x)
        ) + transform.position.y;
      }
    }

    /// <summary>Gets the normal vector of the ground at the specified location</summary>
    /// <param name="x">Location at which the normal vector will be returned</param>
    /// <returns>The normal vector at the specified location</returns>
    public Vector2 GetGroundNormalAt(float x) {
      enforceAtLeastTwoHeightsAssigned();

      // Special case if there is only one height. Zero heights will be caught by the guard
      // in debug builds and cause an IndexOutOfRangeException here if a release build is
      // compiled containing misbehaving code (fix it at the source, not here!)
      int length = this.Heights.Length;
      if(length <= 1) {
        return Vector2.up;
      }

      Vector2 normal;

      int index = getIndexByX(x);
      if(index == -1) { // -1 means that no support point was left of X
        normal.x = this.Heights[0].y - this.Heights[1].y;
        normal.y = this.Heights[1].x - this.Heights[0].x;
      } else if(index >= length - 1) { // means X is right of all support points
        normal.x = this.Heights[length - 2].y - this.Heights[length - 1].y;
        normal.y = this.Heights[length - 1].x - this.Heights[length - 2].x;
      } else {
        normal.x = this.Heights[index].y - this.Heights[index + 1].y;
        normal.y = this.Heights[index + 1].x - this.Heights[index].x;
      }

      return normal.normalized;
    }

#if UNITY_EDITOR

    /// <summary>Called to visualize the walkable area in the editor</summary>
    protected virtual void OnDrawGizmos() {
      Matrix4x4 oldMatrix = Gizmos.matrix;
      Color oldColor = Gizmos.color;
      try {
        if(this.JumpTargets != null) {
          drawJumpGizmos();
        }
        if(this.Heights != null) {
          drawGroundGizmo();
        }
      }
      finally {
        Gizmos.color = oldColor;
        Gizmos.matrix = oldMatrix;
      }
    }

    /// <summary>Draws the editor gizmo for the ground</summary>
    private void drawGroundGizmo() {
      Vector3 position = transform.position;

      for(int index = 0; index < this.Heights.Length - 1; ++index) {
        var left = new Vector2(this.Heights[index].x, this.Heights[index].y);
        var right = new Vector2(this.Heights[index + 1].x, this.Heights[index + 1].y);

        if(left.x < right.x) {
          Gizmos.color = new Color(0.333f, 1.0f, 0.333f, 0.667f);
        } else {
          Gizmos.color = new Color(1.0f, 0.333f, 0.333f, 0.667f);
        }

        // Calculate the angle of this line segment
        var up = new Vector2(left.y - right.y, right.x - left.x);
        up.Normalize();
        float angle = Mathf.Atan2(up.y, up.x) * Mathf.Rad2Deg - 90.0f;

        // Select a matrix that translates to the where the ground segment
        // begins and is rotated to the normal of the ground at that location
        Quaternion orientation = Quaternion.AngleAxis(angle, Vector3.forward);
        var translation = new Vector3(position.x + left.x, position.y + left.y, position.z);
        Gizmos.matrix = Matrix4x4.TRS(translation, orientation, Vector3.one);

        // Using the transformation matrix from before, draw the line segment
        // as a cube (because this uses the depth buffer and makes ground placement
        // much easier to check)
        float length = (right - left).magnitude;
        Gizmos.DrawCube(
          new Vector3(length / 2.0f, -0.05f, 0.0f),
          new Vector3(length, 0.1f, 0.5f)
        );
      }
    }

    /// <summary>Draws the jump targets of the ground into the scene</summary>
    private void drawJumpGizmos() {
      for(int index = 0; index < this.JumpTargets.Length; ++index) {
        if(this.JumpTargets[index] == null) {
          continue;
        }
        Segment2? jumpPath = JumpCalculator.GetJumpPath(
          this, this.JumpTargets[index], 0.0f, Constraints.Infinite
        );
        if(jumpPath.HasValue) {
          var curve = new CubicBezierCurve() {
            P0 = jumpPath.Value.From,
            P3 = jumpPath.Value.To
          };
          float apex = Math.Max(jumpPath.Value.From.y, jumpPath.Value.To.y) + 0.5f;
          curve.P1 = new Vector3(Mathf.Lerp(curve.P0.x, curve.P3.x, 0.50f), apex, 0.0f);
          curve.P2 = new Vector3(Mathf.Lerp(curve.P0.x, curve.P3.x, 0.75f), apex, 0.0f);

          if(curve.P0.y > curve.P3.y) {
            Gizmos.color = Color.blue;
          } else {
            Gizmos.color = Color.red;
          }

          drawCubicBezierCurve(curve);
        } else {
          Vector3 from;
          from.x = Center;
          from.y = GetGroundHeightAt(from.x);
          from.z = 0.0f;

          Vector3 to;
          to.x = this.JumpTargets[index].Center;
          to.y = this.JumpTargets[index].GetGroundHeightAt(to.x);
          to.z = 0.0f;

          Gizmos.color = Color.white;
          Gizmos.DrawLine(from, to);
        }
      }
    }

    /// <summary>Draws a cubic bezier curve using the provided supporting points</summary>
    /// <param name="curve">Curve with supporting points that will be drawn</param>
    private static void drawCubicBezierCurve(CubicBezierCurve curve) {
      Vector3 previous;
      Vector3 current = curve.P0;
      for(int index = 1; index < 10; ++index) {
        previous = current;
        current = curve.GetPointAtTime((float)index / 10.0f);
        Gizmos.DrawLine(previous, current);
      }
      Gizmos.DrawLine(current, curve.P3);
    }

#endif // UNITY_EDITOR

    /// <summary>Determines the index of closest supporting point left of X</summary>
    /// <param name="x">X coordinate to which the closest support point will be found</param>
    /// <returns>The index of the closest supporting point left of X of -1 if none</returns>
    private int getIndexByX(float x) {
      int result = -1;

      x -= transform.position.x;

      // Assumption: typical ground nodes have so few supporting points that
      // a binary search would take longer than just iterating over the points.
      for(int index = 0; index < this.Heights.Length; ++index) {
        if(this.Heights[index].x < x) {
          result = index;
        } else {
          break;
        }
      }

      return result;
    }

    /// <summary>
    ///   Throws an exception if there is are at least two supporting points
    /// </summary>
    [Conditional("DEBUG")]
    private void enforceAtLeastTwoHeightsAssigned() {
      if((this.Heights == null) || (this.Heights.Length < 2)) {
        throw new InvalidOperationException(
          "Navigation.Ground node must have at least two heights assigned"
        );
      }
    }

  }

} // namespace Framework.Navigation.Platformer