/*****************************************************************************
*                                                                            *
*   DEMODPM.SPL  Copyright (C) 2019 DSP Development Corporation              *
*                               All Rights Reserved                          *
*                                                                            *
*   Author:      Randy Race                                                  *
*                                                                            *
*   Synopsis:    Phase Demodulation using the Hilbert Transform              *
*                                                                            *
*   Revisions:   10 Jan 2019 RRR  Creation                                   *
*                                                                            *
*****************************************************************************/

#if @HELP_DEMODPM

    DEMODPM

    Purpose: Demodulates a phase modulated series using the Hilbert Transform

    Syntax:  DEMODPM(s, fc, pdev, p0)

              s - the input series

             fc - Optional, a real. The center frequency of the phase
                  modulated result. Defaults to -1, estimate the center
                  frequency using the FFT.

           pdev - Optional, a real. The phase deviation. Defaults to 
                  pi / 2;

             p0 - Optional, a real. The initial phase. Defaults to 0.0.

    Returns: A series or array, the demodulated series.

    Example:
             W1: gtriwave(1000,.001, 4)
             W2: W1 * 100 + 20
             W3: modpm(w1)
             W4: demodpm(w3)

             W2 represents the scaled information signal and W3 is the
             resulting phase modulated signal. The amplitude of W2
             determines the instantaneous phase of W3. The instantaneous
             phase of W3 ranges from:

                             min(w3) == 0 radians

                                     to

                             max(w3) == pi / 2 radians


             W4 is the demodulated result.

    Remarks:
             DEMODPM uses HILB to calculate the Hilbert Transform.

             If FC is < 0, the center frequency is estimated using
             SINFIT3.

             See MODPM to phase modulate a series.

    See Also:
             Demodam
             Demodfm
             Hilb
             Modam
             Modfm
             Modpm
             Sinfit3

#endif


/* demodulate a phase modulated series using the Hilbert transform */
ITERATE demodpm(s, fc, pdev, p0)
{
        local h, pm;

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

                                /* estimate frequency */
                                fc = -1;
                        }
                        
                        pdev = pi / 2;
                }

                p0 = 0;
        }
        
        if (fc < 0)
        {
                /* estimate frequency */
                (fit, coef) = sinfit3(s);

                fc = coef[2];
        }

        /* hilbert transform */
        h  = hilb(s) * exp(-i * 2 * pi * (fc * xvals(s) + p0));

        /* unwrapped phase */
        pm = demodpm_zuphase(h);

        /* amplitude scaling */
        if (pdev > 0)
        {
                pm /= pdev;
        }

        /* restore the offset */
        setxoffset(pm, xoffset(s));

        return(pm);
}



static udmv = "";

/* unwrapped phase from complex values */
demodpm_zuphase(z, delay)
{
        local x, y, xty, dp, phi1, phi, phi1, f;

        if (argc < 2)
        {
                if (argc < 1) error("zuphase - input series required");
                
                delay = 0;
        }

        /* frequency values */
        f = isxy(z) ? xvals(z) : {};

        /* first phase value |phi1| <= pi */
        phi1 = (real(z[1]) == 0) ? 0 : atan2(imag(z[1]), real(z[1]));

        /* imag(z) / real(z) avoiding divide by 0 */
        z = tan(atan2(imag(z), real(z)));

        z[find(isnan(z))] = 0;

        /* z[i] and z[i-1] */
        y = extract(z, 1, length(z) - 1);
        x = extract(z, 2, -1);

        xty = x * y;

        /* force 1/0 to inf */
        udmv = getconf("use_default_math_value");
        setconf("use_default_math_value", "0");

        /* phase difference, from -pi/2 to pi/2 */
        dp = atan((x - y) / (1 + xty));

        /* integrate, first dp set to 0.0 */
        phi = extract(cumsum(dp), delay, -1, 0.0);

        if (not(isnan(phi1)))
        {
                phi += phi1;
        }


        /* handle DC phase value */
        phi0 = phi1;

        if (length(f) > 0)
        {
                if (f[1] == 0)
                {
                        phi[1] = phi0;
                }

                /* explicit frequency values, return XY */
                phi = xy(f, phi);
        }
        else
        {
                phi[1] = phi0;
        }

        setconf("use_default_math_value", udmv);
        udmv = "";

        if (outargc > 1)
        {
                /* separate components */
                return(phi, xvals(phi));
        }
        else
        {
                return(phi);
        }
}


/* error handler */
demodpm_zuphase_error(errnum, errmes)
{
        if (strlen(udmv) > 0)
        {
                setconf("use_default_math_value", udmv);
                udmv = "";
        }

        error(errmes);
}