#pragma once

#include "StgSharpGraphic.h"
#include "SSGinternal.h"
#include <stdio.h>
#include <stdalign.h>

#include <xmmintrin.h>
#include <pmmintrin.h>
#include <immintrin.h>



/*

Basic functions as a copy of SIMD intrisic function

*/
SSGC_API __m128* __cdecl load(float x, float y, float z, float w)
{
    float array[4] = {x,y,z,w};
    __m128* ptr = _aligned_malloc(sizeof(__m128),16);

    if (NULL != ptr)
    {
        *ptr = _mm_loadu_ps(&array);
        return ptr;
    }
    else
    {
        return -1;
    }
}

SSGC_API __m128* __cdecl load_cpy(__m128* ptr)
{
    __m128* ret = _aligned_malloc(sizeof(__m128*),16);

    if (NULL != ret)
    {
        *ret = *ptr;
    }

    return ret;
}

SSGC_API __m128* __cdecl addps(__m128* ptr1, __m128* ptr2)
{
    
    __m128* ret = _aligned_malloc(sizeof(__m128*),16);

    if (NULL != ret)
    {
        *ret = _mm_add_ps(*ptr1, *ptr2);
        return ret;
    }
    else
    {
        return -1;
    }

}

SSGC_API __m128* __cdecl subps(__m128* ptr1, __m128* ptr2)
{
    __m128* ret = _aligned_malloc(sizeof(__m128*), 16);

    if (NULL != ret)
    {
        *ret = _mm_sub_ps(*ptr1, *ptr2);
        return ret;
    }
    else
    {
        return -1;
    }
}

SSGC_API __m128* __cdecl mulps_const(__m128* ptr, float value)
{
    float c[4] = { value, value, value, value };
    __m128* ret = _aligned_malloc(sizeof(__m128),16);

    __m128 valueVec = _mm_loadu_ps(&c);

    if (NULL != ret)
    {
        *ret = _mm_mul_ps(*ptr,valueVec);
        return ret;
    }
    else
    {
        return -1;
    }
}

SSGC_API __m128* __cdecl mulps(__m128* ptr1, __m128* ptr2)
{
    __m128* ret = _aligned_malloc(sizeof(__m128), 16);

    if (NULL != ret)
    {
        *ret = _mm_mul_ps(*ptr1, *ptr2);
        return ret;
    }
    else
    {
        return -1;
    }
}

SSGC_API float __cdecl mulps_sum(__m128* ptr1, __m128* ptr2)
{
    __m128 ret = _mm_mul_ps(*ptr1,*ptr2);
    ret = _mm_hadd_ps(ret,ret);
    ret = _mm_hadd_ps(ret,ret);
    return *(float*)&ret;
}

SSGC_API __m128* __cdecl divps_const(__m128* ptr, float value)
{

    float c[4] = { value, value, value, value };
    __m128* ret = _aligned_malloc(sizeof(__m128), 16);

    __m128 valueVec = _mm_loadu_ps(&c);

    if (NULL != ret)
    {
        *ret = _mm_div_ps(*ptr, valueVec);
        return ret;
    }
    else
    {
        return -1;
    }
}

SSGC_API __m128* __cdecl divps(__m128* ptr1, __m128* ptr2)
{
    __m128* ret = _aligned_malloc(sizeof(__m128), 16);

    if (NULL != ret)
    {
        *ret = _mm_div_ps(*ptr1, *ptr2);
        return ret;
    }
    else
    {
        return -1;
    }
}

SSGC_API float __cdecl sqrtss_const(float a)
{
    __m128 ret = _mm_sqrt_ss(*(__m128*) & a);

    return ret.m128_f32[0];

    

}



/*

Some complex functions for matrix calculations.

*/

SSGC_API __m256i* __cdecl load_matmap(uint64_t c0, uint64_t c1, uint64_t c2, uint64_t c3)
{
    __m256i* retPtr = _aligned_malloc(sizeof(__m256i), sizeof(__m256i));

    uint64_t ptr[4] = {c0,c1,c2,c3 };

    if (NULL != retPtr)
    {
        *retPtr = _mm256_loadu_epi64(&ptr);
    }

    return retPtr;

}

SSGC_API __m256i* __cdecl tranpose_4x4matmap(__m256i* matPtr)
{

    __m256i* ret = _aligned_malloc(sizeof(__m256i),sizeof(__m256i));

    printf("%llu\n",(uint64_t)ret);

    __m128* columPtr[4];
    columPtr[0] = _aligned_malloc(sizeof(__m128),16);
    columPtr[1] = _aligned_malloc(sizeof(__m128),16);
    columPtr[2] = _aligned_malloc(sizeof(__m128),16);
    columPtr[3] = _aligned_malloc(sizeof(__m128),16);

    if (NULL != ret)
    {
        *ret = _mm256_loadu_epi64(*(uint64_t*)&columPtr);
    }

    __m128* c0Ptr = (__m128*) (matPtr->m256i_i64[0]);
    __m128* c1Ptr = (__m128*) (matPtr->m256i_i64[1]);
    __m128* c2Ptr = (__m128*)(matPtr->m256i_i64[2]);
    __m128* c3Ptr = (__m128*)(matPtr->m256i_i64[3]);

    __m128 cache0Ptr = _mm_shuffle_ps(*c0Ptr,*c1Ptr,0b00010001);
    __m128 cache1Ptr = _mm_shuffle_ps(*c0Ptr,*c1Ptr,0b01110111);
    __m128 cache2Ptr = _mm_shuffle_ps(*c2Ptr, *c3Ptr, 0b00010001);
    __m128 cache3Ptr = _mm_shuffle_ps(*c2Ptr, *c3Ptr, 0b01110111);

    *(columPtr[0]) = _mm_shuffle_ps(cache0Ptr, cache2Ptr, 0b00100010);
    *(columPtr[1]) = _mm_shuffle_ps(cache0Ptr, cache2Ptr, 0b01110111);
    *(columPtr[2]) = _mm_shuffle_ps(cache1Ptr, cache3Ptr, 0b00100010);
    *(columPtr[3]) = _mm_shuffle_ps(cache1Ptr, cache3Ptr, 0b01110111);

    return ret;
}

SSGC_API __m128* __cdecl set_m128ptr_default()
{
    return defaultVecPtr;
}

sse_matmap* __cdecl transpose_ssemat_2(sse_matmap* matPtr)
{
    sse_matmap* ret = malloc(sizeof(sse_matmap));
    if (NULL != ret)
        {
            ret->c0ptr = _aligned_malloc(sizeof(__m128), 16);
            ret->c1ptr = _aligned_malloc(sizeof(__m128), 16);
            ret->c2ptr = defaultVecPtr;
            ret->c3ptr = defaultVecPtr;
        }


    __m128 t0 = _mm_shuffle_ps(*(matPtr->c0ptr), *(matPtr->c1ptr), _MM_SHUFFLE(0,1,0,1));
    __m128 t1 = _mm_shuffle_ps(*(matPtr->c2ptr), *(matPtr->c3ptr), _MM_SHUFFLE(0, 1, 0, 1));

    if (NULL != ret->c0ptr)
    {
        *(ret->c0ptr) = _mm_shuffle_ps(t0, t1, _MM_SHUFFLE(0, 2, 0, 2));
    }
    if (NULL != ret->c1ptr)
    {
        *(ret->c1ptr) = _mm_shuffle_ps(t0, t1, _MM_SHUFFLE(1, 3, 1, 3));
    }

    return ret;


}

sse_matmap* __cdecl transpose_ssemat_3(sse_matmap* matPtr)
{
    sse_matmap* ret = malloc(sizeof(sse_matmap));

    if (NULL != ret )
    {
        ret->c0ptr = _aligned_malloc(sizeof(__m128),16);
        ret->c1ptr = _aligned_malloc(sizeof(__m128),16);
        ret->c2ptr = _aligned_malloc(sizeof(__m128),16);
        ret->c3ptr = defaultVecPtr;

        __m128 t0 = _mm_shuffle_ps( *(matPtr->c0ptr), *(matPtr->c1ptr), _MM_SHUFFLE(0, 1, 0, 1));
        __m128 t1 = _mm_shuffle_ps( *(matPtr->c0ptr), *(matPtr->c1ptr), _MM_shuffle(2, 3, 2, 3));

        if (defaultVecPtr == matPtr->c2ptr)
        {
            __m128 t2 = _mm_shuffle_ps( *(matPtr->c2ptr), *(matPtr->c3ptr), _MM_SHUFFLE(0, 1, 0, 1));
            __m128 t3 = _mm_shuffle_ps( *(matPtr->c2ptr), *(matPtr->c3ptr), _MM_SHUFFLE(2, 3, 2, 3));

            *(ret->c0ptr) = _mm_shuffle_ps(t0, t2, _MM_SHUFFLE(0, 2, 0, 2));
            *(ret->c1ptr) = _mm_shuffle_ps(t0, t2, _MM_SHUFFLE(1, 3, 1, 3));
            *(ret->c3ptr) = _mm_shuffle_ps(t1, t3, _MM_SHUFFLE(0, 2, 0, 2));

        }
        else
        {
            *(ret->c0ptr) = _mm_shuffle_ps(t0, *defaultVecPtr, _MM_SHUFFLE(0, 2, 0, 2));
            *(ret->c1ptr) = _mm_shuffle_ps(t0, *defaultVecPtr, _MM_SHUFFLE(1, 3, 1, 3));
            *(ret->c2ptr) = _mm_shuffle_ps(t1, *defaultVecPtr, _MM_SHUFFLE(0, 2, 0, 2));
        }

    }
    return ret;
}








/*

Functions to clean up colum and matrix resources

*/

SSGC_API void __cdecl deinitM128(__m128* ptr)
{
    free(ptr);
}

void __cdecl deinit_matmap_sse(sse_matmap* mapptr)
{
    free(mapptr);
}