#include "drill_safe_gpio.h"

#include <assert.h>
#include <stdio.h>
#include <string.h>

typedef struct {
    uint32_t moder;
    uint32_t otyper;
    uint32_t ospeedr;
    uint32_t pupdr;
    uint32_t bsrr;
} fake_gpio;

static void bind_port(drill_safe_gpio_port *port, fake_gpio *regs,
                      uint16_t outputs, uint16_t high) {
    port->moder = &regs->moder;
    port->otyper = &regs->otyper;
    port->ospeedr = &regs->ospeedr;
    port->pupdr = &regs->pupdr;
    port->bsrr = &regs->bsrr;
    port->output_mask = outputs;
    port->high_mask = high;
}

static void check_port(const drill_safe_gpio_port *port,
                       const fake_gpio *before, const fake_gpio *after) {
    uint32_t mode_mask = 0u;
    uint32_t expected_mode = before->moder;
    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;
        mode_mask |= 3u << shift;
        expected_mode = (expected_mode & ~(3u << shift)) | (1u << shift);
    }
    assert(after->moder == expected_mode);
    assert((after->otyper & port->output_mask) == 0u);
    assert((after->ospeedr & mode_mask) == 0u);
    assert((after->pupdr & mode_mask) == 0u);
    assert(((after->otyper ^ before->otyper) & ~(uint32_t)port->output_mask) == 0u);
    assert(((after->ospeedr ^ before->ospeedr) & ~mode_mask) == 0u);
    assert(((after->pupdr ^ before->pupdr) & ~mode_mask) == 0u);
    const uint16_t low = (uint16_t)(port->output_mask & (uint16_t)~port->high_mask);
    assert(after->bsrr == ((uint32_t)port->high_mask | ((uint32_t)low << 16u)));
}

int main(void) {
    fake_gpio regs[DRILL_SAFE_GPIO_PORTS];
    fake_gpio before[DRILL_SAFE_GPIO_PORTS];
    drill_safe_gpio_set set;
    memset(&set, 0, sizeof(set));

    for (uint32_t i = 0u; i < DRILL_SAFE_GPIO_PORTS; ++i) {
        regs[i].moder = 0xAAAAAAAAu;
        regs[i].otyper = 0xFFFFu;
        regs[i].ospeedr = 0xFFFFFFFFu;
        regs[i].pupdr = 0xFFFFFFFFu;
        regs[i].bsrr = 0xA5A5A5A5u;
    }
    bind_port(&set.port[0], &regs[0],
              (1u << 5u) | (1u << 7u) | (1u << 8u) |
              (1u << 9u) | (1u << 10u) | (1u << 12u),
              (1u << 12u)); /* PA12 CHARGE_EN_N must idle high. */
    bind_port(&set.port[1], &regs[1], (1u << 0u) | (1u << 1u), 0u);
    bind_port(&set.port[2], &regs[2], (1u << 14u), 0u);
    memcpy(before, regs, sizeof(regs));

    assert(drill_safe_gpio_validate(&set));
    assert(drill_safe_gpio_preload(&set));
    for (uint32_t i = 0u; i < DRILL_SAFE_GPIO_PORTS; ++i) {
        assert(regs[i].moder == before[i].moder);
        assert(regs[i].bsrr != before[i].bsrr);
    }
    assert(drill_safe_gpio_configure_outputs(&set));
    for (uint32_t i = 0u; i < DRILL_SAFE_GPIO_PORTS; ++i)
        check_port(&set.port[i], &before[i], &regs[i]);

    drill_safe_gpio_set invalid = set;
    invalid.port[1].high_mask |= (1u << 4u);
    memcpy(before, regs, sizeof(regs));
    assert(!drill_safe_gpio_validate(&invalid));
    assert(!drill_safe_gpio_preload(&invalid));
    assert(!drill_safe_gpio_configure_outputs(&invalid));
    assert(memcmp(before, regs, sizeof(regs)) == 0);

    puts("PASS: drill safe GPIO latch-before-output sequence and pin masks");
    return 0;
}
