#include <math.h>
#include <stddef.h>
#include "ibm.h"
#include "device.h"
#include "mem.h"
#include "thread.h"
#include "video.h"
#include "vid_svga.h"
#include "vid_voodoo.h"
#include "vid_voodoo_common.h"
#include "vid_voodoo_blitter.h"
#include "vid_voodoo_dither.h"
#include "vid_voodoo_regs.h"
#include "vid_voodoo_render.h"

enum {
        BLIT_COMMAND_SCREEN_TO_SCREEN = 0,
        BLIT_COMMAND_CPU_TO_SCREEN = 1,
        BLIT_COMMAND_RECT_FILL = 2,
        BLIT_COMMAND_SGRAM_FILL = 3
};

enum {
        BLIT_SRC_1BPP = (0 << 3),
        BLIT_SRC_1BPP_BYTE_PACKED = (1 << 3),
        BLIT_SRC_16BPP = (2 << 3),
        BLIT_SRC_24BPP = (3 << 3),
        BLIT_SRC_24BPP_DITHER_2X2 = (4 << 3),
        BLIT_SRC_24BPP_DITHER_4X4 = (5 << 3)
};

enum { BLIT_SRC_RGB_ARGB = (0 << 6), BLIT_SRC_RGB_ABGR = (1 << 6), BLIT_SRC_RGB_RGBA = (2 << 6), BLIT_SRC_RGB_BGRA = (3 << 6) };

enum {
        BLIT_COMMAND_MASK = 7,
        BLIT_SRC_FORMAT = (7 << 3),
        BLIT_SRC_RGB_FORMAT = (3 << 6),
        BLIT_SRC_CHROMA = (1 << 10),
        BLIT_DST_CHROMA = (1 << 12),
        BLIT_CLIPPING_ENABLED = (1 << 16)
};

enum { BLIT_ROP_DST_PASS = (1 << 0), BLIT_ROP_SRC_PASS = (1 << 1) };

#define MIX(src_dat, dst_dat, rop)                                                                                               \
        switch (rop) {                                                                                                           \
        case 0x0:                                                                                                                \
                dst_dat = 0;                                                                                                     \
                break;                                                                                                           \
        case 0x1:                                                                                                                \
                dst_dat = ~(src_dat | dst_dat);                                                                                  \
                break;                                                                                                           \
        case 0x2:                                                                                                                \
                dst_dat = ~src_dat & dst_dat;                                                                                    \
                break;                                                                                                           \
        case 0x3:                                                                                                                \
                dst_dat = ~src_dat;                                                                                              \
                break;                                                                                                           \
        case 0x4:                                                                                                                \
                dst_dat = src_dat & ~dst_dat;                                                                                    \
                break;                                                                                                           \
        case 0x5:                                                                                                                \
                dst_dat = ~dst_dat;                                                                                              \
                break;                                                                                                           \
        case 0x6:                                                                                                                \
                dst_dat = src_dat ^ dst_dat;                                                                                     \
                break;                                                                                                           \
        case 0x7:                                                                                                                \
                dst_dat = ~(src_dat & dst_dat);                                                                                  \
                break;                                                                                                           \
        case 0x8:                                                                                                                \
                dst_dat = src_dat & dst_dat;                                                                                     \
                break;                                                                                                           \
        case 0x9:                                                                                                                \
                dst_dat = ~(src_dat ^ dst_dat);                                                                                  \
                break;                                                                                                           \
        case 0xa:                                                                                                                \
                dst_dat = dst_dat;                                                                                               \
                break;                                                                                                           \
        case 0xb:                                                                                                                \
                dst_dat = ~src_dat | dst_dat;                                                                                    \
                break;                                                                                                           \
        case 0xc:                                                                                                                \
                dst_dat = src_dat;                                                                                               \
                break;                                                                                                           \
        case 0xd:                                                                                                                \
                dst_dat = src_dat | ~dst_dat;                                                                                    \
                break;                                                                                                           \
        case 0xe:                                                                                                                \
                dst_dat = src_dat | dst_dat;                                                                                     \
                break;                                                                                                           \
        case 0xf:                                                                                                                \
                dst_dat = 0xffff;                                                                                                \
                break;                                                                                                           \
        }

void voodoo_v2_blit_start(voodoo_t *voodoo) {
        uint64_t dat64;
        int size_x = ABS(voodoo->bltSizeX), size_y = ABS(voodoo->bltSizeY);
        int x_dir = (voodoo->bltSizeX > 0) ? 1 : -1;
        int y_dir = (voodoo->bltSizeY > 0) ? 1 : -1;
        int dst_x;
        int src_y = voodoo->bltSrcY & 0x7ff, dst_y = voodoo->bltDstY & 0x7ff;
        int src_stride = (voodoo->bltCommand & BLTCMD_SRC_TILED) ? ((voodoo->bltSrcXYStride & 0x3f) * 32 * 2)
                                                                 : (voodoo->bltSrcXYStride & 0xff8);
        int dst_stride = (voodoo->bltCommand & BLTCMD_DST_TILED) ? ((voodoo->bltDstXYStride & 0x3f) * 32 * 2)
                                                                 : (voodoo->bltDstXYStride & 0xff8);
        uint32_t src_base_addr = (voodoo->bltCommand & BLTCMD_SRC_TILED) ? ((voodoo->bltSrcBaseAddr & 0x3ff) << 12)
                                                                         : (voodoo->bltSrcBaseAddr & 0x3ffff8);
        uint32_t dst_base_addr = (voodoo->bltCommand & BLTCMD_DST_TILED) ? ((voodoo->bltDstBaseAddr & 0x3ff) << 12)
                                                                         : (voodoo->bltDstBaseAddr & 0x3ffff8);
        int x, y;

        /*        pclog("blit_start: command=%08x srcX=%i srcY=%i dstX=%i dstY=%i sizeX=%i sizeY=%i color=%04x,%04x\n",
                        voodoo->bltCommand, voodoo->bltSrcX, voodoo->bltSrcY, voodoo->bltDstX, voodoo->bltDstY, voodoo->bltSizeX,
           voodoo->bltSizeY, voodoo->bltColorFg, voodoo->bltColorBg);*/

        voodoo_wait_for_render_thread_idle(voodoo);

        switch (voodoo->bltCommand & BLIT_COMMAND_MASK) {
        case BLIT_COMMAND_SCREEN_TO_SCREEN:
                for (y = 0; y <= size_y; y++) {
                        uint16_t *src = (uint16_t *)&voodoo->fb_mem[src_base_addr + src_y * src_stride];
                        uint16_t *dst = (uint16_t *)&voodoo->fb_mem[dst_base_addr + dst_y * dst_stride];
                        int src_x = voodoo->bltSrcX, dst_x = voodoo->bltDstX;

                        for (x = 0; x <= size_x; x++) {
                                uint16_t src_dat = src[src_x];
                                uint16_t dst_dat = dst[dst_x];
                                int rop = 0;

                                if (voodoo->bltCommand & BLIT_CLIPPING_ENABLED) {
                                        if (dst_x < voodoo->bltClipLeft || dst_x >= voodoo->bltClipRight ||
                                            dst_y < voodoo->bltClipLowY || dst_y >= voodoo->bltClipHighY)
                                                goto skip_pixel_blit;
                                }

                                if (voodoo->bltCommand & BLIT_SRC_CHROMA) {
                                        int r = (src_dat >> 11);
                                        int g = (src_dat >> 5) & 0x3f;
                                        int b = src_dat & 0x1f;

                                        if (r >= voodoo->bltSrcChromaMinR && r <= voodoo->bltSrcChromaMaxR &&
                                            g >= voodoo->bltSrcChromaMinG && g <= voodoo->bltSrcChromaMaxG &&
                                            b >= voodoo->bltSrcChromaMinB && b <= voodoo->bltSrcChromaMaxB)
                                                rop |= BLIT_ROP_SRC_PASS;
                                }
                                if (voodoo->bltCommand & BLIT_DST_CHROMA) {
                                        int r = (dst_dat >> 11);
                                        int g = (dst_dat >> 5) & 0x3f;
                                        int b = dst_dat & 0x1f;

                                        if (r >= voodoo->bltDstChromaMinR && r <= voodoo->bltDstChromaMaxR &&
                                            g >= voodoo->bltDstChromaMinG && g <= voodoo->bltDstChromaMaxG &&
                                            b >= voodoo->bltDstChromaMinB && b <= voodoo->bltDstChromaMaxB)
                                                rop |= BLIT_ROP_DST_PASS;
                                }

                                MIX(src_dat, dst_dat, voodoo->bltRop[rop]);

                                dst[dst_x] = dst_dat;
                        skip_pixel_blit:
                                src_x += x_dir;
                                dst_x += x_dir;
                        }

                        src_y += y_dir;
                        dst_y += y_dir;
                }
                break;

        case BLIT_COMMAND_CPU_TO_SCREEN:
                voodoo->blt.dst_x = voodoo->bltDstX;
                voodoo->blt.dst_y = voodoo->bltDstY;
                voodoo->blt.cur_x = 0;
                voodoo->blt.size_x = size_x;
                voodoo->blt.size_y = size_y;
                voodoo->blt.x_dir = x_dir;
                voodoo->blt.y_dir = y_dir;
                voodoo->blt.dst_stride = (voodoo->bltCommand & BLTCMD_DST_TILED) ? ((voodoo->bltDstXYStride & 0x3f) * 32 * 2)
                                                                                 : (voodoo->bltDstXYStride & 0xff8);
                break;

        case BLIT_COMMAND_RECT_FILL:
                for (y = 0; y <= size_y; y++) {
                        uint16_t *dst;
                        int dst_x = voodoo->bltDstX;

                        if (SLI_ENABLED) {
                                if ((!(voodoo->initEnable & INITENABLE_SLI_MASTER_SLAVE) && (voodoo->blt.dst_y & 1)) ||
                                    ((voodoo->initEnable & INITENABLE_SLI_MASTER_SLAVE) && !(voodoo->blt.dst_y & 1)))
                                        goto skip_line_fill;
                                dst = (uint16_t *)&voodoo->fb_mem[dst_base_addr + (dst_y >> 1) * dst_stride];
                        } else
                                dst = (uint16_t *)&voodoo->fb_mem[dst_base_addr + dst_y * dst_stride];

                        for (x = 0; x <= size_x; x++) {
                                if (voodoo->bltCommand & BLIT_CLIPPING_ENABLED) {
                                        if (dst_x < voodoo->bltClipLeft || dst_x >= voodoo->bltClipRight ||
                                            dst_y < voodoo->bltClipLowY || dst_y >= voodoo->bltClipHighY)
                                                goto skip_pixel_fill;
                                }

                                dst[dst_x] = voodoo->bltColorFg;
                        skip_pixel_fill:
                                dst_x += x_dir;
                        }
                skip_line_fill:
                        dst_y += y_dir;
                }
                break;

        case BLIT_COMMAND_SGRAM_FILL:
                /*32x32 tiles - 2kb*/
                dst_y = voodoo->bltDstY & 0x3ff;
                size_x = voodoo->bltSizeX & 0x1ff; // 512*8 = 4kb
                size_y = voodoo->bltSizeY & 0x3ff;

                dat64 = voodoo->bltColorFg | ((uint64_t)voodoo->bltColorFg << 16) | ((uint64_t)voodoo->bltColorFg << 32) |
                        ((uint64_t)voodoo->bltColorFg << 48);

                for (y = 0; y <= size_y; y++) {
                        uint64_t *dst;

                        /*This may be wrong*/
                        if (!y) {
                                dst_x = voodoo->bltDstX & 0x1ff;
                                size_x = 511 - dst_x;
                        } else if (y < size_y) {
                                dst_x = 0;
                                size_x = 511;
                        } else {
                                dst_x = 0;
                                size_x = voodoo->bltSizeX & 0x1ff;
                        }

                        dst = (uint64_t *)&voodoo->fb_mem[(dst_y * 512 * 8 + dst_x * 8) & voodoo->fb_mask];

                        for (x = 0; x <= size_x; x++)
                                dst[x] = dat64;

                        dst_y++;
                }
                break;

        default:
                fatal("bad blit command %08x\n", voodoo->bltCommand);
        }
}

void voodoo_v2_blit_data(voodoo_t *voodoo, uint32_t data) {
        int src_bits = 32;
        uint32_t base_addr = (voodoo->bltCommand & BLTCMD_DST_TILED) ? ((voodoo->bltDstBaseAddr & 0x3ff) << 12)
                                                                     : (voodoo->bltDstBaseAddr & 0x3ffff8);
        uint32_t addr;
        uint16_t *dst;

        if ((voodoo->bltCommand & BLIT_COMMAND_MASK) != BLIT_COMMAND_CPU_TO_SCREEN)
                return;

        if (SLI_ENABLED) {
                addr = base_addr + (voodoo->blt.dst_y >> 1) * voodoo->blt.dst_stride;
                dst = (uint16_t *)&voodoo->fb_mem[addr];
        } else {
                addr = base_addr + voodoo->blt.dst_y * voodoo->blt.dst_stride;
                dst = (uint16_t *)&voodoo->fb_mem[addr];
        }

        if (addr >= voodoo->front_offset && voodoo->row_width) {
                int y = (addr - voodoo->front_offset) / voodoo->row_width;
                if (y < voodoo->v_disp)
                        voodoo->dirty_line[y] = 2;
        }

        while (src_bits && voodoo->blt.cur_x <= voodoo->blt.size_x) {
                int r = 0, g = 0, b = 0;
                uint16_t src_dat = 0, dst_dat;
                int x = (voodoo->blt.x_dir > 0) ? (voodoo->blt.dst_x + voodoo->blt.cur_x)
                                                : (voodoo->blt.dst_x - voodoo->blt.cur_x);
                int rop = 0;

                switch (voodoo->bltCommand & BLIT_SRC_FORMAT) {
                case BLIT_SRC_1BPP:
                case BLIT_SRC_1BPP_BYTE_PACKED:
                        src_dat = (data & 1) ? voodoo->bltColorFg : voodoo->bltColorBg;
                        data >>= 1;
                        src_bits--;
                        break;
                case BLIT_SRC_16BPP:
                        switch (voodoo->bltCommand & BLIT_SRC_RGB_FORMAT) {
                        case BLIT_SRC_RGB_ARGB:
                        case BLIT_SRC_RGB_RGBA:
                                src_dat = data & 0xffff;
                                break;
                        case BLIT_SRC_RGB_ABGR:
                        case BLIT_SRC_RGB_BGRA:
                                src_dat = ((data & 0xf800) >> 11) | (data & 0x07c0) | ((data & 0x0038) << 11);
                                break;
                        }
                        data >>= 16;
                        src_bits -= 16;
                        break;
                case BLIT_SRC_24BPP:
                case BLIT_SRC_24BPP_DITHER_2X2:
                case BLIT_SRC_24BPP_DITHER_4X4:
                        switch (voodoo->bltCommand & BLIT_SRC_RGB_FORMAT) {
                        case BLIT_SRC_RGB_ARGB:
                                r = (data >> 16) & 0xff;
                                g = (data >> 8) & 0xff;
                                b = data & 0xff;
                                break;
                        case BLIT_SRC_RGB_ABGR:
                                r = data & 0xff;
                                g = (data >> 8) & 0xff;
                                b = (data >> 16) & 0xff;
                                break;
                        case BLIT_SRC_RGB_RGBA:
                                r = (data >> 24) & 0xff;
                                g = (data >> 16) & 0xff;
                                b = (data >> 8) & 0xff;
                                break;
                        case BLIT_SRC_RGB_BGRA:
                                r = (data >> 8) & 0xff;
                                g = (data >> 16) & 0xff;
                                b = (data >> 24) & 0xff;
                                break;
                        }
                        switch (voodoo->bltCommand & BLIT_SRC_FORMAT) {
                        case BLIT_SRC_24BPP:
                                src_dat = (b >> 3) | ((g & 0xfc) << 3) | ((r & 0xf8) << 8);
                                break;
                        case BLIT_SRC_24BPP_DITHER_2X2:
                                r = dither_rb2x2[r][voodoo->blt.dst_y & 1][x & 1];
                                g = dither_g2x2[g][voodoo->blt.dst_y & 1][x & 1];
                                b = dither_rb2x2[b][voodoo->blt.dst_y & 1][x & 1];
                                src_dat = (b >> 3) | ((g & 0xfc) << 3) | ((r & 0xf8) << 8);
                                break;
                        case BLIT_SRC_24BPP_DITHER_4X4:
                                r = dither_rb[r][voodoo->blt.dst_y & 3][x & 3];
                                g = dither_g[g][voodoo->blt.dst_y & 3][x & 3];
                                b = dither_rb[b][voodoo->blt.dst_y & 3][x & 3];
                                src_dat = (b >> 3) | ((g & 0xfc) << 3) | ((r & 0xf8) << 8);
                                break;
                        }
                        src_bits = 0;
                        break;
                }

                if (SLI_ENABLED) {
                        if ((!(voodoo->initEnable & INITENABLE_SLI_MASTER_SLAVE) && (voodoo->blt.dst_y & 1)) ||
                            ((voodoo->initEnable & INITENABLE_SLI_MASTER_SLAVE) && !(voodoo->blt.dst_y & 1)))
                                goto skip_pixel;
                }

                if (voodoo->bltCommand & BLIT_CLIPPING_ENABLED) {
                        if (x < voodoo->bltClipLeft || x >= voodoo->bltClipRight || voodoo->blt.dst_y < voodoo->bltClipLowY ||
                            voodoo->blt.dst_y >= voodoo->bltClipHighY)
                                goto skip_pixel;
                }

                dst_dat = dst[x];

                if (voodoo->bltCommand & BLIT_SRC_CHROMA) {
                        r = (src_dat >> 11);
                        g = (src_dat >> 5) & 0x3f;
                        b = src_dat & 0x1f;

                        if (r >= voodoo->bltSrcChromaMinR && r <= voodoo->bltSrcChromaMaxR && g >= voodoo->bltSrcChromaMinG &&
                            g <= voodoo->bltSrcChromaMaxG && b >= voodoo->bltSrcChromaMinB && b <= voodoo->bltSrcChromaMaxB)
                                rop |= BLIT_ROP_SRC_PASS;
                }
                if (voodoo->bltCommand & BLIT_DST_CHROMA) {
                        r = (dst_dat >> 11);
                        g = (dst_dat >> 5) & 0x3f;
                        b = dst_dat & 0x1f;

                        if (r >= voodoo->bltDstChromaMinR && r <= voodoo->bltDstChromaMaxR && g >= voodoo->bltDstChromaMinG &&
                            g <= voodoo->bltDstChromaMaxG && b >= voodoo->bltDstChromaMinB && b <= voodoo->bltDstChromaMaxB)
                                rop |= BLIT_ROP_DST_PASS;
                }

                MIX(src_dat, dst_dat, voodoo->bltRop[rop]);

                dst[x] = dst_dat;

        skip_pixel:
                voodoo->blt.cur_x++;
        }

        if (voodoo->blt.cur_x > voodoo->blt.size_x) {
                voodoo->blt.size_y--;
                if (voodoo->blt.size_y >= 0) {
                        voodoo->blt.cur_x = 0;
                        voodoo->blt.dst_y += voodoo->blt.y_dir;
                }
        }
}

void voodoo_fastfill(voodoo_t *voodoo, voodoo_params_t *params) {
        int y;
        int low_y, high_y;

        if (params->fbzMode & (1 << 17)) {
                int y_origin = (voodoo->type >= VOODOO_BANSHEE) ? (voodoo->y_origin_swap + 1) : voodoo->v_disp;

                high_y = y_origin - params->clipLowY;
                low_y = y_origin - params->clipHighY;
        } else {
                low_y = params->clipLowY;
                high_y = params->clipHighY;
        }

        if (params->fbzMode & FBZ_RGB_WMASK) {
                int r, g, b;
                uint16_t col;

                r = ((params->color1 >> 16) >> 3) & 0x1f;
                g = ((params->color1 >> 8) >> 2) & 0x3f;
                b = (params->color1 >> 3) & 0x1f;
                col = b | (g << 5) | (r << 11);

                if (SLI_ENABLED) {
                        for (y = low_y; y < high_y; y += 2) {
                                uint16_t *cbuf =
                                        (uint16_t *)&voodoo
                                                ->fb_mem[(params->draw_offset + (y >> 1) * voodoo->row_width) & voodoo->fb_mask];
                                int x;

                                for (x = params->clipLeft; x < params->clipRight; x++)
                                        cbuf[x] = col;
                        }
                } else {
                        for (y = low_y; y < high_y; y++) {
                                if (voodoo->col_tiled) {
                                        uint16_t *cbuf =
                                                (uint16_t *)&voodoo->fb_mem[(params->draw_offset + (y >> 5) * voodoo->row_width +
                                                                             (y & 31) * 128) &
                                                                            voodoo->fb_mask];
                                        int x;

                                        for (x = params->clipLeft; x < params->clipRight; x++) {
                                                int x2 = (x & 63) | ((x >> 6) * 128 * 32 / 2);
                                                cbuf[x2] = col;
                                        }
                                } else {
                                        uint16_t *cbuf =
                                                (uint16_t *)&voodoo
                                                        ->fb_mem[(params->draw_offset + y * voodoo->row_width) & voodoo->fb_mask];
                                        int x;

                                        for (x = params->clipLeft; x < params->clipRight; x++)
                                                cbuf[x] = col;
                                }
                        }
                }
        }
        if (params->fbzMode & FBZ_DEPTH_WMASK) {
                if (SLI_ENABLED) {
                        for (y = low_y; y < high_y; y += 2) {
                                uint16_t *abuf =
                                        (uint16_t *)&voodoo
                                                ->fb_mem[(params->aux_offset + (y >> 1) * voodoo->row_width) & voodoo->fb_mask];
                                int x;

                                for (x = params->clipLeft; x < params->clipRight; x++)
                                        abuf[x] = params->zaColor & 0xffff;
                        }
                } else {
                        for (y = low_y; y < high_y; y++) {
                                if (voodoo->aux_tiled) {
                                        uint16_t *abuf =
                                                (uint16_t *)&voodoo->fb_mem[(params->aux_offset +
                                                                             (y >> 5) * voodoo->aux_row_width + (y & 31) * 128) &
                                                                            voodoo->fb_mask];
                                        int x;

                                        for (x = params->clipLeft; x < params->clipRight; x++) {
                                                int x2 = (x & 63) | ((x >> 6) * 128 * 32 / 2);
                                                abuf[x2] = params->zaColor & 0xffff;
                                        }
                                } else {
                                        uint16_t *abuf =
                                                (uint16_t *)&voodoo->fb_mem[(params->aux_offset + y * voodoo->aux_row_width) &
                                                                            voodoo->fb_mask];
                                        int x;

                                        for (x = params->clipLeft; x < params->clipRight; x++)
                                                abuf[x] = params->zaColor & 0xffff;
                                }
                        }
                }
        }
}
