import serial
import queue
import threading
import numpy as np
from scipy.signal import blackmanharris


class InductionHeater:
    """Interface to the MHS5200A function generator driving the LC tank"""
    def __init__(self, port, induction_min, induction_max):
        self.port = port
        self.induction_min = induction_min
        self.induction_max = induction_max
        self.queue = queue.Queue(1)

    def start(self):
        self.serial = serial.Serial(self.port, 57600)
        self.thread = threading.Thread(target=self.thread_target)
        self.thread.start()

    def thread_target(self):
        while True:
            amount = self.queue.get()
            if amount is None:
                break

            amount = max(min(amount, 0.5), -0.5)
            freq = ((self.induction_min + self.induction_max)/2
                    + amount*(self.induction_max - self.induction_min))

            command = ":s1f{:010d}\n".format(int(freq*1e2))
            self.serial.write(command.encode())
            self.serial.readline()

    def set(self, amount):
        self.queue.put(amount, block=False)

    def stop(self):
        self.queue.put(None, block=True)
        self.thread.join()
        self.serial.close()


# https://gist.github.com/endolith/255291
def parabolic(f, x):
    xv = 1/2. * (f[x-1] - f[x+1]) / (f[x-1] - 2 * f[x] + f[x+1]) + x
    yv = f[x] - 1/4. * (f[x-1] - f[x+1]) * (xv - x)
    return (xv, yv)


class Stabilizer:
    def __init__(self, freq_sample, amp_threshold, freq_target, k, tuner):
        self.freq_sample = freq_sample
        self.amp_threshold = amp_threshold
        self.freq_target = freq_target
        self.k = k
        self.tuner = tuner

    def input(self, samples):
        spectrum = np.abs(np.fft.fft(samples*blackmanharris(len(samples)))[0:len(samples)//2])
        for i in range(len(spectrum)//100):
            spectrum[i] = 0
            spectrum[-i] = 0
        i = np.argmax(spectrum)
        true_i, amplitude = parabolic(spectrum, i)
        freq = 0.5 * self.freq_sample * true_i / len(spectrum)

        if amplitude > threshold:
            tuning = (freq - target_freq)*k
        else:
            tuning = 0.0
        self.tuner.set(tuning)