#include "sn_intrinsic.h"

INTERNAL void SN_DECL f32_fma_sse(MAT_PANEL_SSE(int) * left)
{
}

// ans = left * right + ans
// right is not transposed
INTERNAL void SN_DECL f32_fma_sse(MAT_KERNEL(float) const *restrict left,
                                  MAT_KERNEL(float) const *restrict right,
                                  MAT_KERNEL(float) *restrict ans)
{
        register __m128 source0 = _mm_load_ps(&left->xmm[0]);
        register __m128 source1 = _mm_load_ps(&left->xmm[1]);
        register __m128 source2 = _mm_load_ps(&left->xmm[2]);
        register __m128 source3 = _mm_load_ps(&left->xmm[3]);

        register __m128 ans0 = _mm_load_ps(&ans->xmm[0]);
        register __m128 ans1 = _mm_load_ps(&ans->xmm[1]);
        register __m128 ans2 = _mm_load_ps(&ans->xmm[2]);
        register __m128 ans3 = _mm_load_ps(&ans->xmm[3]);

        register __m128 broadcast;
        for (size_t i = 0; i < 4; i++) {
                broadcast = _mm_load_ps(&right->xmm[i]);

                ans0 = _mm_add_ps(ans0,
                                  _mm_mul_ps(source0, _mm_shuffle_ps(broadcast, broadcast, 0x00)));
                ans1 = _mm_add_ps(ans1,
                                  _mm_mul_ps(source1, _mm_shuffle_ps(broadcast, broadcast, 0x55)));
                ans2 = _mm_add_ps(ans2,
                                  _mm_mul_ps(source2, _mm_shuffle_ps(broadcast, broadcast, 0xAA)));
                ans3 = _mm_add_ps(ans3,
                                  _mm_mul_ps(source3, _mm_shuffle_ps(broadcast, broadcast, 0xFF)));
        }

        _mm_store_ps(&ans->xmm[0], ans0);
        _mm_store_ps(&ans->xmm[1], ans1);
        _mm_store_ps(&ans->xmm[2], ans2);
        _mm_store_ps(&ans->xmm[3], ans3);
}

// ans = left * right + ans
INTERNAL void SN_DECL f32_fma_avx(MAT_KERNEL(float) const *restrict left,
                                  MAT_KERNEL(float) const *restrict right,
                                  MAT_KERNEL(float) *restrict ans)
{
        register __m256 source0 = _mm256_load_ps(&left->ymm[0]);
        register __m256 source1 = _mm256_load_ps(&left->ymm[1]);

        register __m256 ans0 = _mm256_load_ps(&ans->ymm[0]);
        register __m256 ans1 = _mm256_load_ps(&ans->ymm[1]);

        register __m128 bs0, bs1;
        register __m256 broadcast;
        for (size_t i = 0; i < 4; i++) {
                bs0 = _mm_load_ps(&right->xmm[i]);

                __m128 lo = _mm_shuffle_ps(bs0, bs0, 0x00);
                __m128 hi = _mm_shuffle_ps(bs0, bs0, 0x55);
                broadcast = _mm256_castps128_ps256(lo);
                broadcast = _mm256_insertf128_ps(broadcast, bs0, 1);
                ans0 = _mm256_add_ps(ans0, _mm256_mul_ps(source0, broadcast));

                lo = _mm_shuffle_ps(bs0, bs0, 0xAA);
                hi = _mm_shuffle_ps(bs0, bs0, 0xFF);
                broadcast = _mm256_castps128_ps256(lo);
                broadcast = _mm256_insertf128_ps(broadcast, bs0, 1);
                ans1 = _mm256_add_ps(ans1, _mm256_mul_ps(source1, broadcast));
        }

        _mm256_store_ps(&ans->ymm[0], ans0);
        _mm256_store_ps(&ans->ymm[1], ans1);
}

// ans = left * right + ans
INTERNAL void SN_DECL f32_fma_512(MAT_KERNEL(float) const *restrict left,
                                  MAT_KERNEL(float) const *restrict right,
                                  MAT_KERNEL(float) *restrict ans)
{
        /*
        * to be implemented
        */
}