#pragma once
#include <Arduino.h>

class MotorDriver {
private:
    int _l1, _l2, _r3, _r4;
    int  _lastSpeed = 0;  // Последняя вычисленная скорость (−255..255)
    char _lastMode  = 'N'; // Последний режим: D / N / R
    bool _armed = false;  // Аппаратный разрешающий флаг (ARM switch)

    // Низкоуровневая подача ШИМ-сигналов на чип MX1508 по новому стандарту ядра 3.х
    // ВАЖНО: всегда сначала гасим ОБА плеча моста и только потом подаем ШИМ —
    // иначе при асинхронном переключении ledc возможен сквозной ток (shoot-through).
    void setMotorSpeed(int pin1, int pin2, int speed) {
        ledcWrite(pin1, 0);
        ledcWrite(pin2, 0);
        if (speed == 0) return;
        delayMicroseconds(2); // мёртвое время на закрытие транзисторов моста

        if (speed > 0) {
            ledcWrite(pin1, speed);
        } else {
            ledcWrite(pin2, abs(speed));
        }
    }

public:
    MotorDriver(int l1, int l2, int r3, int r4) : _l1(l1), _l2(l2), _r3(r3), _r4(r4) {}

    void begin() {
        // В ядре ESP32 3.x одна функция настраивает частоту (15кГц), разрешение (8 бит) 
        // и сразу привязывает ШИМ к физическому GPIO. Каналы выделяются автоматически!
        ledcAttach(_l1, 15000, 8);
        ledcAttach(_l2, 15000, 8);
        ledcAttach(_r3, 15000, 8);
        ledcAttach(_r4, 15000, 8);

        stop();
    }

    // Глобальный предохранитель: пока не взведён ARM-тумблер на пульте,
    // drive() не пропускает газ, даже если связь есть и стики не в нуле.
    void setArmed(bool armed) {
        _armed = armed;
        if (!_armed) stop();
    }
    bool isArmed() { return _armed; }

    void drive(int steering, int throttle, int gearboxSwitch) {
        if (!_armed) {
            stop();
            _lastSpeed = 0;
            _lastMode  = 'N';
            return;
        }

        // Переводим стик газа в ШИМ (0..255)
        int rawSpeed = map(throttle, 1000, 2000, 0, 255);
        if (rawSpeed < 15) rawSpeed = 0; 

        // Переводим руль в диапазон (-255..255)
        int cmdSteering = map(steering, 1000, 2000, -255, 255);
        if (abs(cmdSteering) < 15) cmdSteering = 0; 

        int finalThrottle = 0;

        // Выбор режима хода (Газ на нейтрали блокируется в 0)
        if (gearboxSwitch < 1300) {
            finalThrottle = rawSpeed;       // [D] - Скорость вперед
            _lastMode = 'D';
        } 
        else if (gearboxSwitch > 1700) {
            finalThrottle = -rawSpeed;      // [R] - Скорость назад
            _lastMode = 'R';
        } 
        else {
            finalThrottle = 0;              // [N] - Газ заблокирован, но руль активен
            _lastMode = 'N';
        }
        _lastSpeed = finalThrottle;

        // Танковый микшер
        int leftPower  = constrain(finalThrottle + cmdSteering, -255, 255);
        int rightPower = constrain(finalThrottle - cmdSteering, -255, 255);

        // Отправляем ШИМ-команды напрямую на физические пины GPIO
        setMotorSpeed(_l1, _l2, leftPower);
        setMotorSpeed(_r3, _r4, rightPower);
    }

    void stop() {
        setMotorSpeed(_l1, _l2, 0);
        setMotorSpeed(_r3, _r4, 0);
    }

    int  getLastSpeed() { return _lastSpeed; }
    char getLastMode()  { return _lastMode;  }
};