#include "sms_pad.h"

void pad_init(Pad *pad, Tristate *TH)
{
    pad->pressed = 0xff;
    pad->TH = TH;
}

void pad_setbtn(Pad *pad, PAD_BTN btn, bool pressed)
{
    if (pressed) {
        pad->pressed &= ~(1 << btn);
    } else {
        pad->pressed |= (1 << btn);
    }
}

uint8_t pad_rd(Pad *pad)
{
    uint8_t res;
    if (*pad->TH == TRI_LOW) { // TH selected
        // A and START shifted in from bits 7:6 into 5:4
        res = (pad->pressed & 0xf) | ((pad->pressed & 0xc0) >> 2);
    } else {
        res = pad->pressed & 0x3f;
    }
    // Bits 7:6 are always high
    res |= 0b11000000;
    return res;
}