defmodule BMI323 do
  @moduledoc """
  Driver for the Bosch BMI323 6-DoF inertial measurement unit.

  Communicates over I²C (or any other [Wafer](https://hex.pm/packages/wafer)
  transport that implements the `Wafer.I2C` protocol).

  ## Protocol notes

  The BMI323 uses an unusual word-oriented register protocol:

    * Every register is 16 bits wide, little-endian.
    * Every read returns **two dummy bytes** before the payload; this driver
      strips them transparently.
    * Writes have no dummy bytes — just the 8-bit address followed by the
      16-bit value(s).
    * Multi-word reads and writes auto-increment the register address.

  ## Example

      {:ok, i2c} = Wafer.Driver.Circuits.I2C.acquire(bus_name: "i2c-1", address: 0x68)
      {:ok, bmi} = BMI323.acquire(conn: i2c)
  """

  alias BMI323.Config
  alias BMI323.Registers
  alias Wafer.Chip
  alias Wafer.Conn

  defstruct conn: nil, accelerometer_range: nil, gyroscope_range: nil

  @type t :: %__MODULE__{
          conn: Conn.t(),
          accelerometer_range: Config.accelerometer_range() | nil,
          gyroscope_range: Config.gyroscope_range() | nil
        }
  @type chip_id :: byte
  @type axes :: %{x: float, y: float, z: float}
  @type imu_sample :: %{accelerometer: axes, gyroscope: axes, temperature: float}
  @type acquire_option ::
          {:conn, Conn.t()}
          | {:soft_reset, boolean}
          | {:verify_chip_id, boolean}

  @behaviour Wafer.Conn

  @default_i2c_address 0x68
  @expected_chip_id 0x43
  @soft_reset_command_le <<0xAF, 0xDE>>
  @post_reset_delay_ms 10
  @gravity_ms2 9.80665
  @sensor_time_tick_us 39.0625

  @acc_data_start 0x03
  @imu_burst_bytes 14

  @doc """
  The default 7-bit I²C address (`0x68`, SDO pin tied to GND).

  The alternate address `0x69` is selected by tying SDO to VDDIO.
  """
  @spec default_i2c_address() :: 0x68
  def default_i2c_address, do: @default_i2c_address

  @doc """
  The expected `CHIP_ID` value (`0x43`) returned by an unmodified BMI323.
  """
  @spec expected_chip_id() :: 0x43
  def expected_chip_id, do: @expected_chip_id

  @doc """
  Wrap an existing Wafer connection in a `BMI323` struct.

  ## Options

    * `:conn` (required) — a Wafer connection that implements the `Wafer.I2C`
      protocol, e.g. `Wafer.Driver.Circuits.I2C` or `Wafer.Driver.Fake`.
    * `:soft_reset` (default `false`) — when `true`, issue a soft reset
      immediately after wrapping the connection. See `soft_reset/1`.
    * `:verify_chip_id` (default `true`) — when `true`, read `CHIP_ID` and
      return `{:error, {:chip_id_mismatch, got: byte, expected: 0x43}}` if the
      device does not identify as a BMI323.
  """
  @impl Wafer.Conn
  @spec acquire([acquire_option]) :: {:ok, t} | {:error, term}
  def acquire(opts) when is_list(opts) do
    with {:ok, conn} <- fetch_conn(opts),
         bmi = %__MODULE__{conn: conn},
         {:ok, bmi} <- maybe_soft_reset(bmi, opts) do
      maybe_verify_chip_id(bmi, opts)
    end
  end

  @doc """
  Read the device's `CHIP_ID` register, returning the 7-bit identifier
  (high byte of the 16-bit register is reserved and discarded).
  """
  @spec chip_id(t) :: {:ok, chip_id} | {:error, term}
  def chip_id(%__MODULE__{} = bmi) do
    with {:ok, <<id, _reserved>>} <- Registers.read_chip_id(bmi) do
      {:ok, id}
    end
  end

  @doc """
  Issue a soft reset by writing the `0xDEAF` command to the `CMD` register.

  The device returns to suspend mode and all user configuration is restored
  to its power-on default. Blocks for #{@post_reset_delay_ms} ms to satisfy
  the device's post-reset start-up time before returning.
  """
  @spec soft_reset(t) :: {:ok, t} | {:error, term}
  def soft_reset(%__MODULE__{} = bmi) do
    with {:ok, bmi} <- Registers.write_cmd(bmi, @soft_reset_command_le) do
      Process.sleep(@post_reset_delay_ms)
      {:ok, bmi}
    end
  end

  @doc """
  Configure the accelerometer and cache the chosen range for later scaling.

  See `BMI323.Config.encode_acc_conf/1` for the supported options. `:mode` and
  `:odr` are required; `:range`, `:bandwidth`, and `:averaging` have defaults.
  """
  @spec configure_accelerometer(t, keyword) :: {:ok, t} | {:error, term}
  def configure_accelerometer(%__MODULE__{} = bmi, opts) when is_list(opts) do
    data = Config.encode_acc_conf(opts)
    range = Keyword.get(opts, :range, 8)

    with {:ok, bmi} <- Registers.write_acc_conf(bmi, data) do
      {:ok, %{bmi | accelerometer_range: range}}
    end
  end

  @doc """
  Configure the gyroscope and cache the chosen range for later scaling.

  See `BMI323.Config.encode_gyr_conf/1` for the supported options.
  """
  @spec configure_gyroscope(t, keyword) :: {:ok, t} | {:error, term}
  def configure_gyroscope(%__MODULE__{} = bmi, opts) when is_list(opts) do
    data = Config.encode_gyr_conf(opts)
    range = Keyword.get(opts, :range, 2000)

    with {:ok, bmi} <- Registers.write_gyr_conf(bmi, data) do
      {:ok, %{bmi | gyroscope_range: range}}
    end
  end

  @doc """
  Populate the cached ranges by reading `ACC_CONF` and `GYR_CONF`.

  Useful after `acquire/1` when the device has already been configured by
  some other process and you need to scale samples without reconfiguring.
  """
  @spec detect_ranges(t) :: {:ok, t} | {:error, term}
  def detect_ranges(%__MODULE__{} = bmi) do
    with {:ok, acc_data} <- Registers.read_acc_conf(bmi),
         {:ok, gyr_data} <- Registers.read_gyr_conf(bmi) do
      %{range: acc_range} = Config.decode_acc_conf(acc_data)
      %{range: gyr_range} = Config.decode_gyr_conf(gyr_data)
      {:ok, %{bmi | accelerometer_range: acc_range, gyroscope_range: gyr_range}}
    end
  end

  @doc """
  Read the accelerometer x/y/z sample and return scaled values in m/s².

  Requires the accelerometer range to be cached on the struct — call
  `configure_accelerometer/2` or `detect_ranges/1` first.
  """
  @spec read_accelerometer(t) :: {:ok, axes} | {:error, term}
  def read_accelerometer(%__MODULE__{accelerometer_range: nil}),
    do: {:error, :accelerometer_range_not_set}

  def read_accelerometer(%__MODULE__{accelerometer_range: range} = bmi) do
    with {:ok, <<x::little-signed-16, y::little-signed-16, z::little-signed-16>>} <-
           Chip.read_register(bmi, @acc_data_start, 6) do
      {:ok, %{x: accel_to_ms2(x, range), y: accel_to_ms2(y, range), z: accel_to_ms2(z, range)}}
    end
  end

  @doc """
  Read the gyroscope x/y/z sample and return scaled values in rad/s.

  Requires the gyroscope range to be cached on the struct — call
  `configure_gyroscope/2` or `detect_ranges/1` first.
  """
  @spec read_gyroscope(t) :: {:ok, axes} | {:error, term}
  def read_gyroscope(%__MODULE__{gyroscope_range: nil}),
    do: {:error, :gyroscope_range_not_set}

  def read_gyroscope(%__MODULE__{gyroscope_range: range} = bmi) do
    with {:ok, <<x::little-signed-16, y::little-signed-16, z::little-signed-16>>} <-
           Chip.read_register(bmi, 0x06, 6) do
      {:ok, %{x: gyro_to_rads(x, range), y: gyro_to_rads(y, range), z: gyro_to_rads(z, range)}}
    end
  end

  @doc """
  Read the temperature sample and return °C. Returns `{:error, :invalid_sample}`
  if the device reports the invalid sentinel `0x8000`.
  """
  @spec read_temperature(t) :: {:ok, float} | {:error, term}
  def read_temperature(%__MODULE__{} = bmi) do
    with {:ok, <<raw::little-signed-16>>} <- Registers.read_temp_data(bmi) do
      decode_temperature(raw)
    end
  end

  @doc """
  Read accelerometer + gyroscope + temperature in a single 7-word burst.

  Requires both ranges to be cached on the struct.
  """
  @spec read_imu(t) :: {:ok, imu_sample} | {:error, term}
  def read_imu(%__MODULE__{accelerometer_range: nil}),
    do: {:error, :accelerometer_range_not_set}

  def read_imu(%__MODULE__{gyroscope_range: nil}),
    do: {:error, :gyroscope_range_not_set}

  def read_imu(%__MODULE__{accelerometer_range: acc_range, gyroscope_range: gyr_range} = bmi) do
    with {:ok, payload} <- Chip.read_register(bmi, @acc_data_start, @imu_burst_bytes) do
      <<ax::little-signed-16, ay::little-signed-16, az::little-signed-16, gx::little-signed-16,
        gy::little-signed-16, gz::little-signed-16, temp::little-signed-16>> = payload

      with {:ok, temperature} <- decode_temperature(temp) do
        {:ok,
         %{
           accelerometer: %{
             x: accel_to_ms2(ax, acc_range),
             y: accel_to_ms2(ay, acc_range),
             z: accel_to_ms2(az, acc_range)
           },
           gyroscope: %{
             x: gyro_to_rads(gx, gyr_range),
             y: gyro_to_rads(gy, gyr_range),
             z: gyro_to_rads(gz, gyr_range)
           },
           temperature: temperature
         }}
      end
    end
  end

  @doc """
  Read the 32-bit sensor time counter.

  One LSB equals #{@sensor_time_tick_us} µs. Multiply by
  `sensor_time_tick_us/0` to get microseconds.
  """
  @spec read_sensor_time(t) :: {:ok, non_neg_integer} | {:error, term}
  def read_sensor_time(%__MODULE__{} = bmi) do
    with {:ok, <<ticks::little-32>>} <- Chip.read_register(bmi, 0x0A, 4) do
      {:ok, ticks}
    end
  end

  @doc "The sensor time counter's tick period in microseconds (#{@sensor_time_tick_us} µs)."
  @spec sensor_time_tick_us() :: float
  def sensor_time_tick_us, do: @sensor_time_tick_us

  defp accel_to_ms2(raw, range_g) do
    raw * @gravity_ms2 * range_g / 32_768
  end

  defp gyro_to_rads(raw, range_dps) do
    raw * :math.pi() * range_dps / (32_768 * 180)
  end

  defp decode_temperature(-32_768), do: {:error, :invalid_sample}
  defp decode_temperature(raw), do: {:ok, raw / 512 + 23}

  defp fetch_conn(opts) do
    case Keyword.fetch(opts, :conn) do
      {:ok, conn} -> {:ok, conn}
      :error -> {:error, "`:conn` option is required"}
    end
  end

  defp maybe_soft_reset(bmi, opts) do
    if Keyword.get(opts, :soft_reset, false), do: soft_reset(bmi), else: {:ok, bmi}
  end

  defp maybe_verify_chip_id(bmi, opts) do
    if Keyword.get(opts, :verify_chip_id, true), do: verify_chip_id(bmi), else: {:ok, bmi}
  end

  defp verify_chip_id(bmi) do
    case chip_id(bmi) do
      {:ok, @expected_chip_id} -> {:ok, bmi}
      {:ok, got} -> {:error, {:chip_id_mismatch, got: got, expected: @expected_chip_id}}
      {:error, _} = error -> error
    end
  end
end

defimpl Wafer.Chip, for: BMI323 do
  @moduledoc """
  `Wafer.Chip` implementation that adapts the BMI323's word-oriented register
  protocol to Wafer's byte-oriented API.

  All register payloads must be a whole number of 16-bit little-endian words.
  Reads transparently skip the two dummy bytes prepended by the device.
  """

  alias Wafer.I2C

  @dummy_bytes 2

  def read_register(%BMI323{conn: inner}, address, bytes)
      when is_integer(address) and address in 0..0xFF and
             is_integer(bytes) and bytes > 0 and rem(bytes, 2) == 0 do
    with {:ok, payload, _inner} <- I2C.write_read(inner, <<address>>, @dummy_bytes + bytes, []) do
      <<_dummy::binary-size(@dummy_bytes), data::binary-size(bytes)>> = payload
      {:ok, data}
    end
  end

  def read_register(_conn, address, bytes) do
    {:error,
     "Invalid argument: address=#{inspect(address)} bytes=#{inspect(bytes)} " <>
       "(bytes must be a positive even integer, address in 0..0xFF)"}
  end

  def write_register(%BMI323{conn: inner} = conn, address, data)
      when is_integer(address) and address in 0..0xFF and
             is_binary(data) and byte_size(data) > 0 and rem(byte_size(data), 2) == 0 do
    with {:ok, inner} <- I2C.write(inner, <<address, data::binary>>, []) do
      {:ok, %{conn | conn: inner}}
    end
  end

  def write_register(_conn, address, data) do
    {:error,
     "Invalid argument: address=#{inspect(address)} data=#{inspect(data)} " <>
       "(data must be a non-empty binary with an even byte count, address in 0..0xFF)"}
  end

  def swap_register(conn, address, data) when is_binary(data) do
    with {:ok, old} <- read_register(conn, address, byte_size(data)),
         {:ok, conn} <- write_register(conn, address, data) do
      {:ok, old, conn}
    end
  end

  def swap_register(_conn, _address, data),
    do: {:error, "Invalid argument: data must be a binary, got #{inspect(data)}"}
end