#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
Control an FMCW radar based on the Innosent IVS-162.

Daniel Sjoberg, 2018-02-04
"""

# Import libraries
from __future__ import print_function
from pylab import *   # Loads many 'matlab-like' commands
from ADCDACPi import ADCDACPi
import RPi.GPIO as GPIO

# Settings for using the ADCDACPi
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(22, GPIO.OUT)
GPIO.output(22, False)
adcdac = ADCDACPi(1)

# Radar parameters
c0 = 299792458.              # Speed of light in vacuum
f1 = 24e9                    # Start frequency
f2 = 24.425e9                # Stop frequency, using external amplifier
f0 = (f1 + f2)/2             # Center frequency
lambda0 = c0/f0              # Center wavelength
B = f2 - f1                  # Absolute bandwidth
V0 = 2.048                   # Maximum modulation voltage
Rgate = 1.0                  # Gate for detection distance

# Signal processing parameters
N = 128                      # Number of samples in the up-chirp, same number in down-chirp
Npad = 1                     # Factor to expand data vector with zeros
Nave = 1                     # Number of averaging pulses
Ivec = zeros(N)              # Storage for I values
Qvec = zeros(N)              # Storage for Q values
ramp = linspace(0, 1, N)     # Unit ramp function, needed to subtract offset

# Plotting parameters
PlotMode = 'Text'        # Choose between 'LivePlot', 'Capture', 'Text'
Nrange = min(80, int(N/2))   # Number of range values in 'Text' plot

# Define a convenient signal processing function
def EstimateAmplitudeOffset(f, f0):
    """Estimate amplitude and offset of f0 to minimize distance to f."""
    A = sum(f0*f0)
    B = sum(f0)
    C = sum(f0*f)
    D = sum(f)
    N = len(f)
    amplitude = 1/(A*N - B**2)*(N*C - B*D)
    offset = 1/(A*N - B**2)*(-B*C + A*D)
    return(amplitude, offset)

if PlotMode == 'LivePlot':      # Some settings are necessary to enable
    ion()                       # a plot that is updated as the measurements
    fig = figure()              # proceed. Note that this takes significant 
    ax = fig.add_subplot(111)   # processing resources and slows the radar down.
    line1, = ax.plot(Ivec)
    line2, = ax.plot(Ivec)
    xlabel('Range (m)')
    ylabel('Amplitude spectrum')
    xlim(-10, 10)
    ylim(0, 2)

# Start the measurements
StartTime0 = datetime.datetime.now() # Get global start time
adcdac.set_dac_voltage(2, 0.0)       # Enable radar module

# Infinite measurement loop, break by ctrl-c or set PlotMode='Capture'
while True:
    a1 = np.zeros(N, dtype=complex)  # Initialize analytical signal
    a2 = np.zeros(N, dtype=complex)  # Initialize analytical signal
    for m in np.arange(0, Nave):     # Outer loop for averaging
        # Up-chirp pulse
        StartTime1 = datetime.datetime.now()         # Get start time 
        for n in np.arange(0, N):                    # Get N samples
            adcdac.set_dac_voltage(1, V0*n/N)        # Set an increasing voltage
            Ivec[n] = adcdac.read_adc_voltage(1, 0)  # Store I value
            Qvec[n] = adcdac.read_adc_voltage(2, 0)  # Store Q value
        a1 = a1 + (Ivec + 1j*Qvec)/Nave              # Store analytic signal, with averaging taken into account
        EndTime1 = datetime.datetime.now()           # Get end time
        T1 = (EndTime1 - StartTime1).total_seconds() # Compute total time

        # Down-chirp pulse
        StartTime2 = datetime.datetime.now()         # Get start time
        for n in np.arange(0, N):                    # Get N samples
            adcdac.set_dac_voltage(1, V0*(N - n - 1)/N) # Set a decreasing voltage
            Ivec[n] = adcdac.read_adc_voltage(1, 0)  # Store I value
            Qvec[n] = adcdac.read_adc_voltage(2, 0)  # Store Q value
        a2 = a2 + (Ivec + 1j*Qvec)/Nave              # Store analytic signal, with  averaging taken into account
        EndTime2 = datetime.datetime.now()           # Get end time
        T2 = (EndTime2 - StartTime2).total_seconds() # Compute total time

    # Estimate the amplitude and offset of the ramp in up-chirp and down-chirp,
    # then subtract the result
    a10 = a1  # Save raw analytical signal for later plotting
    a20 = a2  # Save raw analytical signal for later plotting
    amplitude, offset = EstimateAmplitudeOffset(a1, ramp)
    a1 = a1 - (amplitude*ramp + offset)
    amplitude, offset = EstimateAmplitudeOffset(a2, ramp)
    a2 = a2 - (amplitude*ramp + offset)

    # Convert to frequency domain and analyze
    # The 'Npad' factor enables zero-padding the data for interpolation and nicer plots
    a1_f = fftshift(fft(a1, n=Npad*N))
    a2_f = fftshift(fft(a2, n=Npad*N))
    f1 = fftshift(fftfreq(Npad*N, d=T1/N))
    f2 = fftshift(fftfreq(Npad*N, d=T2/N))

    # The signal has a huge component at small ranges, this is gated out
    fgate = 4*B/(c0*(T1+T2))*Rgate
    a1_f[abs(f1)<fgate] = 0.0
    a2_f[abs(f2)<fgate] = 0.0

    # Set a simple detection threshold, based on the average value of 
    # the up-chirp signal (could use down-chirp as well). Only used 
    # in the 'Text' plotting mode.
    threshold = mean(abs(a1_f))*10

    # Compute the delta frequencies corresponding to the maximum peaks 
    # in up-chirp and down-chirp
    df1 = f1[abs(a1_f) == max(abs(a1_f))][0]
    df2 = f2[abs(a2_f) == max(abs(a2_f))][0]

    # Convert frequency scales f1 and f2 to range scales x1 and x2
    x1 = c0/(2*B)*T1*f1
    x2 = c0/(2*B)*T2*f2

    # Compute range and velocity based on the delta frequencies
    R = c0/(4*B)*(T2*df2 - T1*df1)
    v = c0/(4*f0)*(df1 + df2)
    A = (max(abs(a1_f)) + max(abs(a2_f)))/2
    if not PlotMode == 'Text':  # Output text data only if not text plotting
        print('R = {0:7.2f} m, v = {1:7.3f} m/s, df1 = {2:7.1f} Hz, df2 = {3:7.1f} Hz, '.format(R, v, df1, df2))


    if PlotMode == 'LivePlot':     # Update plot
        line1.set_xdata(x1)
        line1.set_ydata(abs(a1_f))
        line2.set_xdata(x2)
        line2.set_ydata(abs(a2_f))
        fig.canvas.draw()
    elif PlotMode == 'Text':       # Make a text based plot, updates faster
        rplot = ''
        for n in range(0, Nrange):
            if abs(a2_f[int(Npad*N/2.)+n]) > threshold:
                rplot = rplot + '*'
            else:
                rplot = rplot + '.'
        print(rplot)
    elif PlotMode == 'Capture':   # Make some plots and exit
        figure()                    # Plots can be saved using GUI
        plot(real(concatenate((a10, a20))), 'b-')
        plot(imag(concatenate((a10, a20))), 'r--')
        xlabel('Sample index')
        ylabel('Raw analytical signals')

        figure()
        plot(real(concatenate((a1, a2))), 'b-')
        plot(imag(concatenate((a1, a2))), 'r--')
        xlabel('Sample index')
        ylabel('Analytical signals without ramp')

        figure()
        plot(x1, abs(a1_f), 'b-')
        plot(x2, abs(a2_f), 'r--')
        plot((x1 + x2)/2, threshold*ones(Npad*N), 'g-')
        grid(True)
        xlabel('Range (m)')
        ylabel('Amplitude spectrum')

        show()
        exit()