using UnityEngine;

namespace NanoBrain.Braitenberg {
    using Unity;

    public class Motor : MonoBehaviour {
        public string outputNeuronName;
        public float speed;
        public float maxTorque = 1;

        public WheelCollider wheelCollider;

        protected Cluster brain;
        public Neuron motorNeuron;

        protected virtual void Awake() {
            Vehicle vehicle = GetComponentInParent<Vehicle>();
            if (vehicle != null)
                brain = vehicle.brain;
            if (brain != null)
                motorNeuron = brain.GetNeuron(outputNeuronName);
            wheelCollider = GetComponent<WheelCollider>();
        }

        void FixedUpdate() {
            if (motorNeuron == null)
                return;
                
            this.speed = motorNeuron.outputValue.z;
            Debug.DrawRay(this.transform.position, this.transform.forward * this.speed, Color.magenta);

            float desiredRpm = speed * 60; // target wheel RPM
            float currentRpm = wheelCollider.rpm;
            float rpmError = desiredRpm - currentRpm;
            float kp = 0.02f; // proportional gain — tune
            float torque = kp * rpmError;
            wheelCollider.motorTorque = Mathf.Clamp(torque, -maxTorque, maxTorque);
            if (float.IsNaN(wheelCollider.rpm))
                Debug.Log($"{speed} {currentRpm / 60} {torque}");
        }
    }

}