"""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 numpy as np
import numpy.typing as npt

from where_fi.application import CSIApplication
from where_fi.collection.ingest import RealtimeCSIProducer
from where_fi.config import config
from where_fi.visualise import server as visualise

# Connect to a FeitCSI host
app = CSIApplication(visualise_raw=True)

# Register visualisations
app.register_figure(
    "magn-diff",
    visualise.figures.PerAntennaFigure(
        [
            visualise.figures.SimpleLineChart(
                "Magnitude diff", "Subcarrier", "Magnitude"
            ),
        ],
        [lambda x: x],
    ),
)
app.register_figure(
    "phase-diff",
    visualise.figures.PerAntennaFigure(
        [
            visualise.figures.SimpleLineChart("Phase diff", "Subcarrier", "Phase"),
        ],
        [lambda x: x],
    ),
)

MAGN_THRESHOLD = 0.2
PHASE_THRESHOLD = 0.02
QUEUE_SIZE = 20

# Stores historical data for each receiving antenna, for each subcarrier
historical = np.zeros(
    (QUEUE_SIZE, config.subcarriers, config.antennas.count, 1), dtype=np.complex64
)
sample_position = 0


@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
    """
    global sample_position

    historical[sample_position] = sample
    sample_position = (sample_position + 1) % QUEUE_SIZE

    mean = np.mean(historical, axis=0)
    magn_diff = np.abs(mean - sample)
    phase_diff = np.abs(np.angle(mean) - np.angle(sample))

    app.visualise_data(magn_diff, "magn-diff")
    app.visualise_data(phase_diff, "phase-diff")

    if (magn_diff > MAGN_THRESHOLD).any() or (phase_diff > PHASE_THRESHOLD).any():
        print("Motion detected!")
    else:
        print("No motion detected!")


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