#if UNITY_5_3_OR_NEWER
using System.Collections;
using UnityEngine;
using LinearAlgebra;

namespace RoboidControl.Unity {
    public class RelativeEncoder : Thing {

        /// <summary>
        /// Create the Unity representation
        /// </summary>
        /// <param name="core">The core motor</param>
        /// <returns>The Unity representation of a motor</returns>
        public static RelativeEncoder Create(RoboidControl.RelativeEncoder core) {
            GameObject prefab = (GameObject)Resources.Load("IncrementalEncoder");
            if (prefab != null) {
                // Use resource prefab when available
                GameObject gameObj = Instantiate(prefab);
                RelativeEncoder component = gameObj.GetComponent<RelativeEncoder>();
                if (component != null)
                    component.core = core;
                return component;
            }
            else {
                // Fallback implementation
                GameObject gameObj = new(core.name);
                RelativeEncoder component = gameObj.AddComponent<RelativeEncoder>();
                component.Init(core);

                Rigidbody rb = gameObj.AddComponent<Rigidbody>();
                rb.isKinematic = true;
                return component;
            }
        }
        RoboidControl.RelativeEncoder coreEncoder => this.core as RoboidControl.RelativeEncoder;

        /// <summary>
        /// The rotation speed in degrees per second
        /// </summary>
        public float rotationSpeed = 0.0f;

        protected override void HandleBinary() {
            this.rotationSpeed = coreEncoder.angularSpeed;
        }

        private Quaternion lastRotation;
        protected override void Update() {
            base.Update();
            if (this.transform.childCount == 0)
                return;

            Transform child = this.transform.GetChild(0);

            // Get the delta rotation since last frame
            Quaternion deltaRotation = Quaternion.Inverse(lastRotation) * child.localRotation;

            // Normalize angles to range [-180..180)
            // Note: it is not possible to track rotation speeds higher than 180 degrees per frame because of representation limitations of Quaternions
            float deltaAngle = Angle.Normalize(deltaRotation.eulerAngles.x);
            this.rotationSpeed = deltaAngle / Time.deltaTime;
        }

        IEnumerator UpdateCore() {
            WaitForSeconds wait = new(0.1f);

            while (true) {
                this.coreEncoder.angularSpeed = this.rotationSpeed;
                yield return wait;
            }
        }
    }
}
#endif