#if UNITY_5_3_OR_NEWER
using UnityEngine;

namespace RoboidControl.Unity {

    /// <summary>
    /// The Unity representation of a Roboid Control differential drive
    /// </summary>
    public class DifferentialDrive : Thing {

        /// <summary>
        /// The core differential drive
        /// </summary>
        public RoboidControl.DifferentialDrive coreDrive => core as RoboidControl.DifferentialDrive;

        /// <summary>
        /// Create the Unity representation
        /// </summary>
        /// <param name="coreDrive">The core touch sensor</param>
        /// <returns>The Unity representation of the touch sensor</returns>
        /// This uses a 'DifferentialDrive' resource when available for the Unity representation.
        /// If this is not available, a default representation is created.
        public static DifferentialDrive Create(RoboidControl.DifferentialDrive coreDrive) {
            DifferentialDrive differentialDrive;
            GameObject prefab = (GameObject)Resources.Load("DifferentialDrive");
            if (prefab != null) {
                // Use resource prefab when available
                GameObject gameObj = Instantiate(prefab);
                differentialDrive = gameObj.GetComponent<DifferentialDrive>();
                differentialDrive.Init(coreDrive);
            }
            else {
                // Default implementation
                GameObject gameObj = new(coreDrive.name);
                differentialDrive = gameObj.AddComponent<DifferentialDrive>();
                differentialDrive.Init(coreDrive);

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

        public Motor leftMotor;
        public Motor rightMotor;
        /// <summary>
        /// The left wheel of the differential drive
        /// </summary>
        public Wheel leftWheel;
        /// <summary>
        /// The right wheel of the differential drive
        /// </summary>
        public Wheel rightWheel;
        /// <summary>
        /// The caster wheel to keep the differential drive horizontal
        /// </summary>
        public SphereCollider casterWheel;

        /// <summary>
        /// The rigidbody of the differential drive
        /// </summary>
        private Rigidbody rb = null;

        /// <summary>
        /// Start the Unity representation
        /// </summary>
        protected virtual void Start() {
            rb = GetComponent<Rigidbody>();
        }

        /// <summary>
        /// Handle the binary event indicating a configuration change
        /// </summary>
        protected override void HandleBinary() {
            HandleWheelBinary(coreDrive.leftWheel, ref leftMotor, ref leftWheel);
            HandleWheelBinary(coreDrive.rightWheel, ref rightMotor, ref rightWheel);

            if (casterWheel == null) {
                GameObject gameObj = new("Caster wheel");
                gameObj.transform.parent = this.transform;
                casterWheel = gameObj.AddComponent<SphereCollider>();
                casterWheel.material = Wheel.slidingWheel;
            }
            if (coreDrive.wheelRadius > 0 && coreDrive.leftWheel != null && coreDrive.rightWheel != null) {
                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);
            }
        }

        private void HandleWheelBinary(RoboidControl.Motor coreMotor, ref Motor motor, ref Wheel wheel) {
            if (coreMotor == null)
                return;

            if (motor == null) {
                motor = coreMotor.component as Motor;
                if (motor == null)
                    motor = Motor.Create(coreMotor);
                wheel.transform.SetParent(motor.transform);
            }
            else if (motor.core.id != coreMotor.id) {
                motor = coreMotor.component as Motor;
                if (motor != null)
                    wheel.transform.SetParent(motor.transform);
            }
        }

        /// <summary>
        /// Update the Unity representation state
        /// </summary>
        protected override void FixedUpdate() {
            base.FixedUpdate();

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

                if (leftWheel != null && rightWheel != null) {
                    float wheelSeparation = Vector3.Distance(leftWheel.transform.position, rightWheel.transform.position);
                    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;

                    core.position = LinearAlgebra.Spherical.FromVector3(this.transform.localPosition);
                    core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation);

                }
            }
        }
    }

}
#endif