using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class GameCamera : MonoBehaviour
{
	public bool lockAngle = false;
	public float distance = 5.0f;
	public float slack = 1.0f;
	public float height = 2.0f;
	public float followSpeed = 6.0f;
	public float rotSpeed = 3.0f;
	public float aimHeight = 0.5f;
	public float adjustSpeed = 100.0f;

	public List<Transform> targets = new List<Transform>();

	Vector3 velocity = Vector3.zero;
	Vector3 targetPos;
	Vector3 targetDir = Vector3.zero;
	bool firstTime = true;

	Vector3 Follow(Vector3 current, Vector3 goal) {
		goal.y += height;
		Vector3 dir = current - goal;
		dir.y = 0.0f;

		float dist = Mathf.Clamp(dir.magnitude, distance-slack, distance+slack);

		return goal + dir.normalized*dist;
	}

	Vector3 LockedTracking(Vector3 current, Vector3 goal) {
		float angle = Game.game.indoorAngle * Mathf.Deg2Rad;
		Vector3 lookDir = new Vector3(Mathf.Cos(angle), 0, Mathf.Sin(angle));

		Matrix4x4 m = Matrix4x4.LookAt(goal, goal-lookDir, Vector3.up);
		Matrix4x4 inv = new Matrix4x4();
		Matrix4x4.Inverse3DAffine(m, ref inv);

		Vector3 vec = inv.MultiplyPoint(current);

		vec.x = Mathf.Clamp(vec.x, -slack, slack);
		vec.y = Mathf.Clamp(vec.y, height-slack, height+slack);
		vec.z = Mathf.Clamp(vec.z, distance-slack, distance+slack);
		current = m.MultiplyPoint(vec);
		return current;
	}

	static Vector3 RotateAroundOrigin(Vector3 origin, Vector3 pos, Quaternion rot) {
		return rot * (pos - origin) + origin;
	}

	Vector3 UpdateTarget() {
		Vector3 center = Vector3.zero;
		Vector3 dir = Vector3.zero;
		float total = 0.0f;

		// Track the average of all targets.
		foreach (Transform t in targets) {
			if (!t.gameObject.activeInHierarchy)
				continue;

			// See where we should be aiming.
			Vector3 aimAt = Vector3.up*aimHeight;
			Vector3 motion = Vector3.zero;
			Rigidbody body = t.GetComponent<Rigidbody>();
			if (body) {
				aimAt += body.worldCenterOfMass;
				motion = body.velocity;
			} else {
				aimAt += t.position;
			}

			center += aimAt;
			dir += motion;
			total += 1.0f;
		}

		if (total > 0) {
			center /= total;
			dir /= total;
		}

		if (dir.magnitude > 0.5f)
			targetDir = dir.normalized;

		if (lockAngle) {
			targetPos = LockedTracking(targetPos, center);
		} else {
			// Limit our target position to within the target's range.
			targetPos = Follow(targetPos, center);
		}

		return center;
	}

	void LateUpdate()
    {
		if (targets.Count <= 0)
			return;

		float dt = UIWrangler.deltaTime;
		float camx = Input.GetAxisRaw("Camera X");
		float camy = Input.GetAxisRaw("Camera Y");

		Vector3 aimAt = UpdateTarget();

		if (firstTime) {
			transform.position = targetPos;
			transform.rotation = Quaternion.LookRotation(aimAt - targetPos);
			firstTime = false;
		}

		// Move towards the target position.
		Vector3 velocity = (targetPos - transform.position) * followSpeed;

		// Integrate.
		transform.position += velocity*dt;

		// Aim at the target.
		{
			Quaternion newrot = Quaternion.FromToRotation(transform.forward, aimAt-transform.position);

			// Turn at a velocity dependent on the angle (same behaviour we
			// use for position)
			newrot.ToAngleAxis(out float angle, out Vector3 axis);
			newrot = Quaternion.AngleAxis(angle*Mathf.Min(dt*rotSpeed, 1.0f), axis);

			// Apply.
			transform.rotation = newrot * transform.rotation;
		}

		// Adjust to match target velocity.
		// TODO: this is kinda chuft, but it's better with than without right now.
		//{
			//Vector3 ourDir = Vector3.ProjectOnPlane(transform.forward, Vector3.up);
			//Quaternion newrot = Quaternion.FromToRotation(ourDir, targetDir);

			//// Turn at a velocity dependent on the angle (same behaviour we
			//// use for position)
			//newrot.ToAngleAxis(out float angle, out Vector3 axis);
			//newrot = Quaternion.AngleAxis(angle*Mathf.Min(dt*rotSpeed*0.1f, 1.0f), axis);

			//// Apply.
			//targetPos = RotateAroundOrigin(aimAt, targetPos, newrot);
			//transform.position = RotateAroundOrigin(aimAt, transform.position, newrot);
			//transform.rotation = newrot * transform.rotation;
		//}

		// Apply manual overrides.
		if (!lockAngle) {
			Quaternion adjustX = Quaternion.AngleAxis(-camx*dt*adjustSpeed, Vector3.up);
			targetPos = RotateAroundOrigin(aimAt, targetPos, adjustX);
			transform.position = RotateAroundOrigin(aimAt, transform.position, adjustX);
			transform.rotation = transform.rotation * adjustX;
		}

		// Keep upright.
		transform.rotation = Quaternion.LookRotation(transform.forward);
    }
}
