#include <Arduino.h>
#include <BluetoothSerial.h>

// 使用 2 片 TB6612 控制 4 个电机
// 使用差速控制转向

// TB6612 #1 控制 A B 电机
#define MOTOR_AB_STBY 14 // TB6612 #1 待机控制

// A 电机 左前轮
#define MOTOR_A_PWMA 25 // 电机 A PWM
#define MOTOR_A_AIN1 26 // 电机 A 方向控制1
#define MOTOR_A_AIN2 27 // 电机 A 方向控制2

// B 电机 左后轮
#define MOTOR_B_PWMB 32 // 电机 B PWM
#define MOTOR_B_BIN1 33 // 电机 B 方向控制1
#define MOTOR_B_BIN2 34 // 电机 B 方向控制2

// TB6612 #2 控制 C D 电机
#define MOTOR_CD_STBY 19 // TB6612 #2 待机控制

// C 电机 右前轮
#define MOTOR_C_PWMA 16 // 电机 C PWM
#define MOTOR_C_AIN1 17 // 电机 C 方向控制1
#define MOTOR_C_AIN2 18 // 电机 C 方向控制2

// D 电机 右后轮
#define MOTOR_D_PWMB 21 // 电机 D PWM
#define MOTOR_D_BIN1 22 // 电机 D 方向控制1
#define MOTOR_D_BIN2 23 // 电机 D 方向控制2

// 运动控制参数
#define MAX_SPEED 255        // 最大速度
#define BASE_SPEED 200       // 基础速度
#define MIN_SPEED 50         // 最小速度
#define TURN_RATIO 0.6      // 转向时的速度比例
#define SPEED_INCREMENT 10   // 速度增量

// 全局变量
int currentSpeed = 0;        // 当前速度
int turnOffset = 0;         // 转向偏移量 (-100 到 100)
bool isMoving = false;      // 运动状态
bool isTurning = false;     // 转向状态

BluetoothSerial SerialBT;

// 电机控制函数
void motorControl(int pwmPin, int in1Pin, int in2Pin, int speed) {
  if (speed > 0) {
    digitalWrite(in1Pin, HIGH);
    digitalWrite(in2Pin, LOW);
  } else if (speed < 0) {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, HIGH);
    speed = -speed;
  } else {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, LOW);
  }
  analogWrite(pwmPin, speed);
}

// 小车运动控制函数
void carControl(int leftSpeed, int rightSpeed) {
  motorControl(MOTOR_A_PWMA, MOTOR_A_AIN1, MOTOR_A_AIN2, leftSpeed);  // 左前
  motorControl(MOTOR_B_PWMB, MOTOR_B_BIN1, MOTOR_B_BIN2, leftSpeed);  // 左后
  motorControl(MOTOR_C_PWMA, MOTOR_C_AIN1, MOTOR_C_AIN2, rightSpeed); // 右前
  motorControl(MOTOR_D_PWMB, MOTOR_D_BIN1, MOTOR_D_BIN2, rightSpeed); // 右后
}

// 计算左右轮速度
void calculateWheelSpeeds(int baseSpeed, int turnOffset, int &leftSpeed, int &rightSpeed) {
  // turnOffset 范围：-100 到 100
  // 负值表示向左转，正值表示向右转
  float turnMultiplier = 1.0 - (abs(turnOffset) / 100.0) * (1.0 - TURN_RATIO);
  
  if (turnOffset < 0) {  // 向左转
    leftSpeed = baseSpeed * turnMultiplier;
    rightSpeed = baseSpeed;
  } else if (turnOffset > 0) {  // 向右转
    leftSpeed = baseSpeed;
    rightSpeed = baseSpeed * turnMultiplier;
  } else {  // 直行
    leftSpeed = baseSpeed;
    rightSpeed = baseSpeed;
  }
}

// 更新小车运动
void updateCarMovement() {
  int leftSpeed = 0, rightSpeed = 0;
  
  if (isMoving) {
    calculateWheelSpeeds(currentSpeed, turnOffset, leftSpeed, rightSpeed);
    if (!isTurning) {
      // 逐渐加速
      currentSpeed = min(currentSpeed + SPEED_INCREMENT, BASE_SPEED);
    }
  } else {
    // 逐渐减速
    currentSpeed = max(currentSpeed - SPEED_INCREMENT, 0);
    if (currentSpeed > 0) {
      calculateWheelSpeeds(currentSpeed, turnOffset, leftSpeed, rightSpeed);
    }
  }
  
  carControl(leftSpeed, rightSpeed);
}

// CCD 巡线相关函数
#define CCD_PIN 35          // CCD 数据引脚
#define CCD_CLK_PIN 36      // CCD 时钟引脚
#define CCD_SI_PIN 39       // CCD 启动引脚
#define CCD_PIXELS 128      // CCD 像素数

int ccdData[CCD_PIXELS];    // CCD 数据数组

// 读取 CCD 数据
void readCCD() {
  // 启动信号
  digitalWrite(CCD_SI_PIN, HIGH);
  digitalWrite(CCD_CLK_PIN, HIGH);
  digitalWrite(CCD_SI_PIN, LOW);
  digitalWrite(CCD_CLK_PIN, LOW);
  
  // 读取像素数据
  for(int i = 0; i < CCD_PIXELS; i++) {
    digitalWrite(CCD_CLK_PIN, HIGH);
    ccdData[i] = analogRead(CCD_PIN);
    digitalWrite(CCD_CLK_PIN, LOW);
  }
}

// 计算巡线偏移量
int calculateLineOffset() {
  // 这里添加线位置检测算法
  // 返回值范围：-100 到 100
  // 0 表示线在中间，负值表示线在左边，正值表示线在右边
  return 0; // 临时返回值
}

void setup()
{
  // 初始化串口
  Serial.begin(115200);
  
  // 初始化蓝牙
  SerialBT.begin("ESP32_Car"); // 蓝牙设备名称
  
  // 设置引脚模式
  pinMode(MOTOR_AB_STBY, OUTPUT);
  pinMode(MOTOR_CD_STBY, OUTPUT);
  
  pinMode(MOTOR_A_PWMA, OUTPUT);
  pinMode(MOTOR_A_AIN1, OUTPUT);
  pinMode(MOTOR_A_AIN2, OUTPUT);
  
  pinMode(MOTOR_B_PWMB, OUTPUT);
  pinMode(MOTOR_B_BIN1, OUTPUT);
  pinMode(MOTOR_B_BIN2, OUTPUT);
  
  pinMode(MOTOR_C_PWMA, OUTPUT);
  pinMode(MOTOR_C_AIN1, OUTPUT);
  pinMode(MOTOR_C_AIN2, OUTPUT);
  
  pinMode(MOTOR_D_PWMB, OUTPUT);
  pinMode(MOTOR_D_BIN1, OUTPUT);
  pinMode(MOTOR_D_BIN2, OUTPUT);
  
  // 启用电机驱动
  digitalWrite(MOTOR_AB_STBY, HIGH);
  digitalWrite(MOTOR_CD_STBY, HIGH);
  
  // 添加 CCD 引脚初始化
  // pinMode(CCD_CLK_PIN, OUTPUT);
  // pinMode(CCD_SI_PIN, OUTPUT);
  // pinMode(CCD_PIN, INPUT);
}

void loop()
{
  if (SerialBT.available()) {
    char cmd = SerialBT.read();
    
    switch(cmd) {
      case 'F': // 前进
        isMoving = true;
        turnOffset = 0;
        break;
      case 'B': // 后退
        isMoving = true;
        currentSpeed = -BASE_SPEED;
        turnOffset = 0;
        break;
      case 'L': // 左转
        isTurning = true;
        turnOffset = -100;
        break;
      case 'R': // 右转
        isTurning = true;
        turnOffset = 100;
        break;
      case 'S': // 停止
        isMoving = false;
        isTurning = false;
        turnOffset = 0;
        break;
      case 'l': // 微左转
        turnOffset = max(turnOffset - 20, -100);
        break;
      case 'r': // 微右转
        turnOffset = min(turnOffset + 20, 100);
        break;
    }
  }
  
  // 更新小车运动
  updateCarMovement();
  
  // 巡线模式代码（后续启用）
  /*
  readCCD();
  int lineOffset = calculateLineOffset();
  turnOffset = lineOffset;
  isMoving = true;
  updateCarMovement();
  */
  
  delay(20); // 控制更新频率
}