from datetime import datetime, timedelta

import numpy as np


class Plot_Data:
    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)

        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, left=np.nan, right=np.nan)
                self.data[key] = 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]
                if len(drop_index) != 0:
                    drop_index = drop_index[-1]
                    self.queues[key] = (
                        self.queues[key][0][drop_index:],
                        self.queues[key][1][drop_index:],
                    )
                    print(drop_index)