using UnityEngine;

namespace NanoBrain.Unity.Braitenberg {

    /// <summary>
    /// A powered wheel collider, controlled by a NanoBrain::Neuron
    /// </summary>
    [RequireComponent(typeof(WheelCollider))]
    public class Motor : MonoBehaviour {
        /// <summary>
        /// The name of the NanoBrain::Neuron to control this wheel
        /// </summary>
        /// This is used to connect the motor to the right output neuron of the Vehicle::brain
        [Tooltip("The name of the Nanobrain Neuron to control this wheel")]
        public string outputNeuronName;
        /// <summary>
        /// The speed of the motor in rotations per second
        /// </summary>
        [Tooltip("The speed of the motor in rotations per second")]
        public float speed { get; protected set; }
        /// <summary>
        /// The maximum torque used to drive the wheel collider
        /// </summary>
        [Tooltip("The maximum torque used to drive the wheel collider")]
        public float maxTorque = 1;

        /// <summary>
        /// The wheel collider used to drive the car
        /// </summary>
        /// \see [Unity WheelCollider](https://docs.unity3d.com/ScriptReference/WheelCollider.html)
        [Tooltip("The wheel collider used to drive the car")]
        public WheelCollider wheelCollider;
        /// <summary>
        /// The NanoBrain::Neuron to control this wheel
        /// </summary>
        [Tooltip("The NanoBrain Neuron to control this wheel")]
        public Neuron motorNeuron;

        /// <summary>
        /// Unity calls Awake when loading an instance of a script component.
        /// </summary>
        protected virtual void Awake() {
            wheelCollider = GetComponent<WheelCollider>();
            GetMotorNeuron();
        }

        /// <summary>
        /// Retrieve the motor neuron from the outputNeuronName;
        /// </summary>
        private void GetMotorNeuron() {
            if (motorNeuron != null)
                return;

            Vehicle vehicle = GetComponentInParent<Vehicle>();
            if (vehicle == null)
                return;

            Cluster brain = vehicle.brain;
            if (brain != null)
                motorNeuron = brain.GetNeuron(outputNeuronName);
        }

        /// <summary>
        /// Update called at regular, fixed intervals as part of Unity's physics update loop.
        /// </summary>
        void FixedUpdate() {
            if (motorNeuron == null)
                return;
                
            this.speed = motorNeuron.outputValue.z;
            Debug.DrawRay(this.transform.position, this.transform.forward * this.speed, Color.blue);

            float desiredRpm = speed * 60; // target wheel RPM
            float currentRpm = wheelCollider.rpm;
            float rpmError = desiredRpm - currentRpm;
            float kP = 0.02f; // proportional gain
            float torque = wheelCollider.motorTorque + kP * rpmError;
            wheelCollider.motorTorque = Mathf.Clamp(torque, -maxTorque, maxTorque);
        }
    }

}