/*
 * composite_line_yuv_sse2_simple.c
 * Copyright (C) 2003-2014 Meltytech, LLC
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <inttypes.h>
#include <stdio.h>
#include <string.h>

const static unsigned char const1[] =
{
    0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0xFF, 0x00
};
const static unsigned char const2[] =
{
    0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00, 0x80, 0x00
};

#define LOAD_CONSTS     \
        "pxor           %%xmm0, %%xmm0          \n\t"   /* clear zero register */       \
        "movdqu         (%[const1]), %%xmm9     \n\t"   /* load const1 */               \
        "movdqu         (%[const2]), %%xmm10    \n\t"   /* load const2 */

#define LOAD_WEIGHT     \
        "movd           %[weight], %%xmm1       \n\t"   /* load weight and decompose */ \
        "movlhps        %%xmm1, %%xmm1          \n\t"                                   \
        "pshuflw        $0, %%xmm1, %%xmm1      \n\t"                                   \
        "pshufhw        $0, %%xmm1, %%xmm1      \n\t"

#define LOAD_SRC_A      \
        "movq           (%[src_a]), %%xmm2      \n\t"   /* load source alpha */         \
        "punpcklbw      %%xmm0, %%xmm2          \n\t"   /* unpack alpha 8 8-bits alphas to 8 16-bits values */

#define SRC_A_PREMUL    \
        "pmullw         %%xmm1, %%xmm2          \n\t"   /* premultiply source alpha */  \
        "psrlw          $8, %%xmm2              \n\t"

# define DST_A_CALC \
        /* DSTa = DSTa + (SRCa * (0xFF - DSTa)) >> 8  */ \
        "movq           (%[dest_a]), %%xmm3     \n\t"   /* load dst alpha */    \
        "punpcklbw      %%xmm0, %%xmm3          \n\t"   /* unpack dst 8 8-bits alphas to 8 16-bits values */    \
        "movdqa         %%xmm9, %%xmm4          \n\t"   \
        "psubw          %%xmm3, %%xmm4          \n\t"   \
        "pmullw         %%xmm2, %%xmm4          \n\t"   \
        "movdqa         %%xmm4, %%xmm5          \n\t"   \
        "psrlw          $8, %%xmm4              \n\t"   \
        "paddw          %%xmm5, %%xmm4          \n\t"   \
        "paddw          %%xmm10, %%xmm4         \n\t"   \
        "psrlw          $8, %%xmm4              \n\t"   \
        "paddw          %%xmm4, %%xmm3          \n\t"   \
        "packuswb       %%xmm0, %%xmm3          \n\t"   \
        "movq           %%xmm3, (%[dest_a])     \n\t"   /* save dst alpha */

#define DST_PIX_CALC \
        "movdqu         (%[src]), %%xmm3        \n\t"   /* load src */          \
        "movdqu         (%[dest]), %%xmm4       \n\t"   /* load dst */          \
        "movdqa         %%xmm3, %%xmm5          \n\t"   /* dub src */           \
        "movdqa         %%xmm4, %%xmm6          \n\t"   /* dub dst */           \
        "punpcklbw      %%xmm0, %%xmm5          \n\t"   /* unpack src low */    \
        "punpcklbw      %%xmm0, %%xmm6          \n\t"   /* unpack dst low */    \
        "punpckhbw      %%xmm0, %%xmm3          \n\t"   /* unpack src high */   \
        "punpckhbw      %%xmm0, %%xmm4          \n\t"   /* unpack dst high */   \
        "movdqa         %%xmm2, %%xmm7          \n\t"   /* dub alpha */         \
        "movdqa         %%xmm2, %%xmm8          \n\t"   /* dub alpha */         \
        "movlhps        %%xmm7, %%xmm7          \n\t"   /* dub low */           \
        "movhlps        %%xmm8, %%xmm8          \n\t"   /* dub high */          \
        "pshuflw        $0x50, %%xmm7, %%xmm7   \n\t"                           \
        "pshuflw        $0x50, %%xmm8, %%xmm8   \n\t"                           \
        "pshufhw        $0xFA, %%xmm7, %%xmm7   \n\t"                           \
        "pshufhw        $0xFA, %%xmm8, %%xmm8   \n\t"                           \
        "psubw          %%xmm4, %%xmm3          \n\t"   /* src = src - dst */   \
        "psubw          %%xmm6, %%xmm5          \n\t"                           \
        "pmullw         %%xmm8, %%xmm3          \n\t"   /* src = src * alpha */ \
        "pmullw         %%xmm7, %%xmm5          \n\t"                           \
        "pmullw         %%xmm9, %%xmm4          \n\t"   /* dst = dst * 0xFF */  \
        "pmullw         %%xmm9, %%xmm6          \n\t"                           \
        "paddw          %%xmm3, %%xmm4          \n\t"   /* dst = dst + src */   \
        "paddw          %%xmm5, %%xmm6          \n\t"                           \
        "movdqa         %%xmm4, %%xmm3          \n\t"   /* dst = ((dst >> 8) + dst + 128) >> 8 */ \
        "movdqa         %%xmm6, %%xmm5          \n\t"                           \
        "psrlw          $8, %%xmm4              \n\t"                           \
        "psrlw          $8, %%xmm6              \n\t"                           \
        "paddw          %%xmm3, %%xmm4          \n\t"                           \
        "paddw          %%xmm5, %%xmm6          \n\t"                           \
        "paddw          %%xmm10, %%xmm4         \n\t"                           \
        "paddw          %%xmm10, %%xmm6         \n\t"                           \
        "psrlw          $8, %%xmm4              \n\t"                           \
        "psrlw          $8, %%xmm6              \n\t"                           \
        "packuswb       %%xmm4, %%xmm6          \n\t"                           \
        "movdqu         %%xmm6, (%[dest])       \n\t"   /* store dst */

#define PIX_POINTER_INC \
        "add            $0x10, %[src]           \n\t"   \
        "add            $0x10, %[dest]          \n\t"   \
        "dec            %[width]                \n\t"

#define CLOBBER_XMM \
        "%xmm0", "%xmm1", "%xmm2", "%xmm3", "%xmm4", "%xmm5", "%xmm6", "%xmm7", "%xmm8", "%xmm9", "%xmm10"

static void blend_case7(uint8_t *dest, uint8_t *src, int width, uint8_t *src_a, uint8_t *dest_a, int weight)
{
    int w8 = width / 8;
    __asm__ volatile
    (
        LOAD_CONSTS
        LOAD_WEIGHT
        "loop_start7:                           \n\t"
        LOAD_SRC_A
        SRC_A_PREMUL
        DST_A_CALC
        DST_PIX_CALC
        "add            $0x08, %[src_a]         \n\t"
        "add            $0x08, %[dest_a]        \n\t"
        PIX_POINTER_INC
        "jnz            loop_start7             \n\t"
        : [weight] "+r" (weight), [src_a] "+r" (src_a), [src] "+r" (src),
            [dest] "+r" (dest), [dest_a] "+r" (dest_a), [width] "+r" (w8)
        : [const1] "r" (const1), [const2] "r" (const2)
        : CLOBBER_XMM
    );
};

//  | 3   | dest_a == NULL | src_a != NULL | weight != 256 | blend: premultiply src alpha
static void blend_case3(uint8_t *dest, uint8_t *src, int width, uint8_t *src_a, int weight)
{
    int w8 = width / 8;
    __asm__ volatile
    (
        LOAD_CONSTS
        LOAD_WEIGHT
        "loop_start3:                           \n\t"
        LOAD_SRC_A
        SRC_A_PREMUL
        DST_PIX_CALC
        "add            $0x08, %[src_a]         \n\t"
        PIX_POINTER_INC
        "jnz            loop_start3             \n\t"
        : [weight] "+r" (weight), [src_a] "+r" (src_a), [src] "+r" (src), [dest] "+r" (dest), [width] "+r" (w8)
        : [const1] "r" (const1), [const2] "r" (const2)
        : CLOBBER_XMM
    );
};

//  | 2   | dest_a == NULL | src_a != NULL | weight == 255 | blend: only src alpha
static void blend_case2(uint8_t *dest, uint8_t *src, int width, uint8_t *src_a)
{
    int w8 = width / 8;
    __asm__ volatile
    (
        LOAD_CONSTS
        "loop_start2:                           \n\t"
        LOAD_SRC_A
        DST_PIX_CALC
        "add            $0x08, %[src_a]         \n\t"
        PIX_POINTER_INC
        "jnz            loop_start2             \n\t"
        : [src_a] "+r" (src_a), [src] "+r" (src), [dest] "+r" (dest), [width] "+r" (w8)
        : [const1] "r" (const1), [const2] "r" (const2)
        : CLOBBER_XMM
    );
};


//  | 1   | dest_a == NULL | src_a == NULL | weight != 256 | blend: with given alpha
static void blend_case1(uint8_t *dest, uint8_t *src, int width, int weight)
{
    int w8 = width / 8;
    __asm__ volatile
    (
        LOAD_CONSTS
        LOAD_WEIGHT
        "loop_start1:                           \n\t"
        "movdqa         %%xmm1, %%xmm2          \n\t"   /* src alpha cames from weight */
        DST_PIX_CALC
        PIX_POINTER_INC
        "jnz            loop_start1             \n\t"
        : [weight] "+r" (weight), [src] "+r" (src), [dest] "+r" (dest), [width] "+r" (w8)
        : [const1] "r" (const1), [const2] "r" (const2)
        : CLOBBER_XMM
    );
};

//  | 5   | dest_a != NULL | src_a == NULL | weight != 256 | blend: with given alpha
static void blend_case5(uint8_t *dest, uint8_t *src, int width, uint8_t *dest_a, int weight)
{
    int w8 = width / 8;
    __asm__ volatile
    (
        LOAD_CONSTS
        LOAD_WEIGHT
        "loop_start5:                           \n\t"
        "movdqa         %%xmm1, %%xmm2          \n\t"   /* source alpha comes from weight */
        DST_A_CALC
        DST_PIX_CALC
        "add            $0x08, %[dest_a]        \n\t"
        PIX_POINTER_INC
        "jnz            loop_start5             \n\t"
        : [weight] "+r" (weight), [src] "+r" (src), [dest] "+r" (dest), [dest_a] "+r" (dest_a), [width] "+r" (w8)
        : [const1] "r" (const1), [const2] "r" (const2)
        : CLOBBER_XMM
    );
};

//  | 6   | dest_a != NULL | src_a != NULL | weight == 256 | blend: full blend without src alpha premutiply
static void blend_case6(uint8_t *dest, uint8_t *src, int width, uint8_t *src_a, uint8_t *dest_a)
{
    int w8 = width / 8;
    __asm__ volatile
    (
        LOAD_CONSTS
        "loop_start6:                           \n\t"
        LOAD_SRC_A
        DST_A_CALC
        DST_PIX_CALC
        "add            $0x08, %[src_a]         \n\t"
        "add            $0x08, %[dest_a]        \n\t"
        PIX_POINTER_INC
        "jnz            loop_start6             \n\t"
        : [src_a] "+r" (src_a), [src] "+r" (src), [dest] "+r" (dest), [dest_a] "+r" (dest_a), [width] "+r" (w8)
        : [const1] "r" (const1), [const2] "r" (const2)
        : CLOBBER_XMM
    );
};


void composite_line_yuv_sse2_simple(uint8_t *dest, uint8_t *src, int width, uint8_t *src_a, uint8_t *dest_a, int weight)
{
    weight >>= 8;

    /*
        | 0   | dest_a == NULL | src_a == NULL | weight == 256 | blit
        | 1   | dest_a == NULL | src_a == NULL | weight != 256 | blend: with given alpha
        | 2   | dest_a == NULL | src_a != NULL | weight == 256 | blend: only src alpha
        | 3   | dest_a == NULL | src_a != NULL | weight != 256 | blend: premultiply src alpha
        | 4   | dest_a != NULL | src_a == NULL | weight == 256 | blit: blit and set dst alpha to FF
        | 5   | dest_a != NULL | src_a == NULL | weight != 256 | blend: with given alpha
        | 6   | dest_a != NULL | src_a != NULL | weight == 256 | blend: full blend without src alpha premutiply
        | 7   | dest_a != NULL | src_a != NULL | weight != 256 | blend: full (origin version)
    */

    int cond = ((dest_a != NULL)?4:0) + ((src_a != NULL)?2:0) + ((weight != 256)?1:0);

    switch(cond)
    {
        case 0:
            memcpy(dest, src, 2 * width);
            break;
        case 1: blend_case1(dest, src, width, weight); break;
        case 2: blend_case2(dest, src, width, src_a); break;
        case 3: blend_case3(dest, src, width, src_a, weight); break;
        case 4:
            memcpy(dest, src, 2 * width);
            memset(dest_a, 0xFF, width);
            break;
        case 5: blend_case5(dest, src, width, dest_a, weight); break;
        case 6: blend_case6(dest, src, width, src_a, dest_a); break;
        case 7: blend_case7(dest, src, width, src_a, dest_a, weight); break;
    };
};
