#if UNITY_5_3_OR_NEWER
using UnityEngine;

namespace RoboidControl.Unity {
    public class DifferentialDrive : Thing {

        public Wheel leftWheel;
        public Wheel rightWheel;
        public SphereCollider casterWheel;

        protected RoboidControl.DifferentialDrive coreDrive => core as RoboidControl.DifferentialDrive;

        /// <summary>
        /// Create the Unity representation
        /// </summary>
        /// <param name="core">The core touch sensor</param>
        /// <returns>The Unity representation of the touch sensor</returns>
        public static DifferentialDrive Create(RoboidControl.DifferentialDrive core) {
            DifferentialDrive component = null;
            Rigidbody rb = null;
            GameObject prefab = (GameObject)Resources.Load("DifferentialDrive");
            if (prefab != null) {
                // Use resource prefab when available
                GameObject gameObj = Instantiate(prefab);
                component = gameObj.GetComponent<DifferentialDrive>();
                if (component != null)
                    component.core = core;

                rb = component.GetComponent<Rigidbody>();
            }
            else {
                // Fallback implementation
                GameObject gameObj = new(core.name);
                component = gameObj.AddComponent<DifferentialDrive>();
                component.Init(core);

                rb = gameObj.AddComponent<Rigidbody>();
                rb.isKinematic = false;
                rb.mass = 0.5f;
            }
            return component;
        }

        private Rigidbody rb = null;

        protected virtual void Awake() {
            rb = GetComponent<Rigidbody>();
        }

        protected override void HandleBinary() {
            Debug.Log("Diff drive handle Binary");
            if (coreDrive.wheelRadius <= 0 || coreDrive.wheelSeparation <= 0)
                return;

            // if (leftWheel == null) {
            //     motors = GetComponentsInChildren<Wheel>();
            //     foreach (var motor in motors) {
            //         if (motor.core.id == coreDrive.leftWheel.id)
            //             leftWheel = motor;
            //     }
            //     if (leftWheel == null)
            //         // Create placeholder wheel
            //         leftWheel = Wheel.Create(this.GetComponent<Rigidbody>(), coreDrive.leftWheel.id);
            // }
            if (leftWheel != null) {
                leftWheel.core ??= coreDrive.leftWheel;
                SphereCollider leftWheelCollider = leftWheel.GetComponent<SphereCollider>();
                leftWheelCollider.radius = coreDrive.wheelRadius;
                // leftWheelCollider.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0);
            }

            // if (rightWheel == null) {
            //     if (motors == null)
            //         motors = GetComponentsInChildren<Wheel>();
            //     foreach (var motor in motors) {
            //         if (motor.core.id == coreDrive.rightWheel.id)
            //             rightWheel = motor;
            //     }
            //     if (rightWheel == null)
            //         rightWheel = Wheel.Create(this.GetComponent<Rigidbody>(), coreDrive.rightWheel.id);
            // }
            if (rightWheel != null) {
                rightWheel.core ??= coreDrive.rightWheel;
                SphereCollider rightWheelCollider = rightWheel.GetComponent<SphereCollider>();
                rightWheelCollider.radius = coreDrive.wheelRadius;
                // rightWheelCollider.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0);
            }

            if (casterWheel == null) {
                // Create it
            }
            if (casterWheel != null) {
                casterWheel.radius = coreDrive.wheelRadius;
                // Put it in the middle of the back
                casterWheel.center = new Vector3(0, 0, -coreDrive.wheelSeparation);
            }
        }


        protected override void FixedUpdate() {
            base.FixedUpdate();

            float leftWheelVelocity = leftWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;
            float rightWheelVelocity = rightWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;

            float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f;
            float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / coreDrive.wheelSeparation;

            // Use smoothing to emulate motor inertia
            rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;
            rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up;
        }
    }

}
#endif