Level Mapping & Z-Axis Logic

import numpy as np
import networkx as nx
from scipy.cluster.hierarchy import linkage, fcluster
from dataclasses import dataclass, field
from typing import List, Tuple, Dict, Optional
import logging

logging.basicConfig(level=logging.INFO, format="%(levelname)s: %(message)s")

@dataclass
class ZLevelConfig:
    vertical_tolerance: float = 0.35  # meters
    min_cluster_size: int = 5
    outlier_iqr_factor: float = 1.5
    graph_type: str = "MultiDiGraph"

def filter_elevation_noise(
    z_values: np.ndarray, 
    iqr_factor: float = 1.5
) -> np.ndarray:
    """Remove elevation outliers using IQR clipping."""
    q75, q25 = np.percentile(z_values, [75, 25])
    iqr = q75 - q25
    lower_bound = q25 - iqr_factor * iqr
    upper_bound = q75 + iqr_factor * iqr
    mask = (z_values >= lower_bound) & (z_values <= upper_bound)
    filtered = z_values[mask]
    if len(filtered) < 5:
        raise ValueError("Too few valid elevation samples after IQR filtering.")
    return filtered

def cluster_floors(
    z_values: np.ndarray, 
    tolerance: float = 0.35, 
    min_size: int = 5
) -> Tuple[np.ndarray, Dict[int, np.ndarray]]:
    """Discretize continuous Z-values into logical floor clusters."""
    if len(z_values) < min_size:
        raise ValueError("Insufficient samples for clustering.")
    
    # Reshape for scipy linkage
    z_reshaped = z_values.reshape(-1, 1)
    Z_linkage = linkage(z_reshaped, method='ward')
    
    # Convert tolerance to max cophenetic distance threshold
    # fcluster uses distance threshold directly
    cluster_labels = fcluster(Z_linkage, t=tolerance, criterion='distance')
    
    # Map labels to sequential 0-based indices
    unique_labels = np.unique(cluster_labels)
    label_to_idx = {lbl: idx for idx, lbl in enumerate(unique_labels)}
    z_indices = np.array([label_to_idx[lbl] for lbl in cluster_labels])
    
    # Group original values by cluster
    floor_centroids = {}
    for idx in range(len(unique_labels)):
        mask = z_indices == idx
        if np.sum(mask) >= min_size:
            floor_centroids[idx] = z_values[mask]
        else:
            logging.warning(f"Cluster {idx} dropped (size < {min_size})")
            z_indices[mask] = -1  # Mark as invalid
            
    return z_indices, floor_centroids

def build_multilevel_graph(
    floor_centroids: Dict[int, np.ndarray],
    vertical_connectors: Optional[List[Tuple[int, int, str]]] = None
) -> nx.MultiDiGraph:
    """Construct a routable multi-level graph from clustered floors."""
    G = nx.MultiDiGraph()
    
    # Add floor nodes with centroid metadata
    for floor_idx, elevations in floor_centroids.items():
        centroid_z = float(np.median(elevations))
        G.add_node(floor_idx, z_centroid=centroid_z, floor_id=floor_idx)
        
    # Add vertical edges (stairs, elevators, ramps)
    if vertical_connectors:
        for src, dst, conn_type in vertical_connectors:
            if src in G.nodes and dst in G.nodes:
                G.add_edge(src, dst, type=conn_type, weight=1.0)
                G.add_edge(dst, src, type=conn_type, weight=1.0)
                
    return G

def normalize_z_pipeline(
    raw_elevations: np.ndarray,
    config: ZLevelConfig = ZLevelConfig(),
    vertical_connectors: Optional[List[Tuple[int, int, str]]] = None
) -> nx.MultiDiGraph:
    """End-to-end Z-axis normalization pipeline."""
    logging.info("Starting Z-axis normalization pipeline...")
    
    # Phase 1: Filter
    filtered_z = filter_elevation_noise(raw_elevations, config.outlier_iqr_factor)
    logging.info(f"Filtered {len(raw_elevations)} -> {len(filtered_z)} valid samples")
    
    # Phase 2: Cluster
    z_indices, floor_centroids = cluster_floors(
        filtered_z, config.vertical_tolerance, config.min_cluster_size
    )
    logging.info(f"Identified {len(floor_centroids)} routable floors")
    
    # Phase 3: Graph
    G = build_multilevel_graph(floor_centroids, vertical_connectors)
    logging.info(f"Graph constructed: {G.number_of_nodes()} nodes, {G.number_of_edges()} edges")
    
    return G

# --- Example Execution ---
if __name__ == "__main__":
    # Simulate raw LiDAR/CAD elevation samples with noise
    np.random.seed(42)
    floor_0 = np.random.normal(0.0, 0.05, 150)
    floor_1 = np.random.normal(3.2, 0.08, 180)
    floor_2 = np.random.normal(6.5, 0.06, 140)
    noise = np.random.uniform(-2.0, 10.0, 30)  # Outliers
    raw_data = np.concatenate([floor_0, floor_1, floor_2, noise])
    
    connectors = [(0, 1, "stair"), (1, 2, "elevator"), (0, 2, "ramp")]
    
    graph = normalize_z_pipeline(raw_data, vertical_connectors=connectors)
    print("\nFloor Metadata:")
    for n, d in graph.nodes(data=True):
        print(f"  Floor {n}: Z-Centroid = {d['z_centroid']:.2f}m")