#include <graphics.h>
#include <math.h>
#include <stdlib.h>

image_data_t image_create(uint16_t width, uint16_t height) {
    image_data_t data;
    data.width = width;
    data.height = height;
    size_t length = width * height * sizeof(image_color_t);
    data.pixels = (image_color_t*)malloc(length);

    memset(data.pixels, 255, length);

    return data;
}

void image_free(image_data_t image) {
    if (image.pixels) {
        free(image.pixels);
    }
}

static inline void set_point(image_data_t image, uint16_t x, uint16_t y, image_color_t color) {
    image.pixels[x + y * image.width] = color;
}

void image_draw_point(image_data_t image, uint16_t x, uint16_t y, image_color_t color) {
    if (x >= image.width || y >= image.height) {
        return;
    }
    set_point(image, x, y, color);
}

void image_draw_hline(image_data_t image, uint16_t x1, uint16_t x2, uint16_t y, image_color_t color) {
    if (x1 > x2) {
        uint16_t temp = x1;
        x1 = x2;
        x2 = temp;
    }
    if (x1 > image.width) {
        return;
    }
    x2 = min(x2, image.width);

    do {
        image_draw_point(image, x1++, y, color);
    } while (x1 < x2);
}

void image_draw_vline(image_data_t image, uint16_t x, uint16_t y1, uint16_t y2, image_color_t color) {
    if (y1 > y2) {
        uint16_t temp = y1;
        y1 = y2;
        y2 = temp;
    }
    if (y1 > image.width) {
        return;
    }
    y2 = min(y2, image.height);

    do {
        image_draw_point(image, x, y1++, color);
    } while (y1 < y2);
}

void image_draw_line(image_data_t image, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, image_color_t color) {
    int dx = abs(x2 - x1);
    int dy = abs(y2 - y1);
    int sx = (x1 < x2) ? 1 : -1;
    int sy = (y1 < y2) ? 1 : -1;
    int err = dx - dy;
    int e2;

    while (1) {
        if (x1 >= image.width || y1 >= image.height) {
            break;
        }
        set_point(image, x1, y1, color);
        if (x1 == x2 && y1 == y2) {
            break;
        }
        e2 = 2 * err;
        if (e2 > -dy) {
            err -= dy;
            x1 += sx;
        }
        if (e2 < dx) {
            err += dx;
            y1 += sy;
        }
    }
}

void image_draw_rect(image_data_t image, uint16_t x, uint16_t y, uint16_t w, uint16_t h, image_color_t color) {
    image_draw_hline(image, x, x + w - 1, y, color);
    image_draw_hline(image, x, x + w, y + h - 1, color);

    image_draw_vline(image, x, y, y + h - 1, color);
    image_draw_vline(image, x + w - 1, y, y + h - 1, color);
}

void image_fill_rect(image_data_t image, uint16_t x, uint16_t y, uint16_t w, uint16_t h, image_color_t color) {
    while (h--) {
        image_draw_hline(image, x, x + w, y++, color);
    };
}