# ----------------------------------------------------------------------------
#
#  Copyright (C) 2013-2015 Fons Adriaensen <fons@linuxaudio.org>
#    
#  This program is free software; you can redistribute it and/or modify
#  it under the terms of the GNU General Public License as published by
#  the Free Software Foundation; either version 3 of the License, or
#  (at your option) any later version.
#
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#
#  You should have received a copy of the GNU General Public License
#  along with this program.  If not, see <http:#www.gnu.org/licenses/>.
#
# ----------------------------------------------------------------------------


import numpy as np
from math import pi, floor, atan2
from cmath import polar


def adjust_freq (Freq, Fsamp, nsamp):
    """ 
    Adjust 'Freq' for an integer number of cycles in 'nsamp' samples.
    """
    t = nsamp / Fsamp
    k = int (Freq * t + 0.5)
    return k / t


def gen_sinewave (Amp, Freq, Fsamp, nsamp):
    """
    Generate sine wave test signal.
    """
    # Phase array, better use double precision for this.
    w = 2 * pi * Freq / Fsamp
    R = np.linspace (0, w * nsamp, nsamp, endpoint = False, dtype = np.float64)
    S = Amp * np.sin (R)
    return S    
        

def gen_complex (Freq, Fsamp, nsamp, window = True):
    """
    Generate windowed reference signal for selective measurement.
    """
    # Phase array, better use double precision for this.
    w = 2 * pi * Freq / Fsamp
    R = np.linspace (0, w * nsamp, nsamp, endpoint = False, dtype = np.float64)
    # Generate complex (cos(R), sin(R))
    S = np.exp ((0 + 1j) * R)
    if window:
        # Phase array for raised cosine window.
        W = np.linspace (0, 2 * pi, nsamp, endpoint = False, dtype = np.float32)
        S *= (1 - np.cos (W))
    return S


def sigdetect (Ainp, Aref):
    """
    Frequency selective measurement. Detect signal in Ainp at the
    frequency determined by Aref, generated by 'gen_complex' above.
    Equivalent to one bin of an FFT.
    Returns amplitude and phase in radians.
    """
    return polar (2 * np.dot (Ainp, Aref) / Aref.shape [0])


def sigtrack (Ainp, Freq, Fsamp, size, step):
    """
    """
    R = gen_complex (Freq, Fsamp, size, True)
    n = (Ainp.shape [0] - size) // step + 1
    M = np.zeros ((n,), dtype = np.float64)
    A = np.zeros ((n,), dtype = np.float64)
    da = step * Freq / Fsamp
    da -= floor (da)
    r = 0
    k = 0;
    for i in range (n):
        m, a = polar (np.dot (Ainp [k:k + size], R))
        a /= -2 * pi
        d = a - r - da + 0.5
        d -= floor (d) + 0.5
        M [i] = 2 * m / size
        A [i] = d
        r = a
        k += step
    return M, A


