import numpy as np
from datetime import datetime, timedelta


class PlotData():
    def __init__(self):

        #resolution * timeout must be at least 2-3 times higher then the slowest refresh rate
        self.resolution = 100  # ms
        self.timeout = 100 #cycles
        self.data = {"time": np.array([])}
        self.start_time = datetime.utcnow()
        self.queues = {}

    def clear(self):
        print("clear")
        for key in self.data:
            self.data[key] = np.array([])
        self.start_time = datetime.utcnow()

    def get(self, key):
        return self.data.get(key, np.full_like(self.data["time"], np.nan, dtype=np.double))

    def append_data(self, sensor_name, data):
        """

        """
        if sensor_name not in self.data:
            self.queues[sensor_name] = ([],[])
            self.data[sensor_name] = np.full_like(self.data["time"], np.nan, dtype=np.double)
        for time, dat in zip(data[0], data[1]):
            self.queues[sensor_name][0].append((time-self.start_time)/ timedelta(milliseconds=self.resolution))
            self.queues[sensor_name][1].append(dat)
        self.extend_timeline()
        self.sort_in_queue()
        #self.drop_old()

    def extend_timeline(self):
        #extend numpy arrays with passed time
        #unknown values are filled with nans
        end = (datetime.utcnow()-self.start_time)/timedelta(milliseconds=self.resolution)
        print(end, self.start_time)

        self.data["time"] = np.arange(0, end, 1).astype(np.double)
        for key in self.data:
            if key != "time":
                pad_length = self.data["time"].size-self.data[key].size
                self.data[key] = np.pad(self.data[key], (0,pad_length), mode="constant", constant_values=np.nan)

    def sort_in_queue(self):
        #linear interpolation and adding it to the array
        for key in self.queues:
            time = np.array(self.queues[key][0])
            data = np.array(self.queues[key][1])
            if time.size > 1:
                inter = np.interp(self.data["time"], time, data)
                self.data[key] = inter#np.where(np.isnan(inter), self.data[key], inter)

    def drop_old(self):
        for key in self.queues:
            time = np.array(self.queues[key][0])
            old = self.data["time"][-1]-(self.timeout)
            if time.size>2:
                drop_index = (np.where(time < old)[0])[-1]
                self.queues[key] = (self.queues[key][0][drop_index:],self.queues[key][1][drop_index:])