etsi_3gpp_decoder.py

#!/usr/bin/env python3
"""
Décodeur pour le champ position ETSI/3GPP 23.032.

La norme 3GPP 23.032 définit le format de codage de la position géographique
utilisé dans les services de localisation (LCS) des réseaux 3GPP.
"""

import struct
from dataclasses import dataclass
from typing import Tuple, Optional


@dataclass
class Position:
    """Représente une position géographique décodée."""
    latitude: float
    longitude: float
    uncertainty: Optional[float] = None
    confidence: Optional[int] = None
    altitude: Optional[float] = None
    altitude_uncertainty: Optional[float] = None


def decode_position_23032(data: bytes) -> Position:
    """
    Décode le champ position ETSI/3GPP 23.032.
    
    Format simplifié (11 octets minimum):
    - Octet 0: Type de position (bit 7-5), Confiance (bit 4-0)
    - Octets 1-3: Latitude (3 octets, complément à 2)
    - Octets 4-6: Longitude (3 octets, complément à 2)
    - Octets 7-10: Champs optionnels (incertitude, altitude, etc.)
    
    Args:
        data: Bytes contenant l'encodage position 3GPP 23.032
        
    Returns:
        Position: Objet contenant la latitude, longitude et métadonnées
    """
    if len(data) < 7:
        raise ValueError("Les données position doivent contenir au minimum 7 octets")
    
    # Octet 0: Type et confiance
    type_and_confidence = data[0]
    position_type = (type_and_confidence >> 5) & 0x07
    confidence = type_and_confidence & 0x1F
    
    # Latitude (3 octets, signé)
    lat_bytes = data[1:4]
    latitude_raw = int.from_bytes(lat_bytes, byteorder='big', signed=True)
    latitude = (latitude_raw / (2**23)) * 90  # Conversion en degrés (-90 à +90)
    
    # Longitude (3 octets, signé)
    lon_bytes = data[4:7]
    longitude_raw = int.from_bytes(lon_bytes, byteorder='big', signed=True)
    longitude = (longitude_raw / (2**23)) * 180  # Conversion en degrés (-180 à +180)
    
    uncertainty = None
    altitude = None
    altitude_uncertainty = None
    
    # Traitement des champs optionnels
    if len(data) >= 8:
        uncertainty = decode_uncertainty(data[7])
    
    if len(data) >= 11 and position_type in [2, 3]:  # Avec altitude
        altitude_bytes = data[8:10]
        altitude_raw = int.from_bytes(altitude_bytes, byteorder='big', signed=True)
        altitude = float(altitude_raw)
        
        if len(data) >= 11:
            altitude_uncertainty = data[10] * 4.0  # Pas de 4 mètres
    
    return Position(
        latitude=latitude,
        longitude=longitude,
        uncertainty=uncertainty,
        confidence=confidence,
        altitude=altitude,
        altitude_uncertainty=altitude_uncertainty
    )


def decode_uncertainty(uncertainty_byte: int) -> float:
    """
    Décode le byte d'incertitude selon 3GPP 23.032.
    
    La formule est: r = C * (1 + a)^b
    où les paramètres dépendent de la valeur du byte.
    """
    if uncertainty_byte == 0:
        return 0.0
    elif uncertainty_byte <= 127:
        # r = C * (1 + a)^b
        # Approximation: r ≈ 10 * uncertainty_byte (en mètres)
        return float(uncertainty_byte) * 10.0
    else:
        # r = (20 + (uncertainty_byte - 127)) * 100
        return float(20 + (uncertainty_byte - 127)) * 100.0


def encode_position_23032(latitude: float, longitude: float, 
                          uncertainty: float = 0.0,
                          confidence: int = 0) -> bytes:
    """
    Encode une position en format ETSI/3GPP 23.032.
    
    Args:
        latitude: Latitude en degrés (-90 à +90)
        longitude: Longitude en degrés (-180 à +180)
        uncertainty: Incertitude en mètres
        confidence: Niveau de confiance (0-31)
        
    Returns:
        bytes: Données encodées
    """
    # Validation
    if not (-90 <= latitude <= 90):
        raise ValueError("Latitude doit être entre -90 et +90")
    if not (-180 <= longitude <= 180):
        raise ValueError("Longitude doit être entre -180 et +180")
    if not (0 <= confidence <= 31):
        raise ValueError("Confidence doit être entre 0 et 31")
    
    # Octet 0: Type (0 = sans altitude) et confiance
    position_type = 0
    byte0 = (position_type << 5) | (confidence & 0x1F)
    
    # Latitude: conversion en entier signé 3 octets
    latitude_raw = int((latitude / 90.0) * (2**23))
    lat_bytes = latitude_raw.to_bytes(3, byteorder='big', signed=True)
    
    # Longitude: conversion en entier signé 3 octets
    longitude_raw = int((longitude / 180.0) * (2**23))
    lon_bytes = longitude_raw.to_bytes(3, byteorder='big', signed=True)
    
    # Incertitude
    uncertainty_byte = encode_uncertainty(uncertainty)
    
    return bytes([byte0]) + lat_bytes + lon_bytes + bytes([uncertainty_byte])


def encode_uncertainty(uncertainty_meters: float) -> int:
    """Encode l'incertitude en byte selon 3GPP 23.032."""
    if uncertainty_meters == 0:
        return 0
    elif uncertainty_meters <= 1270:
        return int(uncertainty_meters / 10.0)
    else:
        return 127 + int((uncertainty_meters - 2000) / 100.0)


def main():
    """Exemples d'utilisation."""
    
    # Exemple 1: Décodage d'une position
    print("=== Exemple 1: Décodage ===")
    # Position: 48.8°N, 2.3°E (Paris), sans incertitude
    example_data = bytes([
        0x00,                          # Type et confiance
        0x56, 0x5F, 0xFD,            # Latitude (≈ 48.8°)
        0x04, 0xA0, 0x00,            # Longitude (≈ 2.3°)
        0x00                          # Incertitude nulle
    ])
    
    try:
        pos = decode_position_23032(example_data)
        print(f"Latitude:  {pos.latitude:.6f}°")
        print(f"Longitude: {pos.longitude:.6f}°")
        print(f"Confiance: {pos.confidence}")
        print(f"Incertitude: {pos.uncertainty} m")
    except Exception as e:
        print(f"Erreur: {e}")
    
    # Exemple 2: Encodage d'une position
    print("\n=== Exemple 2: Encodage ===")
    encoded = encode_position_23032(48.8566, 2.3522, uncertainty=50.0, confidence=15)
    print(f"Données encodées: {encoded.hex()}")
    
    # Décodage pour vérification
    decoded = decode_position_23032(encoded)
    print(f"Latitude:  {decoded.latitude:.6f}°")
    print(f"Longitude: {decoded.longitude:.6f}°")
    print(f"Incertitude: {decoded.uncertainty} m")


if __name__ == "__main__":
    main()