#include <Arduino.h>
#include <SimpleFOC.h>
/* =====================================================================
 * SimpleFOC 开环步进 — Nucleo-G474RE + A4950 + 42步进
 * 兼容: motor_control.py Commander + Industrial_ui gimbaldll
 *
 * gimbaldll 串口习惯（从 DLL 字符串还原）:
 *   open:  AE1 / BE1
 *   speed: %cE1  %cC1  %c<vel>
 *   pos:   %cC2  %c<pos> <speed?>
 * Commander: C1=velocity → 映射 velocity_openloop
 *            C2=angle    → 映射 angle_openloop（无编码器）
 *
 * A4950: AIN1=5 AIN2=6 BIN1=9 BIN2=10；A/B 前缀同一台电机
 * 串口: VCP USART2 PA2/PA3 → COM14 @ 115200
 * ===================================================================== */

const uint8_t PIN_AIN1 = 5;
const uint8_t PIN_AIN2 = 6;
const uint8_t PIN_BIN1 = 9;
const uint8_t PIN_BIN2 = 10;

const int   STEPPER_STEPS  = 200;
const float SUPPLY_VOLTAGE = 12.0f;
const float VOLTAGE_LIMIT  = 4.5f;
const float MAX_TARGET_VEL = 20.0f;
const float TARGET_RAMP_RPS = 8.0f;

StepperMotor motor(STEPPER_STEPS);
StepperDriver4PWM driver(PIN_AIN1, PIN_AIN2, PIN_BIN1, PIN_BIN2);
Commander command = Commander(Serial);

float cmdTarget = 0.0f;
float rampedTarget = 0.0f;
bool  velRampEnable = true;   // 仅速度模式做斜坡

static void forceOpenLoop() {
    // 无 sensor：把闭环模式改成对应开环，否则 move() 不驱动
    if (motor.controller == MotionControlType::velocity)
        motor.controller = MotionControlType::velocity_openloop;
    else if (motor.controller == MotionControlType::angle)
        motor.controller = MotionControlType::angle_openloop;
}

static bool isVelMode() {
    return motor.controller == MotionControlType::velocity ||
           motor.controller == MotionControlType::velocity_openloop;
}

void onMotorCmd(char* cmd) {
    command.motor(&motor, cmd);
    forceOpenLoop();
    velRampEnable = isVelMode();
    if (velRampEnable) {
        cmdTarget = motor.target;
    }
    // 角度模式：斜坡状态与目标对齐，避免回切速度时突跳
    rampedTarget = motor.target;
}

void setup() {
    Serial.begin(115200);
    delay(400);

    Serial.println();
    Serial.println(F("========== SimpleFOC STEPPER + A4950 =========="));
    Serial.println(F("open-loop + gimbal Commander (C1/C2 remap)"));
    Serial.println(F("cmd: AE1  A2  AC2 +A0.17  or Industrial_ui"));

    driver.voltage_power_supply = SUPPLY_VOLTAGE;
    driver.voltage_limit = VOLTAGE_LIMIT;
    driver.pwm_frequency = 20000;
    driver.init();

    motor.linkDriver(&driver);
    motor.controller = MotionControlType::velocity_openloop;
    motor.voltage_limit = VOLTAGE_LIMIT;
    motor.velocity_limit = MAX_TARGET_VEL;

    motor.PID_velocity.P = 0.2f;
    motor.PID_velocity.I = 1.0f;
    motor.PID_velocity.D = 0.0f;
    motor.PID_velocity.output_ramp = 50.0f;
    motor.LPF_velocity.Tf = 0.02f;

    motor.init();
    motor.disable();

    command.add('A', onMotorCmd, (char*)"stepper");
    command.add('B', onMotorCmd, (char*)"stepper_alias");

    Serial.println(F("Ready. AE1 then A2 / industrial SingleToPosition"));
    Serial.println(F("========================================"));
}

void loop() {
    static uint32_t lastUs = micros();
    uint32_t now = micros();
    float dt = (now - lastUs) * 1e-6f;
    lastUs = now;
    if (dt > 0.05f) dt = 0.05f;

    if (velRampEnable && isVelMode()) {
        float maxStep = TARGET_RAMP_RPS * dt;
        float err = cmdTarget - rampedTarget;
        if (err > maxStep) err = maxStep;
        if (err < -maxStep) err = -maxStep;
        rampedTarget += err;
        motor.target = rampedTarget;
    }
    // 角度模式：保留 Commander 写入的 motor.target，不做速度斜坡覆盖

    motor.move();
    command.run();
}