using UnityEngine;

namespace Breitenberg {

    public class Vehicle : MonoBehaviour {
        [Header("Sensors")]
        public Sensor sensorLeft;
        public Sensor sensorRight;

        [Header("Motors")]
        public float baseSpeed = 0f;    // optional forward bias
        public float gain = 5f;         // motor response strength
        public bool crossWiring = false; // false = same-side wiring (excitatory); true = cross
        public float leftSpeed;
        public float rightSpeed;

        [Header("Wheels")]
        public Transform wheelLeft;
        public Transform wheelRight;

        [Header("Physics")]
        public Rigidbody rb;
        public float turnTorque = 5f;   // rotational influence

        void FixedUpdate() {
            float sL = sensorLeft.output;
            float sR = sensorRight.output;

            // Wiring: same-side (default) maps sL->left motor, sR->right motor.
            // crossWiring = true maps sL->right motor (crossed).
            float leftInput = crossWiring ? sR : sL;
            float rightInput = crossWiring ? sL : sR;

            leftSpeed = baseSpeed + gain * leftInput;
            Debug.DrawRay(wheelLeft.position, wheelLeft.forward * leftSpeed, Color.magenta);
            rightSpeed = baseSpeed + gain * rightInput;
            Debug.DrawRay(wheelRight.position, wheelRight.forward * rightSpeed, Color.magenta);

            // Convert differential wheel speeds into forward force and torque.
            float forward = (leftSpeed + rightSpeed) * 0.5f;
            float rotation = (rightSpeed - leftSpeed) * 0.5f;

            // Apply forward force at center
            Vector3 force = transform.forward * forward;
            rb.AddForce(force, ForceMode.Acceleration);

            // Apply torque around Y axis
            rb.AddTorque(Vector3.down * rotation * turnTorque, ForceMode.Acceleration);

        }
    }

}