/**
 * @file MotorDriver.cpp
 * @brief TB67H450FNG Motor Driver Implementation
 */

#include "MotorDriver.h"

MotorDriver::MotorDriver() 
    : pinIN1(0), pinIN2(0), pinPWM(0), pinCurrentSense(255),
      targetSpeed(0), currentSpeed(0), targetDirection(true),
      loadCompensationEnabled(false), accelRate(10), decelRate(10),
      lastSpeedUpdate(0), Kp(1.0), Ki(0.1), Kd(0.5), 
      integral(0), lastError(0), targetCurrent(0) {}

bool MotorDriver::begin(uint8_t in1Pin, uint8_t in2Pin, uint8_t pwmPin, uint8_t currentSensePin) {
    pinIN1 = in1Pin;
    pinIN2 = in2Pin;
    pinPWM = pwmPin;
    pinCurrentSense = currentSensePin;
    
    // Configure pins
    pinMode(pinIN1, OUTPUT);
    pinMode(pinIN2, OUTPUT);
    pinMode(pinPWM, OUTPUT);
    
    if (pinCurrentSense != 255) {
        pinMode(pinCurrentSense, INPUT);
    }
    
    // Setup PWM
    ledcSetup(pwmChannel, pwmFrequency, pwmResolution);
    ledcAttachPin(pinPWM, pwmChannel);
    ledcWrite(pwmChannel, 0);
    
    // Set initial state (brake)
    digitalWrite(pinIN1, LOW);
    digitalWrite(pinIN2, LOW);
    
    return true;
}

void MotorDriver::setSpeed(uint8_t speed, bool forward) {
    targetSpeed = speed;
    targetDirection = forward;
}

void MotorDriver::emergencyStop() {
    targetSpeed = 0;
    currentSpeed = 0;
    
    // Apply brake
    digitalWrite(pinIN1, HIGH);
    digitalWrite(pinIN2, HIGH);
    ledcWrite(pwmChannel, 0);
}

void MotorDriver::update() {
    updateAcceleration();
    
    if (loadCompensationEnabled && pinCurrentSense != 255) {
        updateLoadCompensation();
    } else {
        applyMotorControl();
    }
}

void MotorDriver::updateAcceleration() {
    unsigned long currentTime = millis();
    
    if (currentTime - lastSpeedUpdate < 50) {
        return; // Update every 50ms
    }
    
    lastSpeedUpdate = currentTime;
    
    if (currentSpeed < targetSpeed) {
        // Accelerate
        uint8_t delta = (accelRate > 0) ? (255 / accelRate) : 1;
        if (currentSpeed + delta >= targetSpeed) {
            currentSpeed = targetSpeed;
        } else {
            currentSpeed += delta;
        }
    } else if (currentSpeed > targetSpeed) {
        // Decelerate
        uint8_t delta = (decelRate > 0) ? (255 / decelRate) : 1;
        if (currentSpeed <= delta || currentSpeed - delta <= targetSpeed) {
            currentSpeed = targetSpeed;
        } else {
            currentSpeed -= delta;
        }
    }
}

void MotorDriver::applyMotorControl() {
    if (currentSpeed == 0) {
        // Stop/brake
        digitalWrite(pinIN1, LOW);
        digitalWrite(pinIN2, LOW);
        ledcWrite(pwmChannel, 0);
        return;
    }
    
    if (currentSpeed == 1) {
        // Emergency stop (brake)
        digitalWrite(pinIN1, HIGH);
        digitalWrite(pinIN2, HIGH);
        ledcWrite(pwmChannel, 0);
        return;
    }
    
    // Map DCC speed (2-127) to PWM (0-255)
    uint16_t pwmValue = map(currentSpeed, 2, 127, 0, 255);
    
    // Set direction
    if (targetDirection) {
        // Forward
        digitalWrite(pinIN1, HIGH);
        digitalWrite(pinIN2, LOW);
    } else {
        // Reverse
        digitalWrite(pinIN1, LOW);
        digitalWrite(pinIN2, HIGH);
    }
    
    // Set PWM
    ledcWrite(pwmChannel, pwmValue);
}

void MotorDriver::updateLoadCompensation() {
    // Read current
    uint16_t currentMa = readCurrent();
    
    // PID control
    float error = targetCurrent - currentMa;
    integral += error;
    
    // Anti-windup
    if (integral > 1000) integral = 1000;
    if (integral < -1000) integral = -1000;
    
    float derivative = error - lastError;
    lastError = error;
    
    float correction = (Kp * error) + (Ki * integral) + (Kd * derivative);
    
    // Apply correction to PWM
    if (currentSpeed == 0 || currentSpeed == 1) {
        applyMotorControl();
        return;
    }
    
    uint16_t basePwm = map(currentSpeed, 2, 127, 0, 255);
    int16_t adjustedPwm = basePwm + (int16_t)correction;
    
    // Clamp PWM
    if (adjustedPwm < 0) adjustedPwm = 0;
    if (adjustedPwm > 255) adjustedPwm = 255;
    
    // Set direction
    if (targetDirection) {
        digitalWrite(pinIN1, HIGH);
        digitalWrite(pinIN2, LOW);
    } else {
        digitalWrite(pinIN1, LOW);
        digitalWrite(pinIN2, HIGH);
    }
    
    ledcWrite(pwmChannel, adjustedPwm);
}

uint16_t MotorDriver::readCurrent() {
    if (pinCurrentSense == 255) {
        return 0;
    }
    
    // Read ADC value
    uint16_t adcValue = analogRead(pinCurrentSense);
    
    // Convert to milliamps (this is hardware dependent)
    // Assuming 3.3V reference, 12-bit ADC, and current sense amplifier
    // Adjust this based on your actual hardware
    uint16_t currentMa = (adcValue * 3300) / 4096; // Simplified conversion
    
    return currentMa;
}

uint16_t MotorDriver::getMotorCurrent() {
    return readCurrent();
}

void MotorDriver::setLoadCompensation(bool enable) {
    loadCompensationEnabled = enable;
    
    if (enable) {
        integral = 0;
        lastError = 0;
    }
}

void MotorDriver::setPIDParameters(float kp, float ki, float kd) {
    Kp = kp;
    Ki = ki;
    Kd = kd;
}

void MotorDriver::setAccelRate(uint8_t rate) {
    accelRate = rate;
}

void MotorDriver::setDecelRate(uint8_t rate) {
    decelRate = rate;
}