localiserd/lib/localiser/localisation/sensor/server.ex

defmodule Localiser.Localisation.Sensor.Server do
  use GenServer

  require Logger

  alias Localiser.Domain.Sensors
  alias Localiser.Domain.Schema.{Sensor, SensorCalibration}
  alias Localiser.MQTT.Connection, as: MQTTConnection

  @default_rssi_ref -59
  @default_path_loss_exp 2.0

  # mode: :ok | {:calibrating, buffer :: [integer()], target :: pos_integer()}
  defstruct [:sensor_id, :sensor_db_id, :floor_x, :floor_y, :rssi_ref, :path_loss_exp, mode: :ok]

  def start_link({sensor, room}) do
    GenServer.start_link(__MODULE__, {sensor, room}, name: via(sensor.sensor_id))
  end

  def via(sensor_id) do
    {:via, Registry, {Localiser.Registry, {:sensor, sensor_id}}}
  end

  # Returns %{sensor_id, floor_x, floor_y, distance} for a raw RSSI reading.
  def measure(sensor_id, rssi) do
    GenServer.call(via(sensor_id), {:measure, rssi})
  end

  # Returns true if the sensor is currently collecting calibration samples.
  def calibrating?(sensor_id) do
    GenServer.call(via(sensor_id), :calibrating?)
  end

  # Feeds a raw RSSI value into the calibration buffer.
  def calibration_reading(sensor_id, rssi) do
    GenServer.cast(via(sensor_id), {:calibration_reading, rssi})
  end

  # Starts calibration mode. sample_target: number of RSSI samples to collect.
  def begin_calibration(sensor_id, sample_target \\ 50) do
    GenServer.cast(via(sensor_id), {:begin_calibration, sample_target})
  end

  # Aborts an in-progress calibration without saving.
  def abort_calibration(sensor_id) do
    GenServer.cast(via(sensor_id), :abort_calibration)
  end

  @impl true
  def init({sensor, room}) do
    Phoenix.PubSub.subscribe(Localiser.PubSub, "sensors")

    calibration = Sensors.latest_calibration(sensor)
    {rssi_ref, path_loss_exp} = calibration_params(calibration)

    state = %__MODULE__{
      sensor_id: sensor.sensor_id,
      sensor_db_id: sensor.id,
      floor_x: (room.offset_x || 0.0) + (sensor.x || 0.0),
      floor_y: (room.offset_y || 0.0) + (sensor.y || 0.0),
      rssi_ref: rssi_ref,
      path_loss_exp: path_loss_exp
    }

    {:ok, state}
  end

  @impl true
  def handle_call({:measure, rssi}, _from, state) do
    distance = rssi_to_distance(rssi, state.rssi_ref, state.path_loss_exp)

    measurement = %{
      sensor_id: state.sensor_id,
      floor_x: state.floor_x,
      floor_y: state.floor_y,
      distance: distance
    }

    {:reply, measurement, state}
  end

  @impl true
  def handle_call(:calibrating?, _from, state) do
    {:reply, match?({:calibrating, _, _}, state.mode), state}
  end

  @impl true
  def handle_cast({:begin_calibration, target}, state) do
    MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_start"}))
    {:noreply, %{state | mode: {:calibrating, [], target}}}
  end

  @impl true
  def handle_cast(:abort_calibration, %{mode: {:calibrating, _, _}} = state) do
    MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_stop"}))
    {:noreply, %{state | mode: :ok}}
  end

  def handle_cast(:abort_calibration, state), do: {:noreply, state}

  @impl true
  def handle_cast({:calibration_reading, rssi}, %{mode: {:calibrating, buffer, target}} = state) do
    buffer = [rssi | buffer]

    if length(buffer) >= target do
      finalize_calibration(buffer, state)
    else
      {:noreply, %{state | mode: {:calibrating, buffer, target}}}
    end
  end

  def handle_cast({:calibration_reading, _rssi}, state), do: {:noreply, state}

  # Position updated (sensor dragged in layout).
  @impl true
  def handle_info({:sensor_enrolled, %Sensor{sensor_id: sid} = sensor, room}, %{sensor_id: sid} = state) do
    floor_x = (room.offset_x || 0.0) + (sensor.x || 0.0)
    floor_y = (room.offset_y || 0.0) + (sensor.y || 0.0)
    {:noreply, %{state | floor_x: floor_x, floor_y: floor_y}}
  end

  # Ignore PubSub messages not relevant to this server.
  def handle_info(_msg, state), do: {:noreply, state}

  # Private

  defp finalize_calibration(buffer, state) do
    rssi_ref = median(buffer)
    sensor_struct = %Sensor{id: state.sensor_db_id, sensor_id: state.sensor_id}

    case Sensors.add_calibration(sensor_struct, %{
           rssi_ref: rssi_ref,
           path_loss_exp: state.path_loss_exp,
           calibrated_at: DateTime.utc_now()
         }) do
      {:ok, _calibration} ->
        Logger.info("[Sensor.Server] Calibration complete for #{state.sensor_id}: rssi_ref=#{rssi_ref}")
        MQTTConnection.publish("localiser/sensor/#{state.sensor_id}/cmd", ~s({"action":"calibrate_stop"}))
        Phoenix.PubSub.broadcast(Localiser.PubSub, "sensors", {:calibration_complete, state.sensor_id})
        {:noreply, %{state | rssi_ref: rssi_ref, mode: :ok}}

      {:error, reason} ->
        Logger.error("[Sensor.Server] Failed to save calibration for #{state.sensor_id}: #{inspect(reason)}")
        {:noreply, %{state | mode: :ok}}
    end
  end

  defp median(list) do
    sorted = Enum.sort(list)
    len = length(sorted)
    mid = div(len, 2)

    if rem(len, 2) == 0 do
      round((Enum.at(sorted, mid - 1) + Enum.at(sorted, mid)) / 2)
    else
      Enum.at(sorted, mid)
    end
  end

  # d = 10 ^ ((rssi_ref - rssi) / (10 * n))
  defp rssi_to_distance(rssi, rssi_ref, path_loss_exp) do
    :math.pow(10.0, (rssi_ref - rssi) / (10.0 * path_loss_exp))
  end

  defp calibration_params(nil), do: {@default_rssi_ref, @default_path_loss_exp}

  defp calibration_params(%SensorCalibration{rssi_ref: rssi_ref, path_loss_exp: path_loss_exp}) do
    {rssi_ref, path_loss_exp}
  end
end