#if UNITY_5_3_OR_NEWER
using UnityEngine;

namespace RoboidControl.Unity {
    public class DifferentialDrive : Thing {

        public Wheel leftWheel;
        public Wheel rightWheel;

        /// <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) {
            GameObject prefab = (GameObject)Resources.Load("DifferentialDrive");
            // if (prefab != null) {
            //     // Use resource prefab when available
            //     GameObject gameObj = Instantiate(prefab);
            //     DifferentialDrive component = gameObj.GetComponent<DifferentialDrive>();
            //     if (component != null)
            //         component.core = core;
            //     return component;
            // }
            // else {
            // Fallback implementation
            GameObject gameObj = new(core.name);
            DifferentialDrive component = gameObj.AddComponent<DifferentialDrive>();
            component.Init(core);

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

        protected override void HandleBinary() {
            Debug.Log("Diff drive handle Binary");
            RoboidControl.DifferentialDrive coreDrive = core as RoboidControl.DifferentialDrive;
            RoboidControl.Unity.Wheel[] motors = null;
            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)
                    leftWheel = Wheel.Create(this.GetComponent<Rigidbody>(), coreDrive.leftWheel.id);
            }
            if (leftWheel != null) {
                leftWheel.wheelCollider.radius = coreDrive.wheelRadius;
                leftWheel.wheelCollider.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0);
                leftWheel.transform.position = Vector3.zero; // position is done with the center, but only X direction is supported right now...
            }

            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 && coreDrive.wheelRadius > 0 && coreDrive.wheelSeparation > 0) {
                rightWheel.wheelCollider.radius = coreDrive.wheelRadius;
                rightWheel.wheelCollider.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0);
                rightWheel.transform.position = Vector3.zero; // position is done with the center, but only X direction is supported right now...
            }
        }
    }

}
#endif