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

public enum TestStates { MovementSpeed, RotationSpeed } // enum to determine test scenarios

[RequireComponent(typeof(CozmoMovement))]
public class CozmoMovementTester : MonoBehaviour
{

    private const float DISTANCE_TO_MEASURE_SPEED = 1f; // Distance that is used to calculate cozmos movementspeed (in m)
    private const float ROTATION_ANGLE = 90f;          // Angle that is used to calculate cozmos rotationspeed 

    private CozmoMovement movement;             // Reference to the movement script of cozmo
    private Vector3 startPositionCozmo;         // Original position of cozmo
    private float lastY;                        // Last Y of cozmo
    private Stopwatch movementWatch;            // Stopwatch to measure the time for the movement usecase
    private Stopwatch rotationWatch;            // Stopwatch to measure the time for the rotation usecase  

    [Tooltip("Choose which speed of the virtual cozmo should be tested.")]
    public TestStates testState = TestStates.MovementSpeed;

    [Tooltip("Start and stop testing")]
    public bool isTesting = false;


    void Start()
    {
        movement = GetComponent<CozmoMovement>();
        movementWatch = new Stopwatch();
        rotationWatch = new Stopwatch();


        startPositionCozmo = movement.transform.position;   // Cache the starting position of Cozmo    
        lastY = movement.transform.rotation.y;              // Cache the rotation Vector of Cozmo    
    }

    private void FixedUpdate()
    {
        if (isTesting)
        {
            if (testState == TestStates.MovementSpeed)
            {
                TestMovementSpeed();
            }
            else if (testState == TestStates.RotationSpeed)
            {
                TestRotationSpeed();
            }
        }
    }

    private void TestRotationSpeed()
    {

        float currentY = movement.transform.rotation.eulerAngles.y;

        // If distance between start and current position of cozmo reached the  DISTANCE_TO_MEASURE_SPEED stop the stopwatch
        if (currentY >= lastY + ROTATION_ANGLE || currentY <= lastY - ROTATION_ANGLE)
        {
            if (rotationWatch.IsRunning)
            {
                rotationWatch.Stop();
                UnityEngine.Debug.Log("Cozmo: " + gameObject.name +
                    "\nElapsed time in milliseconds (Rotation): " + rotationWatch.ElapsedMilliseconds);
            }

            isTesting = false;
            return;
        }
        else
        {
            // Start stopwatch to measure the time cozmo needs for a specific rotation
            if (!rotationWatch.IsRunning)
            {
                rotationWatch = Stopwatch.StartNew();
            }

            // Turn right
            movement.Turn(1);
        }
    }

    private void TestMovementSpeed()
    {
        // If distance between start and current position of cozmo reached the  DISTANCE_TO_MEASURE_SPEED stop the stopwatch
        if (Vector3.Distance(startPositionCozmo, movement.transform.position) >= DISTANCE_TO_MEASURE_SPEED)
        {
            if (movementWatch.IsRunning)
            {
                movementWatch.Stop();
                UnityEngine.Debug.Log("Cozmo: " + gameObject.name +
                    "/nElapsed time in milliseconds (Movement): " + movementWatch.ElapsedMilliseconds);
            }

            isTesting = false;
            return;
        }
        else
        {
            // Start stopwatch to measure the time cozmo needs for a specific distance
            if (!movementWatch.IsRunning)
            {
                movementWatch = Stopwatch.StartNew();
            }

            // Move cozmo in forward direction
            movement.Move(1);
        }
    }
}