#include "drill_safe_gpio.h"

static bool port_valid(const drill_safe_gpio_port *port) {
    return port && port->moder && port->otyper && port->ospeedr &&
           port->pupdr && port->bsrr &&
           (port->high_mask & (uint16_t)~port->output_mask) == 0u;
}

bool drill_safe_gpio_validate(const drill_safe_gpio_set *set) {
    if (!set) return false;
    for (uint32_t i = 0u; i < DRILL_SAFE_GPIO_PORTS; ++i) {
        if (!port_valid(&set->port[i])) return false;
    }
    return true;
}

bool drill_safe_gpio_preload(const drill_safe_gpio_set *set) {
    if (!drill_safe_gpio_validate(set)) return false;
    for (uint32_t i = 0u; i < DRILL_SAFE_GPIO_PORTS; ++i) {
        const drill_safe_gpio_port *port = &set->port[i];
        const uint16_t low_mask = (uint16_t)(port->output_mask & (uint16_t)~port->high_mask);
        *port->bsrr = (uint32_t)port->high_mask | ((uint32_t)low_mask << 16u);
    }
    return true;
}

bool drill_safe_gpio_configure_outputs(const drill_safe_gpio_set *set) {
    if (!drill_safe_gpio_validate(set)) return false;
    for (uint32_t i = 0u; i < DRILL_SAFE_GPIO_PORTS; ++i) {
        const drill_safe_gpio_port *port = &set->port[i];
        for (uint32_t pin = 0u; pin < 16u; ++pin) {
            const uint32_t bit = 1u << pin;
            if ((port->output_mask & bit) == 0u) continue;
            const uint32_t shift = pin * 2u;
            *port->otyper &= ~bit;
            *port->ospeedr &= ~(3u << shift);
            *port->pupdr &= ~(3u << shift);
            *port->moder = (*port->moder & ~(3u << shift)) | (1u << shift);
        }
    }
    return true;
}
