/**
 * @file MotorController.cpp
 * @brief Implementation of DC motor control
 */

#include "MotorController.h"

/**
 * @brief Constructor - initialize with safe defaults
 */
MotorController::MotorController() : currentSpeed(0), currentDirection(1) {
}

void MotorController::begin() {
    // Configure pins
    pinMode(MOTOR_DIR_PIN, OUTPUT);
    pinMode(MOTOR_BRAKE_PIN, OUTPUT);
    
    // Setup PWM
    ledcSetup(PWM_CHANNEL, PWM_FREQUENCY, PWM_RESOLUTION);
    ledcAttachPin(MOTOR_PWM_PIN, PWM_CHANNEL);
    
    // Initialize to safe state
    digitalWrite(MOTOR_BRAKE_PIN, HIGH); // Release brake (active low)
    digitalWrite(MOTOR_DIR_PIN, HIGH);   // Forward direction
    ledcWrite(PWM_CHANNEL, 0);           // Zero speed
    
    Serial.println("Motor Controller initialized");
    Serial.printf("PWM Pin: %d, DIR Pin: %d, BRAKE Pin: %d\n", 
                  MOTOR_PWM_PIN, MOTOR_DIR_PIN, MOTOR_BRAKE_PIN);
}

void MotorController::setSpeed(uint8_t speed, uint8_t direction) {
    currentSpeed = speed;
    currentDirection = direction;
    
    // Release brake
    digitalWrite(MOTOR_BRAKE_PIN, HIGH);
    
    // Set direction
    digitalWrite(MOTOR_DIR_PIN, direction ? HIGH : LOW);
    
    // Set PWM duty cycle
    // Speed is 0-100, convert to 0-255
    uint16_t pwmValue = map(speed, 0, 100, 0, 255);
    ledcWrite(PWM_CHANNEL, pwmValue);
    
    Serial.printf("Motor: Speed=%d%%, Direction=%s, PWM=%d\n", 
                  speed, direction ? "FWD" : "REV", pwmValue);
}

void MotorController::stop() {
    currentSpeed = 0;
    ledcWrite(PWM_CHANNEL, 0);
    digitalWrite(MOTOR_BRAKE_PIN, HIGH); // Release brake
    Serial.println("Motor stopped");
}

void MotorController::brake() {
    ledcWrite(PWM_CHANNEL, 0);
    digitalWrite(MOTOR_BRAKE_PIN, LOW); // Activate brake (active low)
    currentSpeed = 0;
    Serial.println("Motor brake activated");
}

void MotorController::update() {
    // Placeholder for future safety checks or smooth acceleration
}