#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))
        {
                high_y = voodoo->v_disp - params->clipLowY;
                low_y = voodoo->v_disp - 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;
                                }
                        }
                }
        }
}
