Shaker/Shaker.Model/Tools/Tools.cs

using FxpConvert.Common;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Linq.Expressions;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
using System.Xml.Schema;

namespace Shaker.Models.Tools
{
    public static  class Tools
    {
        public static ICalc Calc { get; } = new SIMDFxpConvert.SIMDCalc();
        /// <summary>
        /// 单边谱扩展到双边谱
        /// </summary>
        /// <param name="unilateralSpectrum"></param>
        /// <returns></returns>
        public static double[] UnilateralSpectrumToBilateralSpectrum(double[] unilateralSpectrum) 
        {
            double[] result = new double[unilateralSpectrum.Length << 1];
            Unsafe.CopyBlock(ref Unsafe.As<double, byte>(ref result[0]), ref Unsafe.As<double, byte>(ref unilateralSpectrum[0]), (uint)(unilateralSpectrum.Length * Unsafe.SizeOf<double>()));
            result[unilateralSpectrum.Length] = unilateralSpectrum[^1];
            double[] temp = unilateralSpectrum.Reverse().ToArray();
            Unsafe.CopyBlock(ref Unsafe.As<double, byte>(ref result[unilateralSpectrum.Length+1]), ref Unsafe.As<double, byte>(ref temp[0]), (uint)((unilateralSpectrum.Length-1) * Unsafe.SizeOf<double>()));
            return result;
        }

        public static void CalcSlope(double[] x, double[] y,ref double k,ref double b)
        {
            if(x ==null || y==null || x.Length!=2||y.Length!=2)
            {
                k = 0;
                b = 0;
                return;
            }
            k = (y[1] - y[0]) / (x[1] - x[0]);
            b = y[0] - x[0] * k;
        }
        public static int FindFrequencyIndex(this SweepConfigModel model, double freq)
        {
            if (model.SweepItems.Count < 2) return -1;
            uint uintfreq = (uint)(freq * 100);
            for (int i = 0; i < model.SweepItems.Count - 1; i++)
            {
                if ((uint)(model.SweepItems[i].Frequency * 100) <= uintfreq && uintfreq <= (uint)(model.SweepItems[i + 1].Frequency * 100)) return i;
            }
            return model.SweepItems.Count - 1;
        }
        public static  void CalcItemSpectrum(double[] offset, [In] double[] f, [In] double[] fy_array, [In] double deltaf, [In] double[] turn_freq, ref double spectrumdata)
        {
            for (int i = 0; i < offset.Length - 1; i++)
            {
                int index = (int)((turn_freq[i] - turn_freq[0]) / deltaf);
                uint len = (uint)((turn_freq[i + 1] - turn_freq[i]) / deltaf);
                if (offset[i + 1] == offset[i])
                {
                    var x = Math.Pow(10, offset[i] / 10);
                    Calc.Multiply.Multiply(ref fy_array[index], x, len, ref Unsafe.Add(ref spectrumdata, index));
                }
                else
                {
                    double db1_psd = Math.Pow(10, offset[i] / 10) * fy_array[index];
                    double db2_psd = Math.Pow(10, offset[i + 1] / 10) * fy_array[index + len];
                    var x = Math.Log10(db2_psd / db1_psd) / (Math.Log2(turn_freq[i + 1] / turn_freq[i])) * 10;
                    for (int j = 0; j < len; j++)
                    {
                        ref double result = ref Unsafe.Add(ref spectrumdata, index + j);
                        result = Math.Pow(10, Math.Log2(f[index + j] / turn_freq[i]) * x / 10) * db1_psd;
                    }
                }
            }
            ref double temp = ref Unsafe.Add(ref spectrumdata, fy_array.Length - 1);
            temp = Math.Pow(10, offset[^1] / 10) * fy_array[^1];
        }

        public static void CalcRefSpectrum(double[] f_array, double[] y_array, RandomValueType[] k_array, double deltaf, out double[] fy_array, out double[] f, out double[] turningfreq)
        {
            int n = f_array.Length;
            // 计算圆整频率值及其对应的谱值
            double[] round_f = new double[n];
            double[] round_y = new double[n];
            turningfreq = new double[n];
            for (int i = 0; i < n; i++)
            {
                round_f[i] = Math.Ceiling(f_array[i] / deltaf) * deltaf;
                turningfreq[i] = round_f[i];
            }

            // 若第一个点值未知时，需先计算第一个点的值
            if (k_array[0] == RandomValueType.Slope && k_array[1] == RandomValueType.Slope)
            {
                y_array[0] = y_array[1];
            }

            if (k_array[0] == RandomValueType.Slope && k_array[1] == RandomValueType.Value)
            {
                k_array[0] = 0;
                y_array[0] = y_array[1];
            }

            if (k_array[0] == RandomValueType.Slope)
            {
                double kk = y_array[1];
                y_array[0] = y_array[2] / (Math.Pow(10, kk / 10 * Math.Log(f_array[1] / f_array[0], 2)));
                y_array[1] = y_array[2];
                k_array[0] = 0;
                k_array[1] = 0;
            }

            // 计算第二点及其后点的值
            for (int i = 1; i < n; i++)
            {
                if (f_array[i] == 0)
                {
                    double kk = y_array[i];
                    y_array[i] = y_array[i + 1];
                    f_array[i] = Math.Pow(2, (10 * Math.Log(y_array[i + 1] / y_array[i - 1], 10) / kk)) * f_array[i - 1];
                }
                else
                {
                    if (k_array[i] == RandomValueType.Slope)
                    {
                        y_array[i] = Math.Pow(10, y_array[i] / 10 * Math.Log(f_array[i] / f_array[i - 1], 2)) * y_array[i - 1];
                        k_array[i] = 0;
                    }
                }
            }

            // 验证结果值
            double[] k_exam = new double[n - 1];
            for (int i = 0; i < n; i++)
            {
                if (i < n - 1)
                {
                    k_exam[i] = 10 * Math.Log(y_array[i + 1] / y_array[i], 10) / Math.Log(f_array[i + 1] / f_array[i], 2);
                }
            }

            // 计算圆整后频率对应的谱值
            for (int i = 0; i < n; i++)
            {
                if (round_f[i] == f_array[i])
                {
                    round_y[i] = y_array[i];
                }

                if (i < n - 1)
                {
                    if (round_f[i] > f_array[i])
                    {
                        round_y[i] = Math.Pow(10, k_exam[i] / 10 * Math.Log(round_f[i] / f_array[i], 2)) * y_array[i];
                    }
                }

                if (i == n - 1)
                {
                    round_y[i] = Math.Pow(10, k_exam[i - 1] / 10 * Math.Log(round_f[i] / f_array[i], 2)) * y_array[i];
                }

                if (round_f[i] < f_array[i])
                {
                    round_y[i] = Math.Pow(10, k_exam[i - 1] / 10 * Math.Log(f_array[i] / round_f[i], 2)) * y_array[i];
                }
            }

            int parts = k_exam.Length; // 将谱按斜率分成几段，parts为总的段数\
            f = Enumerable.Range(0, (int)((round_f[n - 1] - round_f[0]) / deltaf) + 1).Select(x => x * deltaf + round_f[0]).ToArray();
            fy_array = new double[f.Length];
            fy_array[0] = round_y[0];
            fy_array[^1] = round_y[^1];
            int part_begin = 0;

            for (int i = 0; i < parts; i++)
            {
                fy_array[part_begin] = round_y[i];
                int n_part = (int)Math.Round((round_f[i + 1] - round_f[i]) / deltaf);

                if (k_exam[i] == 0)
                {
                    for (int k = part_begin + 1; k <= part_begin + n_part; k++)
                    {
                        fy_array[k] = fy_array[k - 1];
                    }
                }
                else
                {
                    if (n_part > 0)
                    {
                        for (int k = part_begin + 1; k <= part_begin + n_part; k++)
                        {
                            fy_array[k] = Math.Pow(10, k_exam[i] / 10 * Math.Log(f[k] / f[k - 1], 2)) * fy_array[k - 1];
                        }
                    }
                }

                part_begin += n_part;
            }
        }
        public static void CalcAmpt(this SweepConfigModel model, double freq,ref double value,ref double upstop,ref double upwarn,ref double downstop,ref double downwarn)
        {
            int index = model.FindFrequencyIndex(freq);
            if (index == -1) return;
            value = 0;
            SweepItemModel nowmodel = model.SweepItems[index];
            switch (nowmodel.SweepValueType)
            {
                case SweepValueType.FixedDisplacement:
                    value = freq * nowmodel.Value * freq * 4 * Math.PI * Math.PI / 9800f;
                    break;
                case SweepValueType.FixedVelocity:
                    value = freq * 2 * Math.PI * nowmodel.Value / 9.8f;
                    break;
                case SweepValueType.FixedAcceleration:
                    value = nowmodel.Value;
                    break;
                case SweepValueType.DynamicAcceleration:
                    {
                        if (index == model.SweepItems.Count - 1)
                        {
                            value = nowmodel.Value;
                        }
                        else
                        {
                            SweepItemModel nextmodel = model.SweepItems[index + 1];
                            double k = 0;
                            double b = 0;
                            Tools.CalcSlope([Math.Log10(nowmodel.Frequency), Math.Log10(nextmodel.Frequency)], [Math.Log10(nowmodel.Value), Math.Log10(nextmodel.Value)], ref k, ref b);
                            value = Math.Pow(10, k * Math.Log10(freq) + b); 
                        }
                    }
                    break;
            }
            upstop = Math.Pow(10, nowmodel.UpStop / 20) * value;
            upwarn = Math.Pow(10, nowmodel.UpWarn / 20) * value;
            downstop = Math.Pow(10, nowmodel.DownStop / 20) * value;
            downwarn = Math.Pow(10, nowmodel.DownWarn / 20) * value;
        }
        public static double CalcAmpt(this SweepConfigModel model, double freq)
        {
            int index = model.FindFrequencyIndex(freq);
            if (index == -1) return 0;
            double value = 0;
            SweepItemModel nowmodel = model.SweepItems[index];
            switch (nowmodel.SweepValueType)
            {
                case SweepValueType.FixedDisplacement:
                    value = freq * nowmodel.Value * freq * 4 * MathF.PI*MathF.PI / 9800f;
                    break;
                case SweepValueType.FixedVelocity:
                    value = freq * 2 * MathF.PI * nowmodel.Value / 9.8f;
                    break;
                case SweepValueType.FixedAcceleration:
                    value = nowmodel.Value;
                    break;
                case SweepValueType.DynamicAcceleration:
                    {
                        if (index == model.SweepItems.Count - 1)
                        {
                            value = nowmodel.Value;
                        }
                        else
                        {
                            SweepItemModel nextmodel = model.SweepItems[index + 1];
                            double k = 0;
                            double b = 0;
                            Tools.CalcSlope(new double[] { Math.Log10(nowmodel.Frequency), Math.Log10(nextmodel.Frequency) }, new double[] { Math.Log10(nowmodel.Value), Math.Log10(nextmodel.Value) }, ref k, ref b);
                            value = Math.Pow(10, k * Math.Log10(freq) + b);
                        }
                    }
                    break;
            }
            return value;
        }
        public static double TimeToOCT(this SweepConfigModel model, double time)
        {
            double oct = 0;
            switch(model.SweepType)
            {
                case SweepType.Linear:
                    oct = (model.EndFrequency - model.StartFrequency) / time * 60;
                    break;
                case SweepType.Log:
                default:
                    oct = Math.Log2(model.EndFrequency / model.StartFrequency) / time * 60;
                    break;
            }
            return oct;
        }
        public static double OCTToTime(this SweepConfigModel model, double oct)
        {
            double time = 0;
            switch(model.SweepType)
            {
                case SweepType.Linear:
                    time = (model.EndFrequency - model.StartFrequency) / oct * 60;
                    break;
                case SweepType.Log:
                default:
                    time = Math.Log2(model.EndFrequency / model.StartFrequency) / oct * 60;
                    break;
            }
            return time;
        }

        public static double DisplacementToVelocity(double displacement, double freq)
        {
            return displacement * 2 * Math.PI * freq / 1000;
        }
        public static double DisplacementToAcceleration(double displacement, double freq)
        {
            return displacement * 2 * Math.PI * freq * 2*Math.PI*freq / 9800;
        }
        public static double VelocityToAcceleration(double velocity, double freq)
        {
            return velocity * 2 * Math.PI * freq / 9.8;
        }
        public static double VelocityToDisplacement(double velocity, double freq)
        {
            return velocity * 1000 / (2 * Math.PI * freq);
        }
        public static double AccelerationToDisplacement(double acceleration, double freq)
        {
            return acceleration * 9800 / (2 * Math.PI * freq * 2 * Math.PI * freq);
        }
        public static double AccelerationToVelocity(double acceleration, double freq)
        {
            return acceleration * 9.8f / (2 * Math.PI * freq );
        }
        /// <summary>
        /// 工程量转电压
        /// </summary>
        /// <param name="value">工程量</param>
        /// <param name="sensitivity">灵敏度</param>
        /// <returns>电压</returns>
        public static double QuantitiesToVoltage(double value, double sensitivity)
        {
            return value * sensitivity / 1000;
        }

        /// <summary>
        /// 工程量转电压
        /// </summary>
        /// <param name="value">工程量</param>
        /// <param name="sensitivity">灵敏度</param>
        /// <returns>电压</returns>
        public static float QuantitiesToVoltage(float value, float sensitivity)
        {
            return value * sensitivity / 1000;
        }
        /// <summary>
        /// 电压转工程量
        /// </summary>
        /// <param name="value">电压</param>
        /// <param name="sensitivity">灵敏度</param>
        /// <returns>工程量</returns>
        public static double VoltageToQuantities(double value, double sensitivity)
        {
            return value * 1000 / sensitivity;
        }
        /// <summary>
        /// 电压转工程量
        /// </summary>
        /// <param name="value">电压</param>
        /// <param name="sensitivity">灵敏度</param>
        /// <returns>工程量</returns>
        public static float VoltageToQuantities(float value, float sensitivity)
        {
            return value * 1000 / sensitivity;
        }
        /// <summary>
        /// 计算计数的间隔时间
        /// </summary>
        /// <param name="maxCount">最大计数</param>
        /// <returns></returns>

        public static double CalcInterval(uint maxCount)
        {
            return 1d / (maxCount << 1);
        }
        /// <summary>
        /// 二维数组转置
        /// </summary>
        /// <typeparam name="T"></typeparam>
        /// <param name="source"></param>
        /// <param name="destination"></param>
        /// <param name="rowcount"></param>
        /// <param name="colnumcount"></param>
        public static void ArrayTranspose<T>(ref T source, ref T destination, int rowcount, int colnumcount,int maxcolnumcount)
        {
            if (rowcount == 0 || colnumcount == 0) return;
            for (int i = 0; i < rowcount; i++)
            {
                for (int j = 0; j < colnumcount; j++)
                {
                    Unsafe.Add(ref destination, i * maxcolnumcount + j) = Unsafe.Add(ref source, j * rowcount + i);
                }
            }
        }
        public static double CalcLevel(double currentlevel,double level)
        {
            return Math.Pow(10, level / 20) * currentlevel;
        }
        public static float CalcLevel(float currentlevel, float level)
        {
            return MathF.Pow(10, level / 20) * currentlevel;
        }
        /// <summary>
        /// 解交织
        /// </summary>
        /// <typeparam name="T"></typeparam>
        /// <param name="source"></param>
        /// <param name="destination"></param>
        /// <param name="rowcount"></param>
        /// <param name="colnumcount"></param>
        public static void Deinterweaving<T>(ref T source, ref T destination, int rowcount, int colnumcount,int maxcolnumcount) => ArrayTranspose(ref source, ref destination, rowcount, colnumcount,maxcolnumcount);
        /// <summary>
        /// 压缩数据包
        /// </summary>
        /// <param name="arrays"></param>
        /// <returns></returns>
        public static byte[] CompressionBytes(byte[] arrays)
        {
            if (arrays == null || arrays.Length == 0) return new byte[0];
            using (MemoryStream ms =new MemoryStream())
            {
                using (System.IO.Compression.DeflateStream ds = new System.IO.Compression.DeflateStream(ms, System.IO.Compression.CompressionLevel.SmallestSize, true))
                {
                    ds.Write(arrays, 0, arrays.Length);
                }
                return ms.ToArray();
            }
        }
        /// <summary>
        /// 解压数据包
        /// </summary>
        /// <param name="arrays"></param>
        /// <returns></returns>
        public static byte[] DecompressionBytes(byte[] arrays)
        {
            if (arrays == null || arrays.Length == 0) return new byte[0];
            using (MemoryStream ms = new MemoryStream(arrays))
            {
                using (System.IO.Compression.DeflateStream ds = new System.IO.Compression.DeflateStream(ms, System.IO.Compression.CompressionMode.Decompress,true))
                {
                    using (MemoryStream memoryStream = new MemoryStream())
                    {
                        ds.CopyTo(memoryStream);
                        return memoryStream.ToArray();
                    }
                }
            }
        }
        public static byte[] CompressionWaves<T>(ref T f,uint count) where T : unmanaged
        {
            if(count==0)return new byte[0];
            byte[] temp = new byte[Unsafe.SizeOf<T>() * count];
            Unsafe.CopyBlock(ref temp[0], ref Unsafe.As<T, byte>(ref f), (uint)temp.Length);
            return CompressionBytes(temp);
        }
        public static T[] DecompressionWaves<T>(byte[] f) where T : unmanaged
        {
            if (f ==null || f.Length == 0) return new T[0];
            byte[] temp = DecompressionBytes(f);
            T[] result = new T[temp.Length / Unsafe.SizeOf<T>()];
            Unsafe.CopyBlock( ref Unsafe.As<T, byte>(ref result[0]),ref temp[0], (uint)temp.Length);
            return result;
        }
        public static HistogramData[] Histogram(ref float value,uint count,ref float max,ref float min,int histogramCount = 256)
        {
            if (count == 0 || histogramCount ==0) return new HistogramData[0];
            max = float.MaxValue;
            min = float.MinValue;
            for(int i=0;i<count;i++)
            {
                max = MathF.Max(Unsafe.Add(ref value, i), max);
                min = MathF.Min(Unsafe.Add(ref value, i), min);
            }
            float invert = (max - min) / histogramCount;
            float temomin = min;
            var result =  Enumerable.Range(0, (int)histogramCount).Select(x =>new HistogramData(x * invert + temomin, (x + 1) * invert + temomin, (uint)0)).ToArray();
            for(int i=0;i<count;i++)
            {
                int index = (int)Math.Floor((Unsafe.Add(ref value, i) - min) / invert);
                if (index == histogramCount) index--;
                result[index].H++;
            }
            return result;
        }
        public static HistogramData[] Histogram(ref float value, uint count, float max, float min, int histogramCount = 256)
        {
            if (count == 0 || histogramCount == 0) return new HistogramData[0];

            float invert = (max - min) / histogramCount;
            var result = Enumerable.Range(0, (int)histogramCount).Select(x => new HistogramData(x * invert + min, (x + 1) * invert + min, (uint)0)).ToArray();
            for (int i = 0; i < count; i++)
            {
                int index = (int)Math.Floor((Unsafe.Add(ref value, i) - min) / invert);
                if (index == histogramCount) index--;
                result[index].H++;
            }
            return result;
        }
        /// <summary>
        /// 按SI单位制为数字添加后缀
        /// </summary>
        /// <param name="value">需要转换的数字</param>
        /// <param name="decimals">小数位数</param>
        /// <param name="unit">附加单位</param>
        /// <returns></returns>
        public static string ValueChangeToSI(double value,double exp = 1000, int decimals = 1, string unit = "", bool invalid = false)
        {
            string formatstring = "#0";
            if (!invalid)
            {
                if (decimals > 0)
                {
                    formatstring += ".";
                    for (int i = 0; i < decimals; i++) formatstring += "#";
                }
            }
            else
            {
                formatstring = "N" + decimals;
            }
            if (double.IsNaN(value)) return "NaN";
            if (value == 0)
            {
                return value.ToString(formatstring) + unit;
            }
            if (Math.Abs(value) < 0.0000000000001) return Math.Round(0f, decimals) + unit;
            string SI = "yzafpnμmDkMGTPEZY";
            double d = Math.Log(Math.Abs(value), exp);
            double number = Math.Round(value / Math.Pow(exp, Math.Floor(d)), decimals);
            d += 8;
            string s = SI.Substring((int)d, 1);
            if (s == "D") s = "";
            return number.ToString(formatstring) + s + unit;
        }
        public static void CalcAmpt(ref float value, uint count,float max,float min, ref float highlevel,ref float lowlevel,ref float ampt)
        {
            if (count == 0) return;
            if(count ==1)
            {
                highlevel = value;
                lowlevel = value;
                ampt = 0;
                return;
            }
            var hisdata = Histogram(ref value, count,max,min);
            var orderdata = hisdata.OrderBy(x => x.H).ToArray();
            if((orderdata[^1].H + orderdata[^2].H)/(float)count>=0.05f)
            {
                int index1 = Array.FindIndex(hisdata, x => x.H == orderdata[^1].H);
                int index2 = Array.FindIndex(hisdata, x => x.H == orderdata[^2].H);
                if(index1<index2)
                {
                    lowlevel = hisdata[index1].Center;
                    highlevel = hisdata[index2].Center;
                }
                else
                {
                    lowlevel = hisdata[index1].Center;
                    highlevel = hisdata[index1].Center;
                }
                ampt = highlevel - lowlevel;
            }
            else
            {
                highlevel = max;
                lowlevel = min;
                ampt = max - min;
            }
        }

        public struct HistogramData
        {
            public HistogramData()
            {

            }

            public HistogramData(float start, float end, uint h)
            {
                Start = start;
                End = end;
                H = h;
            }

            public float Start;
            public float End;
            public uint H;
            public float Center => (Start + End) / 2;
        }
    }
}