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

namespace BS {
	[RequireComponent(typeof(Rigidbody2D))]
	[RequireComponent(typeof(CircleCollider2D))]
    public class NodalPathfinding2DAgent : MonoBehaviour
    {
        public NodalPathfinding2D pathFinder;

        public Vector3 destination;
        public List<Vector3> path = new List<Vector3>();
		public float moveSpeed;

        float t = 0.5f;

		private Rigidbody2D rb;

        public bool isGizmos = true;

        private void OnDrawGizmos()
        {
            if (!isGizmos)
                return;
            Gizmos.color = Color.white;
            if (path.Count > 0)
            {
                Gizmos.DrawLine(transform.position, path[0]);
                for (int i = 0; i < path.Count - 1; i++)
                {
                    Gizmos.DrawLine(path[i] + new Vector3(0,0,1), path[i + 1] + new Vector3(0, 0, 1));
                }
                Gizmos.color = Color.red;
                Gizmos.DrawSphere(destination + new Vector3(0, 0, 1), 0.2f);
            }
            Gizmos.color = Color.red;
            if (rb == null)
                rb = GetComponent<Rigidbody2D>();
            Gizmos.DrawLine(transform.position, transform.position + new Vector3(rb.velocity.x, rb.velocity.y));
        }

		private void Start()
		{
			rb = GetComponent<Rigidbody2D>();
		}

     public void MoveTo(Vector3 destination)
        {
            this.destination = destination;
			//path = pathFinder.GetPathGreedy(transform.position, destination);
			if (path.Count > 1 && Vector2.Distance(path[path.Count - 1], destination) < pathFinder.pointInterval)
				path[path.Count - 1] = destination;
			else
			{
				path = pathFinder.GetPathAstar(transform.position, destination);
			}
		}

		public void SetPath(List<Vector3> path)
		{
			this.destination = path[path.Count - 1];
			List<Vector3> newPath = new List<Vector3>();
			newPath.Add(transform.position);
			foreach (var next in path)
			{
				newPath.AddRange(pathFinder.GetPathAstar(newPath[newPath.Count - 1], next));
			}
			this.path = newPath;
		}

		public void Move(Vector2 direction)
		{

		}

		private void FixedUpdate()
		{
			if (path.Count > 0)
			{
				transform.position += (path[0] - transform.position).normalized * moveSpeed * Time.fixedDeltaTime;
				rb.velocity = Vector3.Lerp(rb.velocity, (path[0] - transform.position).normalized * moveSpeed, 0.1f);
				//Debug.Log((path[0] - transform.position).normalized.magnitude);
				//rb.velocity = (path[0] - transform.position).normalized * moveSpeed;

				if (path.Count > 1 && Vector2.Distance(transform.position, path[0]) < 0.1f && Vector2.Dot(path[1] - path[0], path[0] - transform.position) < 0)
				{
					path.RemoveAt(0);
				}
				else if (path.Count > 1 && Vector2.Distance(transform.position, path[0]) < pathFinder.pointInterval / 10)
				{
					path.RemoveAt(0);
				}
				else if (Vector2.Distance(transform.position, destination) < 0.05f)
				{
					rb.velocity = Vector3.zero;
					path.RemoveAt(0);
				}
			}
            if (rb.velocity.magnitude > 0.01f)
            {
                float angle = Vector2.SignedAngle(Vector2.up, rb.velocity);
                transform.rotation = Quaternion.Lerp(transform.rotation, Quaternion.AngleAxis(angle, Vector3.forward), 0.2f);
            }
		}
	}
}