#include <stdlib.h>
#include <string.h>
#include <sys/types.h>

#include <cammy/photo.h>

static uint32_t bayer_matrix[256] = {
    /*
     * Black gamut
     */
    0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000,

    /*
     * Black to dark gray gradient
     */
    0x00018000, 0x00018000, 0x00018000, 0x00018000, 0x00018000,
    0x00018020, 0x00018020, 0x00018020, 0x00018020, 0x00018020,
    0x000180a0, 0x000180a0, 0x000180a0, 0x000180a0, 0x000180a0,
    0x0001a0a0, 0x0001a0a0, 0x0001a0a0, 0x0001a0a0, 0x0001a0a0,
    0x0001a4a0, 0x0001a4a0, 0x0001a4a0, 0x0001a4a0, 0x0001a4a0, 0x0001a4a0,
    0x0001a4a1, 0x0001a4a1, 0x0001a4a1, 0x0001a4a1, 0x0001a4a1,
    0x0001a4a5, 0x0001a4a5, 0x0001a4a5, 0x0001a4a5, 0x0001a4a5,
    0x0001a5a5, 0x0001a5a5, 0x0001a5a5, 0x0001a5a5, 0x0001a5a5,
    0x0001ada5, 0x0001ada5, 0x0001ada5, 0x0001ada5, 0x0001ada5, 0x0001ada5,
    0x0001ada7, 0x0001ada7, 0x0001ada7, 0x0001ada7, 0x0001ada7,
    0x0001adaf, 0x0001adaf, 0x0001adaf, 0x0001adaf, 0x0001adaf,
    0x0001afaf, 0x0001afaf, 0x0001afaf, 0x0001afaf, 0x0001afaf,
    0x0001df5f, 0x0001df5f, 0x0001df5f, 0x0001df5f, 0x0001df5f, 0x0001df5f,
    0x0001df7f, 0x0001df7f, 0x0001df7f, 0x0001df7f, 0x0001df7f,
    0x0001dfff, 0x0001dfff, 0x0001dfff, 0x0001dfff, 0x0001dfff,
    0x0001ffff, 0x0001ffff, 0x0001ffff, 0x0001ffff, 0x0001ffff,

    /*
     * Dark gray to light gray gradient
     */
    0x01028000, 0x01028000, 0x01028000, 0x01028000, 0x01028000, 0x01028000,
    0x01028020, 0x01028020, 0x01028020, 0x01028020, 0x01028020,
    0x010280a0, 0x010280a0, 0x010280a0, 0x010280a0, 0x010280a0,
    0x0102a0a0, 0x0102a0a0, 0x0102a0a0, 0x0102a0a0, 0x0102a0a0,
    0x0102a4a0, 0x0102a4a0, 0x0102a4a0, 0x0102a4a0, 0x0102a4a0, 0x0102a4a0,
    0x0102a4a1, 0x0102a4a1, 0x0102a4a1, 0x0102a4a1, 0x0102a4a1,
    0x0102a4a5, 0x0102a4a5, 0x0102a4a5, 0x0102a4a5, 0x0102a4a5,
    0x0102a5a5, 0x0102a5a5, 0x0102a5a5, 0x0102a5a5, 0x0102a5a5,
    0x0102ada5, 0x0102ada5, 0x0102ada5, 0x0102ada5, 0x0102ada5, 0x0102ada5,
    0x0102ada7, 0x0102ada7, 0x0102ada7, 0x0102ada7, 0x0102ada7,
    0x0102adaf, 0x0102adaf, 0x0102adaf, 0x0102adaf, 0x0102adaf,
    0x0102afaf, 0x0102afaf, 0x0102afaf, 0x0102afaf, 0x0102afaf,
    0x0102df5f, 0x0102df5f, 0x0102df5f, 0x0102df5f, 0x0102df5f, 0x0102df5f,
    0x0102df7f, 0x0102df7f, 0x0102df7f, 0x0102df7f, 0x0102df7f,
    0x0102dfff, 0x0102dfff, 0x0102dfff, 0x0102dfff, 0x0102dfff,
    0x0102ffff, 0x0102ffff, 0x0102ffff, 0x0102ffff, 0x0102ffff,

    /* Light gray to white gradient */
    0x02038000, 0x02038000, 0x02038000, 0x02038000, 0x02038000, 0x02038000,
    0x02038020, 0x02038020, 0x02038020, 0x02038020, 0x02038020,
    0x020380a0, 0x020380a0, 0x020380a0, 0x020380a0, 0x020380a0,
    0x0203a0a0, 0x0203a0a0, 0x0203a0a0, 0x0203a0a0, 0x0203a0a0,
    0x0203a4a0, 0x0203a4a0, 0x0203a4a0, 0x0203a4a0, 0x0203a4a0, 0x0203a4a0,
    0x0203a4a1, 0x0203a4a1, 0x0203a4a1, 0x0203a4a1, 0x0203a4a1,
    0x0203a4a5, 0x0203a4a5, 0x0203a4a5, 0x0203a4a5, 0x0203a4a5,
    0x0203a5a5, 0x0203a5a5, 0x0203a5a5, 0x0203a5a5, 0x0203a5a5,
    0x0203ada5, 0x0203ada5, 0x0203ada5, 0x0203ada5, 0x0203ada5, 0x0203ada5,
    0x0203ada7, 0x0203ada7, 0x0203ada7, 0x0203ada7, 0x0203ada7,
    0x0203adaf, 0x0203adaf, 0x0203adaf, 0x0203adaf, 0x0203adaf,
    0x0203afaf, 0x0203afaf, 0x0203afaf, 0x0203afaf, 0x0203afaf,
    0x0203df5f, 0x0203df5f, 0x0203df5f, 0x0203df5f, 0x0203df5f, 0x0203df5f,
    0x0203df7f, 0x0203df7f, 0x0203df7f, 0x0203df7f, 0x0203df7f,
    0x0203dfff, 0x0203dfff, 0x0203dfff, 0x0203dfff, 0x0203dfff,
    0x0203ffff, 0x0203ffff, 0x0203ffff, 0x0203ffff, 0x0203ffff,
};

static uint8_t tile_2bpp_palette[4] = {
    0, 86, 171, 255
};

static uint8_t rgb_to_grayscale(uint8_t r, uint8_t g, uint8_t b) {
    return (uint8_t)((0.2126 * (float)r)
                   + (0.7152 * (float)g)
                   + (0.0722 * (float)b));
}

static uint8_t rgb_to_tile_2bpp(uint8_t r, uint8_t g, uint8_t b, size_t x, size_t y) {
    uint8_t gray = rgb_to_grayscale(r, g, b);

    uint32_t slot = bayer_matrix[gray];
     uint8_t from = (slot & 0x03000000) >> 24;
     uint8_t to   = (slot & 0x00030000) >> 16;

    return (slot & (0x8000 >> ((y & 3) << 2) >> (x & 3)))?
        to ^ 3: from ^ 3;
}

static inline uint8_t tile_read(cammy_photo *photo, size_t x, size_t y) {
    cammy_tile *tile = &photo->tiles[y>>3][x>>3];

    int tile_x = x & 7,
        tile_y = y & 7;

    return
        ((tile->data[ tile_y<<1]    & (0x80 >> tile_x)) >> (tile_x ^ 7)
      | ((tile->data[(tile_y<<1)|1] & (0x80 >> tile_x)) >> (tile_x ^ 7) << 1))
      ^ 3;
}

static inline void tile_write(cammy_photo *photo,
                              size_t x, size_t y, uint8_t value) {
    cammy_tile *tile = &photo->tiles[y>>3][x>>3];

    int tile_x = x & 7,
        tile_y = y & 7;

    tile->data[ tile_y<<1]    |=  (value & 0x01)       << (tile_x ^ 7);
    tile->data[(tile_y<<1)|1] |= ((value & 0x02) >> 1) << (tile_x ^ 7);
}

static inline void buf_read(uint8_t *buf,
                            size_t x, size_t y, size_t width,
                            uint8_t *r, uint8_t *g, uint8_t *b,
                            int stride) {
    *r = buf[stride*width*y + stride*x],
    *g = buf[stride*width*y + stride*x + 1],
    *b = buf[stride*width*y + stride*x + 2];
}

static inline void buf_write(uint8_t *buf,
                             size_t x, size_t y, size_t width,
                             uint8_t r, uint8_t g, uint8_t b,
                             int stride) {
    buf[stride*width*y + stride*x]     = r;
    buf[stride*width*y + stride*x + 1] = g;
    buf[stride*width*y + stride*x + 2] = b;
}

void cammy_photo_export(cammy_photo *src, uint8_t *dest, int stride) {
    size_t x, y;

    for (y=0; y<CAMMY_PHOTO_HEIGHT; y++) {
        for (x=0; x<CAMMY_PHOTO_WIDTH; x++) {
            uint8_t rgb = tile_2bpp_palette[tile_read(src, x, y)];

            buf_write(dest, x, y, CAMMY_PHOTO_WIDTH, rgb, rgb, rgb, stride);
        }
    }
}

void cammy_photo_import(cammy_photo *dest, uint8_t *src, int stride) {
    size_t x, y;

    memset(&dest->tiles, '\x00', sizeof(dest->tiles));
    memset(&dest->thumb, '\xff', sizeof(dest->thumb));

    for (y=0; y<CAMMY_PHOTO_HEIGHT; y++) {
        for (x=0; x<CAMMY_PHOTO_WIDTH; x++) {
            uint8_t r, g, b;

            buf_read(src, x, y, CAMMY_PHOTO_WIDTH, &r, &g, &b, stride);

            tile_write(dest, x, y, rgb_to_tile_2bpp(r, g, b, x, y));
        }
    }
}

void cammy_photo_dither(uint8_t *dest,
                        uint8_t *src,
                         size_t  width,
                         size_t  height,
                            int  stride) {
    size_t x, y;

    for (y=0; y<height; y++) {
        for (x=0; x<width; x++) {
            uint8_t r, g, b, gray;

            buf_read(src, x, y, width, &r, &g, &b, stride);

            gray = tile_2bpp_palette[rgb_to_tile_2bpp(r, g, b, x, y)];

            buf_write(dest, x, y, width, gray, gray, gray, stride);
        }
    }
}