﻿using UnityEngine;
using UnityEngine.Events;
using System.Collections.Generic;

namespace Nostalgia.Example
{
	[AddComponentMenu("Nostalgia/Example/PlatformController")]
	[RequireComponent(typeof(Rigidbody2D))]
	public sealed class PlatformController : MonoBehaviour
	{
		public float groundThroughTime = 0.1f;
		public float maxClibmAngle = 80.0f;
		public Collider2D footCollider;
		public LayerMask groundLayer = Physics2D.AllLayers;

		[System.Serializable]
		public class TrapTileEvent : UnityEvent<TrapTile>
		{
		}

		[System.NonSerialized]
		public bool isGroundThrough = false;

		[System.NonSerialized]
		public TrapTileEvent trapHitEvent = new TrapTileEvent();

		Rigidbody2D _Rigidbody;
		Collider2D[] _Colliders;

		Dictionary<Collider2D, List<ContactPoint2D>> _Contacts = new Dictionary<Collider2D, List<ContactPoint2D>>();

		public class Ground
		{
			public Tile tile;
			public Collider2D collider;
			public float slope = 0.0f;
			public Vector2 normal;

			public Vector2 Move(Vector2 velocity)
			{
				BeltTile beltTile = tile != null ? tile.GetComponent<BeltTile>() : null;
				if (beltTile != null)
				{
					return velocity;
				}

				float moveDistance = Mathf.Abs(velocity.x);
				float directionX = Mathf.Sign(velocity.x);

				if (velocity.y <= 0.0f)
				{
					if (Mathf.Sign(normal.x) == directionX)
					{
						velocity.x *= Mathf.Cos(slope * Mathf.Deg2Rad);
						velocity.y -= Mathf.Sin(slope * Mathf.Deg2Rad) * moveDistance;
					}
				}
				else
				{
					float velocityY = Mathf.Sin(slope * Mathf.Deg2Rad) * moveDistance;
					if (velocity.y <= velocityY)
					{
						velocity.x *= Mathf.Cos(slope * Mathf.Deg2Rad);
						velocity.y = velocityY;
					}
				}

				return velocity;
			}
		}

		List<Ground> _Grounds = new List<Ground>();

		class ThroughCell
		{
			public bool isGroundThrough;
			public float time;

			public ThroughCell(bool isGroundThrough)
			{
				this.isGroundThrough = isGroundThrough;
				time = Time.fixedTime;
			}
		}
		Dictionary<Cell, ThroughCell> _ThroughCells = new Dictionary<Cell, ThroughCell>();

		Ground _RaycastGround = null;

		Ground _MoveGround = null;
		Vector2 _PreGroundPos;
		Vector2 _GroundLocalPos;

		public bool isGround
		{
			get
			{
				return _Grounds.Count != 0;
			}
		}

		public int groundCount
		{
			get
			{
				return _Grounds.Count;
			}
		}

		public Ground GetGround(int groundIndex)
		{
			return _Grounds[groundIndex];
		}

		public Vector2 velocity
		{
			get
			{
				return Physics2DUtility.GetLinearVelocity(_Rigidbody);
			}
			set
			{
				Physics2DUtility.SetLinearVelocity(_Rigidbody, value);
			}
		}

		public void ClearGround()
		{
			_Grounds.Clear();
			_RaycastGround = null;
			_MoveGround = null;
			_Contacts.Clear();
		}

		public void ClearThroughCell()
		{
			_ThroughCells.Clear();
		}

		Vector2 CalcClimbVelocity(Vector2 velocity)
		{
			int groundCount = _Grounds.Count;
			for (int groundIndex = 0; groundIndex < groundCount; groundIndex++)
			{
				Ground ground = _Grounds[groundIndex];

				velocity = ground.Move(velocity);
			}

			if (_Contacts != null)
			{
				foreach (var pair in _Contacts)
				{
					List<ContactPoint2D> contacts = pair.Value;
					int contactCount = contacts.Count;
					for (int contactIndex = 0; contactIndex < contactCount; contactIndex++)
					{
						ContactPoint2D contact = contacts[contactIndex];
						float slopeAngle = Mathf.Abs(Vector2.Angle(contact.normal, Vector2.up));
						if (slopeAngle > maxClibmAngle && Mathf.Sign(contact.normal.x) != Mathf.Sign(velocity.x))
						{
							velocity.x *= 1.0f - Mathf.Abs(contact.normal.x);
						}
					}
				}
			}

			return velocity;
		}

		public void Move(Vector2 velocity)
		{
			Physics2DUtility.SetLinearVelocity(_Rigidbody, CalcClimbVelocity(velocity));
		}

		void Awake()
		{
			_Rigidbody = GetComponent<Rigidbody2D>();
			_Colliders = GetComponents<Collider2D>();
		}

		static Vector2 ClosestPointSegment(Vector2 a, Vector2 b, Vector2 p, bool higher)
		{
			Vector2 ab = b - a;
			if (ab.x != 0)
			{
				return a + Mathf.Clamp01((p.x - a.x) / ab.x) * ab;
			}
			else
			{
				if (higher)
				{
					return a.y > b.y ? a : b;
				}
				else
				{
					return a.y < b.y ? a : b;
				}
			}
		}

		static Vector2 ClosestPointBox(BoxCollider2D collider, Vector2 position, bool higher)
		{
			Vector2 r = Vector2.zero;
			Vector2 min = collider.offset - collider.size * 0.5f;
			Vector2 max = collider.offset + collider.size * 0.5f;

			r.x = position.x;
			r.x = Mathf.Max(r.x, min.x);
			r.x = Mathf.Min(r.x, max.x);

			r.y = higher ? max.y : min.y;

			return r;
		}

		static Vector2 ClosestPointCircle(CircleCollider2D collider, Vector2 position, bool higher)
		{
			Vector2 normal = (position - collider.offset).normalized;
			if (higher)
			{
				normal.y = Mathf.Abs(normal.y);
			}
			else
			{
				normal.y = -Mathf.Abs(normal.y);
			}
			return normal * collider.radius + collider.offset;
		}

		static Vector2 ClosestPointEdge(EdgeCollider2D collider, Vector2 position, bool higher)
		{
			position -= collider.offset;

			Vector2 closestPoint = position;
			float distance = float.MaxValue;
			float height = higher ? float.MinValue : float.MaxValue;

#if UNITY_2020_1_OR_NEWER
			using (Pool.ListPool<Vector2>.Get(out List<Vector2> points))
			{
				int pointCount = collider.GetPoints(points);
#else
			{
				int pointCount = collider.pointCount;
				IList<Vector2> points = collider.points;
#endif
				for (int pointIndex = 0; pointIndex < pointCount - 1; pointIndex++)
				{
					Vector2 p = ClosestPointSegment(points[pointIndex], points[pointIndex + 1], position, higher);
					float d = Mathf.Abs(p.x - position.x);
					if (d < distance || d == distance && (higher && p.y > height || !higher && p.y < height))
					{
						distance = d;
						height = p.y;
						closestPoint = p;
					}
				}
			}

			return closestPoint + collider.offset;
		}

		static Vector2 ClosestPointPolygon(PolygonCollider2D collider, Vector2 position, bool higher)
		{
			position -= collider.offset;

			Vector2 closestPoint = position;
			float distance = float.MaxValue;
			float height = higher ? float.MinValue : float.MaxValue;

			int pathCount = collider.pathCount;
			for (int pathIndex = 0; pathIndex < pathCount; pathIndex++)
			{
				using(Pool.ListPool<Vector2>.Get(out List<Vector2> points))
				{
					collider.GetPath(pathIndex, points);

					int pointCount = points.Count;
					for (int pointIndex = 0; pointIndex < pointCount; pointIndex++)
					{
						int index0 = pointIndex % pointCount;
						int index1 = (pointIndex + 1) % pointCount;

						Vector2 p = ClosestPointSegment(points[index0], points[index1], position, higher);
						float d = Mathf.Abs(p.x - position.x);
						if (d < distance || d == distance && (higher && p.y > height || !higher && p.y < height))
						{
							distance = d;
							height = p.y;
							closestPoint = p;
						}
					}
				}
			}

			return closestPoint + collider.offset;
		}

		static Vector2 GetClosestPoint(Collider2D collider, Vector2 position, bool higher)
		{
			position = collider.transform.InverseTransformPoint(position);

			if (collider is BoxCollider2D)
			{
				position = ClosestPointBox(collider as BoxCollider2D, position, higher);
			}
			else if (collider is CircleCollider2D)
			{
				position = ClosestPointCircle(collider as CircleCollider2D, position, higher);
			}
			else if (collider is EdgeCollider2D)
			{
				position = ClosestPointEdge(collider as EdgeCollider2D, position, higher);
			}
			else if (collider is PolygonCollider2D)
			{
				position = ClosestPointPolygon(collider as PolygonCollider2D, position, higher);
			}

			return collider.transform.TransformPoint(position);
		}

		void UpdateThroughFloorTile()
		{
			using (new ProfilerScope("UpdateThroughFloorTile"))
			{
				Collider2D[] colliders = _Colliders;
				int colliderCount = colliders.Length;


				bool throughFlag = isGround && isGroundThrough;
				isGroundThrough = false;

				int mapCount = Map.mapCount;
				for (int mapIndex = 0; mapIndex < mapCount; mapIndex++)
				{
					Map map = Map.GetMap(mapIndex);
					if (map == null)
					{
						continue;
					}

					TileSet tileSet = map.tileSet;
					if (tileSet == null)
					{
						continue;
					}

					int tileCount = tileSet.tileCount;
					for (int tileIndex = 0; tileIndex < tileCount; tileIndex++)
					{
						Tile tile = tileSet.GetTile(tileIndex);

						if (!tile.GetComponent<ThroughFloorTile>())
						{
							continue;
						}

						using (Pool.ListPool<Cell>.Get(out List<Cell> cells))
						{
							if (map.TryGetCells(tile, cells))
							{
								int cellCount = cells.Count;

								for (int cellIndex = 0; cellIndex < cellCount; cellIndex++)
								{
									Cell cell = cells[cellIndex];

									if (cell.collider == null)
									{
										continue;
									}

									ThroughCell throughCell = null;
									if (_ThroughCells.TryGetValue(cell, out throughCell))
									{
										if (!throughCell.isGroundThrough || Time.fixedTime - throughCell.time > groundThroughTime)
										{
											Vector3 cellPos = GetClosestPoint(cell.collider, _Rigidbody.position, true);

											if (_Rigidbody.position.y >= cellPos.y)
											{
												for (int colliderIndex = 0; colliderIndex < colliderCount; colliderIndex++)
												{
													Collider2D collider = colliders[colliderIndex];
													if (Physics2D.GetIgnoreCollision(collider, cell.collider) != false)
													{
														Physics2D.IgnoreCollision(collider, cell.collider, false);
													}
												}

												cell.destroyCallback -= OnDestroyCell;
												_ThroughCells.Remove(cell);
											}
										}
									}
									else
									{
										Vector2 position = _Rigidbody.position;
										Vector3 cellPos = GetClosestPoint(cell.collider, position, false);

										if (throughFlag || position.y < cellPos.y)
										{
											Ground ground = FindGround(cell.collider);
											if (ground == null && _RaycastGround != null && _RaycastGround.collider == cell.collider)
											{
												ground = _RaycastGround;
											}

											if (ground != null)
											{
												if (_RaycastGround == ground)
												{
													_RaycastGround = null;
												}
												if (_MoveGround == ground)
												{
													_MoveGround = null;
												}
												_Grounds.Remove(ground);
											}

											for (int colliderIndex = 0; colliderIndex < colliderCount; colliderIndex++)
											{
												Collider2D collider = colliders[colliderIndex];
												if (Physics2D.GetIgnoreCollision(collider, cell.collider) != true)
												{
													Physics2D.IgnoreCollision(collider, cell.collider, true);
												}
											}

											cell.destroyCallback += OnDestroyCell;

											_ThroughCells.Add(cell, new ThroughCell(throughFlag));
										}
									}
								}
							}
						}
					}
				}
			}
		}

		void OnDestroyCell(Cell cell)
		{
			_ThroughCells.Remove(cell);
		}

		Ground FindGround(Collider2D collider)
		{
			int groundCount = _Grounds.Count;
			for (int groundIndex = 0; groundIndex < groundCount; groundIndex++)
			{
				Ground ground = _Grounds[groundIndex];
				if (_RaycastGround != ground && ground.collider == collider)
				{
					return ground;
				}
			}
			return null;
		}

		void UpdateGround(Collision2D collision)
		{
			Collider2D groundObject = null;
			float groundSlope = 0.0f;
			Vector2 groundNormal = Vector2.zero;

			List<ContactPoint2D> contacts = null;
			if (!_Contacts.TryGetValue(collision.collider, out contacts))
			{
				contacts = new List<ContactPoint2D>();
				_Contacts.Add(collision.collider, contacts);
			}
			else
			{
				contacts.Clear();
			}

			int contactLength = collision.contactCount;
			for (int contactIndex = 0; contactIndex < contactLength; contactIndex++)
			{
				ContactPoint2D contact = collision.GetContact(contactIndex);
				contacts.Add(contact);

				if (contact.otherCollider != footCollider)
				{
					continue;
				}

				Vector2 normal = contact.normal;

				RaycastHit2D hit = Physics2D.Linecast(contact.point + new Vector2(0.0f, 0.1f), contact.point);
				if (hit.collider == contact.collider)
				{
					normal = hit.normal;
				}

				float slopeAngle = Mathf.Abs(Vector2.Angle(normal, Vector2.up));

				if (slopeAngle <= maxClibmAngle && (groundObject == null || slopeAngle > groundSlope))
				{
					groundObject = contact.collider;
					groundSlope = slopeAngle;
					groundNormal = contact.normal;
				}
			}

			if (groundObject != null)
			{
				Ground ground = FindGround(groundObject);
				if (ground == null)
				{
					ground = new Ground();
					_Grounds.Add(ground);
				}

				ground.tile = groundObject.GetTile();
				ground.collider = groundObject;
				ground.slope = groundSlope;
				ground.normal = groundNormal;
			}
			else
			{
				Ground ground = FindGround(collision.collider);
				if (ground != null)
				{
					if (ground == _MoveGround)
					{
						_MoveGround = null;
					}

					_Grounds.Remove(ground);
				}
			}
		}

		void OnCollisionEnter2D(Collision2D collision)
		{
			UpdateGround(collision);
		}

		void OnCollisionStay2D(Collision2D collision)
		{
			UpdateGround(collision);

			int contactLength = collision.contactCount;
			for (int contactIndex = 0; contactIndex < contactLength; contactIndex++)
			{
				ContactPoint2D contact = collision.GetContact(contactIndex);
				Debug.DrawLine(contact.point, contact.point + contact.normal * 0.1f, Color.red);
			}
		}

		void OnCollisionExit2D(Collision2D collision)
		{
			_Contacts.Remove(collision.collider);

			Ground ground = FindGround(collision.collider);
			if (ground != null)
			{
				if (_MoveGround == ground)
				{
					_MoveGround = null;
				}
				_Grounds.Remove(ground);
			}
		}

		RaycastHit2D[] _GroundHits = new RaycastHit2D[10];

		void FixedUpdate()
		{
			Vector2 pos = _Rigidbody.position;

			if (_MoveGround != null)
			{
				Vector2 nextPosition = _GroundLocalPos;

				Transform groundTransform = _MoveGround.collider.transform;
				Rigidbody2D groundRigidbody = _MoveGround.collider.attachedRigidbody;

				Vector2 move = Vector2.zero;

				if (!groundRigidbody)
				{
					nextPosition = groundTransform.TransformPoint(_GroundLocalPos);
					move += nextPosition - _PreGroundPos;
					if (move.y >= 0.0f)
					{
						move.y = 0.0f;
					}
				}

				Tile tile = _MoveGround.tile;
				if (tile != null)
				{
					BeltTile beltTile = tile.GetComponent<BeltTile>();
					if (beltTile != null)
					{
						move += beltTile.velocity * Time.fixedDeltaTime;
					}
				}

				pos += move;
				_Rigidbody.position = pos;

				_PreGroundPos = _Rigidbody.position;
				if (groundRigidbody)
				{
					_GroundLocalPos = groundRigidbody.GetPoint(_PreGroundPos);
				}
				else
				{
					_GroundLocalPos = groundTransform.InverseTransformPoint(_PreGroundPos);
				}
			}

			Collider2D groundObject = null;
			float groundSlope = 0.0f;
			Vector2 groundNormal = Vector2.zero;

			Vector2 groundArea = new Vector2(0.0f, 0.1f);

#if UNITY_2023_1_OR_NEWER
			ContactFilter2D contactFilter = new ContactFilter2D()
			{
				useTriggers = Physics2D.queriesHitTriggers,
				useLayerMask = true,
				layerMask = groundLayer,
			};
			int hitCount = Physics2D.Linecast(pos, pos - groundArea, contactFilter, _GroundHits);
#else
			int hitCount = Physics2D.LinecastNonAlloc(pos, pos - groundArea, _GroundHits, groundLayer);
#endif
			for (int hitIndex = 0; hitIndex < hitCount; hitIndex++)
			{
				RaycastHit2D hit = _GroundHits[hitIndex];

				if (hit.transform == transform || hit.collider.isTrigger)
				{
					continue;
				}

				Cell cell = hit.collider.GetCell();
				if (cell != null && _ThroughCells.ContainsKey(cell))
				{
					continue;
				}

				float slopeAngle = Mathf.Abs(Vector2.Angle(hit.normal, Vector2.up));

				if (slopeAngle <= maxClibmAngle)
				{
					if (groundObject == null || slopeAngle > groundSlope)
					{
						groundObject = hit.collider;
						groundSlope = slopeAngle;
						groundNormal = hit.normal;
					}
				}
			}

			if (groundObject != null)
			{
				if (_RaycastGround == null)
				{
					_RaycastGround = new Ground();
					_Grounds.Add(_RaycastGround);
				}

				_RaycastGround.tile = groundObject.GetTile();
				_RaycastGround.collider = groundObject;
				_RaycastGround.slope = groundSlope;
				_RaycastGround.normal = groundNormal;
			}
			else if (_RaycastGround != null)
			{
				_Grounds.Remove(_RaycastGround);

				_RaycastGround = null;
			}

			_MoveGround = null;

			if (_RaycastGround != null)
			{
				_MoveGround = _RaycastGround;
			}
			else if (isGround)
			{
				_MoveGround = _Grounds[0];
			}

			if (_MoveGround != null)
			{
				_PreGroundPos = _Rigidbody.position;

				Transform groundTransform = _MoveGround.collider.transform;
				Rigidbody2D groundRigidbody = _MoveGround.collider.attachedRigidbody;
				if (groundRigidbody)
				{
					_GroundLocalPos = groundRigidbody.GetPoint(_PreGroundPos);
				}
				else
				{
					_GroundLocalPos = groundTransform.InverseTransformPoint(_PreGroundPos);
				}
			}

			UpdateThroughFloorTile();
		}
	}
}
