#include "motor.h"

// Set to true to enable current sensing (requires R_IS and L_IS connected)
#define CURRENT_SENSING_ENABLED true

// Number of samples for offset calibration
#define OFFSET_CALIBRATION_SAMPLES 16

// Static member initialization
bool MotorController::_enablePinsConfigured = false;

// Create two motor controller instances
MotorController motor1(MOTOR1_PINS, "Motor1");
MotorController motor2(MOTOR2_PINS, "Motor2");

// Constructor
MotorController::MotorController(const MotorPins& pins, const char* name)
    : _pins(pins), _name(name) {
}

void MotorController::begin() {
    // Configure shared enable pins only once (first motor to initialize does this)
    if (!_enablePinsConfigured) {
        pinMode(R_EN_PIN, OUTPUT);
        pinMode(L_EN_PIN, OUTPUT);
        
        // Enable the H-bridge (shared by both drivers)
        digitalWrite(R_EN_PIN, HIGH);
        digitalWrite(L_EN_PIN, HIGH);
        
        _enablePinsConfigured = true;
        Serial.println("Shared enable pins configured (HIGH)");
    }
    
    // Configure LEDC PWM channels for this motor
    ledcSetup(_pins.pwm_channel_r, PWM_FREQ, PWM_RESOLUTION);
    ledcSetup(_pins.pwm_channel_l, PWM_FREQ, PWM_RESOLUTION);
    
    // Attach PWM channels to GPIO pins
    ledcAttachPin(_pins.rpwm, _pins.pwm_channel_r);
    ledcAttachPin(_pins.lpwm, _pins.pwm_channel_l);
    
#if CURRENT_SENSING_ENABLED
    // Configure current sense pins as analog inputs
    // GPIO34, 35, 36, 39 are input-only pins, perfect for ADC
    analogSetAttenuation(ADC_11db);  // Full range 0-3.3V
    pinMode(_pins.r_is, INPUT);
    pinMode(_pins.l_is, INPUT);
    
    // Calibrate zero-current offset (ESP32 ADC has inherent offset)
    calibrateCurrentOffset();
    Serial.printf("%s: Current sensing enabled\n", _name);
#endif
    
    // Start stopped
    stop();
    
    Serial.printf("%s: Controller initialized (RPWM=%d, LPWM=%d)\n", 
                  _name, _pins.rpwm, _pins.lpwm);
}

void MotorController::setSpeed(int speed) {
    _speed = constrain(speed, 0, 100);
    applyMotorState();
}

void MotorController::setDirection(int dir) {
    _direction = constrain(dir, -1, 1);
    applyMotorState();
}

void MotorController::stop() {
    // Don't reset _speed - keep last speed setting
    _direction = 0;
    ledcWrite(_pins.pwm_channel_r, 0);
    ledcWrite(_pins.pwm_channel_l, 0);
    Serial.printf("%s: STOPPED (manual)\n", _name);
}

void MotorController::update() {
#if CURRENT_SENSING_ENABLED
    static unsigned long lastPrintTime = 0;
    
    // Read current sensors
    _currentRight = readCurrentSense(_pins.r_is);
    _currentLeft = readCurrentSense(_pins.l_is);
    
    // Log current readings only when motor is running (direction != 0)
    unsigned long now = millis();
    if (_direction != 0 && now - lastPrintTime >= CURRENT_LOG_INTERVAL_MS) {
        lastPrintTime = now;
        Serial.printf("%s CURRENT: R=%.2fA L=%.2fA dir=%d spd=%d\n",
                      _name, _currentRight, _currentLeft, _direction, _speed);
    }
    
#endif
}

int MotorController::getSpeed() {
    return _speed;
}

int MotorController::getDirection() {
    return _direction;
}

float MotorController::getCurrentRight() {
    return _currentRight;
}

float MotorController::getCurrentLeft() {
    return _currentLeft;
}

float MotorController::getCurrentActive() {
    if (_direction > 0) {
        return _currentRight;
    } else if (_direction < 0) {
        return _currentLeft;
    }
    return 0.0f;
}

const char* MotorController::getName() {
    return _name;
}

void MotorController::applyMotorState() {
    // Apply minimum PWM when motor is running
    int effectiveSpeed = _speed;
    if (_speed > 0 && _speed < MIN_PWM_PERCENT) {
        effectiveSpeed = MIN_PWM_PERCENT;
    }
    
    // Convert percentage to 8-bit PWM value
    int pwmValue = map(effectiveSpeed, 0, 100, 0, 255);
    
    if (_direction == 0 || _speed == 0) {
        // Stop - both PWM outputs off
        ledcWrite(_pins.pwm_channel_r, 0);
        ledcWrite(_pins.pwm_channel_l, 0);
    } else if (_direction > 0) {
        // Forward - RPWM active, LPWM off
        ledcWrite(_pins.pwm_channel_r, pwmValue);
        ledcWrite(_pins.pwm_channel_l, 0);
    } else {
        // Reverse - LPWM active, RPWM off
        ledcWrite(_pins.pwm_channel_r, 0);
        ledcWrite(_pins.pwm_channel_l, pwmValue);
    }
    
    Serial.printf("%s: dir=%d, speed=%d%%, pwm=%d\n", _name, _direction, _speed, pwmValue);
}

float MotorController::readCurrentSense(int pin) {
#if CURRENT_SENSING_ENABLED
    // Read ADC value (12-bit, 0-4095)
    int adcValue = analogRead(pin);
    
    // Subtract zero-current offset (calibrated at startup)
    int offset = (pin == _pins.r_is) ? _adcOffsetRight : _adcOffsetLeft;
    adcValue = adcValue - offset;
    if (adcValue < 0) adcValue = 0;  // Clamp to zero
    
    // Convert to voltage (0-3.3V)
    float voltage = (adcValue / ADC_MAX) * ADC_VREF;
    
    // Calculate current
    // IS pin outputs: I_sense = I_load / CURRENT_SENSE_RATIO
    // With sense resistor: V = I_sense * R_sense
    // Therefore: I_load = V * CURRENT_SENSE_RATIO / R_sense
    float current = (voltage * CURRENT_SENSE_RATIO) / SENSE_RESISTOR;
    
    // Apply calibration factor (adjust for real-world measurement differences)
    current *= CURRENT_CALIBRATION;
    
    return current;
#else
    return 0.0f;
#endif
}

void MotorController::calibrateCurrentOffset() {
#if CURRENT_SENSING_ENABLED
    // Sample ADC multiple times and average to get stable offset
    long sumRight = 0;
    long sumLeft = 0;
    
    for (int i = 0; i < OFFSET_CALIBRATION_SAMPLES; i++) {
        sumRight += analogRead(_pins.r_is);
        sumLeft += analogRead(_pins.l_is);
        delay(5);  // Small delay between samples
    }
    
    _adcOffsetRight = sumRight / OFFSET_CALIBRATION_SAMPLES;
    _adcOffsetLeft = sumLeft / OFFSET_CALIBRATION_SAMPLES;
    
    Serial.printf("%s: Current sense offset calibrated: R=%d L=%d (ADC counts)\n",
                  _name, _adcOffsetRight, _adcOffsetLeft);
#endif
}