#include "drivers/pic.hpp"

namespace
{
inline constexpr std::uint8_t PIC_CMD_INIT = 0x10;
inline constexpr std::uint8_t PIC_CMD_READ_IRR = 0x0a;
inline constexpr std::uint8_t PIC_CMD_READ_ISR = 0x0b;
inline constexpr std::uint8_t PIC_INIT_WITH_ICW4 = 0x01;
inline constexpr std::uint8_t PIC_CFG_8086 = 0x01;

void picWriteCmd(std::uint16_t port, std::uint8_t command) noexcept
{
    writePort8(port, command);
}

[[nodiscard]] std::uint8_t picReadResponse(std::uint8_t port) noexcept
{
    return readPort8(port);
}

void picWriteData(std::uint16_t port, std::uint8_t data) noexcept
{
    writePort8(port + 1, data);
}

[[nodiscard]] std::uint8_t picReadData(std::uint16_t port) noexcept
{
    return readPort8(port + 1);
}

[[nodiscard]] std::uint16_t readPICIRQRegisters(std::uint8_t readCommand) noexcept
{
    picWriteCmd(PORT_PIC1, readCommand);
    picWriteCmd(PORT_PIC2, readCommand);
    return (picReadResponse(PORT_PIC1) << 8) | picReadResponse(PORT_PIC2);
}
}

bool initPICs(std::uint8_t masterOffset, std::uint8_t slaveOffset) noexcept
{
    if (!isValidPICOffset(masterOffset) || !isValidPICOffset(slaveOffset))
    {
        return false;
    }

    const uint8_t mask1 = picReadData(PORT_PIC1);
    const uint8_t mask2 = picReadData(PORT_PIC2);

    // ICW1 - init command
    picWriteCmd(PORT_PIC1, PIC_CMD_INIT | PIC_INIT_WITH_ICW4);
    ioWait();
    picWriteCmd(PORT_PIC2, PIC_CMD_INIT | PIC_INIT_WITH_ICW4);
    ioWait();

    // ICW2 - offset
    picWriteData(PORT_PIC1, masterOffset);
    ioWait();
    picWriteData(PORT_PIC2, slaveOffset);
    ioWait();

    // ICW3 - master/slave config
    picWriteData(PORT_PIC1, (1 << 2)); // there is a slave at IRQ2
    ioWait();
    picWriteData(PORT_PIC2, 2); // you are slave no. 2
    ioWait();

    // ICW3 - additional config
    picWriteData(PORT_PIC1, PIC_CFG_8086);
    ioWait();
    picWriteData(PORT_PIC2, PIC_CFG_8086);
    ioWait();

    // restore masks
    picWriteData(PORT_PIC1, mask1);
    picWriteData(PORT_PIC2, mask2);

    return true;
}

__attribute__((no_caller_saved_registers))
void sendEndOfInterrupt(std::uint8_t irq) noexcept
{
    if (irq >= 8) {
        writePort8(PORT_PIC2, PIC_CMD_EOI);
    }
    writePort8(PORT_PIC1, PIC_CMD_EOI);
}

void maskIRQ(std::uint8_t irq) noexcept
{
    std::uint16_t port = PORT_PIC1;
    if (irq >= 8)
    {
        port = PORT_PIC2;
        irq -= 8;
    }
    const std::uint8_t currentMask = picReadData(port);
    picWriteData(port, currentMask | (1 << irq));
}

void unmaskIRQ(std::uint8_t irq) noexcept
{
    std::uint16_t port = PORT_PIC1;
    if (irq >= 8)
    {
        port = PORT_PIC2;
        irq -= 8;
    }
    const std::uint8_t currentMask = picReadData(port);
    picWriteData(port, currentMask & ~(1 << irq));
}

std::uint16_t readPICIRR() noexcept
{
    return readPICIRQRegisters(PIC_CMD_READ_IRR);
}

std::uint16_t readPICISR() noexcept
{
    return readPICIRQRegisters(PIC_CMD_READ_ISR);
}