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

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
app = CSIApplication(visualise_raw=True)


# Stores historical data for each receiving antenna, for each subcarrier
historical: dict[tuple[int, int], Queue[np.complex64]] = {}

MAGN_THRESHOLD = 20
PHASE_THRESHOLD = 0.5
QUEUE_SIZE = 20


@app.on_process
def _(sample: npt.NDArray[np.complex64]) -> None:
    """
    Process the CSI data and detect changes in the environment.

    This function is called by the framework at a fixed interval, with the latest CSI
    sample received.

    It is used to detect changes in the environment caused by motion, by comparing each
    entry in the matrix with a moving average
    """
    change = False
    for antenna in range(sample.shape[1]):
        for subcarrier in range(sample.shape[0]):
            # Get the current subcarrier data
            current = sample[subcarrier, antenna, 0]

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

            # Calculate the average of the historical data for this antenna and
            # subcarrier
            mean = 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 sample 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
            ):
                print(
                    f"Change detected in antenna {antenna}, subcarrier {subcarrier}: "
                    f"magn: {np.abs(mean - current)}, angle: {np.abs(np.angle(mean) - np.angle(current))}"
                )
                change = True
    if change:
        print("Motion detected!")
    else:
        print("No motion detected!")


if __name__ == "__main__":
    # Start the application
    print("Starting app")
    app.set_producer(RealtimeCSIProducer())
    app.start()