#include "AdafruitPWMBoard.hpp"

AdafruitPWMBoard::AdafruitPWMBoard(const uint8_t addr, const uint8_t pwmFreq):
    hwif(addr)
{
    if (!hwif.begin()) {
        good = false;
        return;
    }

    hwif.setPWMFreq(pwmFreq);
    good = true;
}

bool AdafruitPWMBoard::isMotorSlotOccupied(const uint8_t motorIndex) const noexcept
{
    if (motorIndex >= 16) {
        return false;
    }

    return occupiedMotorSlots & (1 << motorIndex);
}

bool AdafruitPWMBoard::occupyMotorSlot(const uint8_t motorIndex) noexcept
{
    if (motorIndex >= 16) {
        return false;
    }

    if (isMotorSlotOccupied(motorIndex)) {
        return false;
    }

    occupiedMotorSlots |= 1 << motorIndex;
    return true;
}

bool AdafruitPWMBoard::releaseMotorSlot(const uint8_t motorIndex) noexcept
{
    if (motorIndex >= 16) {
        return false;
    }

    if (!isMotorSlotOccupied(motorIndex)) {
        return false;
    }

    occupiedMotorSlots &= ~(1 << motorIndex);
    return true;
}