#!/usr/bin/env python3

# For David:
# the idea for the tool is to parse an SPLL debug log (sample below) generated by the wrpc swisstool
# and plot it with live refresh and selection of traces/labels.
# the log consists of traces and events, grouped per signal sources. The sources are the "main", "helper", "aux0..3" and "ext"
# PLLs.
# What we need here:
# - parsing the log either from stdin (in this case, with live refresh) or a text file supplief from the command line
# - live refresh of the plot (inteval can be a constant or a command line parameter)
# - a freeze button/checkbox (matplotlib's internal GUI should be sufficient)
# - stretching the time base. the "sample_id" is the number of the current sample, but as the SoftPLL does not work with a 
#   fixed sampling rate, an additional trace called "time_ms" is provied with each source, with the current time in milliseconds. we need some
#   sort of stretching/mapping of the horizontal scale per each trace to correctly display the time relations between different PLLs.
#   No need for correct singal-theoretic resampling, just a LERP stretch should be fine.
# - display of event labels (a vertical line with a label attached to it at the time when the event occured)
# - sane shutdown (clicking close button on the plot window should close the program, not make it freeze)

# sample log format:

# source signal_name=value(int) [event=event_name]

# example log from the helper PLL:

# helper tag=1401447 ref=4054630 time_ms=19476 sample=27492 y=25564 err=1 
# helper tag=1729125 ref=4382310 time_ms=19481 sample=27512 y=25565 err=-1 
# helper tag=2056808 ref=4709990 time_ms=19487 sample=27532 y=25564 err=2 
# helper tag=2384487 ref=5037670 time_ms=19492 sample=27552 y=25564 err=1  event=lock-acquired

# For the moment I'm too ~~lazy~~ dumb to do a clicky-clicky GUI for this tool, so just edit the lines below
# to configure what you want to see on the screen - Tom

#PLOT_SOURCES = { "helper", "main" }
#PLOT_SIGNALS = { "y", "err", "phase_current", "phase_target", "dtag", "dref" }
PLOT_SOURCES = { "main" }
PLOT_SIGNALS = { "err", "y", "dtag" }
PLOT_EVENTS = True

import sys
from matplotlib import animation
from matplotlib import pyplot as plt
import numpy as np
import threading
import logging
import copy

traces = {
    "helper" : None,
    "main" : None,
    "ext" : None,
    "aux0" : None,
    "aux1" : None,
    "aux2" : None,
    "aux3" : None
};

class SPLLTrace:
    def __init__(self):
        self.data = {}
        self.events = []

    def parse( self, tokens ):
        for t in tokens:
            #print(t)
            try:
                (name, value_str) = t.split('=')
            except ValueError:
                continue

            if name == "event":
                self.events.append( ( value_str, self.data["sample_id"][-1] ) )
            if not name in self.data:
                self.data[name] = []

            self.data[name].append( int( value_str ) )

            #print(name, value_str)

    def remove_outliers(self, x, threshold ):
        y=copy.copy(x)
        win = 3
        for i in range(win, len(x) - win ):
            med = np.median( x[i-win:i+win+1] )
            err = np.abs (x[i] / med )
            if ( err > threshold or err < (1.0/threshold) ):
                y[i] = med
        return y

    def calc_diff(self):
        dref = np.unwrap( np.diff( self.data["ref"] ), period=1<<24 )
        dtag = np.unwrap( np.diff( self.data["tag"] ), period=1<<24 )
        
        dref = self.remove_outliers( dref, 1.05 )
        dtag = self.remove_outliers( dtag, 1.05 )
        dref = dref - np.linspace( np.mean(dref), np.mean(dref), len(dref) )
        dtag = dtag - np.linspace( np.mean(dtag), np.mean(dtag), len(dtag) )

        self.data["dref"] = np.cumsum(dref)
        self.data["dtag"] = np.cumsum(dtag)
        print("calcdiff ", len(self.data["dref"]))
        
def acquisition_thread():
    logging.info("ACQ Thread: starting")
    nsamples = 0
    while True:
        l = sys.stdin.readline().rstrip('\n').rstrip('\r').lstrip(' ').lstrip('\t').rstrip(' ')
        t = l.split(' ')
        if(len(t) < 1):
            continue
        src_id = t[0]
        if src_id in traces:
            if traces[src_id] == None:
                traces[src_id] = SPLLTrace()

            traces[src_id].parse( t[1:] )
            if nsamples % 1000 == 0:
                traces[src_id].calc_diff()
        nsamples += 1

    logging.info("ACQ Thread: quitting")


def animated_refresh(args):
    ax.clear()

    for src in traces.keys():
        if not src in PLOT_SOURCES:
            continue
        trace = traces[src]
        print(trace.data)
        if trace == None:
            continue
        for signal in trace.data.keys():
            print("Plot[?] ", src, signal )
            if not signal in PLOT_SIGNALS:
                continue
            #print(trace.data["sample"]  )
            lplot = len(trace.data[signal])
            lsamp = len(trace.data["sample"])
            if lsamp < lplot:
                lplot = lsamp
            print( "SPLOT: %d" % lplot)
            ax.plot( trace.data["sample"][0:lplot], trace.data[signal][0:lplot], label="%s:%s" % ( src, signal ) )
            plt.legend()


acq_thread = threading.Thread( target=acquisition_thread )
acq_thread.start()

fig, ax = plt.subplots( 1, 1 )
anim = animation.FuncAnimation(fig, animated_refresh, interval=5000)
plt.title("SoftPLL debug")
plt.xlabel("Time [samples]")
plt.ylabel("Trace value [arbitrary units]")
plt.show()

acq_thread.join()
