using System.Collections.Generic;
using UnityEngine;

public class Squishbobs : MonoBehaviour
{
	public Transform model;

	[System.Serializable]
	public class Settings {
		public float time = 1;
		[Range(0, 1)] public float bounce = 0.9f;
		[Range(0, 1)] public float minimumSize = 0.3f;
		[Range(0, 1)] public float horizontalScale = 1.0f;

		public bool preserveVolume = true;
	}

	public Settings walkSettings, rollSettings;

	public class Bob {
		Vector3 contact; // world-space point of impact
		Vector3 plane; // world-space collision direction
		float offset, velocity; // spring-space trajectory
		float range; // spring-space maximum displacement

		Transform squisher;
		DampedSpring spring;
		Vector3 nextScale;
		Settings settings;

		public delegate void OnComplete(Vector3 exitVelocity);
		public OnComplete onComplete;

		public bool complete {
			get {
				return offset > -0.01f && velocity >= -0.01f;
			}
		}

		public Bob() {
			squisher = new GameObject("Squisher").transform;
		}

		public void Create(Vector3 origin, Vector3 dir, float _velocity, float _range,
			Transform child, Settings _settings) {
			plane = dir;
			contact = origin;
			offset = 0;
			velocity = _velocity;
			range = _range;
			settings = _settings;
			spring = new DampedSpring(Time.fixedDeltaTime, settings.time, settings.bounce);

			velocity = Mathf.Lerp(velocity*settings.horizontalScale, velocity, Mathf.Abs(dir.y));

			// Insert a new squisher into the hierarchy.
			squisher.localScale = Vector3.one;
			squisher.localRotation = Quaternion.identity;
			squisher.localPosition = Vector3.zero;
			squisher.SetParent(child.parent, false);
			child.SetParent(squisher, true);

			// Activate it and add it.
			squisher.gameObject.SetActive(true);
			squisher.gameObject.hideFlags = HideFlags.None;
		}

		public void Update() {
			// Move the bob along its path.
			spring.Apply(0.0f, ref offset, ref velocity);

			// Ensure a certain minimum size to the object.
			float limit = -range*(1-settings.minimumSize);
			if (offset < limit) {
				offset = limit;
				velocity = Mathf.Max(velocity, 0.0f);
			}
		}

		public void Delete() {
			// Unsquish its child and remove the bob.
			squisher.localScale = Vector3.one;
			for (int j = 0; j<squisher.childCount; j++)
				squisher.GetChild(j).SetParent(squisher.parent, true);
			squisher.parent = null;

			// Apply its exit velocity back to the rigid body.
			onComplete?.Invoke(velocity*plane);

			onComplete = null;

			// Return it to the freelist.
			squisher.gameObject.SetActive(false);
			squisher.gameObject.hideFlags = HideFlags.HideAndDontSave;
			free.Push(this);
		}

		public void MoveTransform(Collider coll) {
			// See where to start squishing from.
			Vector3 origin = FurthestPointOnColliderAlongDir(coll, -plane);

			// Calculate how much to squish in by.
			float scale = (range+offset) / range;

			// Calculate volume-preserving scale for the other axes.
			// Should theoretically be sqrt(1/scale), but this behaves
			// better at extremes.
			float xyscale = 0.5f + Mathf.Pow(2, -scale);
			if (!settings.preserveVolume)
				xyscale = 1.0f;

			// Configure all the axis for the squishers, but without any scale yet.
			squisher.SetPositionAndRotation(origin, Quaternion.LookRotation(plane));
			squisher.localScale = Vector3.one;

			// Store the scale for later.
			nextScale = new Vector3(xyscale, xyscale, scale);
		}

		public void ApplyScale() {
			squisher.localScale = nextScale;
		}
	}

	List<Bob> bobs = new List<Bob>();
	Rigidbody body;
	Collider coll;
	Transform localCoordFixer;
	Entity entity;

	static Stack<Bob> free = new Stack<Bob>();

	void Awake() {
		body = GetComponent<Rigidbody>();
		entity = GetComponent<Entity>();

		localCoordFixer = new GameObject("Local-coord Fixer").transform;
		localCoordFixer.SetParent(transform, false);
		model.SetParent(localCoordFixer, true);
	}

	static Vector3 FurthestPointOnColliderAlongDir(Collider coll, Vector3 dir) {
		// If we had a GJK-like support function, we could have any convex object.

		Vector3 vscale = coll.transform.lossyScale;
		float scale = Mathf.Max(Mathf.Max(vscale.x, vscale.y), vscale.z);
		Vector3 localDir = coll.transform.InverseTransformDirection(dir);

		SphereCollider sphere = coll as SphereCollider;
		if (sphere) {
			return coll.transform.TransformPoint(sphere.center) + dir*sphere.radius*scale;
		}

		CapsuleCollider cap = coll as CapsuleCollider;
		if (cap) {
			Vector3 axis = Vector3.zero;
			axis[cap.direction] = 1;

			Vector3 sphereCenter;
			if (Vector3.Dot(axis, localDir) > 0)
				sphereCenter = cap.center + axis*(cap.height*0.5f - cap.radius);
			else
				sphereCenter = cap.center - axis*(cap.height*0.5f - cap.radius);

			return coll.transform.TransformPoint(sphereCenter) + dir*cap.radius*scale;
		}

		Debug.LogError("Collider type not supported.");
		return coll.bounds.center;
	}

	void FixedUpdate() {
		// Search for colliders again if they were changed.
		if (!coll || !coll.enabled) {
			coll = null;
			foreach (Collider c in GetComponentsInChildren<Collider>())
				if (c.enabled && !c.isTrigger)
					coll = c;
			if (!coll)
				Debug.LogError("No active collider found.", this);
		}

		for (int i = 0; i<bobs.Count; i++) {
			Bob bob = bobs[i];

			bob.Update();

			if (bob.complete) {
				// Bob has returned to its starting position. Delete it.
				bobs.RemoveAt(i);
				bob.Delete();
				i--;
			} else {
				// Bob is in motion. Re-orient its axis to cover the rigid body.
				bob.MoveTransform(coll);
			}
		}

		// Finish up by placing the model where its unsquished place would be.
		// The idea is that anyone looking at the model's local transform won't
		// see any difference from before.
		localCoordFixer.SetPositionAndRotation(transform.position, transform.rotation);
		localCoordFixer.localScale = transform.localScale;

		// Now our axes are all set up, we can go squish stuff:
		for (int i = 0; i<bobs.Count; i++)
			bobs[i].ApplyScale();
	}

	void OnCollisionEnter(Collision collision) {
		if (collision.contactCount < 0 || !enabled)
			return;

		// Get world-space velocity we received.
		Vector3 velocityVec = collision.impulse / body.mass;
		float velocity = -velocityVec.magnitude;
		Vector3 plane = velocityVec.normalized;

		// Get the average contact point.
		Vector3 hitPoint = Vector3.zero;
		for (int i = 0; i<collision.contactCount; i++)
			hitPoint += collision.GetContact(i).point;
		hitPoint /= collision.contactCount;

		Settings settings = entity.isRolling ? rollSettings : walkSettings;

		// Add the squish.
		Bob bob = AddSquish(hitPoint, plane, velocity, settings);
		if (bob != null)
			bob.onComplete += OnCollisionSquishFinished;
	}

	void OnCollisionSquishFinished(Vector3 exitVelocity) {
		// Kick the rigid body back as the squish gets released.
		body.AddForce(exitVelocity, ForceMode.VelocityChange);
	}

	public Bob AddSquish(Vector3 contact, Vector3 dir, float velocity, Settings settings)
	{
		// Figure out an approximate position on the other side
		// of the body to start squishing in from.
		Vector3 farSide = (body.worldCenterOfMass + (body.worldCenterOfMass - contact));

		// Calculate the maximum range our bob can swing over.
		float range = Vector3.Dot(farSide - contact, dir);

		// Reject any wacky cases.
		if (range < 0.01f || velocity > -0.01f)
			return null;

		// Grab a new bob and set it up.
		Bob bob = (free.Count > 0) ? free.Pop() : new Bob();

		bob.Create(contact, dir, velocity, range, localCoordFixer, settings);
		bobs.Add(bob);

		return bob;
	}

	//void OnDrawGizmosSelected() {
	//	foreach (Bob bob in bobs) {
	//		Gizmos.DrawRay(bob.contact, bob.plane);
	//		Gizmos.DrawWireSphere(bob.contact + bob.plane*bob.offset, 0.03f);
	//	}
	//}
}
