using FxpConvert.Common;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Runtime.CompilerServices;
using System.Runtime.Intrinsics;
using System.Text;
using System.Threading.Tasks;

namespace SIMDFxpConvert
{
    public unsafe sealed class SIMDFxpConverter : IFxpConvert
    {
        public bool IsSupport => true;

        public void DoubleConvertToDxp(ref double value, NiFpga_FxpTypeInfo dxpinfo, ref ulong fxp, uint count)
        {
            if (count == 0) return;
            double pow = (1ul << (dxpinfo.wordLength - dxpinfo.integerWordLength));
            uint onecount = (uint)(512 / (Unsafe.SizeOf<double>() * 8));
            uint c = count / onecount;
            ref var source = ref Unsafe.As<double, System.Runtime.Intrinsics.Vector512<double>>(ref value);
            Vector512<double> max = Vector512<double>.One * dxpinfo.Max;
            Vector512<double> min = Vector512<double>.One * dxpinfo.Min;
             ulong* fxpptr = (ulong*)Unsafe.AsPointer(ref fxp);
            for (int i = 0; i < c; i++)
            {
                var temp = Unsafe.Add(ref source, i);
                temp = Vector512.Max(min, temp);
                temp = Vector512.Min(max, temp);
                temp *= pow;

                if (dxpinfo.isSigned)
                {
                    for (int j = 0; j < onecount; j++)
                    {
                        fxpptr[i * onecount + j] = dxpinfo.wordLength > 32 ? (((ulong)(long)temp[j]) << (64 - dxpinfo.wordLength)) : ((ulong)(long)temp[j]);
                    }

                }
                else
                {
                    for (int j = 0; j < onecount; j++)
                    {
                        fxpptr[i * onecount + j] = dxpinfo.wordLength > 32 ? (((ulong)temp[j]) << (64 - dxpinfo.wordLength)) : ((ulong)temp[j]);
                    }
                }

            }
            uint c1 = count % onecount;
            if (c1 != 0)
            {
                uint start = c * onecount;
                for (uint i = 0; i < c1; i++)
                {
                    double temp = Math.Clamp(Unsafe.Add(ref value, start + i),dxpinfo.Min,dxpinfo.Max);

                    if (dxpinfo.isSigned)
                    {
                        ref long templ = ref Unsafe.As<ulong, long>(ref Unsafe.Add(ref fxp, start + i));
                        templ = dxpinfo.wordLength > 32 ? (((long)(temp * pow)) << (64 - dxpinfo.wordLength)) : ((long)(temp * pow));

                    }
                    else
                    {
                        ref ulong tempu = ref Unsafe.Add(ref fxp, start + i);
                        tempu = dxpinfo.wordLength > 32 ? (((ulong)(temp * pow)) << (64 - dxpinfo.wordLength)) : ((ulong)(temp * pow));
                    }
                }
            }
        }

        public void FloatConvertToDxp(ref float value, NiFpga_FxpTypeInfo dxpinfo, ref ulong fxp, uint count)
        {
            if (count == 0) return;
            float pow = (1ul << (dxpinfo.wordLength - dxpinfo.integerWordLength));
            uint onecount = (uint)(512 / (Unsafe.SizeOf<float>() * 8));
            uint c = count / onecount;
            ref var source = ref Unsafe.As<float, System.Runtime.Intrinsics.Vector512<float>>(ref value);
            ulong* fxpptr = (ulong*)Unsafe.AsPointer(ref fxp); 
            Vector512<float> max = Vector512<float>.One * (float)dxpinfo.Max;
            Vector512<float> min = Vector512<float>.One * (float)dxpinfo.Min;
            for (int i = 0; i < c; i++)
            {
                var temp = Unsafe.Add(ref source, i);
                temp = Vector512.Max(min, temp);
                temp = Vector512.Min(max, temp);
                temp *= pow;
                if (dxpinfo.isSigned)
                {
                    for (int j = 0; j < onecount; j++)
                    {
                        fxpptr[i * onecount + j] = dxpinfo.wordLength>32?(((ulong)(long)temp[j])<<(64-dxpinfo.wordLength)):  ((ulong)(long)temp[j]) ;
                    }

                }
                else
                {
                    for (int j = 0; j < onecount; j++)
                    {
                        fxpptr[i * onecount + j] =dxpinfo.wordLength>32?(((ulong)temp[j])<<(64-dxpinfo.wordLength)) : ((ulong)temp[j]) ;
                    }
                }
            }
            uint c1 = count % onecount;
            if (c1 != 0)
            {
                uint start = c * onecount;
                for (uint i = 0; i < c1; i++)
                {
                    float temp =Math.Clamp(Unsafe.Add(ref value, start + i),(float)dxpinfo.Min,(float)dxpinfo.Max);
                    if (dxpinfo.isSigned)
                    {
                        ref long templ = ref Unsafe.As<ulong, long>(ref Unsafe.Add(ref fxp, start + i));
                        templ = dxpinfo.wordLength>32?(((long)(temp * pow))<<(64-dxpinfo.wordLength)) : ((long)(temp * pow)) ;

                    }
                    else
                    {
                        ref ulong tempu = ref Unsafe.Add(ref fxp, start + i);
                        tempu = dxpinfo.wordLength > 32 ? (((ulong)(temp * pow)) << (64 - dxpinfo.wordLength)) : ((ulong)(temp * pow)) ;
                    }
                }
            }
        }

        public void FxpConvertToDouble(ref ulong fxp, NiFpga_FxpTypeInfo dxpinfo, ref double value, uint count)
        {
            if (count == 0) return;
            double delta = Math.Pow(2, dxpinfo.integerWordLength - dxpinfo.wordLength);
            ulong mask = dxpinfo.wordLength == 64 ? ulong.MaxValue : ((1ul << dxpinfo.wordLength) - 1);
            if (dxpinfo.isSigned)
            {
                ulong signedmask = 1ul << (dxpinfo.wordLength - 1);
                for (int i = 0; i < count; i++)
                {
                    ref double resultref = ref Unsafe.Add(ref value, i);
                    ref ulong tempfxpref = ref Unsafe.Add(ref fxp, i);
                    ulong tempdata = 0;
                    if (dxpinfo.wordLength > 32)
                    {
                        tempdata = (tempfxpref >> (64 - dxpinfo.wordLength)) & mask;
                    }
                    else tempdata = tempfxpref & mask;
                    if ((tempdata & signedmask) != 0)
                    {
                        long signeddata = (long)(tempdata ^ mask);
                        signeddata = (signeddata + 1) * -1;
                        resultref = signeddata * delta;
                    }
                    else resultref = delta * tempdata;
                }
            }
            else
            {
                for (int i = 0; i < count; i++)
                {
                    ref double resultref = ref Unsafe.Add(ref value, i);
                    ref ulong tempfxpref = ref Unsafe.Add(ref fxp, i);
                    ulong tempdata = (dxpinfo.wordLength > 32 ? (tempfxpref >> (64 - dxpinfo.wordLength)) : tempfxpref) & mask;
                    resultref = delta * tempdata;
                }
            }
        }

        public void FxpConvertToFloat(ref ulong fxp, NiFpga_FxpTypeInfo dxpinfo, ref float value, uint count)
        {
            if (count == 0) return;
            float delta = MathF.Pow(2, dxpinfo.integerWordLength - dxpinfo.wordLength);
            ulong mask = dxpinfo.wordLength == 64 ? ulong.MaxValue : ((1ul << dxpinfo.wordLength) - 1);
            if (dxpinfo.isSigned)
            {
                ulong signedmask = 1ul << (dxpinfo.wordLength - 1);
                for (int i = 0; i < count; i++)
                {
                    ref float resultref = ref Unsafe.Add(ref value, i);
                    ref ulong tempfxpref = ref Unsafe.Add(ref fxp, i);
                    ulong tempdata = 0;
                    if (dxpinfo.wordLength>32)
                    {
                        tempdata = (tempfxpref>>(64-dxpinfo.wordLength)) & mask;
                    }
                    else  tempdata = tempfxpref & mask;
                    if ((tempdata & signedmask) != 0)
                    {
                        long signeddata = (long)(tempdata ^ mask);
                        signeddata = (signeddata + 1) * -1;
                        resultref = signeddata * delta;
                    }
                    else resultref = delta * tempdata;
                }
            }
            else
            {
                for (int i = 0; i < count; i++)
                {
                    ref float resultref = ref Unsafe.Add(ref value, i);
                    ref ulong tempfxpref = ref Unsafe.Add(ref fxp, i);
                    ulong tempdata = (dxpinfo.wordLength>32?(tempfxpref>>(64-dxpinfo.wordLength)): tempfxpref) & mask;
                    resultref = delta * tempdata;
                }
            }
        }
    }
}