#include "control.h"
#include "GPIO.h"

extern double g_steer_deviation;

MotorController *motorController[2] = {nullptr, nullptr};

GPIO mortorEN(73);

double mortor_kp = 1000;
double mortor_ki = 300;
double mortor_kd = 0;

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

    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 int encoder_pwmchip[2]  = {0, 3};
    const int encoder_gpioNum[2]  = {75, 72};
    const int encoder_dir[2]      = {1, -1};

    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,
            mortor_kp, mortor_ki, mortor_kd, 0,
            encoder_pwmchip[i], encoder_gpioNum[i], encoder_dir[i]
        );
    }
}

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

    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];
        std::cout << "motor" << i << " deleted\n";
    }
    mortorEN.setValue(0);
}
