/*****************************************************************************
*                                                                            *
*   PHASEDIFF.SPL Copyright (C) 2015 DSP Development Corporation             *
*                           All Rights Reserved                              *
*                                                                            *
*   Author:      Randy Race                                                  *
*                                                                            *
*   Synopsis:    Computes the phase difference between two sinusoids         *
*                                                                            *
*   Revisions:   28 Dec 2015  RRR  Creation                                  *
*                                                                            *
*****************************************************************************/


#if @HELP_PHASEDIFF

    PHASEDIFF

    Purpose: Computes the phase difference between two sinusoids.

    Syntax:  PHASEDIFF(s1, s2, freq)

             (p, f, mag1, mag2) = PHASEDIFF(s1, s2, freq)

                 s1 - A series, the first input sinusoid.

                 s2 - A series, the second input sinusoid.
 
               freq - Optional. A real, the known frequency. Defaults to
                      -1, estimate the frequency.

    Returns: A real, the phase difference in radians.

             (p, f, mag1, mag2) = PHASEDIFF(s1, s2, freq) returns the
             phase difference, frequency and magnitudes of the two
             sinusoids.

    Example:
             W1: gcos(1000, 1/1000, 4)
             W2: gcos(1000, 1/1000, 4, -pi/3)
             W3: {phasediff(w1, w2)}

             W3 == -1.047198 or -pi/3.

    Example:
             W1: gcos(1000, 1/1000, 4) + gnorm(1000, 1/1000, 4) / 10
             W2: gcos(1000, 1/1000, 4, -pi/3) + gnorm(1000, 1/1000, 4) / 10
             W3: {phasediff(w1, w2)}

             W3 == -1.044738 or approximately -pi/3.

    Example:
             W1: 10 * gcos(1000, 1/1000, 4)
             W2: 3 * gcos(1000, 1/1000, 4, -pi)
             W3: (p, f, m1, m2) = phasediff(w1, w2);{p, f, m1, m2}

             W3 == {-3.141593, 4, 10, 3} the phase difference,
             frequency and magnitudes of the input sinusoids in W1
             and W2.

    Remarks:
             PHASEDIFF uses SINFIT3 to compute the least squares fit
             of a sinusoid to a known frequency as per the IEEE 1241
             Standard. If the input frequency is unspecified, the
             frequency is estimated by locating the dominant
             frequency of the FFT and using analytical rectangular
             window interpolation to determine the precise frequency. 
             The FFT interpolation handles estimated frequencies that
             do not lie on a unique FFT bin.

             The phase difference is always negative and expressed in
             radians.

    See Also:
             Addphase
             Angle
             Fft
             Phase
             Sinfit3
#endif


/* phase difference in radians of two sinusoids with the same frequency */
ITERATE phasediff(s1, s2, f)
{
        local fit1, fit2, coef1, coef2, p;

        if (argc < 3)
        {
                if (argc < 2)
                {
                        if (argc < 1) error("phasediff - input series required");

                        s2 = {};
                }
                else if (isscalar(s2))
                {
                        f  = s2;
                        s2 = {};
                }
                else
                {
                        f = -1;
                }
        }

        /* sin fit */
        (fit1, coef1) = sinfit3(s1, f);

        if (iscomplex(coef1))
        {
                /* can't fit a sin */
                coef1 = zeros(length(coef1), 1);
        }

        if (length(s2) > 0)
        {
                /* phase difference, use frequency of s1 */
                (fit2, coef2) = sinfit3(s2, coef1[2]);

                if (iscomplex(coef2))
                {
                        /* can't fit a sin */
                        coef2 = zeros(length(coef2), 1);
                }
        }
        else
        {
                /* set coef2 to find phase of input */
                coef2 = coef1;

                /* phase */
                coef2[3] = 0;
        }

        /* phase difference in radians */
        p = coef2[3] - coef1[3];

        /* force difference to be negative */
        if (p > 0) p -= 2 * pi;

        if (outargc > 1)
        {
                /* phase difference, frequency, magnitudes */
                return(p, coef1[2], coef1[1], coef2[1]);
        }
        else
        {
                /* just phase difference */
                return(p);
        }
}