#include "motor.hpp"
#include "hal/gpio.hpp"
#include "hal/pwm.hpp"
#include "types.hpp"
#include <cstdio>
#include <algorithm>

static PWM*  pwm_left  = nullptr;
static PWM*  pwm_right = nullptr;
static GPIO* dir_left  = nullptr;
static GPIO* dir_right = nullptr;
static GPIO* left_in2  = nullptr;
static GPIO* mortor_en = nullptr;
static bool  initialized = false;

void motor_init()
{
    if (initialized) return;

    mortor_en = new GPIO(MOTOR_ENABLE_GPIO);
    mortor_en->setDirection("out");
    mortor_en->setValue(1);

    left_in2 = new GPIO(MOTOR_LEFT_IN2_GPIO);
    left_in2->setDirection("out");
    left_in2->setValue(0);

    pwm_left  = new PWM(MOTOR_PWMCHIP, MOTOR_LEFT_PWMID,  MOTOR_PWM_PERIOD_NS);
    pwm_right = new PWM(MOTOR_PWMCHIP, MOTOR_RIGHT_PWMID, MOTOR_PWM_PERIOD_NS);
    pwm_left->enable();
    pwm_right->enable();

    dir_left  = new GPIO(MOTOR_LEFT_DIR_GPIO);
    dir_right = new GPIO(MOTOR_RIGHT_DIR_GPIO);
    dir_left->setDirection("out");
    dir_right->setDirection("out");

    initialized = true;
}

void motor_set_pwm(int idx, float duty)
{
    if (!initialized) return;
    duty = std::clamp(duty, -100.0f, 100.0f);

    PWM*  pwm  = (idx == MOTOR_LEFT) ? pwm_left  : pwm_right;
    GPIO* dir  = (idx == MOTOR_LEFT) ? dir_left  : dir_right;

    if (duty >= 0)
    {
        dir->setValue(1);
        unsigned int d_ns = static_cast<unsigned int>(MOTOR_PWM_PERIOD_NS * duty / 100.0f);
        pwm->setDutyCycle(d_ns);
    }
    else
    {
        dir->setValue(0);
        unsigned int d_ns = static_cast<unsigned int>(MOTOR_PWM_PERIOD_NS * (-duty) / 100.0f);
        pwm->setDutyCycle(d_ns);
    }
}

void motor_stop_all()
{
    motor_set_pwm(MOTOR_LEFT, 0);
    motor_set_pwm(MOTOR_RIGHT, 0);
}

void motor_enable(bool on)
{
    if (mortor_en) mortor_en->setValue(on ? 1 : 0);
}