#include "motor.h"

// 静态成员初始化
MotorStatus MotorController::motorA = {{0, 0, 0}, false, false, 0};
MotorStatus MotorController::motorB = {{0, 0, 0}, false, false, 0};
MotorStatus MotorController::motorC = {{0, 0, 0}, false, false, 0};
MotorStatus MotorController::motorD = {{0, 0, 0}, false, false, 0};

void MotorController::init(
    MotorPin aPin,
    MotorPin bPin,
    MotorPin cPin,
    MotorPin dPin)
{
  Serial.println("Motor Init");

  motorA.pin = aPin;
  motorB.pin = bPin;
  motorC.pin = cPin;
  motorD.pin = dPin;

  pinMode(aPin.pwm, OUTPUT);
  pinMode(aPin.in1, OUTPUT);
  pinMode(aPin.in2, OUTPUT);

  pinMode(bPin.pwm, OUTPUT);
  pinMode(bPin.in1, OUTPUT);
  pinMode(bPin.in2, OUTPUT);

  pinMode(cPin.pwm, OUTPUT);
  pinMode(cPin.in1, OUTPUT);
  pinMode(cPin.in2, OUTPUT);

  pinMode(dPin.pwm, OUTPUT);
  pinMode(dPin.in1, OUTPUT);
  pinMode(dPin.in2, OUTPUT);
}

void MotorController::pwmControl(char motor, unsigned char pwm)
{
  int pwmPin;
  switch (motor)
  {
  case 'A':
    pwmPin = motorA.pin.pwm;
    motorA.pwm = pwm;
    break;
  case 'B':
    pwmPin = motorB.pin.pwm;
    motorB.pwm = pwm;
    break;
  case 'C':
    pwmPin = motorC.pin.pwm;
    motorC.pwm = pwm;
    break;
  case 'D':
    pwmPin = motorD.pin.pwm;
    motorD.pwm = pwm;
    break;
  }
  analogWrite(pwmPin, pwm);
}

// 电机控制函数
void MotorController::motorControl(char motor, int speed)
{
  int in1Pin, in2Pin;
  MotorStatus *status;

  switch (motor)
  {
  case 'A':
    in1Pin = motorA.pin.in1;
    in2Pin = motorA.pin.in2;
    status = &motorA;
    break;
  case 'B':
    in1Pin = motorB.pin.in2;
    in2Pin = motorB.pin.in1;
    status = &motorB;
    break;
  case 'C':
    in1Pin = motorC.pin.in2;
    in2Pin = motorC.pin.in1;
    status = &motorC;
    break;
  case 'D':
    in1Pin = motorD.pin.in1;
    in2Pin = motorD.pin.in2;
    status = &motorD;
    break;
  }

  if (speed > 0)
  {
    digitalWrite(in1Pin, HIGH);
    digitalWrite(in2Pin, LOW);
    if (motor == 'A' || motor == 'D')
    {
      status->in1 = true;
      status->in2 = false;
    }
    else
    {
      status->in1 = false;
      status->in2 = true;
    }
  }
  else if (speed < 0)
  {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, HIGH);
    if (motor == 'A' || motor == 'D')
    {
      status->in1 = false;
      status->in2 = true;
    }
    else
    {
      status->in1 = true;
      status->in2 = false;
    }

    speed = -speed;
  }
  else
  {
    digitalWrite(in1Pin, LOW);
    digitalWrite(in2Pin, LOW);
    status->in1 = false;
    status->in2 = false;
  }
  pwmControl(motor, speed);
}

void MotorController::motorXYRControl(int8_t x, int8_t y, int8_t r)
{
  // 将输入范围限制在 -100 到 100
  x = constrain(x, -100, 100);
  y = constrain(y, -100, 100);
  r = constrain(r, -100, 100);

  // 将 -100~100 映射到 -255~255
  int16_t mappedX = map(x, -100, 100, -255, 255);
  int16_t mappedY = map(y, -100, 100, -255, 255);
  int16_t mappedR = map(r, -100, 100, -255, 255);

  // 麦克纳姆轮运动学方程
  // 左前轮(A) = Y - X - R
  // 右前轮(D) = Y + X - R
  // 右后轮(C) = Y - X + R
  // 左后轮(B) = Y + X + R
  int16_t speedA = mappedY - mappedX - mappedR; // 左前轮
  int16_t speedB = mappedY + mappedX + mappedR; // 左后轮
  int16_t speedC = mappedY - mappedX + mappedR; // 右后轮
  int16_t speedD = mappedY + mappedX - mappedR; // 右前轮

  // 找出最大速度的绝对值
  int16_t maxSpeed = max(max(abs(speedA), abs(speedB)),
                         max(abs(speedC), abs(speedD)));

  // 如果最大速度超过255，等比例缩放所有速度
  if (maxSpeed > 255)
  {
    float scale = 255.0f / maxSpeed;
    speedA *= scale;
    speedB *= scale;
    speedC *= scale;
    speedD *= scale;
  }

  // 控制各个电机
  motorControl('A', speedA);
  motorControl('B', speedB);
  motorControl('C', speedC);
  motorControl('D', speedD);
}

void MotorController::rotate(RotateDirection direction, uint8_t speed)
{
  motorControl('A', direction == ROTATE_CLOCKWISE ? speed : -speed);
  motorControl('B', direction == ROTATE_CLOCKWISE ? speed : -speed);
  motorControl('C', direction == ROTATE_CLOCKWISE ? -speed : speed);
  motorControl('D', direction == ROTATE_CLOCKWISE ? -speed : speed);
}

MotorStatus MotorController::getMotorStatus(char motor)
{
  switch (motor)
  {
  case 'A':
    return motorA;
  case 'B':
    return motorB;
  case 'C':
    return motorC;
  case 'D':
    return motorD;
  default:
    return {false, false, 0};
  }
}