using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MIMOTestPlan
{
    public static class PeakSearch
    {
        #region Constants
        const double PeakThreshold = 3.0;   // dB threashold for peak detection
        #endregion

        public static double DbmToVpk(double dDbm)
        {
            const double IMPEDANCE = 50.0;
            if (dDbm < -700)
                return 0.0;
            else
                return Math.Sqrt((Math.Pow(10.0, dDbm / 10.0) * .002 * IMPEDANCE));
        }

        public static float[] ToDBm(float[] data, int points, bool isComplex)
        {
            const double IMPEDANCE = 50.0;
            float[] result = new float[points];
            for (int i = 0, inx = 0; i < points; ++i)
            {
                double real = data[inx++];
                double imag = (isComplex ? data[inx++] : 0.0);
                double mag2 = real * real + imag * imag;
                try
                {
                    result[i] = checked((float)(10.0 * Math.Log10(mag2 * 500.0 / IMPEDANCE)));
                }
                catch
                {
                    result[i] = (mag2 > 1.0 ? 1000f : -1000f);
                }
            }
            return result;
        }

        public static int FindPeak(float[] data)
        {
            if (data == null || data.Length == 0)
                return -1;

            int point = -1;
            double peak = -double.MaxValue;
            int points = data.Length;
            for (int i = 0; i < points; ++i)
            {
                if (data[i] > peak)
                {
                    peak = data[i];
                    point = i;
                }
            }
            return point;
        }

        private static bool IsBinInRange(int iBin, int iStart, int iStop, int iDelta)
        {
            if (iDelta > 0)
                return iBin >= iStart && iBin <= iStop;
            else
                return iBin >= iStop && iBin <= iStart;
        }

        public static int FindQualifiedPeak(float[] data, int index, int iStart, int iStop, int iDelta,
            double yMin, double yMax)
        {
            // If searching for a peak to the left, iDelta will be negative and 
            //   iStart will be bigger than iStop so we will be looking backwards 

            // check iStart/iStop limits..
            if ((iDelta < 0 && index - iStop <= 1) || (iDelta > 0 && iStop - index <= 1))
                return -1;

            int i = index;

            while (IsBinInRange(i, iStart, iStop, iDelta))  // as long as we haven't run outta bins 
            {
                int iLast = i;
                double current = data[i];
                double prev_val = data[i - iDelta];

                // Search for a local maximum within the threshold prev_val  
                //   must be less than the current bin, next_val must be <= the current bin 
                //
                if (current >= yMin && current <= yMax &&
                    current > prev_val &&
                    (i == iStop || current >= data[i + iDelta])
                   )
                {
                    // Qualify the local maximum by finding a trace value that is
                    //   at least a "threshold" lower than the local peak. 
                    //   First, look to "right".   This does not require that the
                    //   points on either side of the peak decrease monotonically, 
                    //   it just looks for something that is eventually a peak
                    //   threshold below the current point. 
                    // 
                    //   If isMaxFound == true, a value was found which is larger 
                    //   than the current value before peakThreshold was found,
                    //   so bail out, we don't have a peak 

                    int j = i;
                    bool isMinFound = false, isMaxFound = false;

                    // Look to the right for peakThreshold below the current peak
                    do
                    {
                        if (j == iStop)
                            break;
                        j += iDelta;
                        double val = data[j];

                        /* Do we have a peak below the current peak? */
                        isMinFound = (current - val >= PeakThreshold);

                        /* Have we found a value to disqualify the peak? */
                        if (!isMaxFound)
                            isMaxFound = (current < val);
                    } while (!isMinFound && !isMaxFound);

                    // Also qualify a local maximum if the points to the right 
                    //   monotonically decrease to the iStop value.

                    if (!isMaxFound && j == iStop && j != i)
                        isMinFound = true;

                    // If isMaxFound, the test has failed so update i and be done with it 
                    if (isMaxFound)
                        i = j - iDelta;
                    else if (isMinFound)
                    {
                        // If we have qualified the local maximum to the "right",
                        //   then check to the "left" to qualify as a peak.

                        isMinFound = isMaxFound = false;
                        j = i;

                        do
                        {
                            if (j == iStart)
                                break;
                            j -= iDelta;
                            double val = data[j];

                            // Have we found a peak to the left?
                            isMinFound = current - val >= PeakThreshold;

                            // Have we found a value to disqualify the current peak? 
                            if (!isMaxFound)
                                isMaxFound = current < val;
                        } while (!isMinFound && !isMaxFound);

                        // If a trace value was found that is at least a "threshold" lower
                        //   than the local peak to the "left" - then a peak has been found. 
                        if (isMinFound)
                            return i;
                    }
                }

                // No peak yet, increase i and keep going
                i += iDelta;
                if (iLast == i)
                    break;  // no change, hit a endless loop
            }

            return -1;  // no peak
        }

        public static int FindNextSmallerPeak(float[] data, int index)
        {
            int bin;
            int iPeak = -1;
            double yPeakDelta = 0.0;

            if (data == null || data.Length == 0 || index < 0)
                return -1;

            double yReference = data[index];

            // finds all the peak to the left of current index, and remember
            //   the highest peak as each peak is found.  This peak has to be 
            //   smaller than current marker y.  It assumes that "next
            //   peak" key is usually pressed after "marker to peak" key

            int iMax = index - 1;
            while ((bin = FindQualifiedPeak(data, iMax, iMax, 0, -1,
                                            -float.MaxValue, float.MaxValue)) >= 0)
            {
                double y = data[bin];
                if (y < yReference)
                {
                    if (iPeak < 0)
                    {
                        // first peak less than current marker y
                        yPeakDelta = yReference - y;
                        iPeak = bin;
                    }
                    else if (yReference - y < yPeakDelta)
                    {
                        // found a higher peak
                        yPeakDelta = yReference - y;
                        iPeak = bin;
                    }
                }
                iMax = bin - 1; // decrease area of searching
            }

            // start with all the bins to the right of current index
            int iMin = index + 1;
            while ((bin = FindQualifiedPeak(data, iMin, iMin, data.Length - 1, 1,
                                            -float.MaxValue, float.MaxValue)) >= 0)
            {
                double y = data[bin];
                if (y < yReference)
                {
                    if (iPeak < 0)
                    {
                        // first peak less than current marker y
                        yPeakDelta = yReference - y;
                        iPeak = bin;
                    }
                    else if (yReference - y < yPeakDelta)
                    {
                        // found a higher peak
                        yPeakDelta = yReference - y;
                        iPeak = bin;
                    }
                }
                iMin = bin + 1; // decrease area of searching
            }

            return iPeak;
        }
    }
}
