#include <interrupt/pic.h>
#include <ports/io.h>
#include <stdint.h>

void sendPicEOI(uint8_t irq) {
    if (irq >= 8) {
        outb(PIC_2_CMD, PIC_EOI);
    }
    outb(PIC_1_CMD, PIC_EOI);
}

void disableIrqs() {
    outb(PIC_2_DATA, PIC_DISABLE);
    outb(PIC_1_DATA, PIC_DISABLE);
}

void setIrqMask(uint8_t irq) {
    uint16_t port;
    uint8_t value;

    if (irq < 8) {
        port = PIC_1_DATA;
    } else {
        irq -= 8;
        port = PIC_2_DATA;
    }

    value = inb(port) | (1 << irq);
    outb(port, value);
}

void unsetIrqMask(uint8_t irq) {
    uint16_t port;
    uint8_t value;

    if (irq < 8) {
        port = PIC_1_DATA;
    } else {
        irq -= 8;
        port = PIC_2_DATA;
    }

    value = inb(port) & ~(1 << irq);
    outb(port, value);
}

void remapPic() {
    uint8_t pic1Mask = inb(PIC_1_DATA);
    uint8_t pic2Mask = inb(PIC_2_DATA);

    // Send Init Control Word 1-4 to both PICs
    // ICW 1: bit 0 - send up to 4 ICWs, bit 4 - Init PIC
    outb(PIC_1_CMD, 0x11);
    ioWait(); // For slower machines
    outb(PIC_2_CMD, 0x11);
    ioWait();

    // ICW 2: Where to map IRW bases in IDT
    outb(PIC_1_DATA, NEW_IRQ_0);
    ioWait();
    outb(PIC_2_DATA, NEW_IRQ_8);
    ioWait();

    // ICW 3: Which IRQ in PIC1 is PIC2; Mapping PIC2 to IRQ 2
    outb(PIC_1_DATA, 0x04);
    ioWait();
    outb(PIC_2_DATA, 0x02);
    ioWait();

    // ICW 4 - Set x86 mode
    outb(PIC_1_DATA, 0x01);
    ioWait();
    outb(PIC_2_DATA, 0x01);
    ioWait();

    outb(PIC_1_DATA, pic1Mask);
    outb(PIC_2_DATA, pic2Mask);
}