//
// Created by bruno on 2.2.2025.
//

#include "memory.h"

#define SFR_OFFSET (0xDF00)

#define GPU_OFFSET (0xEF00)

#define GPU_SIZE (160 * 160)

#define GPU_END (GPU_OFFSET + GPU_SIZE)

uint8_t write_mem(CPU *cpu, uint32_t addr, uint8_t value) {
    if (addr >= MEM_SIZE) {
        return 1;
    }

    switch (addr) {

        case SFR_OFFSET + 0x00:
            pcm_buffer_push(&audioData.pcmVoice, value << 8 | cpu->memory[SFR_OFFSET + 0x01]);
            break;

        case SFR_OFFSET + 0x02:
            audioData.synthVoices[0].volume = value;
            break;

        case SFR_OFFSET + 0x03:
            audioData.synthVoices[0].waveform = value;
            break;

        case SFR_OFFSET + 0x04:
            audioData.synthVoices[0].phase = value;
            break;

        case SFR_OFFSET + 0x05:
            audioData.synthVoices[0].frequency = value << 8 | cpu->memory[SFR_OFFSET + 0x06];
            break;

        case SFR_OFFSET + 0x07:
            audioData.synthVoices[1].volume = value;
            break;

        case SFR_OFFSET + 0x08:
            audioData.synthVoices[1].waveform = value;
            break;

        case SFR_OFFSET + 0x09:
            audioData.synthVoices[1].phase = value;
            break;

        case SFR_OFFSET + 0x0A:
            audioData.synthVoices[1].frequency = value << 8 | cpu->memory[SFR_OFFSET + 0x0B];
            break;

        case SFR_OFFSET + 0x0C:
            audioData.synthVoices[2].volume = value;
            break;

        case SFR_OFFSET + 0x0D:
            audioData.synthVoices[2].waveform = value;
            break;

        case SFR_OFFSET + 0x0E:
            audioData.synthVoices[2].phase = value;
            break;

        case SFR_OFFSET + 0x0F:
            audioData.synthVoices[2].frequency = value << 8 | cpu->memory[SFR_OFFSET + 0x10];
            break;

        case SFR_OFFSET + 0x12:
            displayA.value = value;
            break;

        case SFR_OFFSET + 0x13:
            displayB.value = value;
            break;

        case SFR_OFFSET + 0x14:
            displayC.value = value;
            break;

        case SFR_OFFSET + 0x15:
            displayD.value = value;
            break;

        case SFR_OFFSET + 0x16:
            displayE.value = value;
            break;

        case SFR_OFFSET + 0x17:
            displayF.value = value;
            break;

        case SFR_OFFSET + 0x18:
            displayG.value = value;
            break;

        case SFR_OFFSET + 0x19:
            displayH.value = value;
            break;

        default:
            break;
    }

    if (addr >= GPU_OFFSET && addr < GPU_END) {
        const int index = addr - GPU_OFFSET;
        int x = index % 160;
        int y = index / 160;

        // Proper color scaling from 3-bit and 2-bit channels
        Uint8 r = ((value & 0xE0) >> 5) * 36;  // 0–7 mapped to 0–255
        Uint8 g = ((value & 0x1C) >> 2) * 36;
        Uint8 b = ((value & 0x03)) * 85;  // 0–3 mapped to 0–255

        // Ensure texture format matches RGBA8888
        Uint32 color = (255 << 24) | (r << 16) | (g << 8) | b;

        Uint32 *pixels = (Uint32 *) gpuSurf->pixels;
        pixels[(y * gpuSurf->w) + x] = color;
        gpuSurfDirty = true;
    }

    cpu->memory[addr] = value;
    return 0;
}

uint8_t read_mem(CPU *cpu, uint32_t addr) {
    if (addr >= MEM_SIZE) {
        return 0;
    }
    switch (addr) {

        case SFR_OFFSET + 0x20:
            return (switchesA.value >> 8) & 0xFF;

        case SFR_OFFSET + 0x21:
            return switchesA.value & 0xFF;

        case SFR_OFFSET + 0x22:
            return (switchesB.value >> 8) & 0xFF;

        case SFR_OFFSET + 0x23:
            return switchesB.value & 0xFF;

        case SFR_OFFSET + 0x24:
            return (switchesC.value >> 8) & 0xFF;

        case SFR_OFFSET + 0x25:
            return switchesC.value & 0xFF;

        case SFR_OFFSET + 0x26:
            return (switchesD.value >> 8) & 0xFF;

        case SFR_OFFSET + 0x27:
            return switchesD.value & 0xFF;

        default:
            return cpu->memory[addr];
    }
}

uint16_t read_mem16(CPU *cpu, uint32_t addr) {
    return read_mem(cpu, addr) | (read_mem(cpu, addr + 1) << 8);
}

uint32_t read_mem32(CPU *cpu, uint32_t addr) {
    return read_mem16(cpu, addr) | (read_mem16(cpu, addr + 2) << 16);
}

uint32_t read_mem24(CPU *cpu, uint32_t addr) {
    return read_mem16(cpu, addr) | (read_mem(cpu, addr + 2) << 16);
}

uint32_t read_address_argument(CPU *cpu) {
    uint32_t out = read_mem24(cpu, cpu->pc);
    cpu->pc += 3;
    if (out >= MEM_SIZE) {
        out = MEM_SIZE - 1;
    }
    return out;
}

uint8_t read_register_argument(CPU *cpu) {
    uint8_t out = read_mem(cpu, cpu->pc);
    cpu->pc += 1;
    if (out >= REG_COUNT) {
        out = REG_COUNT - 1;
    }
    return out;
}

// Push a 32-bit program counter (PC) onto the stack
void write_stack(CPU *cpu) {
    if (cpu->pc >= MEM_SIZE) {
        return; // Invalid memory address
    }
    if (cpu->stack_ptr >= STACK_SIZE - 1) {
        return; // Stack overflow protection
    }
    cpu->stack[++cpu->stack_ptr] = cpu->pc;
    for (uint32_t reg = 0; reg < REG_COUNT; reg += 4) {
        uint32_t packed = cpu->regs[reg] |
                          ((reg + 1 < REG_COUNT) ? (cpu->regs[reg + 1] << 8) : 0) |
                          ((reg + 2 < REG_COUNT) ? (cpu->regs[reg + 2] << 16) : 0);
        cpu->stack[++cpu->stack_ptr] = packed;
    }
    cpu->stack[++cpu->stack_ptr] = cpu->flags;
}

// Pop a 32-bit program counter (PC) and registers from the stack
void read_stack(CPU *cpu) {
    if (cpu->stack_ptr == 0) {
        return; // Stack underflow protection
    }

    // Restore flags
    cpu->flags = cpu->stack[cpu->stack_ptr--];

    // Restore registers
    for (int32_t reg = REG_COUNT - 1; reg >= 0; reg -= 4) {
        if (cpu->stack_ptr == 0) {
            return; // Stack underflow protection
        }
        uint32_t packed = cpu->stack[cpu->stack_ptr--];
        cpu->regs[reg] = (packed & 0xFF);
        if (reg - 1 >= 0) cpu->regs[reg - 1] = ((packed >> 8) & 0xFF);
        if (reg - 2 >= 0) cpu->regs[reg - 2] = ((packed >> 16) & 0xFF);
    }

    // Restore PC
    if (cpu->stack_ptr == 0) {
        return; // Stack underflow protection
    }
    cpu->pc = cpu->stack[cpu->stack_ptr--];
}


uint8_t read_reg_number(CPU *cpu) {
    uint8_t out = read_mem(cpu, cpu->pc++);
    if (out >= REG_COUNT) {
        return 0;
    }
    return out;
}

uint8_t read_reg(CPU *cpu, uint8_t number) {
    if (number >= REG_COUNT) {
        return 0;
    }
    return cpu->regs[number];
}

uint8_t write_reg(CPU *cpu, uint8_t number, uint8_t value) {
    if (number >= REG_COUNT) {
        return 1;
    }
    cpu->regs[number] = value;
    return 0;
}

uint8_t write_mem16(CPU *cpu, uint32_t addr, uint16_t value) {
    uint8_t status = write_mem(cpu, addr, value & 0xFF);
    if (status) {
        return status;
    }
    status = write_mem(cpu, addr + 1, (value >> 8) & 0xFF);
    if (status) {
        return status;
    }
    return 0;
}

uint8_t write_mem32(CPU *cpu, uint32_t addr, uint32_t value) {
    uint8_t status = write_mem16(cpu, addr, value & 0xFFFF);
    if (status) {
        return status;
    }
    status = write_mem16(cpu, addr + 2, (value >> 16) & 0xFFFF);
    if (status) {
        return status;
    }
    return 0;
}
