#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.1f;
            }
            return component;
        }

        private Rigidbody rb = null;

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

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

            // Destroy any (generic) thing with the same id
            if (leftWheel == null) {
                Thing[] things = FindObjectsOfType<Thing>();
                foreach (Thing thing in things) {
                    if (thing.core.id == coreDrive.leftWheel.id)
                        Destroy(thing.gameObject);
                }
            }
            if (leftWheel == null && coreDrive.leftWheel != null)
                leftWheel = Wheel.Create(this.transform, 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);
            }

            // Destroy any (generic) thing with the same id
            if (rightWheel == null) {
                Thing[] things = FindObjectsOfType<Thing>();
                foreach (Thing thing in things) {
                    if (thing.core.id == coreDrive.rightWheel.id)
                        Destroy(thing.gameObject);
                }
            }
            if (rightWheel == null && coreDrive.rightWheel != null)
                rightWheel = Wheel.Create(this.transform, 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) {
                GameObject gameObj = new("Caster wheel");
                gameObj.transform.parent = this.transform;
                casterWheel = gameObj.AddComponent<SphereCollider>();
                casterWheel.material = Wheel.slidingWheel;
            }
            casterWheel.radius = coreDrive.wheelRadius;
            // Put it in the middle of the back
            // This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
            float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
            casterWheel.center = new Vector3(0, 0, -wheelSeparation);
        }

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

            if (rb != null && leftWheel != null && rightWheel != null) {
                float leftWheelVelocity = leftWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;
                float rightWheelVelocity = rightWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;

                // This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
                float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
                float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f;
                float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / 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