#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

MotorController motor;

void MotorController::begin() {
    // Configure enable pins as outputs
    pinMode(R_EN_PIN, OUTPUT);
    pinMode(L_EN_PIN, OUTPUT);
    
    // Enable the H-bridge
    digitalWrite(R_EN_PIN, HIGH);
    digitalWrite(L_EN_PIN, HIGH);
    
    // Configure LEDC PWM channels for ESP32
    ledcSetup(PWM_CHANNEL_R, PWM_FREQ, PWM_RESOLUTION);
    ledcSetup(PWM_CHANNEL_L, PWM_FREQ, PWM_RESOLUTION);
    
    // Attach PWM channels to GPIO pins
    ledcAttachPin(RPWM_PIN, PWM_CHANNEL_R);
    ledcAttachPin(LPWM_PIN, PWM_CHANNEL_L);
    
#if CURRENT_SENSING_ENABLED
    // Configure current sense pins as analog inputs
    // GPIO34 and GPIO35 are input-only pins, perfect for ADC
    analogSetAttenuation(ADC_11db);  // Full range 0-3.3V
    pinMode(R_IS_PIN, INPUT);
    pinMode(L_IS_PIN, INPUT);
    
    // Calibrate zero-current offset (ESP32 ADC has inherent offset)
    calibrateCurrentOffset();
    Serial.println("Current sensing enabled");
#endif
    
    // Start stopped
    stop();
    
    Serial.println("Motor controller initialized");
}

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(PWM_CHANNEL_R, 0);
    ledcWrite(PWM_CHANNEL_L, 0);
}

void MotorController::update() {
#if CURRENT_SENSING_ENABLED
    static unsigned long lastPrintTime = 0;
    
    // Read current sensors
    _currentRight = readCurrentSense(R_IS_PIN);
    _currentLeft = readCurrentSense(L_IS_PIN);
    
    // Log current readings frequently for data collection
    unsigned long now = millis();
    if (now - lastPrintTime >= CURRENT_LOG_INTERVAL_MS) {
        lastPrintTime = now;
        Serial.printf("CURRENT: R=%.2fA L=%.2fA dir=%d spd=%d\n",
                      _currentRight, _currentLeft, _direction, _speed);
    }
#endif
    
    // Update pingpong mode
    updatePingpong();
}

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;
}

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(PWM_CHANNEL_R, 0);
        ledcWrite(PWM_CHANNEL_L, 0);
    } else if (_direction > 0) {
        // Forward - RPWM active, LPWM off
        ledcWrite(PWM_CHANNEL_R, pwmValue);
        ledcWrite(PWM_CHANNEL_L, 0);
    } else {
        // Reverse - LPWM active, RPWM off
        ledcWrite(PWM_CHANNEL_R, 0);
        ledcWrite(PWM_CHANNEL_L, pwmValue);
    }
    
    Serial.printf("Motor: dir=%d, speed=%d%%, pwm=%d\n", _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 == R_IS_PIN) ? _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(R_IS_PIN);
        sumLeft += analogRead(L_IS_PIN);
        delay(5);  // Small delay between samples
    }
    
    _adcOffsetRight = sumRight / OFFSET_CALIBRATION_SAMPLES;
    _adcOffsetLeft = sumLeft / OFFSET_CALIBRATION_SAMPLES;
    
    Serial.printf("Current sense offset calibrated: R=%d L=%d (ADC counts)\n",
                  _adcOffsetRight, _adcOffsetLeft);
#endif
}

// Pingpong mode implementation (time-based only)
void MotorController::startPingpong(int speed, int timeMs, int speedRandomPercent, int timeRandomPercent) {
    _pingpongBaseSpeed = constrain(speed, 0, 100);
    _pingpongBaseTime = constrain(timeMs, 100, 30000);
    _pingpongSpeedRandomPercent = constrain(speedRandomPercent, 0, 100);
    _pingpongTimeRandomPercent = constrain(timeRandomPercent, 0, 100);
    
    _pingpongDirection = 1;
    _pingpongCurrentSpeed = applyRandomness(_pingpongBaseSpeed, _pingpongSpeedRandomPercent);
    _pingpongCurrentTime = applyRandomness(_pingpongBaseTime, _pingpongTimeRandomPercent);
    _pingpongLastSwitch = millis();
    _pingpongActive = true;
    
    // Apply initial state
    _speed = _pingpongCurrentSpeed;
    _direction = _pingpongDirection;
    applyMotorState();
    
    Serial.printf("Pingpong started: speed=%d%% (base=%d, rand=%d%%), time=%dms (base=%d, rand=%d%%)\n",
                  _pingpongCurrentSpeed, _pingpongBaseSpeed, _pingpongSpeedRandomPercent,
                  _pingpongCurrentTime, _pingpongBaseTime, _pingpongTimeRandomPercent);
}

void MotorController::stopPingpong() {
    _pingpongActive = false;
    stop();
    Serial.println("Pingpong stopped");
}

bool MotorController::isPingpongActive() {
    return _pingpongActive;
}

int MotorController::getPingpongSpeed() {
    return _pingpongBaseSpeed;
}

int MotorController::getPingpongTime() {
    return _pingpongBaseTime;
}

int MotorController::getPingpongSpeedRandom() {
    return _pingpongSpeedRandomPercent;
}

int MotorController::getPingpongTimeRandom() {
    return _pingpongTimeRandomPercent;
}

void MotorController::updatePingpong() {
    if (!_pingpongActive) return;
    
    unsigned long now = millis();
    
    // Time-based switching
    if ((now - _pingpongLastSwitch) >= (unsigned long)_pingpongCurrentTime) {
        // Switch direction
        _pingpongDirection = -_pingpongDirection;
        
        // Apply randomness for next cycle
        _pingpongCurrentSpeed = applyRandomness(_pingpongBaseSpeed, _pingpongSpeedRandomPercent);
        _pingpongCurrentTime = applyRandomness(_pingpongBaseTime, _pingpongTimeRandomPercent);
        _pingpongLastSwitch = now;
        
        // Apply new state
        _speed = _pingpongCurrentSpeed;
        _direction = _pingpongDirection;
        applyMotorState();
        
        Serial.printf("Pingpong switch: dir=%d, speed=%d%%, next_time=%dms\n",
                      _pingpongDirection, _pingpongCurrentSpeed, _pingpongCurrentTime);
    }
}

int MotorController::applyRandomness(int baseValue, int randomPercent) {
    if (randomPercent == 0) return baseValue;
    
    int maxVariation = (baseValue * randomPercent) / 100;
    int variation = random(-maxVariation, maxVariation + 1);
    int result = baseValue + variation;
    
    // Ensure result stays positive and reasonable
    return max(1, result);
}