# Delay estimation using the sinc interpolation
# ---------------------------------------------
#
# This script is made to compute the delay between two sampled
# sinewaves using the sinc interpolation. 
#
# It can be used by two different ways. 
# 1: Set NBR_TESTS to 1 to calculate one particular delay set 
# by the delay_ps variable.
# 2: Set NBR_TESTS to more than 1 to make different tests 
# with a time shiffting increment = 1/FS/NBR_TESTS in ps. An 
# offset can be added by setting delay_ps to a value different 
# than 0.
# 
# Two sine are created with a delay between them. These sines are
# sampled and quantized before beeing processed.
#
# In the function MinusCorrLag0, we compute function :
# (correlation without lag) = f(delay)
# To realise this, we calculate new samples from one of the sines.
# These samples are computed at time delay from the previous ones.
# The method used to compute these samples is the sinc interpolation.
# Then the value of the correlation between the new samples and the
# samples from the other sine is computed. The function returns 
# the oposite of the correlation's value estimated at lag = 0.
#
# The function (correlation without lag) = f(delay)
# is computed and its maximum is determined by the Brent's method.
# This maximum corresponds to the estimation of the delay between
# the two sines.

# Import packages
import matplotlib.pyplot as plt
import numpy as np
from scipy import signal,optimize
from textwrap import wrap

# Startup values
F = 50                 # Set the frequence of the signal in MHz
FS = 125               # Set the sampling frequency in MHz
NBR_SAMPLES = 64       # Set the number of samples taken for the calculation
ENOB = 12              # ENOB of the ADC
V_MAX = 1              # Voltage range of the ADC (V_MAX = 1V => 2Vpp)
SNR = 1e6              # SNR of the input signal
NBR_TESTS = 100        # Number of tests to be done
RMS_JITTER = 150       # In fs
delay_ps = 0           # Starting value for the tests


# Return the signal quantized
def quantize(signal, nbr_bits, max_val):
    possible_val = np.linspace(-max_val, max_val, 2**nbr_bits)
    switching_val = np.zeros(len(possible_val) - 1)
    for i in range (0, len(possible_val) - 1):
        switching_val[i] = (possible_val[i] + possible_val[i + 1])/2
    sampled = []
    for sample in signal:
        index = 0
        while index < len(switching_val) and sample > switching_val[index]:
            index += 1
        sampled.append(possible_val[index])
    return sampled

# Return the best possible estimation 
def CRLB(snr, nbr_samples, amplitude):
    CRLB = 2*1/snr*amplitude / (nbr_samples*amplitude**2)
    return CRLB

# Return the opposite of the function :
# (correlation without lag) = f(delay).
# The minimum is sent to be used with the Brent's
# interpolation method.
def MinusCorrLag0(delay, fixed_sig, delayed_sig, apod):
    new_points = np.zeros(NBR_SAMPLES)
    nbr_samples = len(fixed_sig)
    abscisse = np.arange(delay, nbr_samples + delay, 1) 
    for m in range(0, nbr_samples):
        for i in range(0, nbr_samples):
            # Compute the points with the sinc interpolation
            new_points[m] += fixed_sig[i]*np.sinc(abs(i - abscisse[m]))
        corr = signal.correlate(new_points*apod, delayed_sig*apod)
        lag0 = 0
        # Interpolation of the correlation at lag = 0
        for i in range(0, NBR_SAMPLES*2 - 1):
            lag0 += corr[i] * np.sinc(abs(i - (nbr_samples - 1)))
    return -lag0

def AddJitter(sampling_points, fs, rms_jitter):
    nbr_samples = len(sampling_points)
    jittered = np.zeros(nbr_samples)
    jittered = sampling_points + np.random.normal(0, rms_jitter*fs/1e9, nbr_samples)
    return jittered 

ts1 = AddJitter(np.arange(NBR_SAMPLES), FS, RMS_JITTER)
ts2 = AddJitter(np.arange(NBR_SAMPLES), FS, RMS_JITTER)

# Create fixed sine
sine1 = np.sin(2*np.pi*F/FS*ts1)
sine1 = sine1 + np.random.normal(0, np.sqrt(1/SNR), NBR_SAMPLES)
sine1 = quantize(sine1, ENOB, V_MAX)
result = np.zeros(NBR_TESTS)
delay = np.zeros(NBR_TESTS)

for e in range (0, NBR_TESTS):
    sine2 = np.sin(2*np.pi*F/FS*ts2 + 2*np.pi*F*delay_ps/1e6)
    sine2 = sine2 + np.random.normal(0, np.sqrt(1/SNR), NBR_SAMPLES)
    sine2 = quantize(sine2, ENOB, V_MAX)

    # test with the hanning window
    hanning = np.hanning(NBR_SAMPLES)
    # Function optimisation using Brent's method
    max = optimize.brent(MinusCorrLag0, (sine1, sine2, hanning), maxiter = 100)
    result[e] = max/FS * 1e6 - delay_ps
    delay[e] = delay_ps
    print('Error = ' + str(result[e])
          + ' ps. ' + 'Real delay = ' + str(delay_ps))
    delay_ps += 1/FS * 1e6 /NBR_TESTS/4
    print('Cramer = ' + str(np.sqrt(CRLB(SNR, NBR_SAMPLES, 1))*1e6/2/np.pi/F))


print("The standart deviation = "  + str(np.std(result))
                         + " ps.")
# Plot everything
if (NBR_TESTS>1):
    fig = plt.figure()
    ax = fig.add_subplot(111)
    title = ax.set_title("\n".join(wrap("Error vs delay. F = " + str(F)
                         + ". FS = " + str(FS) + ". NBR_SAMPLES = "
                         + str(NBR_SAMPLES) +  ". ENOB = " + str(ENOB)
                         + ". SNR = " + str(10*np.log10(SNR)) + " dB."
                         + " The standart deviation = "  + str(np.std(result))
                         + " ps." + " clock jitter = " + str(RMS_JITTER) 
                         + " fs RMS", 65)))
    
    ax.plot(delay,result)
    fig.tight_layout()
    title.set_y(1.05)
    fig.subplots_adjust(top = 0.8)
    plt.ylabel('Error (ps)')
    plt.xlabel('Delay [ps]')
    plt.show()

