/*
 * Copyright (C) 2020 Linux Studio Plugins Project <https://lsp-plug.in/>
 *           (C) 2020 Vladimir Sadovnikov <sadko4u@gmail.com>
 *
 * This file is part of lsp-dsp-lib
 * Created on: 31 мар. 2020 г.
 *
 * lsp-dsp-lib 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 3 of the License, or
 * any later version.
 *
 * lsp-dsp-lib 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 lsp-dsp-lib. If not, see <https://www.gnu.org/licenses/>.
 */

#ifndef PRIVATE_DSP_ARCH_X86_SSE4_3DMATH_H_
#define PRIVATE_DSP_ARCH_X86_SSE4_3DMATH_H_

#ifndef PRIVATE_DSP_ARCH_X86_SSE4_IMPL
    #error "This header should not be included directly"
#endif /* PRIVATE_DSP_ARCH_X86_SSE4_IMPL */

namespace lsp
{
    namespace sse4
    {
        using namespace dsp;

        IF_ARCH_X86(
            static const uint32_t X_MASK0111[] __lsp_aligned16  = { 0xffffffff, 0xffffffff, 0xffffffff, 0 };
            static const float X_3DPOINT[] __lsp_aligned16      = { 0.0f, 0.0f, 0.0f, 1.0f };
            static const float X_3D_TOLERANCE[] __lsp_aligned16 = { LSP_DSP_VEC4(DSP_3D_TOLERANCE) };
            static const float X_3D_MTOLERANCE[] __lsp_aligned16 = { LSP_DSP_VEC4(-DSP_3D_TOLERANCE) };
            static const uint32_t IONE[] __lsp_aligned16        = { LSP_DSP_VEC4(1) };
        )

        /* Input:
         * x0 = a0 a1 a2 a3
         * x1 = b0 b1 b2 b3
         *
         * Output:
         * x0 = b0 b1 b2 b3
         * x1 = a0 a1 a2 a3
         */
        #define VECTOR_XCHG(x0, x1) \
            __ASM_EMIT("xorps      %" x0 ", %" x1)     /* xmm1 = b ^ a */ \
            __ASM_EMIT("xorps      %" x1 ", %" x0)     /* xmm0 = a ^ ( b ^ a) = b */ \
            __ASM_EMIT("xorps      %" x0 ", %" x1)     /* xmm1 = (b ^ a) ^ b = a    */

        /* 3x vector multiplication
         * Input:
         *   x0 = vector1 [dx dy dz ? ]
         *   x1 = vector2 [dx dy dz ? ]
         *   x2 = vector3 [dx dy dz ? ]
         *
         * Output:
         *   x0 = vector1 * vector2 [ vz vx vy ? ]
         *   x1 = vector2 * vector3 [ vz vx vy ? ]
         *   x2 = vector3 * vector1 [ vz vx vy ? ]
         */
        #define VECTOR_MUL3(x0, x1, x2, x3, x4, x5, x6, x7) \
            __ASM_EMIT("movaps      %" x0 ", %" x3)          /* xmm3 = dx0 dy0 dz0 dw0 */ \
            __ASM_EMIT("movaps      %" x1 ", %" x4)          /* xmm4 = dx1 dy1 dz1 dw1 */ \
            __ASM_EMIT("movaps      %" x2 ", %" x5)          /* xmm5 = dx2 dy2 dz2 dw2 */ \
            __ASM_EMIT("shufps      $0xc9, %" x3 ", %" x3)   /* xmm3 = dy0 dz0 dx0 dw0 */ \
            __ASM_EMIT("shufps      $0xc9, %" x4 ", %" x4)   /* xmm4 = dy1 dz1 dx1 dw1 */ \
            __ASM_EMIT("shufps      $0xc9, %" x5 ", %" x5)   /* xmm5 = dy2 dz2 dx2 dw2 */ \
            __ASM_EMIT("movaps      %" x0 ", %" x6)          /* xmm6 = dx0 dy0 dz0 dw0 */ \
            __ASM_EMIT("movaps      %" x3 ", %" x7)          /* xmm7 = dy0 dz0 dx0 dw0 */ \
            \
            __ASM_EMIT("mulps       %" x4 ", %" x0)          /* xmm0 = dx0*dy1 dy0*dz1 dz0*dx1 dw0*dw1 */ \
            __ASM_EMIT("mulps       %" x1 ", %" x3)          /* xmm3 = dy0*dx1 dz0*dy1 dx0*dz1 dw0*dw1 */ \
            __ASM_EMIT("mulps       %" x5 ", %" x1)          /* xmm1 = dx1*dy2 dy1*dz2 dz1*dx2 dw1*dw2 */ \
            __ASM_EMIT("mulps       %" x2 ", %" x4)          /* xmm4 = dy1*dx2 dz1*dy2 dx1*dz2 dw1*dw2 */ \
            __ASM_EMIT("mulps       %" x7 ", %" x2)          /* xmm2 = dx2*dy0 dy2*dz0 dz2*dx0 dw2*dw0 */ \
            __ASM_EMIT("mulps       %" x6 ", %" x5)          /* xmm5 = dy2*dx0 dz2*dy0 dx2*dz0 dw2*dw0 */ \
            __ASM_EMIT("subps       %" x3 ", %" x0)          /* xmm0 = nz0 nx0 ny0 nw0 */ \
            __ASM_EMIT("subps       %" x4 ", %" x1)          /* xmm1 = nz1 nx1 ny1 nw1 */ \
            __ASM_EMIT("subps       %" x5 ", %" x2)          /* xmm2 = nz2 nx2 ny2 nw2 */

        /* 3x scalar multiplication
         * Input:
         *   x0 = vector1 [dx dy dz ? ]
         *   x1 = vector2 [dx dy dz ? ]
         *   x2 = vector3 [dx dy dz ? ]
         *
         * Output:
         *   x0 = vector1 * vector2 [ S0 ? ? ? ]
         *   x1 = vector2 * vector3 [ S1 ? ? ? ]
         *   x2 = vector3 * vector1 [ S2 ? ? ? ]
         */
        #define SCALAR_MUL3(x0, x1, x2, x3) \
            __ASM_EMIT("movaps      %" x0 ", %" x3) \
            __ASM_EMIT("dpps        $0x71, %" x1 ", %" x0)      /* xmm0 = dx0*dx1+dy0*dy1+dz0*dz1 */ \
            __ASM_EMIT("dpps        $0x71, %" x2 ", %" x1)      /* xmm1 = dx0*dx1+dy0*dy1+dz0*dz1 */ \
            __ASM_EMIT("dpps        $0x71, %" x3 ", %" x2)      /* xmm2 = dx0*dx1+dy0*dy1+dz0*dz1 */

        /*
         * Input:
         *   x0 = vector 1 = dx1 dy1 dz1 dw1
         *   x1 = vector 2 = dx2 dy2 dz2 dw2
         *
         * Output:
         *   x0 = dx1*dx2 + dy1*dy2 + dz1*dz2
         */
        #define SCALAR_MUL(x0, x1, x2) \
            __ASM_EMIT("dpps        $0x71, %" x1 ", %" x0)  /* xmm0 = dx1*dx2 + dy1*dy2 + dz1*dz2 */

        /*
         * Input:
         *   x0 = vector/point to normalize
         * Output:
         *   x0 = normalized vector
         */
        #define NORMALIZE(x0, x1, x2) \
            __ASM_EMIT("movaps      %" x0 ", %" x1)         /* xmm1 = dx dy dz dw */ \
            __ASM_EMIT("xorps       %" x2 ", %" x2)         /* xmm2 = 0 */ \
            __ASM_EMIT("dpps        $0x7f, %" x1 ", %" x1)  /* xmm1 = dx*dx+dy*dy+dz*dz */ \
            __ASM_EMIT("sqrtps      %" x1", %" x1)          /* xmm1 = sqrt(x*x + y*y + z*z) = w */ \
            __ASM_EMIT("ucomiss     %" x2 ", %" x1)         /* xmm2 =?= xmm1 */ \
            __ASM_EMIT("jle         1000000f") \
            __ASM_EMIT("divps       %" x1 ", %" x0)         /* xmm0 = dx/w dy/w dz/w dw/w */ \
            __ASM_EMIT("1000000:")

        /*
         * Input:
         *   x0 = vector/point to scale
         *   x1 = radius-vector to match length
         *
         * Output:
         *   x0 = scaled vector
        */
        #define SCALE(x0, x1, x2, x3) \
            __ASM_EMIT("movaps      %" x0 ", %" x3)         /* xmm3 = x y z w */ \
            __ASM_EMIT("xorps       %" x2 ", %" x2)         /* xmm2 = 0 */ \
            __ASM_EMIT("dpps        $0x7f, %" x3 ", %" x3)  /* xmm3 = dx*dx+dy*dy+dz*dz */ \
            __ASM_EMIT("shufps      $0x00, %" x1 ", %" x1)  /* xmm1 = r r r r */ \
            __ASM_EMIT("sqrtps      %" x3", %" x3)          /* xmm3 = sqrt(x*x + y*y + z*z) = W W W W */ \
            __ASM_EMIT("ucomiss     %" x2 ", %" x3)         /* xmm3 =?= xmm2 */ \
            __ASM_EMIT("jle         1000000f") \
            __ASM_EMIT("divps       %" x3 ", %" x0)         /* xmm0 = r/W r/W r/W r/W */ \
            __ASM_EMIT("mulps       %" x1 ", %" x0)         /* xmm0 = x*r/W y*r/W z*r/W w*r/W */ \
            __ASM_EMIT("1000000:")

        void normalize_point(point3d_t *p)
        {
            float x0, x1, x2;

            ARCH_X86_ASM
            (
                __ASM_EMIT("movups      (%[p]), %[x0]")         // xmm0 = x y z w
                NORMALIZE("[x0]", "[x1]", "[x2]")
                __ASM_EMIT("andps       %[zmask], %[x0]")       // xmm0 = x y z 0
                __ASM_EMIT("orps        %[omask], %[x0]")       // xmm0 = x y z 1
                __ASM_EMIT("movups      %[x0], (%[p])")

                : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2)
                : [p] "r" (p),
                  [zmask] "m" (X_MASK0111),
                  [omask] "m" (X_3DPOINT)
                : "cc", "memory"
            );
        }

        void scale_point1(point3d_t *p, float r)
        {
            float x0, x1, x2;

            ARCH_X86_ASM
            (
                __ASM_EMIT("movups      (%[p]), %[x0]")         // xmm0 = x y z w
                SCALE("[x0]", "[x3]", "[x1]", "[x2]")
                __ASM_EMIT("andps       %[zmask], %[x0]")       // xmm0 = x y z 0
                __ASM_EMIT("orps        %[omask], %[x0]")       // xmm0 = x y z 1
                __ASM_EMIT("movups      %[x0], (%[p])")

                : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
                : [p] "r" (p),
                  [zmask] "m" (X_MASK0111),
                  [omask] "m" (X_3DPOINT)
                : "cc", "memory"
            );
        }

        void scale_point2(point3d_t *p, const point3d_t *s, float r)
        {
            float x0, x1, x2;

            ARCH_X86_ASM
            (
                __ASM_EMIT("movups      (%[s]), %[x0]")         // xmm0 = x y z w
                SCALE("[x0]", "[x3]", "[x1]", "[x2]")
                __ASM_EMIT("andps       %[zmask], %[x0]")       // xmm0 = x y z 0
                __ASM_EMIT("orps        %[omask], %[x0]")       // xmm0 = x y z 1
                __ASM_EMIT("movups      %[x0], (%[p])")

                : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
                : [p] "r" (p), [s] "r" (s),
                  [zmask] "m" (X_MASK0111),
                  [omask] "m" (X_3DPOINT)
                : "cc", "memory"
            );
        }

        void normalize_vector(vector3d_t *v)
        {
            float x0, x1, x2;

            ARCH_X86_ASM
            (
                __ASM_EMIT("movups      (%[v]), %[x0]")     // xmm0 = dx dy dz dw
                NORMALIZE("[x0]", "[x1]", "[x2]")
                __ASM_EMIT("movups      %[x0], (%[v])")

                : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2)
                : [v] "r" (v)
                : "cc", "memory"
            );
        }

        void scale_vector1(vector3d_t *v, float r)
        {
            float x0, x1, x2;

            ARCH_X86_ASM
            (
                __ASM_EMIT("movups      (%[v]), %[x0]")     // xmm0 = dx dy dz dw
                SCALE("[x0]", "[x3]", "[x1]", "[x2]")
                __ASM_EMIT("andps       %[zmask], %[x0]")
                __ASM_EMIT("movups      %[x0], (%[v])")

                : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
                : [v] "r" (v),
                  [zmask] "m" (X_MASK0111)
                : "cc", "memory"
            );
        }

        void scale_vector2(vector3d_t *v, const vector3d_t *s, float r)
        {
            float x0, x1, x2;

            ARCH_X86_ASM
            (
                __ASM_EMIT("movups      (%[s]), %[x0]")     // xmm0 = dx dy dz dw
                SCALE("[x0]", "[x3]", "[x1]", "[x2]")
                __ASM_EMIT("andps       %[zmask], %[x0]")
                __ASM_EMIT("movups      %[x0], (%[v])")

                : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
                : [v] "r" (v), [s] "r" (s),
                  [zmask] "m" (X_MASK0111)
                : "cc", "memory"
            );
        }

        float check_point3d_on_triangle_tp(const triangle3d_t *t, const point3d_t *p)
        {
            float x0, x1, x2, x3, x4, x5, x6, x7;

            ARCH_X86_ASM
            (
                /* Load vectors */
                __ASM_EMIT("movups      (%[p]), %[x3]")         /* xmm3 = px py pz pw */
                __ASM_EMIT("movups      0x00(%[t]), %[x0]")     /* xmm0 = x0 y0 z0 w0 */
                __ASM_EMIT("movups      0x10(%[t]), %[x1]")     /* xmm1 = x1 y1 z1 w1 */
                __ASM_EMIT("movups      0x20(%[t]), %[x2]")     /* xmm2 = x2 y2 z2 w2 */
                __ASM_EMIT("subps       %[x3], %[x0]")          /* xmm0 = dx0 dy0 dz0 dw0 */
                __ASM_EMIT("subps       %[x3], %[x1]")          /* xmm1 = dx1 dy1 dz1 dw1 */
                __ASM_EMIT("subps       %[x3], %[x2]")          /* xmm2 = dx2 dy2 dz2 dw2 */
                /* 3x vector multiplications */
                VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]")
                /* 3x scalar multiplications */
                SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
                /* Compare with zeros */
                __ASM_EMIT("xorps       %[x4], %[x4]")
                __ASM_EMIT("ucomiss     %[x4], %[x0]")
                __ASM_EMIT("jb          110f")
                __ASM_EMIT("ucomiss     %[x4], %[x1]")
                __ASM_EMIT("jb          109f")
                __ASM_EMIT("ucomiss     %[x4], %[x2]")
                __ASM_EMIT("jb          108f")
                __ASM_EMIT("mulss       %[x1], %[x0]")
                __ASM_EMIT("mulss       %[x2], %[x0]")
                __ASM_EMIT("ucomiss     %[x4], %[x0]")
                __ASM_EMIT("jne         110f")
                /* There is somewhere zero, need additional check */
                /* Load vectors */
                __ASM_EMIT("movups      (%[p]), %[x3]")         /* xmm3 = px py pz pw */
                __ASM_EMIT("movups      0x00(%[t]), %[x0]")     /* xmm0 = x0 y0 z0 w0 */
                __ASM_EMIT("movups      0x10(%[t]), %[x1]")     /* xmm1 = x1 y1 z1 w1 */
                __ASM_EMIT("movups      0x20(%[t]), %[x2]")     /* xmm2 = x2 y2 z2 w2 */
                __ASM_EMIT("subps       %[x3], %[x0]")          /* xmm0 = dx0 dy0 dz0 dw0 */
                __ASM_EMIT("subps       %[x3], %[x1]")          /* xmm1 = dx1 dy1 dz1 dw1 */
                __ASM_EMIT("subps       %[x3], %[x2]")          /* xmm2 = dx2 dy2 dz2 dw2 */
                /* Do 3x scalar multiplications */
                SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
                __ASM_EMIT("mulss       %[x1], %[x0]")
                __ASM_EMIT("mulss       %[x2], %[x0]")
                __ASM_EMIT("jmp         110f")

                __ASM_EMIT("108:")
                __ASM_EMIT("movss       %[x2], %[x0]")
                __ASM_EMIT("jmp         110f")
                __ASM_EMIT("109:")
                __ASM_EMIT("movss       %[x1], %[x0]")
                __ASM_EMIT("110:")
                : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
                  [x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7)
                : [p] "r" (p), [t] "r" (t)
            );

            return x0;
        }

        float check_point3d_on_triangle_pvp(const point3d_t *pv, const point3d_t *p)
        {
            float x0, x1, x2, x3, x4, x5, x6, x7;

            ARCH_X86_ASM
            (
                /* Load vectors */
                __ASM_EMIT("movups      (%[p]), %[x3]")         /* xmm3 = px py pz pw */
                __ASM_EMIT("movups      0x00(%[pv]), %[x0]")    /* xmm0 = x0 y0 z0 w0 */
                __ASM_EMIT("movups      0x10(%[pv]), %[x1]")    /* xmm1 = x1 y1 z1 w1 */
                __ASM_EMIT("movups      0x20(%[pv]), %[x2]")    /* xmm2 = x2 y2 z2 w2 */
                __ASM_EMIT("subps       %[x3], %[x0]")          /* xmm0 = dx0 dy0 dz0 dw0 */
                __ASM_EMIT("subps       %[x3], %[x1]")          /* xmm1 = dx1 dy1 dz1 dw1 */
                __ASM_EMIT("subps       %[x3], %[x2]")          /* xmm2 = dx2 dy2 dz2 dw2 */
                /* 3x vector multiplications */
                VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]")
                /* 3x scalar multiplications */
                SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
                /* Compare with zeros */
                __ASM_EMIT("xorps       %[x4], %[x4]")
                __ASM_EMIT("ucomiss     %[x4], %[x0]")
                __ASM_EMIT("jb          110f")
                __ASM_EMIT("ucomiss     %[x4], %[x1]")
                __ASM_EMIT("jb          109f")
                __ASM_EMIT("ucomiss     %[x4], %[x2]")
                __ASM_EMIT("jb          108f")
                __ASM_EMIT("mulss       %[x1], %[x0]")
                __ASM_EMIT("mulss       %[x2], %[x0]")
                __ASM_EMIT("ucomiss     %[x4], %[x0]")
                __ASM_EMIT("jne         110f")
                /* There is somewhere zero, need additional check */
                /* Load vectors */
                __ASM_EMIT("movups      (%[p]), %[x3]")         /* xmm3 = px py pz pw */
                __ASM_EMIT("movups      0x00(%[pv]), %[x0]")    /* xmm0 = x0 y0 z0 w0 */
                __ASM_EMIT("movups      0x10(%[pv]), %[x1]")    /* xmm1 = x1 y1 z1 w1 */
                __ASM_EMIT("movups      0x20(%[pv]), %[x2]")    /* xmm2 = x2 y2 z2 w2 */
                __ASM_EMIT("subps       %[x3], %[x0]")          /* xmm0 = dx0 dy0 dz0 dw0 */
                __ASM_EMIT("subps       %[x3], %[x1]")          /* xmm1 = dx1 dy1 dz1 dw1 */
                __ASM_EMIT("subps       %[x3], %[x2]")          /* xmm2 = dx2 dy2 dz2 dw2 */
                /* Do 3x scalar multiplications */
                SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
                __ASM_EMIT("mulss       %[x1], %[x0]")
                __ASM_EMIT("mulss       %[x2], %[x0]")
                __ASM_EMIT("jmp         110f")

                __ASM_EMIT("108:")
                __ASM_EMIT("movss       %[x2], %[x0]")
                __ASM_EMIT("jmp         110f")
                __ASM_EMIT("109:")
                __ASM_EMIT("movss       %[x1], %[x0]")
                __ASM_EMIT("110:")
                : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
                  [x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7)
                : [pv] "r" (pv), [p] "r" (p)
            );

            return x0;
        }

        float check_point3d_on_triangle_p3p(const point3d_t *p1, const point3d_t *p2, const point3d_t *p3, const point3d_t *p)
        {
            float x0, x1, x2, x3, x4, x5, x6, x7;

            ARCH_X86_ASM
            (
                /* Load vectors */
                __ASM_EMIT("movups      (%[p]), %[x3]")         /* xmm3 = px py pz pw */
                __ASM_EMIT("movups      (%[p1]), %[x0]")        /* xmm0 = x0 y0 z0 w0 */
                __ASM_EMIT("movups      (%[p2]), %[x1]")        /* xmm1 = x1 y1 z1 w1 */
                __ASM_EMIT("movups      (%[p3]), %[x2]")        /* xmm2 = x2 y2 z2 w2 */
                __ASM_EMIT("subps       %[x3], %[x0]")          /* xmm0 = dx0 dy0 dz0 dw0 */
                __ASM_EMIT("subps       %[x3], %[x1]")          /* xmm1 = dx1 dy1 dz1 dw1 */
                __ASM_EMIT("subps       %[x3], %[x2]")          /* xmm2 = dx2 dy2 dz2 dw2 */
                /* 3x vector multiplications */
                VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]")
                /* 3x scalar multiplications */
                SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
                /* Compare with zeros */
                __ASM_EMIT("xorps       %[x4], %[x4]")
                __ASM_EMIT("ucomiss     %[x4], %[x0]")
                __ASM_EMIT("jb          110f")
                __ASM_EMIT("ucomiss     %[x4], %[x1]")
                __ASM_EMIT("jb          109f")
                __ASM_EMIT("ucomiss     %[x4], %[x2]")
                __ASM_EMIT("jb          108f")
                __ASM_EMIT("mulss       %[x1], %[x0]")
                __ASM_EMIT("mulss       %[x2], %[x0]")
                __ASM_EMIT("ucomiss     %[x4], %[x0]")
                __ASM_EMIT("jne         110f")
                /* There is somewhere zero, need additional check */
                /* Load vectors */
                __ASM_EMIT("movups      (%[p]), %[x3]")         /* xmm3 = px py pz pw */
                __ASM_EMIT("movups      (%[p1]), %[x0]")        /* xmm0 = x0 y0 z0 w0 */
                __ASM_EMIT("movups      (%[p2]), %[x1]")        /* xmm1 = x1 y1 z1 w1 */
                __ASM_EMIT("movups      (%[p3]), %[x2]")        /* xmm2 = x2 y2 z2 w2 */
                __ASM_EMIT("subps       %[x3], %[x0]")          /* xmm0 = dx0 dy0 dz0 dw0 */
                __ASM_EMIT("subps       %[x3], %[x1]")          /* xmm1 = dx1 dy1 dz1 dw1 */
                __ASM_EMIT("subps       %[x3], %[x2]")          /* xmm2 = dx2 dy2 dz2 dw2 */
                /* Do 3x scalar multiplications */
                SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
                __ASM_EMIT("mulss       %[x1], %[x0]")
                __ASM_EMIT("mulss       %[x2], %[x0]")
                __ASM_EMIT("jmp         110f")

                __ASM_EMIT("108:")
                __ASM_EMIT("movss       %[x2], %[x0]")
                __ASM_EMIT("jmp         110f")
                __ASM_EMIT("109:")
                __ASM_EMIT("movss       %[x1], %[x0]")
                __ASM_EMIT("110:")
                : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
                  [x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7)
                : [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3), [p] "r" (p)
            );

            return x0;
        }

        size_t colocation_v1p3(const vector3d_t *pl, const point3d_t *p0, const point3d_t *p1, const point3d_t *p2)
        {
            float x0, x1, x2, x3, x4;
            float res[4] __lsp_aligned16;

            ARCH_X86_ASM
            (
                __ASM_EMIT("movups      (%[pl]), %[x3]")        /* xmm3 = pl    */
                __ASM_EMIT("movups      (%[p0]), %[x0]")        /* xmm0 = p0    */
                __ASM_EMIT("movups      (%[p1]), %[x1]")        /* xmm1 = p1    */
                __ASM_EMIT("movups      (%[p2]), %[x2]")        /* xmm2 = p2    */

                __ASM_EMIT("dpps        $0xf1, %[x3], %[x0]")   /* xmm0 = p0 * pl */
                __ASM_EMIT("dpps        $0xf2, %[x3], %[x1]")   /* xmm1 = p1 * pl */
                __ASM_EMIT("dpps        $0xf4, %[x3], %[x2]")   /* xmm2 = p2 * pl */
                __ASM_EMIT("orps        %[x1], %[x0]")          /* xmm0 = k0 k1 0 0 */
                __ASM_EMIT("orps        %[x2], %[x0]")          /* xmm0 = k0 k1 k2 0 */
                __ASM_EMIT("movaps      %[x0], %[x1]")          /* xmm1 = k0 k1 k2 0 */

                __ASM_EMIT("cmpps       $2, %[PTOL], %[x0]")    /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
                __ASM_EMIT("cmpps       $1, %[MTOL], %[x1]")    /* xmm1 = k0 < -TOL  k1 < -TOL k2 < -TOL ? */
                __ASM_EMIT("andps       %[IONE], %[x0]")        /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
                __ASM_EMIT("andps       %[IONE], %[x1]")        /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
                __ASM_EMIT("paddd       %[x1], %[x0]")
                __ASM_EMIT("movdqa      %[x0], (%[res])")
                __ASM_EMIT32("movl      0x00(%[res]), %[p0]")
                __ASM_EMIT32("movl      0x04(%[res]), %[p1]")
                __ASM_EMIT32("movl      0x08(%[res]), %[p2]")
                __ASM_EMIT64("movl      0x00(%[res]), %k[p0]")
                __ASM_EMIT64("movl      0x04(%[res]), %k[p1]")
                __ASM_EMIT64("movl      0x08(%[res]), %k[p2]")
                __ASM_EMIT("lea         (%[p1], %[p2], 4), %[p1]")
                __ASM_EMIT("lea         (%[p0], %[p1], 4), %[p0]")
                : [p0] "+r" (p0), [p1] "+r" (p1), [p2] "+r" (p2),
                  [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
                  [x4] "=&x" (x4)
                : [pl] "r" (pl),
                  [res] "r" (res),
                  [PTOL] "m" (X_3D_TOLERANCE),
                  [MTOL] "m" (X_3D_MTOLERANCE),
                  [IONE] "m" (IONE)
                : "cc"
            );

            return size_t(p0);
        }

        size_t colocation_v1pv(const vector3d_t *pl, const point3d_t *pv)
        {
            float x0, x1, x2, x3, x4;
            float res[4] __lsp_aligned16;
            size_t pt;

            ARCH_X86_ASM
            (
                __ASM_EMIT("movups      (%[pl]), %[x3]")        /* xmm3 = pl    */
                __ASM_EMIT("movups      0x00(%[pv]), %[x0]")    /* xmm0 = p0    */
                __ASM_EMIT("movups      0x10(%[pv]), %[x1]")    /* xmm1 = p1    */
                __ASM_EMIT("movups      0x20(%[pv]), %[x2]")    /* xmm2 = p2    */

                __ASM_EMIT("dpps        $0xf1, %[x3], %[x0]")   /* xmm0 = p0 * pl */
                __ASM_EMIT("dpps        $0xf2, %[x3], %[x1]")   /* xmm1 = p1 * pl */
                __ASM_EMIT("dpps        $0xf4, %[x3], %[x2]")   /* xmm2 = p2 * pl */
                __ASM_EMIT("orps        %[x1], %[x0]")          /* xmm0 = k0 k1 0 0 */
                __ASM_EMIT("orps        %[x2], %[x0]")          /* xmm0 = k0 k1 k2 0 */
                __ASM_EMIT("movaps      %[x0], %[x1]")          /* xmm1 = k0 k1 k2 0 */

                __ASM_EMIT("cmpps       $2, %[PTOL], %[x0]")    /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
                __ASM_EMIT("cmpps       $1, %[MTOL], %[x1]")    /* xmm1 = k0 < -TOL  k1 < -TOL k2 < -TOL ? */
                __ASM_EMIT("andps       %[IONE], %[x0]")        /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
                __ASM_EMIT("andps       %[IONE], %[x1]")        /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
                __ASM_EMIT("paddd       %[x1], %[x0]")
                __ASM_EMIT("movdqa      %[x0], (%[res])")
                __ASM_EMIT32("movl      0x00(%[res]), %[pt]")
                __ASM_EMIT32("movl      0x04(%[res]), %[pl]")
                __ASM_EMIT32("movl      0x08(%[res]), %[pv]")
                __ASM_EMIT64("movl      0x00(%[res]), %k[pt]")
                __ASM_EMIT64("movl      0x04(%[res]), %k[pl]")
                __ASM_EMIT64("movl      0x08(%[res]), %k[pv]")
                __ASM_EMIT("lea         (%[pl], %[pv], 4), %[pl]")
                __ASM_EMIT("lea         (%[pt], %[pl], 4), %[pt]")
                : [pl] "+r" (pl), [pv] "+r" (pv), [pt] "=&r" (pt),
                  [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
                  [x4] "=&x" (x4)
                : [res] "r" (res),
                  [PTOL] "m" (X_3D_TOLERANCE),
                  [MTOL] "m" (X_3D_MTOLERANCE),
                  [IONE] "m" (IONE)
                : "cc"
            );

            return pt;
        }
    }
}

#endif /* PRIVATE_DSP_ARCH_X86_SSE4_3DMATH_H_ */
