#include "nds.h"
#include "memory.h"

static u8 heap[HEAP_SIZE][256];  // the page heap.
static u8 unallocated[256];      // page to use for all unallocated reads and writes.
static u8 heap_index[HEAP_SIZE];

u8 mem_read1(MemoryDevice *mem) {
    u8 output = mem->point1[mem->byte1++];
    if (mem->byte1 == 0) {
        mem->page1++;
        mem_get_page1(mem);
    }
    return output;
}

u8 mem_read2(MemoryDevice *mem) {
    u8 output = mem->point2[mem->byte2++];
    if (mem->byte2 == 0) {
        mem->page2++;
        mem_get_page2(mem);
    }
    return output;
}

void mem_write1(MemoryDevice *mem, u8 value) {
    mem->point1[mem->byte1++] = value;
    if (mem->byte1 == 0) {
        mem->page1++;
        mem_get_page1(mem);
    }
}

void mem_write2(MemoryDevice *mem, u8 value) {
    mem->point2[mem->byte2++] = value;
    if (mem->byte2 == 0) {
        mem->page2++;
        mem_get_page2(mem);
    }
}


void mem_load_cache1(MemoryDevice *mem) {
    // Set all cached pointers to unallocated.
    for (int i=0; i<256; i++) {
        mem->cache1[i] = (u8*) &unallocated;
    }
    // Iterate over all heap pages, gather our ones.
    int count = 0;
    int cached = 0;
    for (int i=0; i<HEAP_SIZE; i++) {
        if (heap_index[i] == mem->id) {
            if (count >= mem->offset1) {
                mem->cache1[cached] = (u8*) &heap[i];
                cached++;
            }
            count++;
            if (cached == 256) break;
        }
    }
}

void mem_load_cache2(MemoryDevice *mem) {
    // Set all cached pointers to unallocated.
    for (int i=0; i<256; i++) {
        mem->cache2[i] = (u8*) &unallocated;
    }
    // Iterate over all heap pages, gather our ones.
    int count = 0;
    int cached = 0;
    for (int i=0; i<HEAP_SIZE; i++) {
        if (heap_index[i] == mem->id) {
            if (count >= mem->offset2) {
                mem->cache2[cached] = (u8*) &heap[i];
                cached++;
            }
            count++;
            if (cached == 256) break;
        }
    }
}

// Fetch the page1 pointer from cache1.
void mem_get_page1(MemoryDevice *mem) {
    mem->point1 = mem->cache1[mem->page1];
}

// Fetch the page2 pointer from cache2.
void mem_get_page2(MemoryDevice *mem) {
    mem->point2 = mem->cache2[mem->page2];
}

void mem_allocate(MemoryDevice *mem) {
    int count = 0;

    // ALLOCATING
    if (mem->count_write > mem->count) {
        int to_allocate = mem->count_write - mem->count;
        for (int i=0; i<HEAP_SIZE; i++) {
            if (heap_index[i] == 0) {
                heap_index[i] = mem->id;
                count++;
                if (count == to_allocate) {
                    break;
                }
            }
        }
    // DEALLOCATING
    } else if (mem->count_write < mem->count) {
        for (int i=0; i<HEAP_SIZE; i++) {
            if (heap_index[i] == mem->id) {
                count++;
                if (count > mem->count_write) {
                    heap_index[i] = 0;
                    memset(heap[i], 0, 256);
                }
            }
        }
    }

    // Count the number of pages allocated to us.
    mem->count = 0;
    for (int i=0; i<HEAP_SIZE; i++) {
        if (heap_index[i]==mem->id) mem->count++;
    }
    // Reload the pointer caches for each head.
    mem_load_cache1(mem);
    mem_load_cache2(mem);
}


void mem_do_copy(MemoryDevice *mem) {
    int src = mem->offset2;
    int dest = mem->offset1;
    if (src == dest) return;
    for (int i=0; i<mem->copy_write; i++) {
        if (src>=mem->count || dest>=mem->count) {
            return;
        }
        memcpy(&heap[dest++], &heap[src++], 256);
    }
}