import logging
from queue import Queue
from typing import Any, Callable

import numpy as np
import numpy.typing as npt
from scipy.signal import butter, correlate, sosfilt, sosfilt_zi

from where_fi.collection import CSIMatrix

from ..config import config
from ..visualise import server as visualise

logger = logging.getLogger(__name__)
logger.setLevel(logging.DEBUG)

np.seterr(invalid="ignore")


class Preprocessor:
    def __init__(self) -> None:
        self.prev_entries: Queue[CSIMatrix] = Queue(maxsize=100)
        self.short_term_avg = np.zeros((1,), dtype=np.complex64)
        self.long_term_avg = np.zeros((1,), dtype=np.complex64)
        # self.filter = butter(
        #     5,
        #     config.preprocessing.bandpass.bounds,
        #     fs=config.sample_rate,
        #     btype="band",
        #     output="sos",
        # )

    def remove_sto(self, csi: CSIMatrix) -> CSIMatrix:
        """
        Remove sampling time offsets caused by:
            - Sampling frequency offset
            - Packet detection delay

        According to [1]:
            > Conjugate multiplication and division are the only two methods to
            > eliminate the SFO and PDD.

        No citation or explanation is provided, so not sure why/whether it works.
        Something similar is also done in [2] without explanation.

        [1] - https://tns.thss.tsinghua.edu.cn/wst/docs/sanitization
        [2] - https://doi.org/10.1109/ICC51166.2024.10623053
        """
        # Conjugate multiplication
        csi_remove_sto = np.zeros_like(csi)
        for antenna in range(csi.shape[1]):
            antenna_nxt = (antenna + 1) % csi.shape[1]
            csi_remove_sto[:, antenna, :] = np.multiply(
                csi[:, antenna, :], csi[:, antenna_nxt, :].conj()
            )
        return csi_remove_sto

    def preprocess(
        self,
        h: CSIMatrix,
        visualiser: None
        | Callable[[npt.NDArray[Any], visualise.figures.Figure], None] = None,
    ) -> CSIMatrix:
        # CSI data is not available for pilot subcarriers.
        h_hat: CSIMatrix = np.where(
            np.expand_dims(h[:, 0, 0] == 0, axis=(1, 2)),
            correlate(h, [[[1 / 2]], [[0]], [[1 / 2]]], mode="same"),
            h,
        )

        logger.debug(f"CSI shape: {h_hat.shape}")

        # h_hat = np.multiply(h_hat, h_hat.conj() / abs(h_hat.conj()))
        h_hat = np.nan_to_num(h_hat)

        # h_hat = correlate(h_hat, np.ones((3, 1, 1)) / 3)
        # h_hat = correlate(h_hat, [[[1 / 4]], [[1 / 2]], [[1 / 4]]], mode="valid")

        # Unwrap phase and remove linear fit
        unwrapped = np.unwrap(np.angle(h_hat[:, :, 0]), axis=0).reshape(
            h_hat.shape[0], h_hat.shape[1], 1
        )
        if visualiser:
            visualiser(unwrapped, visualise.figures.Figure.UNWRAPPED_PHASE)
        for antenna in range(h_hat.shape[1]):
            tau, rho = np.linalg.lstsq(
                np.vstack([np.arange(h_hat.shape[0]), np.ones(h_hat.shape[0])]).T,
                unwrapped[:, antenna, 0],
            )[0]
            h_hat[:, antenna, 0] = np.abs(h_hat[:, antenna, 0]) * np.exp(
                1j
                * (
                    np.angle(h_hat[:, antenna, 0])
                    - (tau * np.arange(h_hat.shape[0]) + rho)
                )
            )

        # Skip subcarrierss per config
        h_hat = h_hat[:: config.preprocessing.subcarrier_step, :, :]

        # h_hat = self.remove_sto(h_hat)

        # Assume that all csi matrices will have the same shape
        if self.long_term_avg.shape != h_hat.shape:
            self.long_term_avg = np.zeros(h_hat.shape, dtype=np.complex64)

        self.long_term_avg = (
            self.long_term_avg * (1 - config.preprocessing.moving_average_alpha)
            + h_hat * config.preprocessing.moving_average_alpha
        )

        # Remove long term average, to remove static paths
        return h_hat
        h_hat -= self.long_term_avg

        # Apply bandpass filter to remove low and high frequency noise
        if not hasattr(self, "filter_zi"):
            self.filter_zi = (
                np.expand_dims(sosfilt_zi(self.filter), axis=(-1, -2, -3)) * h_hat
            )

        h_hat_filt, self.filter_zi = sosfilt(
            self.filter, [h_hat], zi=self.filter_zi, axis=0
        )

        return h_hat_filt[0]