using UnityEngine;
using UnityEngine.AI;
using System.Collections.Generic;
using System.IO;
using System;

public class WalkPos_LerpNew : MonoBehaviour
{
    // Waypoints and Rotations
    private Tuple<List<float>, List<Vector3>, List<Quaternion>, List<float>>[][] timePosRotList;

    // Private Journey Settings
    private int[][] currentStartPoint;
    private float startTime;

    //Animator
    private GameObject[][] humansGO;
    private Animator[][] humansA;
    private NavMeshAgent[][] humansNMA;

    //Update possible
    private bool doUpdate = false;

    private void Start()
    {
        // Get information from InstatiatePrefab
        humansGO = gameObject.GetComponent<InstantiatePrefab>().humanGameObject;
        humansA = gameObject.GetComponent<InstantiatePrefab>().humanAnimator;
        humansNMA = gameObject.GetComponent<InstantiatePrefab>().humanNavMeshAgent;

        // Read from CSV file and save time, position, rotation in matrix
        int index = gameObject.GetComponent<WriteInCSV>().index;
        string folderName = gameObject.GetComponent<WriteInCSV>().folderName;
        string dir = Directory.GetCurrentDirectory();
        string reference = @"\Assets\Data_position\"+ folderName + @"\Walk" + index + @".csv";
        timePosRotList = gameObject.GetComponent<ReadFromCSV>().ReadFromCSVFile(dir + reference);

        // Set initial position and rotation
        currentStartPoint = new int[humansGO.Length][];
        for (int i = 0; i < humansGO.Length; ++i)
        {
            currentStartPoint[i] = new int[humansGO[i].Length];
            for (int j = 0; j < humansGO[i].Length; ++j)
            {
                currentStartPoint[i][j] = 0;
                humansGO[i][j].transform.position = timePosRotList[0][0].Item2[0]; // First entry of Position
                humansGO[i][j].transform.rotation = timePosRotList[0][0].Item3[0]; // First entry of Rotation
                // Animation Idle
                humansA[i][j].SetBool("isWalking", false);

                humansNMA[i][j].enabled = false;
            }
        }

        startTime = Time.time;
        
        // All required parameters for Update have been set
        doUpdate = true;
    }

    void FixedUpdate()
    {
        if (!doUpdate) return;

        for(int i = 0; i < humansGO.Length; ++i)
        {
            for(int j = 0; j < humansGO[i].Length; ++j)
            {
                Vector3 startPos = timePosRotList[i][j].Item2[currentStartPoint[i][j]];
                Vector3 endPos = timePosRotList[i][j].Item2[currentStartPoint[i][j] + 1];
                Quaternion startRot = timePosRotList[i][j].Item3[currentStartPoint[i][j]];
                Quaternion endRot = timePosRotList[i][j].Item3[currentStartPoint[i][j] + 1];

                float distCovered = (Time.time - startTime) * humansA[i][j].speed;
                float fracJourney = 1f;
                float journeyLength = Vector3.Distance(startPos, endPos);
                if (journeyLength != 0 && distCovered != 0) fracJourney = distCovered / journeyLength;

                // Animation Idle, if startPoint == endPoint and startRot == endRot
                if (journeyLength <= 0 && startRot.Equals(endRot))
                {
                    humansA[i][j].SetBool("isWalking", false);
                }
                else if (journeyLength > 0)
                {
                    humansA[i][j].SetBool("isWalking", true);
                }

                humansGO[i][j].transform.position = Vector3.Lerp(startPos, endPos, fracJourney);
                humansGO[i][j].transform.rotation = Quaternion.Lerp(startRot, endRot, fracJourney);
                if (fracJourney >= 1f &&
                    currentStartPoint[i][j] + 2 < timePosRotList[i][j].Item2.Count &&
                    currentStartPoint[i][j] + 2 < timePosRotList[i][j].Item3.Count)
                {
                    currentStartPoint[i][j]++;
                }
            }
        }
    }
}