/*****************************************************************************
*                                                                            *
*   SINFIT.SPL    Copyright (C) 1999-2012 DSP Development Corporation        *
*                               All Rights Reserved                          *
*                                                                            *
*   Author:       Randy Race                                                 *
*                                                                            *
*   Synopsis:     Fits y(x) = A + B * sin(C*x + D) using the FFT             *
*                                                                            *
*   Revisions:    22 Jun 1999  RRR  Creation                                 *
*                  7 May 2012  RRR  Hanning window interpolation method      *
*                                                                            *
*****************************************************************************/


#if @HELP_SINFIT

    SINFIT

    Purpose: Fits y(x) = A + B * sin(C*x + D) using the FFT

    Syntax:  SINFIT(s, wfreq)

                  s - A series, the input.

              wfreq - Optional. A real, the known frequency in radians.
                      Defaults to -1, estimate frequency.


    Returns: A series, the fitted sine curve

             (fit, coef) = sinfit(s)

             returns both the fit and the coefficients as a series.


    Example:
             W1: 5 + 3 * gsin(100, .01, 4, pi) + gnorm(100, .01)

             W2: sinfit(w1);overp(w1, lred)

             Overplots the original data with the calculated sin fit.


             (fit, coef) = sinfit(w1)

             fit is the same series as in W2

             coef == {5.0000, 2.9737, 25.1554, 3.1449}

             Note: since C == coef[3] == 2*pi*F, in this case,
                   F == 25.1554 / (2*pi) = 4.004

    Remarks:
             SINFIT uses the FFT to find the dominate frequency present
             in the series by multiplying the input with a Hanning
             window and using Hanning window interpolation in the
             frequency domain to estimate the frequency, magnitude and
             phase.  The Hanning window interpolation scheme handles
             estimated frequencies that do not precisely correspond to
             FFF frequency bin values.

             The frequency term, C == coef[3] and phase term, D == coef[4],
             are in radians.

             SINFIT3 uses LSINFIT to perform the least squares fit as
             per the IEEE 1241 Standard.

             See SINFIT4 for an implementation of the 4 term iterative
             sinusoid fit as outlined in the IEEE 1241 Standard.

             See SINFIT for a similar routine that uses Hanning window
             interpolation and returns the frequency coefficient in
             radians.

             See SINTREND to fit a sine with a linear trend.

    See Also:
             FFT
             Hanning
             Sinfit3
             Sinfit4
             Sintrend
             Trend
#endif



/* fit y = A + B * sin(C*x + D) */
ITERATE sinfit(s, wfreq)
{
        if (argc < 2)
        {
                if (argc < 1) error("sinfit - input series required");

                /* estimate frequency */
                wfreq = -1;
        }

        /* devolved case */
        if (length(s) <= 1)
        {
                if (wfreq < 0)
                {
                        wfreq = 0;
                }

                return({max(s)}, {0, max(s), wfreq, 0});
        }

        /* original xvals */
        xv = xvals(s);

        if (isxy(s))
        {
                /* interpolate XY to interval series */
                s = xyinterp(s);
        }

        /* best power of 2 FFT length */
        n = bestpow2(s);

        /* hanning windowed FFT */
        f = fft(hanning(demean(s), n));

        /* first half */
        f1 = extract(f, 1, int(n/2));

        /* normalized magnitude */
        fmag = 4 * mag(f1) / length(s);

        if (wfreq < 0)
        {
                /* index of primary frequency */
                kmax = maxidx(fmag);

                /* use left or right side of max for interpolation */
                kleft = (kmax > 1) ? (fmag[kmax - 1] > fmag[kmax + 1]) : 0;

                /* hanning window interpolation */
                if (kleft)
                {
                        dr  = fmag[kmax - 1] / max(fmag);
                        dfx = (kmax - 1) + (1.0 - 2.0 * dr) / (1.0 + dr);
                }
                else
                {
                        dr  = fmag[kmax + 1] / max(fmag);
                        dfx = (kmax - 1) - (1.0 - 2.0 * dr) / (1.0 + dr);
                }

                /* frequency in Hz */
                C = dfx * deltax(fmag);
        }
        else
        {
                /* known frequency, set kmax to get magnitude and phase */
                C    = wfreq / (2 * pi);
                dfx  = C / deltax(fmag);
                kmax = int(dfx) + 1;
        }

        /* convert to radian frequency */
        C *= 2 * pi;

        /* find magnitude and phase */
        dx = dfx - kmax + 1;
        dy = pi * dx;

        /* magnitude */
        if (abs(dx) <= eps)
        {
                B = fmag[kmax];
        }
        else
        {
                B = -fmag[kmax] * (dy / sin(dy)) * (dx - 1.0) * (dx + 1.0);
        }

        /* phase in radians */
        cx = f1[kmax];
        cy = cos(dy) - i * sin(dy);
        cz = cx * cy;

        D = atan2(imag(cz), real(cz));

        /* add pi/2 to reference to sine */
        D += pi/2;

        /* account for X offset */
        D -= C * xoffset(s);

        /* build fit and coefficients */
        fit = B * sin(C * xv + D);

        /* add mean to fit */
        A = mean(s - fit);

        fit += A;

        /* units */
        sethunits(fit, gethunits(s));
        setvunits(fit, getvunits(s));

        /* coefficients */
        coef = {A, B, C, D};

        if (outargc > 1)
        {
                return(fit, coef);
        }
        else
        {
                return(fit);
        }
}