sidereon

defmodule Sidereon.ErrorMetrics do
  @moduledoc """
  Position error metrics derived from supplied covariance matrices.

  Covariances are in square metres. ENU axes are east, north, up. ECEF
  covariances are rotated at the supplied WGS84 geodetic reference before
  metrics are computed.
  """

  alias __MODULE__.ErrorEllipse
  alias __MODULE__.PercentileRadius
  alias __MODULE__.PositionCovariance
  alias __MODULE__.PositionErrorMetrics
  alias Sidereon.NIF

  @type covariance3 :: [[number()]]
  @type geodetic_radians_m :: {number(), number(), number()}
  @type error_reason ::
          :non_finite
          | :not_positive_semidefinite
          | :invalid_probability
          | :rotation
          | term()

  defmodule ErrorEllipse do
    @moduledoc """
    Horizontal one-sigma error ellipse.
    """

    @enforce_keys [:semi_major_m, :semi_minor_m, :orientation_rad]
    defstruct [:semi_major_m, :semi_minor_m, :orientation_rad]

    @type t :: %__MODULE__{
            semi_major_m: float(),
            semi_minor_m: float(),
            orientation_rad: float()
          }
  end

  defmodule PercentileRadius do
    @moduledoc """
    Exact percentile circle or sphere radius plus the named approximation.
    """

    @enforce_keys [:probability, :radius_m, :approx_m, :approx_valid]
    defstruct [:probability, :radius_m, :approx_m, :approx_valid]

    @type t :: %__MODULE__{
            probability: float(),
            radius_m: float(),
            approx_m: float(),
            approx_valid: boolean()
          }
  end

  defmodule PositionCovariance do
    @moduledoc """
    Position covariance in ECEF and local ENU coordinates.
    """

    @enforce_keys [:ecef_m2, :enu_m2]
    defstruct [:ecef_m2, :enu_m2]

    @type t :: %__MODULE__{
            ecef_m2: Sidereon.ErrorMetrics.covariance3(),
            enu_m2: Sidereon.ErrorMetrics.covariance3()
          }
  end

  defmodule PositionErrorMetrics do
    @moduledoc """
    Standard horizontal, vertical, and three-dimensional position error metrics.
    """

    @enforce_keys [
      :ellipse,
      :sigma_e_m,
      :sigma_n_m,
      :sigma_u_m,
      :cep_m,
      :r95_m,
      :r99_m,
      :drms_m,
      :two_drms_m,
      :vep_m,
      :sep_m,
      :mrse_m
    ]
    defstruct [
      :ellipse,
      :sigma_e_m,
      :sigma_n_m,
      :sigma_u_m,
      :cep_m,
      :r95_m,
      :r99_m,
      :drms_m,
      :two_drms_m,
      :vep_m,
      :sep_m,
      :mrse_m
    ]

    @type t :: %__MODULE__{
            ellipse: ErrorEllipse.t(),
            sigma_e_m: float(),
            sigma_n_m: float(),
            sigma_u_m: float(),
            cep_m: PercentileRadius.t(),
            r95_m: PercentileRadius.t(),
            r99_m: PercentileRadius.t(),
            drms_m: float(),
            two_drms_m: float(),
            vep_m: float(),
            sep_m: PercentileRadius.t(),
            mrse_m: float()
          }
  end

  @doc """
  Compute position error metrics from an ENU covariance matrix in square metres.
  """
  @spec from_enu_covariance(covariance3()) :: {:ok, PositionErrorMetrics.t()} | {:error, error_reason()}
  def from_enu_covariance(covariance_enu_m2) do
    case NIF.position_error_metrics_from_enu_covariance(rows(covariance_enu_m2)) do
      {:ok, metrics} -> {:ok, metrics(metrics)}
      {:error, _} = err -> err
      other -> {:error, other}
    end
  rescue
    e in ErlangError -> {:error, e.original}
  end

  @doc """
  Compute position error metrics from an ECEF covariance matrix in square metres.

  The receiver is `{lat_rad, lon_rad, height_m}`.
  """
  @spec from_ecef_covariance(covariance3(), geodetic_radians_m()) ::
          {:ok, PositionErrorMetrics.t()} | {:error, error_reason()}
  def from_ecef_covariance(covariance_ecef_m2, receiver_llh_rad_m) do
    case NIF.position_error_metrics_from_ecef_covariance(rows(covariance_ecef_m2), vec3(receiver_llh_rad_m)) do
      {:ok, metrics} -> {:ok, metrics(metrics)}
      {:error, _} = err -> err
      other -> {:error, other}
    end
  rescue
    e in ErlangError -> {:error, e.original}
  end

  @doc """
  Compute position error metrics from a position covariance value.
  """
  @spec from_position_covariance(PositionCovariance.t() | map()) ::
          {:ok, PositionErrorMetrics.t()} | {:error, error_reason()}
  def from_position_covariance(%PositionCovariance{} = covariance) do
    from_position_covariance(Map.from_struct(covariance))
  end

  def from_position_covariance(%{ecef_m2: ecef_m2, enu_m2: enu_m2}) do
    case NIF.position_error_metrics_from_position_covariance(rows(ecef_m2), rows(enu_m2)) do
      {:ok, metrics} -> {:ok, metrics(metrics)}
      {:error, _} = err -> err
      other -> {:error, other}
    end
  rescue
    e in ErlangError -> {:error, e.original}
  end

  @doc """
  Compute position error metrics from a kinematic solution-like map or struct.

  The value must provide `:position_m` and `:position_covariance_m2`.
  """
  @spec from_kinematic_solution(map()) :: {:ok, PositionErrorMetrics.t()} | {:error, error_reason()}
  def from_kinematic_solution(solution) when is_map(solution) do
    position_m = Map.fetch!(solution, :position_m)
    covariance_m2 = Map.fetch!(solution, :position_covariance_m2)

    case NIF.position_error_metrics_from_kinematic_solution(vec3(position_m), rows(covariance_m2)) do
      {:ok, metrics} -> {:ok, metrics(metrics)}
      {:error, _} = err -> err
      other -> {:error, other}
    end
  rescue
    e in ErlangError -> {:error, e.original}
  end

  @doc """
  Compute the horizontal one-sigma error ellipse from an ENU covariance matrix.
  """
  @spec error_ellipse_from_enu_covariance(covariance3()) :: {:ok, ErrorEllipse.t()} | {:error, error_reason()}
  def error_ellipse_from_enu_covariance(covariance_enu_m2) do
    case NIF.position_error_metrics_error_ellipse_from_enu_covariance(rows(covariance_enu_m2)) do
      {:ok, ellipse} -> {:ok, ellipse(ellipse)}
      {:error, _} = err -> err
      other -> {:error, other}
    end
  rescue
    e in ErlangError -> {:error, e.original}
  end

  @doc """
  Compute a horizontal percentile circle radius from an ENU covariance matrix.
  """
  @spec horizontal_radius_at(covariance3(), number()) :: {:ok, PercentileRadius.t()} | {:error, error_reason()}
  def horizontal_radius_at(covariance_enu_m2, probability) do
    case NIF.position_error_metrics_horizontal_radius_at(rows(covariance_enu_m2), probability / 1.0) do
      {:ok, radius} -> {:ok, percentile(radius)}
      {:error, _} = err -> err
      other -> {:error, other}
    end
  rescue
    e in ErlangError -> {:error, e.original}
  end

  @doc """
  Compute a three-dimensional percentile sphere radius from an ENU covariance matrix.
  """
  @spec spherical_radius_at(covariance3(), number()) :: {:ok, PercentileRadius.t()} | {:error, error_reason()}
  def spherical_radius_at(covariance_enu_m2, probability) do
    case NIF.position_error_metrics_spherical_radius_at(rows(covariance_enu_m2), probability / 1.0) do
      {:ok, radius} -> {:ok, percentile(radius)}
      {:error, _} = err -> err
      other -> {:error, other}
    end
  rescue
    e in ErlangError -> {:error, e.original}
  end

  @doc """
  Compute a vertical percentile radius from an up variance in square metres.
  """
  @spec vertical_radius_at(number(), number()) :: {:ok, float()} | {:error, error_reason()}
  def vertical_radius_at(sigma_u_m2, probability) do
    case NIF.position_error_metrics_vertical_radius_at(sigma_u_m2 / 1.0, probability / 1.0) do
      {:ok, radius_m} -> {:ok, radius_m}
      {:error, _} = err -> err
      other -> {:error, other}
    end
  rescue
    e in ErlangError -> {:error, e.original}
  end

  defp metrics(value) do
    %PositionErrorMetrics{
      ellipse: ellipse(value.ellipse),
      sigma_e_m: value.sigma_e_m,
      sigma_n_m: value.sigma_n_m,
      sigma_u_m: value.sigma_u_m,
      cep_m: percentile(value.cep_m),
      r95_m: percentile(value.r95_m),
      r99_m: percentile(value.r99_m),
      drms_m: value.drms_m,
      two_drms_m: value.two_drms_m,
      vep_m: value.vep_m,
      sep_m: percentile(value.sep_m),
      mrse_m: value.mrse_m
    }
  end

  defp ellipse(value) do
    %ErrorEllipse{
      semi_major_m: value.semi_major_m,
      semi_minor_m: value.semi_minor_m,
      orientation_rad: value.orientation_rad
    }
  end

  defp percentile(value) do
    %PercentileRadius{
      probability: value.probability,
      radius_m: value.radius_m,
      approx_m: value.approx_m,
      approx_valid: value.approx_valid
    }
  end

  defp rows(matrix), do: Enum.map(matrix, fn row -> Enum.map(row, &(&1 / 1.0)) end)
  defp vec3({x, y, z}), do: {x / 1.0, y / 1.0, z / 1.0}
  defp vec3([x, y, z]), do: {x / 1.0, y / 1.0, z / 1.0}
end