#include "motor_control.h"

#if defined(ESP32)
Servo esp_servo; // 使用 ESP32Servo 库的 Servo 对象
#else
Servo standard_servo; // 使用标准 Servo 库的 Servo 对象
#endif

void motor_setup() {
    // --- 电机引脚设置 ---
    pinMode(MOTOR_LEFT_IN1_PIN, OUTPUT);
    pinMode(MOTOR_LEFT_IN2_PIN, OUTPUT);
    pinMode(MOTOR_RIGHT_IN3_PIN, OUTPUT);
    pinMode(MOTOR_RIGHT_IN4_PIN, OUTPUT);

#if defined(ESP32)
    // 配置 ESP32 LEDC PWM 通道
    ledcSetup(PWM_CHANNEL_LEFT, PWM_FREQUENCY, PWM_RESOLUTION);
    ledcSetup(PWM_CHANNEL_RIGHT, PWM_FREQUENCY, PWM_RESOLUTION);
    // 将 LEDC 通道附加到电机 ENA/ENB 引脚
    ledcAttachPin(MOTOR_LEFT_ENA_PIN, PWM_CHANNEL_LEFT);
    ledcAttachPin(MOTOR_RIGHT_ENB_PIN, PWM_CHANNEL_RIGHT);
#else
    // 对于非ESP32板，ENA/ENB引脚通常直接用 analogWrite 控制
    pinMode(MOTOR_LEFT_ENA_PIN, OUTPUT);
    pinMode(MOTOR_RIGHT_ENB_PIN, OUTPUT);
#endif

    motor_stop(); // 初始状态停止电机
    Serial.println("电机驱动已初始化。");

    // --- 舵机初始化 ---
    servo_attach();
    servo_center(); // 舵机初始居中
    Serial.println("舵机已初始化并居中。");
}

// --- 内部辅助函数设置单个电机速度和方向 ---
void set_motor_speed(char motor_char, int speed, bool forward) {
    // speed: 0-255
    int actual_speed = constrain(speed, 0, 255);

    if (motor_char == 'L') { // 左电机
        digitalWrite(MOTOR_LEFT_IN1_PIN, forward ? HIGH : LOW);
        digitalWrite(MOTOR_LEFT_IN2_PIN, forward ? LOW : HIGH);
#if defined(ESP32)
        ledcWrite(PWM_CHANNEL_LEFT, actual_speed);
#else
        analogWrite(MOTOR_LEFT_ENA_PIN, actual_speed);
#endif
    } else if (motor_char == 'R') { // 右电机
        digitalWrite(MOTOR_RIGHT_IN3_PIN, forward ? HIGH : LOW);
        digitalWrite(MOTOR_RIGHT_IN4_PIN, forward ? LOW : HIGH);
#if defined(ESP32)
        ledcWrite(PWM_CHANNEL_RIGHT, actual_speed);
#else
        analogWrite(MOTOR_RIGHT_ENB_PIN, actual_speed);
#endif
    }
}

// --- 公开的电机控制函数 ---
void motor_move_forward(int speed) {
    set_motor_speed('L', speed, true);
    set_motor_speed('R', speed, true);
    Serial.print("电机前进，速度: "); Serial.println(speed);
}

void motor_move_backward(int speed) {
    set_motor_speed('L', speed, false);
    set_motor_speed('R', speed, false);
    Serial.print("电机后退，速度: "); Serial.println(speed);
}

void motor_stop() {
    set_motor_speed('L', 0, true); // 速度为0，方向无所谓
    set_motor_speed('R', 0, true);
    Serial.println("电机停止。");
}

void motor_set_raw_speeds(int left_speed, int right_speed) {
    bool left_forward = left_speed >= 0;
    bool right_forward = right_speed >= 0;
    set_motor_speed('L', abs(left_speed), left_forward);
    set_motor_speed('R', abs(right_speed), right_forward);
    Serial.print("设置原始速度 左:"); Serial.print(left_speed);
    Serial.print(" 右:"); Serial.println(right_speed);
}

void motor_turn_left(int speed, float turn_intensity, int steer_angle) {
    servo_set_angle(steer_angle);
    int outer_speed = speed;
    int inner_speed = speed * (1.0 - constrain(turn_intensity, 0.0, 1.0));
    motor_set_raw_speeds(inner_speed, outer_speed); 
    Serial.print("左转: 基础速度="); Serial.print(speed);
    Serial.print(", 强度="); Serial.print(turn_intensity);
    Serial.print(", 舵机角度="); Serial.println(steer_angle);
}

void motor_turn_right(int speed, float turn_intensity, int steer_angle) {
    servo_set_angle(steer_angle);
    int outer_speed = speed;
    int inner_speed = speed * (1.0 - constrain(turn_intensity, 0.0, 1.0));
    motor_set_raw_speeds(outer_speed, inner_speed);
    Serial.print("右转: 基础速度="); Serial.print(speed);
    Serial.print(", 强度="); Serial.print(turn_intensity);
    Serial.print(", 舵机角度="); Serial.println(steer_angle);
}

// --- 舵机控制函数 ---
void servo_attach() {
#if defined(ESP32)
    esp_servo.attach(SERVO_PIN);
#else
    standard_servo.attach(SERVO_PIN);
#endif
    Serial.println("舵机已连接。");
}

void servo_set_angle(int angle) {
    int constrained_angle = constrain(angle, 0, 180); // 标准舵机角度范围
#if defined(ESP32)
    esp_servo.write(constrained_angle);
#else
    standard_servo.write(constrained_angle);
#endif
    // Serial.print("舵机角度设置为: "); Serial.println(constrained_angle);
}

void servo_center() {
    servo_set_angle(90); // 假设90度是舵机中心位置
    Serial.println("舵机已居中 (90度)。");
}

void servo_detach() {
#if defined(ESP32)
    esp_servo.detach();
#else
    standard_servo.detach();
#endif
    Serial.println("舵机已断开连接。");
}