///-----------------------------------------------------------------
/// Namespace: <CozmoHelpers>
/// Class: <CozmoMovementTester>
/// Description: <Used to measure the time for rotating and moving the virtual cozmo to copy the 
///               real cozmos movement behaviour.>
/// Author: <Tobias Hassel> Date: <29.07.2019>
/// Notes: <>
///-----------------------------------------------------------------
/// 

using Cozmo;
using System.Diagnostics;
using UnityEngine;

namespace CozmoHelpers
{
    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 from the inspector.")]
        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();
                }
            }
        }

        // Measure time for rotation 
        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);
            }
        }

        // Measure time for movement
        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);
            }
        }
    }
}