#!/usr/bin/env python

from time import sleep
from collections import deque
import numpy as np
import scipy.signal

from zoetrope import aniplot as plt
from bstrd import BS, bsstream
plt.blit = False



# config
cam_name = "SARES11-XMI125-C4P1"
xmin = 0
xmax = 400
ymin = 400
ymax = 700
proj_axis = 0
um_per_px = 2.28
buffer_length = 100



def get_data(cam):
    img = 0
    for i in range(1):
        img += cam.get()
        next(bsstream)
    cropped = img[xmin:xmax, ymin:ymax]
    projx = cropped.mean(axis=proj_axis)
    projy = cropped.mean(axis=int(not proj_axis))
    return cropped, projx, projy


def find_spacing(py, extend=5):
    py -= py.mean()

    if extend > 1:
        py = np.concatenate((py, np.zeros(len(py) * extend)))

    spectrum = np.abs(np.fft.fft(py))
    freq = np.fft.fftfreq(len(spectrum))

    which = (freq > 0.005)
    spectrum = spectrum[which]
    freq = freq[which]

    index_peaks = scipy.signal.find_peaks(spectrum)[0]
    peaks_pos = freq[index_peaks]
    peak_height = spectrum[index_peaks]

    cut_freq = peaks_pos[np.argmax(peak_height)]
    return freq, spectrum, cut_freq


def make_xs(arr):
    return np.arange(len(arr))


# create channel
cam = BS(cam_name + ".roi_signal_x_profile", modulo=5)
cropped, width, proj = get_data(cam)
pixel_proj  = make_xs(proj)
pixel_width = make_xs(width)


plt.fig.set_figheight(12)
plt.fig.set_figwidth(9)

plt.suptitle(cam_name)

plt.subplot(321)
plt.title(f"cropped image\n[{xmin}:{xmax}, {ymin}:{ymax}]")
pimg = plt.imshow(cropped, origin="lower")

plt.subplot(322)
plt.title("projection")
pln_proj = plt.plot(proj, pixel_proj)

plt.subplot(323)
plt.title("projection")
pln_width = plt.plot([0])

plt.subplot(324)
plt.title("FFT")
plt.xlabel("frequency in 1/pixel")
pln_fft_spec = plt.plot([0])
plt.xlim(0,0.2)
pln_fft_cut  = plt.plot([0])

plt.subplot(313)
plt.title(f"spacing assuming {um_per_px} μm per pixel")
pln_spac = plt.plot([0], 'o')
plt.ylim(0,250)
plt.tight_layout()


spacings = deque(maxlen=buffer_length)

for counter in plt.show():
    print(counter)
    next(bsstream)


    cropped, width, proj = get_data(cam)

    pimg.set(cropped)
    pln_proj.set(proj, pixel_proj)
    pln_width.set(pixel_width, width)

    freq, spectrum, cut_freq = find_spacing(proj, extend=10)
    spac = um_per_px / cut_freq
    spacings.append(spac)

    pln_spac.set(make_xs(spacings), spacings)
    pln_fft_spec.set(freq, spectrum)
    pln_fft_cut.set([cut_freq]*2, [spectrum.min(), spectrum.max()])
    pln_fft_spec.ax.set_title(f"FFT\ncurrent spacing: {spac:.1f} μm")


    # this, I need to move into the library
    ps = [pln_proj, pln_width, pln_spac, pln_fft_spec]
    for p in ps:
        p.ax.relim()
        p.ax.autoscale_view()



bsstream.close()