#include "ssc_intrinsic.h"
#include "SSC_internal.h"

INTERNAL void __inline SSCDECL dot_41_sse(__4_columnset *transpose, __m128 *vector, __m128 *ans)
{
        *ans = _mm_dp_ps(transpose->column[0], *vector, 0B00011111);
        *ans = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[1], *vector, 0B00011111),
                             SSE_INSERT_SIMPLE(1, 1));
        *ans = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[2], *vector, 0B00011111),
                             SSE_INSERT_SIMPLE(1, 3));
        *ans = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[3], *vector, 0B00011111),
                             SSE_INSERT_SIMPLE(1, 4));
}

INTERNAL void SSCDECL dot_31_sse(__3_columnset *transpose, __m128 *vector, __m128 *ans)
{
        *ans = _mm_dp_ps(transpose->column[0], *vector, 0B00011111);
        *ans = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[1], *vector, 0B00011111),
                             SSE_INSERT_SIMPLE(1, 1));
        *ans = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[2], *vector, 0B00011111),
                             SSE_INSERT_SIMPLE(1, 3));
}

INTERNAL void SSCDECL dot_42_sse(__4_columnset *transpose, __m128 *vector, __m128 *ans)
{
        ans[0] = _mm_dp_ps(transpose->column[0], vector[0], 0B00011111);
        ans[0] = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[1], vector[0], 0B00011111),
                               SSE_INSERT_SIMPLE(1, 1));
        ans[0] = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[2], vector[0], 0B00011111),
                               SSE_INSERT_SIMPLE(1, 3));
        ans[0] = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[3], vector[0], 0B00011111),
                               SSE_INSERT_SIMPLE(1, 4));

        ans[1] = _mm_dp_ps(transpose->column[0], vector[1], 0B00011111);
        ans[1] = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[1], vector[1], 0B00011111),
                               SSE_INSERT_SIMPLE(1, 1));
        ans[1] = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[2], vector[1], 0B00011111),
                               SSE_INSERT_SIMPLE(1, 3));
        ans[1] = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[3], vector[1], 0B00011111),
                               SSE_INSERT_SIMPLE(1, 4));
}

INTERNAL void SSCDECL dot_32_sse(__3_columnset *transpose, __m128 *vector, __m128 *ans)
{
        ans[0] = _mm_dp_ps(transpose->column[0], vector[0], 0B00011111);
        ans[0] = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[1], vector[0], 0B00011111),
                               SSE_INSERT_SIMPLE(1, 1));
        ans[0] = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[2], vector[0], 0B00011111),
                               SSE_INSERT_SIMPLE(1, 3));

        ans[1] = _mm_dp_ps(transpose->column[0], vector[1], 0B00011111);
        ans[1] = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[1], vector[1], 0B00011111),
                               SSE_INSERT_SIMPLE(1, 1));
        ans[1] = _mm_insert_ps(*ans, _mm_dp_ps(transpose->column[2], vector[1], 0B00011111),
                               SSE_INSERT_SIMPLE(1, 3));
}

INTERNAL void SSCDECL dot_42_avx(__4_columnset *transpose, __m128 *vector, __m128 *ans)
{
        __2_columnset *ansPtr = (__2_columnset *)ans;
        __m256 temp = ALIGN256(transpose->half[0]);

        ansPtr->stream[0] = _mm256_dp_ps(temp, *(__m256 *)vector, 0B11110001);
        temp = _mm256_permute2f128_ps(temp, temp, 0B00000001);
        ansPtr->stream[0] = _mm256_blend_ps(
                ansPtr->stream[0], _mm256_dp_ps(temp, *(__m256 *)vector, 0B11110010), 0B001000010);

        temp = ALIGN256(transpose->half[1]);
        ansPtr->stream[0] = _mm256_blend_ps(
                ansPtr->stream[0], _mm256_dp_ps(temp, *(__m256 *)vector, 0B11110100), 0B01000100);
        temp = _mm256_permute2f128_ps(temp, temp, 0B00000001);
        ansPtr->stream[0] = _mm256_blend_ps(
                ansPtr->stream[0], _mm256_dp_ps(temp, *(__m256 *)vector, 0B11111000), 0B10001000);

        ans[1] = _mm_shuffle_ps(ALIGN(ans[1]), ALIGN(ans[1]), _MM_SHUFFLE(2, 3, 0, 1));
}

INTERNAL void SSCDECL dot_32_avx(__3_columnset *transpose, __m128 *vector, __m128 *ans)
{
        __2_columnset *ansPtr = (__2_columnset *)ans;
        __m256 temp = transpose->part;

        ansPtr->stream[0] = _mm256_dp_ps(temp, *(__m256 *)vector, 0B11110001);
        temp = _mm256_permute2f128_ps(temp, temp, 0B00000001);
        ansPtr->stream[0] = _mm256_blend_ps(
                ansPtr->stream[0], _mm256_dp_ps(temp, *(__m256 *)vector, 0B11110010), 0B001000010);

        temp = _mm256_set_m128(transpose->column[2], transpose->column[2]);
        ansPtr->stream[0] = _mm256_blend_ps(
                ansPtr->stream[0], _mm256_dp_ps(temp, *(__m256 *)vector, 0B11110100), 0B01000100);

        ans[1] = _mm_shuffle_ps(ALIGN(ans[1]), ALIGN(ans[1]), _MM_SHUFFLE(2, 2, 0, 1));
}