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

public class AIWaypointFollower : MonoBehaviour {

	public WaypointNode[] waypoints;
	public int _currentNodeIDToTarget;
	public int _maxWaypointID;
	public Transform currentTarget;
	public Vector3 targetPosition;
	private Vector3 targetDir;
	private Rigidbody rb;
	public float Power;
	public float RotSpeed;
	private float _velocity;
	public float MaxVelocity;

	void Start () 
	{
		rb = GetComponent<Rigidbody> ();
		waypoints = FindObjectsOfType<WaypointNode>();
		_maxWaypointID = waypoints.Length;
		_currentNodeIDToTarget = 0;
		SetTarget (_currentNodeIDToTarget);
	}
		
	void Update () 
	{
		if (_currentNodeIDToTarget <= _maxWaypointID) 
		{
			MoveToTarget ();
		}
	}

	void MoveToTarget()
	{
		targetDir = currentTarget.position - transform.position;
		float step = RotSpeed * Time.deltaTime;
		Vector3 lookDir = Vector3.RotateTowards (transform.forward, targetDir, step, 0.0F);

		//transform.LookAt (targetPosition, Vector3.up);
		transform.rotation = Quaternion.LookRotation(lookDir);
		_velocity = rb.velocity.magnitude;
		if (_velocity <= MaxVelocity) 
		{
			rb.AddForce (transform.forward * Power);
		}
	}

	void OnTriggerEnter(Collider coll)
	{
		if (coll.gameObject.tag == "Waypoint" && coll.GetComponent<WaypointNode>().WaypointNumber == _currentNodeIDToTarget) 
		{
			if (_currentNodeIDToTarget <= _maxWaypointID)
			{
				_currentNodeIDToTarget++;
				SetTarget (_currentNodeIDToTarget);
			}
		}
	}

	private void SetTarget(int waypointId)
	{
		foreach (var waypoint in waypoints) 
		{
			if (waypointId == waypoint.WaypointNumber)
			{
				currentTarget = waypoint.transform;
				targetPosition = new Vector3 (currentTarget.position.x, transform.position.y, currentTarget.position.z);
				return;
			}
		}
	}
}
