#include <mygba.h>
#include <stdlib.h>

#define REG_DISPCNT *(unsigned long*)0x4000000
#define MODE_3 0x3
#define BG2_ENABLE 0x400
#define setMode(mode) REG_DISPCNT = (mode)
#define RGB(r, g, b) ((r) + (g << 5) + (b << 10))

MULTIBOOT

const unsigned short NUM_ROWS = 8, NUM_COLS = 8, CELL_SIZE = 20;
const unsigned short BOARD_BLACK = 0x0000, BOARD_WHITE = 0xFFFF;

unsigned short* videoBuffer = (unsigned short*)0x6000000;
unsigned short currentMode;

void circle (int centreX, int centreY, int radius, unsigned short colour);
void clearScreen ();
void fillScreen (unsigned short colour);
void line (int startX, int startY, int endX, int endY, unsigned short colour);
void rect (int startX, int startY, int endX, int endY, unsigned short colour);
void rectFill (int startX, int startY, int endX, int endY, unsigned short colour);
void setPixel (int x, int y, unsigned short colour);

void circle (int centreX, int centreY, int radius, unsigned short colour)
{
    
    int x = 0, y = radius;
    int p = 3 - 2 * radius;
    
    while (x <= y)
    {
        
        setPixel(centreX + x, centreY + y, colour);
        setPixel(centreX - x, centreY + y, colour);
        setPixel(centreX + x, centreY - y, colour);
        setPixel(centreX - x, centreY - y, colour);
        setPixel(centreX + y, centreY + x, colour);
        setPixel(centreX - y, centreY + x, colour);
        setPixel(centreX + y, centreY - x, colour);
        setPixel(centreX - y, centreY - x, colour);
        
        if (p < 0)
        {
            p += 4 * x++ + 6;
        }
        else
        {
            p += 4 * (x++ - y--) + 10;
        }
        
    }
    
}

void clearScreen ()
{
    rectFill(0, 0, 240, 160, 0xFFFF);
}

void fillScreen (unsigned short colour)
{
    rectFill(0, 0, 240, 160, colour);
}

void line (int startX, int startY, int endX, int endY, unsigned short colour)
{
    
    int i, deltaX, deltaY, numPixels;
    int d, dinc1, dinc2;
    int x, xinc1, xinc2;
    int y, yinc1, yinc2;
    
    deltaX = abs(endX - startX);
    deltaY = abs(endY - startY);
    
    if (deltaX >= deltaY)
    {
        
        numPixels = deltaX + 1;
        d = (2 * deltaY) - deltaX;
        
        dinc1 = deltaY << 1;
        dinc2 = (deltaY - deltaX) << 1;
        
        xinc1 = 1;
        xinc2 = 1;
        yinc1 = 0;
        yinc2 = 1;
        
    }
    else
    {
        
        numPixels = deltaY + 1;
        d = (2 * deltaX) - deltaY;
        
        dinc1 = deltaX << 1;
        dinc2 = (deltaX - deltaY) << 1;

        xinc1 = 0;
        xinc2 = 1;
        yinc1 = 1;
        yinc2 = 1;
        
    }
    
    if (startX > endX)
    {
        
        xinc1 = -xinc1;
        xinc2 = -xinc2;
        
    }
    
    if (startY > endY)
    {
        
        yinc1 = -yinc1;
        yinc2 = -yinc2;
        
    }
    
    x = startX;
    y = startY;
    
    for (i = 0; i < numPixels; i++)
    {
        
        setPixel(x, y, colour);
        
        if (d < 0)
        {
            
            d += dinc1;
            x += xinc1;
            y += yinc1;
            
        }
        else
        {
            
            d += dinc2;
            x += xinc2;
            y += yinc2;
            
        }
        
    }
    
}

void rect (int startX, int startY, int endX, int endY, unsigned short colour)
{
    
    int x, y;
    
    for (x = startX; x < endX; x++)
    {
        
        setPixel(x, startY, colour);
        setPixel(x, (endY - 1), colour);
        
    }
    
    for (y = startY; y < endY; y++)
    {
        
        setPixel(startX, y, colour);
        setPixel((endX - 1), y, colour);
        
    }

}

void rectFill (int startX, int startY, int endX, int endY, unsigned short colour)
{
    
    int x, y;

    for (y = startY; y < endY; y++)
    {

        for (x = startX; x < endX; x++)
        {
            setPixel(x, y, colour);
        }

    }
    
}

void setPixel (int x, int y, unsigned short colour)
{
    
    if (currentMode == MODE_3)
    {
        videoBuffer[y * 240 + x] = colour;
    }
    
}

int main ()
{
    
    int row, col;
    unsigned short colour;
    
    currentMode = MODE_3;
    
    setMode(MODE_3 | BG2_ENABLE);
    
    fillScreen(RGB(0, 0, 255));
    
    for (row = 0; row < NUM_ROWS; row++)
    {
        
        for (col = 0; col < NUM_COLS; col++)
        {
            
            if ((col + 1) % 2 == (row + 1) % 2)
            {
                colour = BOARD_WHITE;
            }
            else
            {
                colour = BOARD_BLACK;
            }
            
            rectFill(col * CELL_SIZE, row * CELL_SIZE, (col + 1) * CELL_SIZE, (row + 1) * CELL_SIZE, colour);
            
            if (row < 3 && !((col + 1) % 2 == (row + 1) % 2))
            {
                colour = 0xFFFF;
            }
            else if (row > 4 && ((col + 1) % 2 == (row + 1) % 2))
            {
                colour = 0x0000;
            }
            else
            {
                continue;
            }

            circle(((col * CELL_SIZE) + 9), ((row * CELL_SIZE) + 9), 9, colour);
            
        }
        
    }

   while (1)
   {
   }

   return 0;
   
}

/* END OF FILE */