# coding: utf8

#############################################################################
# anti-drift.py v0.12 by Harald Maiss, DL3HM
#
# For use with the narrow amateur radio band of Es'hail-2 (QO-100)
# Corrects the frequency drift of the LNB using the LEILA-beacons 
# https://www.dl3hm.de/eshail_antidrift.html
#############################################################################

# Standard Python libraries
import time
import re

# Libraries which need to be installed with pip on a plain Python system

# https://www.numpy.org/devdocs/docs/index.html
# Install: pip install numpy
import numpy as np

# https://python-sounddevice.readthedocs.io/en/0.3.12/index.html
# Install: pip install sounddevice
import sounddevice as sd

# https://pyserial.readthedocs.io/en/latest/
# Install: pip install pyserial
import serial

# https://pypi.org/project/simple-pid/
# Install: pip install simple-pid
from simple_pid import PID

# https://pypi.org/project/keyboard/
# Install: pip install keyboard
import keyboard

###############################################################
# set the following parameters according to your needs
###############################################################

COM_beacon = 'COM8'     # CAT-COM-port of the RX listening to the beacon
COM_band = 'COM5'       # CAT-COM-port of the RX listening to the band
baudrate = 57600        # see SDRuno -> RX Control -> SETT. -> CAT

absolute_beacon_frequncy = 10489800000.0    # absolute frequency (Hz) of the upper PSK-beacon 
                                            # for display of the band RX frequency

#print(sd.query_devices())
# the string below is selected from the list generated by the print command above
# do not use numbers, because they change every time a sound device is pluged in/out
virtual_audio_device = "Line 1 (Virtual Audio Cable), Windows DirectSound" 
virtual_audio_sample_rate = 48000   # see VAC Control Panel, when sound device is opened by SDRuno


######################################################
# Configuration of the PID controller
######################################################

frequency_setpoint = 1000.   # frequency setpoint in Hz within the audio signal
                             # no need to tweak this value

# P and I selected by experiment (1st, 2nd parameter)
# no need for a non-zero D (3rd paramter)
pid = PID(0.5, 0.05, 0., setpoint=frequency_setpoint)  
pid.sample_time = 1.    # 1 second update interval to minimize system load of SDRuno
pid.output_limits = (-800, 800)    # max. frequency change in Hz per pid.sample_time
    # -800 is enought to correct the huge drift during the warmup of the LNB 


#############################################################
# Configuration of the serial communication with SDRuno
#############################################################
# https://www.nvadg.org/images/pdfs/radio_manual_kenwood_ts-2000.pdf   see pdf-page 122ff
# Data format 8N1 is the default of pyserial and used by TS2000 (see manual PDF page 58)

serial_beacon = serial.Serial(COM_beacon, baudrate, timeout=0.02) # write blocking, read non-blocking
serial_band = serial.Serial(COM_band, baudrate, timeout=0.02) # write blocking, read non-blocking


sd.default.device = virtual_audio_device
sd.default.dtype = 'float32'     # reassign original default to make it explicit
sd.default.samplerate = virtual_audio_sample_rate  

fft_blocksize = 2048
fft_window = np.hamming(fft_blocksize)    # SDRConsole uses Hamming as default
fft_bin_frequencies = np.fft.rfftfreq(fft_blocksize, 1./sd.default.samplerate)

# buffer for calculation of a running average over the measured beacon frequencies
# 20 < pip.sample_time * virtual_audio_sample_rate / fft_blocksize
center_frequencies = np.full((20,), frequency_setpoint)    

current_frequency = frequency_setpoint


def callback(indata, frames, time, status):
   
   global fft_window, fft_bin_frequencies, center_frequencies, current_frequency
   
   # rfft: FFT with real input signal (normal fft has complex input)
   x = np.abs(np.fft.rfft(fft_window * indata[:,0]))    # don't use standard python abs() for sake of performance
   
   # select only those fft-bins whose amplitude is higher than 0.5 of the max. amplitude
   y = x * (x > (0.5 * np.max(x)))    # factor 0.5 gives lowest ripple in final frequency
   
   # calcualate a runnig average over multiple (20) measurements to reduce ripple
   center_frequencies = np.roll(center_frequencies, 1)
   center_frequencies[0] = np.average(fft_bin_frequencies, weights = y)
   current_frequency = np.mean(center_frequencies)


def CAT_update(serial, control_value):
   serial.write(b'FA;')
   data_string = serial.read(14).decode('ascii')
   FA_regex = r"^FA(\d{11});$"   
   if re.match(FA_regex, data_string) != None:
      frequency = int(re.sub(FA_regex, r"\1", data_string))            
      message_string = "FA{:011d};".format(frequency - control_value)   
      serial.write(message_string.encode('ascii'))
      return frequency - control_value


with sd.InputStream(channels=1, callback=callback, blocksize=fft_blocksize):
   while True:
      control_value = int(round(pid(current_frequency)))

      relative_beacon_frequency = CAT_update(serial_beacon, control_value)
      relative_band_frqeuncy = CAT_update(serial_band, control_value)

      print("band: {:.3f} MHz - audio: {:.1f} Hz - control: {} Hz ".format(
            (absolute_beacon_frequncy - relative_beacon_frequency + relative_band_frqeuncy)/1000. ,
            current_frequency, control_value))   


      # press ESC to quit
      if keyboard.is_pressed('esc'):
         break
               
      time.sleep(pid.sample_time)