{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "8e231f17-2241-4008-a0b6-3f19098ba547",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "--- LOCALIZING: 20230504_085452UTC_V12 ---\n",
      "    Saved spatial localizations: ../01_data/20230504_085452UTC_V12_localizations.csv\n",
      "\n",
      "--- LOCALIZING: 20230504_085315UTC_V12 ---\n",
      "    Saved spatial localizations: ../01_data/20230504_085315UTC_V12_localizations.csv\n",
      "\n",
      "--- LOCALIZING: 20230504_081356UTC_V12 ---\n",
      "    Saved spatial localizations: ../01_data/20230504_081356UTC_V12_localizations.csv\n"
     ]
    }
   ],
   "source": [
    "\"\"\"\n",
    "\n",
    "Module 04: Spatial localization (TDOA to angles)\n",
    "Journal of the Acoustical Society of America (JASA) - Data availability\n",
    "\n",
    "(c) Walter M.X. Zimmer (2025)\n",
    "Modified by Lara Berkenbaum (2025, 2026)\n",
    "Original code produced for the CIAN Tool Kit, associated with the manuscript: \n",
    "Berkenbaum, L., Glotin, H., Sarano, F., Zimmer, W.M.X., Sarano, V., Adam, O., Giraudet, P. (2026). \"Audiovisual diarization of overlapping click trains in sperm whale (Physeter macrocephalus) vocal sparring using a three-hydrophone array,\" J. Acoust. Soc. Am.\n",
    "\n",
    "Description:\n",
    "This script computes the 3D spatial origin of each click based on the Time Difference of Arrival (TDOA) delays extracted in Module 01. \n",
    "It uses a constrained two-dimensional directional approach and aligns the spatial geometry with the camera's orthonormal reference frame for optical fusion (Sec. II.E). \n",
    "\n",
    "Inputs: \n",
    "- Detection parameters: `../01_data/*_detections_strict.csv` (thresholds defined in Sec. II.C)\n",
    "\n",
    "Outputs: \n",
    "- Localization coordinates: `../01_data/*_localizations.csv` \n",
    "`[filename]_localizations.csv`: spatial coordinates including antenna geometric frame angles (AZ_G_deg, EL_G_deg), camera-corrected optical frame angles (AZ_C_deg, EL_C_deg), and spatial residuals (residuals_cm).\n",
    "\n",
    "\"\"\"\n",
    "\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "import glob\n",
    "import os\n",
    "\n",
    "# Import custom rotation function and shared constants from local utils\n",
    "from utils import roty, C_SOUND, HSEL\n",
    "\n",
    "# =================================================================\n",
    "# 1. CONFIGURATION & GEOMETRY MATRIX\n",
    "# =================================================================\n",
    "csv_dir = \"../01_data/\"\n",
    "\n",
    "# Strictly ingesting 'strict' detections for TDOA inference \n",
    "detection_files = glob.glob(os.path.join(csv_dir, \"*_detections_strict.csv\"))\n",
    "\n",
    "if not detection_files:\n",
    "    print(f\"No strict detection files found in {csv_dir}.\")\n",
    "\n",
    "# OPALE array geometry (sensor coordinates in cm, converted to meters; Supplementary material S1)\n",
    "ho = np.array([[0, 0, 0], [0, 51.3, 0], [21.5, 25.65, -30.29]]) / 100\n",
    "dx = np.sqrt(ho[2, 0]**2 + ho[2, 2]**2) * 100\n",
    "bx = 90 + 180 / np.pi * np.arctan2(ho[2, 2], ho[2, 0])\n",
    "\n",
    "hh = np.array([[0, 0], [51.3, 0], [25.65, -dx]]) / 100\n",
    "\n",
    "D_mat = hh[HSEL[:, 1], :] - hh[HSEL[:, 0], :]\n",
    "DI = np.linalg.pinv(D_mat)\n",
    "\n",
    "# =================================================================\n",
    "# 2. SPATIAL LOCALIZATION BATCH\n",
    "# =================================================================\n",
    "for fname in detection_files:\n",
    "    base_name = os.path.basename(fname).replace(\"_detections_strict.csv\", \"\")\n",
    "    print(f\"\\n--- LOCALIZING: {base_name} ---\")\n",
    "    \n",
    "    # Load the TDOA delays and the native sampling frequency\n",
    "    df = pd.read_csv(fname, sep=';')\n",
    "    \n",
    "    if df.empty:\n",
    "        print(\"    No clicks to process in this file.\")\n",
    "        continue\n",
    "        \n",
    "    # Extract dynamic sampling frequency array to maintain mathematical accuracy\n",
    "    fs = df['fs_hz'].values[:, np.newaxis]\n",
    "    \n",
    "    # Extract delays (samples) and convert to physical apparent distance (meters)\n",
    "    B_samples = df[['B01_samples', 'B02_samples', 'B12_samples']].values\n",
    "    B_meters = B_samples * (C_SOUND / fs)\n",
    "    \n",
    "    # Tangential directional vector computation (optimal least-squares solution)\n",
    "    G = B_meters @ DI.T\n",
    "    \n",
    "    # Normal component reconstruction\n",
    "    sum_sq = np.sum(G**2, axis=1)\n",
    "    \n",
    "    # 1. Cone boundary folding: clicks marginally outside the resolvable cone (sum(G^2) > 1, \n",
    "    #typically <1.2) are folded back to the cone boundary (np.abs) rather than discarded, \n",
    "    #assuming the excess stems from measurement noise on near-plane sources (see Sec. II.E.).\n",
    "    # 2. Frontal half-space postulate: positive root selection.\n",
    "    gn = np.sqrt(np.abs(1 - sum_sq)).reshape(-1, 1)\n",
    "    \n",
    "    G_3D = np.hstack((gn, G)) \n",
    "    \n",
    "    # 3D rotation to align with the camera frontal reference frame\n",
    "    C_final = roty(G_3D, bx * np.pi / 180)\n",
    "    \n",
    "    # Antenna geometric frame angles \n",
    "    df['AZ_G_deg'] = np.degrees(np.arctan2(G_3D[:, 1], G_3D[:, 0]))\n",
    "    df['EL_G_deg'] = np.degrees(np.arctan2(G_3D[:, 2], np.sqrt(G_3D[:, 0]**2 + G_3D[:, 1]**2)))\n",
    "    df['AZ_G_rad'] = np.arctan2(G_3D[:, 1], G_3D[:, 0])\n",
    "    df['EL_G_rad'] = np.arctan2(G_3D[:, 2], np.sqrt(G_3D[:, 0]**2 + G_3D[:, 1]**2))\n",
    "    \n",
    "    # Optical corrected frame angles\n",
    "    df['AZ_C_deg'] = np.degrees(np.arctan2(C_final[:, 1], C_final[:, 0]))\n",
    "    df['EL_C_deg'] = np.degrees(np.arctan2(C_final[:, 2], np.sqrt(C_final[:, 0]**2 + C_final[:, 1]**2)))\n",
    "    df['AZ_C_rad'] = np.arctan2(C_final[:, 1], C_final[:, 0])\n",
    "    df['EL_C_rad'] = np.arctan2(C_final[:, 2], np.sqrt(C_final[:, 0]**2 + C_final[:, 1]**2))\n",
    "    \n",
    "    # Spatial residuals calculation for TDOA quality assessment\n",
    "    B_theoretical = (D_mat @ G.T).T\n",
    "    df['residuals_cm'] = np.linalg.norm(B_meters - B_theoretical, axis=1) * 100\n",
    "    \n",
    "    # Export final localizations\n",
    "    FILE_OUT = os.path.join(csv_dir, f\"{base_name}_localizations.csv\")\n",
    "    df.to_csv(FILE_OUT, index=False, sep=';')\n",
    "    print(f\"    Saved spatial localizations: {FILE_OUT}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d71bfa97-0238-4071-bb44-914c7806ac06",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}