#ifdef ENABLE_LCDCAM_TEST

#include "lt24.h"
#include "stdint.h"
#include "stddef.h"

#include "system.h"

#define WIDTH   320
#define HEIGHT  240
#define NPIXELS WIDTH*HEIGHT
#define PIXELSIZE 2 // 16 bits
#define BUFFER_SIZE NPIXELS / PIXELSIZE // in words
#define BUFFER1 0x10000000
#define BUFFER2 BUFFER1 + BUFFER_SIZE
#define BUFFER3 BUFFER2 + BUFFER_SIZE
#define N_BUFFERS 3

struct BufferManager {
    uint32_t* buffers[N_BUFFERS];

    // Buffer assignment semantics:
    //    if null
    //        => device currently has no assigne buffer
    //        => device is currently now showing/capturing
    uint32_t* lcd_buffer;
    uint32_t* cam_buffer;
};

struct BufferManager bman;
struct LT24 *lcd = LCDCONTROLLER_0_BASE;
// struct Camera cam = CAMERA_ADDRESS;

void setupBuffers(struct BufferManager* bman, uint32_t* b1, uint32_t* b2, uint32_t* b3) {
    bman->buffers[0] = b1;
    bman->buffers[1] = b2;
    bman->buffers[2] = b3;
    bman->lcd_buffer = 0;
    bman->cam_buffer = 0;
}

int main_changeme() {
    setupBuffers(&bman, BUFFER1, BUFFER2, BUFFER3);

    LT24_initialize(lcd);
    // startCamera(&cam);
    while(1) {
    	// TODO: put to sleep...
    }
}

void tryLCDShow() {
    char gotBuffer = 0;
    for (int i = 0; i < N_BUFFERS; i++) {
        if (bman.buffers[i] != 0 && bman.buffers[i] != bman.cam_buffer) {
            bman.lcd_buffer = bman.buffers[i];
            gotBuffer = 1;
            break;
        }
    }
    if (gotBuffer) {
        LT24_refresh(lcd);
    } else {
        bman.lcd_buffer = 0;
    }
}

void tryCamCapture() {
    char gotBuffer = 0;
    for (int i = 0; i < N_BUFFERS; i++) {
        if (bman.buffers[i] != bman.lcd_buffer && bman.buffers[i] != bman.cam_buffer) {
            bman.cam_buffer = bman.buffers[i];
            gotBuffer = 1;
            break;
        }
    }
    if (gotBuffer) {
    	// captureImage(bman.lcd_buffer);
    } else {
        bman.cam_buffer = 0;
    }
}

void IRQ_lcd() {
    bman.lcd_buffer = 0;            // Yield buffer
    tryLCDShow();                   // Locate any ready buffer
    if (bman.cam_buffer == 0) {     // If camera currently inactive, re-try capture (we just yielded a buffer)
        tryCamCapture();
    }
}

void IRQ_camera() {
    tryCamCapture();
    if (bman.lcd_buffer == 0) {
        tryLCDShow();
    }
}

#endif