/*
 * control — 电机控制层
 *
 * 开环占空比控制：根据目标速度和偏差计算占空比，直接输出到 PWM。
 * 无编码器反馈，无 PID 闭环。
 */
#include "control.h"
#include "global.h"

extern double g_steer_deviation;

MotorController *motorController[2] = {nullptr, nullptr};
GPIO mortorEN(73);

void ControlInit()
{
    mortorEN.setDirection("out");
    mortorEN.setValue(1);

    // 左电机方向: GPIO12  (右电机方向由 MotorController 自管, 但左In2也一样要设)
    GPIO leftIn2(13);
    leftIn2.setDirection("out");
    leftIn2.setValue(1);

    const int pwmchip[2]  = {8, 8};
    const int pwmnum[2]   = {2, 1};
    const int gpioNum[2]  = {12, 13};
    const unsigned int period_ns = 50000;

    for (int i = 0; i < 2; ++i)
    {
        motorController[i] = new MotorController(
            pwmchip[i], pwmnum[i], gpioNum[i], period_ns
        );
    }
}

void ControlUpdate(double speed, bool zebra_block)
{
    if (zebra_block || !g_cfg.start)
    {
        for (int i = 0; i < 2; ++i)
            if (motorController[i]) motorController[i]->updateduty(0);
        if (!g_cfg.start) mortorEN.setValue(0);
        return;
    }

    // 弯道降速: 偏差越大，速度越低 (最低 60%)
    double curve = 1.0 - std::abs(g_steer_deviation) * 0.4;
    if (curve < 0.6) curve = 0.6;
    double spd = speed * curve;

    for (int i = 0; i < 2; ++i)
        if (motorController[i]) motorController[i]->updateduty(spd);

    mortorEN.setValue(1);
}

void ControlExit()
{
    for (int i = 0; i < 2; ++i)
    {
        delete motorController[i];
        motorController[i] = nullptr;
    }
    mortorEN.setValue(0);
}
