"""Motion Detection example
This example shows how to use the CSI framework to connect to a FeitCSI host and
detect changes in the environment
"""

import logging
from queue import Queue

import numpy as np
import numpy.typing as npt

from where_fi.application import CSIApplication
from where_fi.collection.ingest import RealtimeCSIProducer

# Connect to a FeitCSI host
producer = RealtimeCSIProducer()
app = CSIApplication(producer)

# Store historical CSI data to compare with the current data
# This allows us to detect sudden changes in the environment,
# most likely to be cause by motion

# This is stored individually for each receiving antenna, for each subcarrier
historical: dict[tuple[int, int], Queue[np.complex64]] = {}

MAGN_THRESHOLD = 20
PHASE_THRESHOLD = 0.5


@app.on_process
def _(antenna_data: npt.NDArray[np.complex64]) -> None:
    for antenna in range(antenna_data.shape[1]):
        for subcarrier in range(antenna_data.shape[0]):
            # Get the current subcarrier data
            current = antenna_data[subcarrier, antenna, 0]

            # Get the historical data for this antenna and subcarrier
            if (antenna, subcarrier) not in historical:
                historical[(antenna, subcarrier)] = Queue(maxsize=100)
            historical_data = historical[(antenna, subcarrier)]

            # Calculate the average of the historical data
            mean: np.complex64 = np.mean(historical_data.queue)

            # If we have enough historical data, compare it with the current data
            if historical_data.full():
                historical_data.get()

            # Add the current data to the historical data
            historical_data.put(current)

            # Compare the current data with the historical data
            if (
                np.abs(mean - current) > MAGN_THRESHOLD
                or np.abs(np.angle(mean) - np.angle(current)) > PHASE_THRESHOLD
            ):
                logging.info(
                    f"Change detected on antenna {antenna}, subcarrier {subcarrier} "
                    f"|{np.abs(mean - current)}| <{np.angle(mean) - np.angle(current)}>"
                )


# Start the application
app.start()