zhaoyuan_DataPreProcess/core/processors/built_in/lst.py

import zipfile
import sqlite3
import csv
import tempfile
from pathlib import Path
from typing import List, Dict, Tuple, Optional, Any, NamedTuple
import cantools
import os
import subprocess
import numpy as np
import pandas as pd
from collections import Counter
# from datetime import datetime
import argparse
import sys
from pyproj import Proj
from scipy.spatial import cKDTree

import shutil
import json
from dataclasses import dataclass, field

# --- Constants ---
PLAYER_ID_EGO = int(1)
PLAYER_ID_OBJ = int(2)
PLAYER_ID_PEDESTRIAN = int(5)
DEFAULT_TYPE = int(1)
PEDESTRIAN_TYPE = int(5)
OUTPUT_CSV_OBJSTATE = "ObjState.csv"
OUTPUT_CSV_TEMP_OBJSTATE = "ObjState_temp_intermediate.csv"  # Should be eliminated
OUTPUT_CSV_EGOSTATE = "EgoState.csv"  # Not used in final merge? Check logic if needed.
OUTPUT_CSV_MERGED = "merged_ObjState.csv"
OUTPUT_CSV_OBU = "OBUdata.csv"
OUTPUT_CSV_LANEMAP = "LaneMap.csv"
OUTPUT_CSV_EGOMAP = "EgoMap.csv"
MERGED_CSV_EGOMAP = "merged_egomap.csv"
OUTPUT_CSV_FUNCTION = "Function.csv"
ROADMARK_CSV = "RoadMark.csv"
HD_LANE_CSV = "hd_lane.csv"
HD_LINK_CSV = "hd_link.csv"


# --- Configuration Class ---
@dataclass
class Config:
    """Holds configuration paths and settings."""
    zip_path: Path
    output_path: Path
    json_path: Optional[Path]  # Make json_path optional
    dbc_path: Optional[Path] = None
    engine_path: Optional[Path] = None
    map_path: Optional[Path] = None
    utm_zone: int = 51  # Example UTM zone
    x_offset: float = 0.0
    y_offset: float = 0.0

    # Derived paths
    output_dir: Path = field(init=False)

    def __post_init__(self):
        # Use output_path directly as output_dir to avoid nested directories
        self.output_dir = self.output_path
        self.output_dir.mkdir(parents=True, exist_ok=True)


# --- Zip/CSV Processing ---
class ZipCSVProcessor:
    """Processes DB files within a ZIP archive to generate CSV data."""

    # Define column mappings more clearly
    EGO_COLS_NEW = [
        "simTime", "simFrame", "playerId", "v", "speedX", "speedY", "speedZ",
        "posH", "pitch", "roll", "roll_rate", "pitch_rate", "speedH", "posX", "posY", "accelX", "accelY", "accelZ",
        "travelDist", "composite_v", "relative_dist", "x_relative_dist", "y_relative_dist", "type"  # Added type
    ]
    OBJ_COLS_OLD_SUFFIXED = [
        "v_obj", "speedX_obj", "speedY_obj", "speedZ_obj", "posH_obj", "pitch_obj", "roll_obj", "roll_rate_obj",
        "pitch_rate_obj", "speedH_obj",
        "posX_obj", "posY_obj", "accelX_obj", "accelY_obj", "accelZ_obj", "travelDist_obj"
    ]
    OBJ_COLS_MAPPING = {old: new for old, new in
                        zip(OBJ_COLS_OLD_SUFFIXED, EGO_COLS_NEW[3:19])}  # Map suffixed cols to standard names

    def __init__(self, config: Config):
        self.config = config
        self.dbc = self._load_dbc(config.dbc_path)
        self.projection = Proj(proj='utm', zone=config.utm_zone, ellps='WGS84', preserve_units='m')
        self._init_table_config()
        self._init_keyword_mapping()

    def _load_dbc(self, dbc_path: Optional[Path]) -> Optional[cantools.db.Database]:
        dbc_path = Path(dbc_path)
        if not dbc_path or not dbc_path.exists():
            print("DBC path not provided or file not found.")
            return None
        try:
            return cantools.db.load_file(dbc_path)
        except Exception as e:
            print(f"DBC loading failed: {e}")
            return None

    def _init_table_config(self):
        """Initializes configurations for different table types."""
        self.table_config = {
            "gnss_table": self._get_gnss_config(),
            "can_table": self._get_can_config()
        }

    def _get_gnss_config(self):
        # Keep relevant columns, adjust mapping as needed
        return {
            "output_columns": self.EGO_COLS_NEW,  # Use the standard ego columns + type
            "mapping": {  # Map output columns to source DB columns/signals
                "simTime": ("second", "usecond"),
                "simFrame": "ID",
                "v": "speed",
                "speedY": "y_speed",
                "speedX": "x_speed",
                "speedZ": "z_speed",
                "posH": "yaw",
                "pitch": "tilt",
                "roll": "roll",
                "speedH": "yaw_rate",
                "posX": "latitude_dd",  # Source before projection
                "posY": "longitude_dd",  # Source before projection
                "accelX": "x_acceleration",
                "accelY": "y_acceleration",
                "accelZ": "z_acceleration",
                "travelDist": "total_distance",
                # composite_v/relative_dist might not be direct fields in GNSS, handle later if needed
                "composite_v": "speed",  # Placeholder, adjust if needed
                "relative_dist": "distance",  # Placeholder, likely not in GNSS data
                "x_relative_dist": "x_distance",
                "y_relative_dist": "y_distance",
                "type": None  # Will be set later
            },
            "db_columns": ["ID", "second", "usecond", "speed", "y_speed", "x_speed", "z_speed", "z_acceleration",
                           "yaw", "tilt", "roll", "yaw_rate", "latitude_dd", "longitude_dd", "total_distance",
                           "x_acceleration", "y_acceleration", "total_distance", "distance", "x_distance", "y_distance"]
            # Actual cols to SELECT
        }

    def _get_can_config(self):
        # Define columns needed from DB/CAN signals for both EGO and OBJ
        return {
            "mapping": {  # Map unified output columns to CAN signals or direct fields
                # EGO mappings (VUT = Vehicle Under Test)
                "v": "VUT_Speed_mps",
                "speedX": "VUT_Speed_long_mps",
                "speedY": "VUT_Speed_lat_mps",
                "speedZ": "VUT_Speed_z_mps",
                "speedH": "VUT_Yaw_Rate",
                "posX": "VUT_GPS_Latitude",  # Source before projection
                "posY": "VUT_GPS_Longitude",  # Source before projection
                "posH": "VUT_Heading",
                "pitch": "VUT_Pitch",
                "roll": "VUT_Roll",
                "pitch_rate": None,
                "roll_rate": None,
                "accelX": "VUT_Acc_X",
                "accelY": "VUT_Acc_Y2",
                "accelZ": "VUT_Acc_Z",
                # OBJ mappings (UFO = Unidentified Flying Object / Other Vehicle)
                "v_obj": "Speed_mps",
                "speedX_obj": "UFO_Speed_long_mps",
                "speedY_obj": "UFO_Speed_lat_mps",
                "speedZ_obj": "UFO_Speed_z_mps",
                "speedH_obj": "Yaw_Rate",
                "posX_obj": "GPS_Latitude",  # Source before projection
                "posY_obj": "GPS_Longitude",  # Source before projection
                "posH_obj": "Heading",
                "pitch_obj": None,
                "roll_obj": None,
                "pitch_rate_obj": None,
                "roll_rate_obj": None,
                "accelX_obj": "Acc_X",
                "accelY_obj": "Acc_Y",
                "accelZ_obj": "Acc_Z",
                # Relative Mappings
                "composite_v": "VUT_Rel_speed_long_mps",
                "relative_dist": "VUT_Dist_MRP_Abs",
                "x_relative_dist": "VUT_Dist_MRP_long",
                "y_relative_dist": "VUT_Dist_MRP_lat",
                # travelDist often calculated, not direct CAN signal
                "travelDist": None,  # Placeholder
                "travelDist_obj": None  # Placeholder
            },
            "db_columns": ["ID", "second", "usecond", "timestamp", "canid", "len", "frame"]  # Core DB columns
        }

    def _init_keyword_mapping(self):
        """Maps keywords in filenames to table configurations and output CSV names."""
        self.keyword_mapping = {
            "gnss": ("gnss_table", OUTPUT_CSV_OBJSTATE),
            # GNSS likely represents ego, writing to ObjState first? Revisit logic if needed.
            "can4": ("can_table", OUTPUT_CSV_OBJSTATE),  # Process CAN data into the combined ObjState file
        }

    def process_zip(self) -> None:
        """Extracts and processes DB files from the configured ZIP path."""
        print(f"Processing ZIP: {self.config.zip_path}")
        output_dir = self.config.output_dir  # Already created in Config

        try:
            with zipfile.ZipFile(self.config.zip_path, "r") as zip_ref:
                db_files_to_process = []
                for file_info in zip_ref.infolist():
                    # Check if it's a DB file in the CANdata directory
                    if 'CANdata/' in file_info.filename and file_info.filename.endswith('.db'):
                        # Check if the filename contains any of the keywords
                        match = self._match_keyword(file_info.filename)
                        if match:
                            table_type, csv_name = match
                            db_files_to_process.append((file_info, table_type, csv_name))

                if not db_files_to_process:
                    print("No relevant DB files found in CANdata/ matching keywords.")
                    return

                # Process matched DB files
                with tempfile.TemporaryDirectory() as tmp_dir_str:
                    tmp_dir = Path(tmp_dir_str)
                    for file_info, table_type, csv_name in db_files_to_process:
                        print(f"Processing DB: {file_info.filename} for table type {table_type}")
                        extracted_path = tmp_dir / Path(file_info.filename).name
                        try:
                            # Extract the specific DB file
                            with zip_ref.open(file_info.filename) as source, open(extracted_path, "wb") as target:
                                shutil.copyfileobj(source, target)

                            # Process the extracted DB file
                            self._process_db_file(file_info.filename, extracted_path, output_dir, table_type, csv_name)

                        except (sqlite3.Error, pd.errors.EmptyDataError, FileNotFoundError, KeyError) as e:
                            print(f"Error processing DB file {file_info.filename}: {e}")
                        except Exception as e:
                            print(f"Unexpected error processing DB file {file_info.filename}: {e}")
                        finally:
                            if extracted_path.exists():
                                extracted_path.unlink()  # Clean up extracted file

        except zipfile.BadZipFile:
            print(f"Error: Bad ZIP file: {self.config.zip_path}")
        except FileNotFoundError:
            print(f"Error: ZIP file not found: {self.config.zip_path}")
        except Exception as e:
            print(f"An error occurred during ZIP processing: {e}")

    def _match_keyword(self, filename: str) -> Optional[Tuple[str, str]]:
        """Finds the first matching keyword configuration for a filename."""
        for keyword, (table_type, csv_name) in self.keyword_mapping.items():
            if keyword in filename:
                return table_type, csv_name
        return None

    def _process_db_file(
            self, filename: str, db_path: Path, output_dir: Path, table_type: str, csv_name: str
    ) -> None:
        """Connects to SQLite DB and processes the specified table type."""
        output_csv_path = output_dir / csv_name
        try:
            # Use URI for read-only connection
            conn_str = f"file:{db_path}?mode=ro"
            with sqlite3.connect(conn_str, uri=True) as conn:
                cursor = conn.cursor()
                if not self._check_table_exists(cursor, table_type):
                    print(f"Table '{table_type}' does not exist in {db_path.name}. Skipping.")
                    return
                if self._check_table_empty(cursor, table_type):
                    print(f"Table '{table_type}' in {db_path.name} is empty. Skipping.")
                    return

                print(f"Exporting data from table '{table_type}' to {output_csv_path}")
                if table_type == "can_table":
                    self._process_can_table_optimized(filename, cursor, output_csv_path)
                elif table_type == "gnss_table":
                    # Pass output_path directly, avoid intermediate steps
                    self._process_gnss_table(cursor, output_csv_path)
                else:
                    print(f"Warning: No specific processor for table type '{table_type}'. Skipping.")

        except sqlite3.OperationalError as e:
            print(f"Database operational error for {db_path.name}: {e}. Check file integrity/permissions.")
        except sqlite3.DatabaseError as e:
            print(f"Database error connecting to {db_path.name}: {e}")
        except Exception as e:
            print(f"Unexpected error processing DB {db_path.name}: {e}")

    def _check_table_exists(self, cursor, table_name: str) -> bool:
        """Checks if a table exists in the database."""
        try:
            cursor.execute("SELECT name FROM sqlite_master WHERE type='table' AND name=?;", (table_name,))
            return cursor.fetchone() is not None
        except sqlite3.Error as e:
            print(f"Error checking existence of table {table_name}: {e}")
            return False  # Assume not exists on error

    def _check_table_empty(self, cursor, table_name: str) -> bool:
        """Checks if a table is empty."""
        try:
            cursor.execute(f"SELECT COUNT(*) FROM {table_name}")  # Use COUNT(*) for efficiency
            count = cursor.fetchone()[0]
            return count == 0
        except sqlite3.Error as e:
            # If error occurs (e.g., table doesn't exist after check - race condition?), treat as problematic/empty
            print(f"Error checking if table {table_name} is empty: {e}")
            return True

    def _process_gnss_table(self, cursor, output_path: Path) -> None:
        """Processes gnss_table data and writes directly to CSV."""
        config = self.table_config["gnss_table"]
        db_columns = config["db_columns"]
        output_columns = config["output_columns"]
        mapping = config["mapping"]

        try:
            cursor.execute(f"SELECT {', '.join(db_columns)} FROM gnss_table")
            rows = cursor.fetchall()
            if not rows:
                print("No data found in gnss_table.")
                return

            processed_data = []
            for row in rows:
                row_dict = dict(zip(db_columns, row))
                record = {}

                # Calculate simTime
                record["simTime"] = round(row_dict.get("second", 0) + row_dict.get("usecond", 0) / 1e6, 2)

                # Map other columns
                for out_col in output_columns:
                    if out_col == "simTime": continue  # Already handled
                    if out_col == "playerId":
                        record[out_col] = PLAYER_ID_EGO  # Assuming GNSS is ego
                        continue
                    if out_col == "type":
                        record[out_col] = DEFAULT_TYPE
                        continue

                    source_info = mapping.get(out_col)
                    if source_info is None:
                        record[out_col] = 0.0  # Or np.nan if preferred
                    elif isinstance(source_info, tuple):
                        # This case was only for simTime, handled above
                        record[out_col] = 0.0
                    else:  # Direct mapping from db_columns
                        raw_value = row_dict.get(source_info)
                        if raw_value is not None:
                            # Handle projection for position columns
                            if out_col == "posX":
                                # Assuming source_info = "latitude_dd"
                                lat = row_dict.get(mapping["posX"])
                                lon = row_dict.get(mapping["posY"])
                                if lat is not None and lon is not None:
                                    proj_x, _ = self.projection(lon, lat)
                                    record[out_col] = round(proj_x, 6)
                                else:
                                    record[out_col] = 0.0
                            elif out_col == "posY":
                                # Assuming source_info = "longitude_dd"
                                lat = row_dict.get(mapping["posX"])
                                lon = row_dict.get(mapping["posY"])
                                if lat is not None and lon is not None:
                                    _, proj_y = self.projection(lon, lat)
                                    record[out_col] = round(proj_y, 6)
                                else:
                                    record[out_col] = 0.0
                            elif out_col in ["composite_v", "relative_dist"]:
                                # Handle these based on source if available, else default
                                record[out_col] = round(float(raw_value), 3) if source_info else 0.0
                            else:
                                # General case: round numeric values
                                try:
                                    record[out_col] = round(float(raw_value), 3)
                                except (ValueError, TypeError):
                                    record[out_col] = raw_value  # Keep as is if not numeric
                        else:
                            record[out_col] = 0.0  # Default for missing source data

                processed_data.append(record)

            if processed_data:
                df_final = pd.DataFrame(processed_data)[output_columns].iloc[::4].reset_index(
                    drop=True)  # Ensure column order
                # df_final.to_csv("/home/output/V2I_CSAE53-2020_LeftTurnAssist_LST_01/ObjState_old.csv", index=False, encoding="utf-8")
                # print(df_final)
                # df_final['speedY'] = -df_final['speedY']
                # df_final['accelY'] = -df_final['accelY']

                # df_final['speedZ'] = -df_final['speedZ']
                # df_final['accelZ'] = -df_final['accelZ']
                df_final['simFrame'] = np.arange(1, len(df_final) + 1)
                df_final["pitch_rate"] = df_final["pitch"].diff() / df_final["simTime"].diff()
                df_final["roll_rate"] = df_final["roll"].diff() / df_final["simTime"].diff()
                # print("df_final[\"posH\"] is", df_final["posH"])
                df_final["posH"] = (90 - df_final["posH"])
                stopcar_flag = self.is_valid_interval(df_final)
                # print("stopcar_flag is", stopcar_flag)
                if stopcar_flag:
                    first_gt_1 = df_final['v'].gt(1).idxmax()
                    last_gt_1 = df_final['v'].gt(0.15)[::-1].idxmax()
                    result_df = df_final.loc[first_gt_1:last_gt_1].copy()
                    result_df.to_csv(output_path, index=False, encoding="utf-8")
                else:
                    df_final.to_csv(output_path, index=False, encoding="utf-8")
                print(f"Successfully wrote GNSS data to {output_path}")
            else:
                print("No processable records found in gnss_table.")

        except sqlite3.Error as e:
            print(f"SQL error during GNSS processing: {e}")
        except Exception as e:
            print(f"Unexpected error during GNSS processing: {e}")

    def _process_can_table_optimized(self, filename, cursor, output_path: Path) -> None:
        """Processes CAN data directly into the final merged DataFrame format."""
        config = self.table_config["can_table"]
        db_columns = config["db_columns"]
        mapping = config["mapping"]

        try:
            cursor.execute(f"SELECT {', '.join(db_columns)} FROM can_table")
            rows = cursor.fetchall()
            if not rows:
                print("No data found in can_table.")
                return

            all_records = []
            for row in rows:
                row_dict = dict(zip(db_columns, row))
                # Decode CAN frame if DBC is available
                decoded_signals = self._decode_can_frame(row_dict)
                # Create a unified record combining DB fields and decoded signals
                record = self._create_unified_can_record(row_dict, decoded_signals, mapping)
                if record:  # Only add if parsing was successful
                    all_records.append(record)

            if not all_records:
                print("No CAN records could be successfully processed.")
                return

            # Convert raw records to DataFrame for easier manipulation
            df_raw = pd.DataFrame(all_records)

            # Separate EGO and OBJ data based on available columns
            df_ego = self._extract_vehicle_data(df_raw, PLAYER_ID_EGO)
            # if 'pedestrian' in filename.lower():
            #     df_obj = self._extract_vehicle_data(df_raw, PLAYER_ID_PEDESTRIAN)
            # else:
            df_obj = self._extract_vehicle_data(df_raw, PLAYER_ID_OBJ)

            # Project coordinates
            df_ego = self._project_coordinates(df_ego, 'posX', 'posY')
            # df_ego = self._project_coordinates(df_ego, 'posX', 'posY', 'speedX', 'speedY', 'speedZ', 'accelX', 'accelY', 'accelZ', 'posH', 'pitch', 'roll')
            print("df_ego is", df_ego.columns)
            df_obj = self._project_coordinates(df_obj, 'posX', 'posY')  # Use same column names after extraction
            # df_obj = self._project_coordinates(df_obj, 'posX', 'posY', 'speedX', 'speedY', 'speedZ', 'accelX', 'accelY', 'accelZ', 'posH', 'pitch', 'roll')  # Use same column names after extraction

            # Add calculated/default columns
            df_ego['type'] = DEFAULT_TYPE
            if 'pedestrian' in filename.lower():
                df_obj['type'] = PEDESTRIAN_TYPE
            else:
                df_obj['type'] = DEFAULT_TYPE
            # Note: travelDist is often calculated later or not available directly

            # Ensure both have the same columns before merging
            final_columns = self.EGO_COLS_NEW  # Target columns
            df_ego = df_ego.reindex(columns=final_columns).iloc[::4]
            df_obj = df_obj.reindex(columns=final_columns).iloc[::4]

            # Reindex simFrame of ego and obj
            df_ego['simFrame'] = np.arange(1, len(df_ego) + 1)
            df_obj['simFrame'] = np.arange(1, len(df_obj) + 1)

            # Merge EGO and OBJ dataframes
            df_merged = pd.concat([df_ego, df_obj], ignore_index=True)

            # Sort and clean up
            df_merged.sort_values(by=["simTime", "simFrame", "playerId"], inplace=True)
            df_merged.fillna(0, inplace=True)
            df_merged.reset_index(drop=True, inplace=True)

            # Fill potential NaNs introduced by reindexing or missing data
            # Choose appropriate fill strategy (e.g., 0, forward fill, or leave as NaN)
            # df_merged.fillna(0.0, inplace=True)  # Example: fill with 0.0
            stopcar_flag = self.is_valid_interval(df_merged)
            print("stopcar_flag is", stopcar_flag)
            if stopcar_flag:
                print("筛选非静止车辆数据!")
                first_gt_01 = df_merged[df_merged['playerId'] == 1]['v'].gt(1).idxmax()
                last_gt_01 = df_merged[df_merged['playerId'] == 1]['v'].gt(0.15)[::-1].idxmax()
                result_df = df_merged.loc[first_gt_01:last_gt_01 - 1].copy()
                # Save the final merged DataFrame
                result_df.to_csv(output_path, index=False, encoding="utf-8")
            else:
                df_merged.to_csv(output_path, index=False, encoding="utf-8")
            print(f"Successfully processed CAN data and wrote merged output to {output_path}")

        except sqlite3.Error as e:
            print(f"SQL error during CAN processing: {e}")
        except KeyError as e:
            print(f"Key error during CAN processing - mapping issue? Missing key: {e}")
        except Exception as e:
            print(f"Unexpected error during CAN processing: {e}")
            import traceback
            traceback.print_exc()  # Print detailed traceback for debugging

    def _decode_can_frame(self, row_dict: Dict) -> Dict[str, Any]:
        """Decodes CAN frame using DBC file if available."""
        decoded_signals = {}
        if self.dbc and 'canid' in row_dict and 'frame' in row_dict and 'len' in row_dict:
            can_id = row_dict['canid']
            frame_bytes = bytes(row_dict['frame'][:row_dict['len']])  # Ensure correct length
            try:
                message_def = self.dbc.get_message_by_frame_id(can_id)
                decoded_signals = message_def.decode(frame_bytes, decode_choices=False,
                                                     allow_truncated=True)  # Allow truncated
            except KeyError:
                # Optional: print(f"Warning: CAN ID 0x{can_id:X} not found in DBC.")
                pass  # Ignore unknown IDs silently
            except ValueError as e:
                print(
                    f"Warning: Decoding ValueError for CAN ID 0x{can_id:X} (length {row_dict['len']}, data: {frame_bytes.hex()}): {e}")
            except Exception as e:
                print(f"Warning: Error decoding CAN ID 0x{can_id:X}: {e}")
        return decoded_signals

    def _create_unified_can_record(self, row_dict: Dict, decoded_signals: Dict, mapping: Dict) -> Optional[
        Dict[str, Any]]:
        """Creates a single record combining DB fields and decoded signals based on mapping."""
        record = {}
        try:
            # Handle time and frame ID first
            record["simTime"] = round(row_dict.get("second", 0) + row_dict.get("usecond", 0) / 1e6, 2)
            record["simFrame"] = row_dict.get("ID")
            record["canid"] = f"0x{row_dict.get('canid'):X}"  # Store CAN ID if needed

            # Populate record using the mapping config
            for target_col, source_info in mapping.items():
                if target_col in ["simTime", "simFrame", "canid"]: continue  # Already handled
                if isinstance(source_info, tuple): continue  # Should only be time

                # source_info is now the signal name (or None)
                signal_name = source_info
                if signal_name and signal_name in decoded_signals:
                    # Value from decoded CAN signal
                    raw_value = decoded_signals[signal_name]
                    try:
                        # Apply scaling/offset if needed (cantools handles this)
                        # Round appropriately, especially for floats
                        if isinstance(raw_value, (int, float)):
                            # Be cautious with lat/lon precision before projection
                            if "Latitude" in target_col or "Longitude" in target_col:
                                record[target_col] = float(raw_value)  # Keep precision for projection
                            else:
                                record[target_col] = round(float(raw_value), 6)
                        else:
                            record[target_col] = raw_value  # Keep non-numeric as is (e.g., enums)
                    except (ValueError, TypeError):
                        record[target_col] = raw_value  # Assign raw value if conversion fails
                # If signal not found or source_info is None, leave it empty for now
                # Will be filled later or during DataFrame processing

            return record

        except Exception as e:
            print(f"Error creating unified record for row {row_dict.get('ID')}: {e}")
            return None

    def _extract_vehicle_data(self, df_raw: pd.DataFrame, player_id: int) -> pd.DataFrame:
        """Extracts and renames columns for a specific vehicle (EGO or OBJ)."""
        df_vehicle = pd.DataFrame()
        # df_vehicle["simTime"] = df_raw["simTime"].drop_duplicates().sort_values().reset_index(drop=True)
        # df_vehicle["simFrame"] = np.arange(1, len(df_vehicle) + 1)
        # df_vehicle["playerId"] = int(player_id)
        df_vehicle_temps_ego = pd.DataFrame()
        df_vehicle_temps_obj = pd.DataFrame()

        if player_id == PLAYER_ID_EGO:
            # Select EGO columns (not ending in _obj) + relative columns
            ego_cols = {target: source for target, source in self.table_config['can_table']['mapping'].items()
                        if source and not isinstance(source, tuple) and not target.endswith('_obj')}
            print("ego_cols is", ego_cols)
            rename_map = {}
            select_cols_raw = []
            for target_col, source_info in ego_cols.items():
                if source_info:  # Mapped signal/field name in df_raw
                    select_cols_raw.append(target_col)  # Column names in df_raw are already target names
                    rename_map[target_col] = target_col  # No rename needed here

            # Include relative speed and distance for ego frame
            relative_cols = ["composite_v", "relative_dist"]
            select_cols_raw.extend(relative_cols)
            for col in relative_cols:
                rename_map[col] = col

            # Select and rename
            df_vehicle_temp = df_raw[list(set(select_cols_raw) & set(df_raw.columns))]  # Select available columns
            for col in df_vehicle_temp.columns:
                df_vehicle_temps_ego[col] = df_vehicle_temp[col].dropna().reset_index(drop=True)
            df_vehicle = pd.concat([df_vehicle, df_vehicle_temps_ego], axis=1)

        elif (player_id == PLAYER_ID_OBJ) or (player_id == PLAYER_ID_PEDESTRIAN):
            # Select OBJ columns (ending in _obj)
            obj_cols = {target: source for target, source in self.table_config['can_table']['mapping'].items()
                        if source and not isinstance(source, tuple) and target.endswith('_obj')}
            rename_map = {}
            select_cols_raw = []
            for target_col, source_info in obj_cols.items():
                if source_info:
                    select_cols_raw.append(target_col)  # Original _obj column name
                    # Map from VUT_XXX_obj -> VUT_XXX
                    rename_map[target_col] = self.OBJ_COLS_MAPPING.get(target_col,
                                                                       target_col)  # Rename to standard name

            # Select and rename
            df_vehicle_temp = df_raw[list(set(select_cols_raw) & set(df_raw.columns))]  # Select available columns
            df_vehicle_temp.rename(columns=rename_map, inplace=True)
            for col in df_vehicle_temp.columns:
                df_vehicle_temps_obj[col] = df_vehicle_temp[col].dropna().reset_index(drop=True)
            df_vehicle = pd.concat([df_vehicle, df_vehicle_temps_obj], axis=1)

            # Copy relative speed/distance from ego calculation (assuming it's relative *to* ego)
            if "composite_v" in df_raw.columns:
                df_vehicle["composite_v"] = df_raw["composite_v"].dropna().reset_index(drop=True)
            if "relative_dist" in df_raw.columns:
                df_vehicle["relative_dist"] = df_raw["relative_dist"].dropna().reset_index(drop=True)

        # Drop rows where essential position data might be missing after selection/renaming
        # Adjust required columns as necessary
        # required_pos = ['posX', 'posY', 'posH']
        # df_vehicle.dropna(subset=[col for col in required_pos if col in df_vehicle.columns], inplace=True)

        try:
            df_vehicle["simTime"] = np.round(
                np.linspace(df_raw["simTime"].tolist()[0], df_raw["simTime"].tolist()[0] + 0.01 * (len(df_vehicle)),
                            len(df_vehicle)), 2)
            df_vehicle["simFrame"] = np.arange(1, len(df_vehicle) + 1)
            df_vehicle["playerId"] = int(player_id)
            df_vehicle['playerId'] = pd.to_numeric(df_vehicle['playerId']).astype(int)
            df_vehicle["pitch_rate"] = df_vehicle["pitch"].diff() / df_vehicle["simTime"].diff()
            df_vehicle["roll_rate"] = df_vehicle["roll"].diff() / df_vehicle["simTime"].diff()
            # print("df_vehicle is", df_vehicle)
        except ValueError as ve:
            print(f"{ve}")
        except TypeError as te:
            print(f"{te}")
        except Exception as Ee:
            print(f"{Ee}")

        return df_vehicle

    def _project_coordinates(self, df: pd.DataFrame, lat_col: str, lon_col: str) -> pd.DataFrame:
        """Applies UTM projection to latitude and longitude columns."""
        if lat_col in df.columns and lon_col in df.columns:
            # Ensure data is numeric and handle potential errors/missing values
            lat = pd.to_numeric(df[lat_col], errors='coerce')
            lon = pd.to_numeric(df[lon_col], errors='coerce')
            # speedX = pd.to_numeric(df[speedX_col], errors='coerce')
            # speedY = pd.to_numeric(df[speedY_col], errors='coerce')
            # speedZ = pd.to_numeric(0, errors='coerce')
            # accelX = pd.to_numeric(df[accelX_col], errors='coerce')
            # accelY = pd.to_numeric(df[accelY_col], errors='coerce')
            # accelZ = pd.to_numeric(0, errors='coerce')
            # posh = pd.to_numeric(df[posh_col], errors='coerce')
            # pitch = pd.to_numeric(df[pitch_col], errors='coerce')
            # roll = pd.to_numeric(df[roll_col], errors='coerce')
            valid_coords = lat.notna() & lon.notna()

            if valid_coords.any():
                x, y = self.projection(lon[valid_coords].values, lat[valid_coords].values)
                # Update DataFrame, assign NaN where original coords were invalid
                df.loc[valid_coords, lat_col] = np.round(x, 6)  # Overwrite latitude col with X
                df.loc[valid_coords, lon_col] = np.round(y, 6)  # Overwrite longitude col with Y
                df.loc[~valid_coords, [lat_col, lon_col]] = np.nan  # Set invalid coords to NaN
            else:
                # No valid coordinates found, set columns to NaN or handle as needed
                df[lat_col] = np.nan
                df[lon_col] = np.nan

            # Rename columns AFTER projection for clarity
            df.rename(columns={lat_col: 'posX', lon_col: 'posY'}, inplace=True)

        else:
            # Ensure columns exist even if projection didn't happen
            if 'posX' not in df.columns: df['posX'] = np.nan
            if 'posY' not in df.columns: df['posY'] = np.nan
            print(f"Warning: Latitude ('{lat_col}') or Longitude ('{lon_col}') columns not found for projection.")

        return df

    def is_valid_interval(self, df, threshold=1, window_sec=0.1):
        '''
        检查区间[start_idx, end_idx]是否满足前后window_sec秒的速度均 < threshold
        '''
        # 获取时间范围
        print("获取时间范围...")
        start_time = df['simTime'].tolist()[0]
        end_time = df['simTime'].tolist()[-1]

        # 前5秒数据
        # print("前5秒数据...")
        mask_before = (df['simTime'] >= start_time) & \
                      (df['simTime'] < start_time + window_sec)
        # 后5秒数据
        mask_after = (df['simTime'] < end_time) & \
                     (df['simTime'] >= end_time - window_sec)

        # 判断前后是否均 < threshold
        return (df.loc[mask_before, 'v'].max() < threshold) and \
            (df.loc[mask_after, 'v'].max() < threshold)


# --- Polynomial Fitting (Largely unchanged, minor cleanup) ---
class PolynomialCurvatureFitting:
    """Calculates curvature and its derivative using polynomial fitting."""

    def __init__(self, lane_map_path: Path, degree: int = 3):
        self.lane_map_path = Path(lane_map_path)
        self.degree = degree
        self.data = self._load_data()
        if self.data is not None:
            self.points = self.data[["centerLine_x", "centerLine_y"]].values
            self.x_data, self.y_data = self.points[:, 0], self.points[:, 1]
        else:
            self.points = np.empty((0, 2))
            self.x_data, self.y_data = np.array([]), np.array([])

    def _load_data(self) -> Optional[pd.DataFrame]:
        """Loads lane map data safely."""

        if not self.lane_map_path.exists() or self.lane_map_path.stat().st_size == 0:
            print(f"Warning: LaneMap file not found or empty: {self.lane_map_path}")
            return None
        try:
            return pd.read_csv(self.lane_map_path)
        except pd.errors.EmptyDataError:
            print(f"Warning: LaneMap file is empty: {self.lane_map_path}")
            return None
        except Exception as e:
            print(f"Error reading LaneMap file {self.lane_map_path}: {e}")
            return None

    def curvature(self, coefficients: np.ndarray, x: float) -> float:
        """Computes curvature of the polynomial at x."""
        if len(coefficients) < 3:  # Need at least degree 2 for curvature
            return 0.0
        first_deriv_coeffs = np.polyder(coefficients)
        second_deriv_coeffs = np.polyder(first_deriv_coeffs)
        dy_dx = np.polyval(first_deriv_coeffs, x)
        d2y_dx2 = np.polyval(second_deriv_coeffs, x)
        denominator = (1 + dy_dx ** 2) ** 1.5
        return np.abs(d2y_dx2) / denominator if denominator != 0 else np.inf

    def curvature_derivative(self, coefficients: np.ndarray, x: float) -> float:
        """Computes the derivative of curvature with respect to x."""
        if len(coefficients) < 4:  # Need at least degree 3 for derivative of curvature
            return 0.0
        first_deriv_coeffs = np.polyder(coefficients)
        second_deriv_coeffs = np.polyder(first_deriv_coeffs)
        third_deriv_coeffs = np.polyder(second_deriv_coeffs)

        dy_dx = np.polyval(first_deriv_coeffs, x)
        d2y_dx2 = np.polyval(second_deriv_coeffs, x)
        d3y_dx3 = np.polyval(third_deriv_coeffs, x)

        denominator = (1 + dy_dx ** 2) ** 2.5  # Note the power is 2.5 or 5/2
        if denominator == 0:
            return np.inf

        numerator = d3y_dx3 * (1 + dy_dx ** 2) - 3 * dy_dx * d2y_dx2 * d2y_dx2  # Corrected term order? Verify formula
        # Standard formula: (d3y_dx3*(1 + dy_dx**2) - 3*dy_dx*(d2y_dx2**2)) / ((1 + dy_dx**2)**(5/2)) * sign(d2y_dx2)
        # Let's stick to the provided calculation logic but ensure denominator is correct
        # The provided formula in the original code seems to be for dk/ds (arc length), not dk/dx.
        # Re-implementing dk/dx based on standard calculus:
        term1 = d3y_dx3 * (1 + dy_dx ** 2) ** (3 / 2)
        term2 = d2y_dx2 * (3 / 2) * (1 + dy_dx ** 2) ** (1 / 2) * (2 * dy_dx * d2y_dx2)  # Chain rule
        numerator_dk_dx = term1 - term2
        denominator_dk_dx = (1 + dy_dx ** 2) ** 3

        if denominator_dk_dx == 0:
            return np.inf

        # Take absolute value or not? Original didn't. Let's omit abs() for derivative.
        return numerator_dk_dx / denominator_dk_dx
        # dk_dx = (d3y_dx3 * (1 + dy_dx ** 2) - 3 * dy_dx * d2y_dx2 ** 2) / (
        #         (1 + dy_dx ** 2) ** (5/2) # Original had power 3 ?? Double check this formula source
        # ) * np.sign(d2y_dx2) # Need sign of curvature
        # return dk_dx

    def polynomial_fit(
            self, x_window: np.ndarray, y_window: np.ndarray
    ) -> Tuple[Optional[np.ndarray], Optional[np.poly1d]]:
        """Performs polynomial fitting, handling potential rank warnings."""
        if len(x_window) <= self.degree:
            print(f"Warning: Window size {len(x_window)} is <= degree {self.degree}. Cannot fit.")
            return None, None
        try:
            # Use warnings context manager if needed, but RankWarning often indicates insufficient data variability
            # with warnings.catch_warnings():
            #     warnings.filterwarnings('error', category=np.RankWarning) # Or ignore
            coefficients = np.polyfit(x_window, y_window, self.degree)
            return coefficients, np.poly1d(coefficients)
        except np.RankWarning:
            print(f"Warning: Rank deficient fitting for window. Check data variability.")
            # Attempt lower degree fit? Or return None? For now, return None.
            # try:
            #      coefficients = np.polyfit(x_window, y_window, len(x_window) - 1)
            #      return coefficients, np.poly1d(coefficients)
            # except:
            return None, None
        except Exception as e:
            print(f"Error during polynomial fit: {e}")
            return None, None

    def find_best_window(self, point: Tuple[float, float], window_size: int) -> Optional[int]:
        """Finds the start index of the window whose center is closest to the point."""
        if len(self.x_data) < window_size:
            print("Warning: Not enough data points for the specified window size.")
            return None

        x_point, y_point = point
        min_dist_sq = np.inf
        best_start_index = -1

        # Calculate window centers more efficiently
        # Use rolling mean if window_size is large, otherwise simple loop is fine
        num_windows = len(self.x_data) - window_size + 1
        if num_windows <= 0: return None

        for start in range(num_windows):
            x_center = np.mean(self.x_data[start: start + window_size])
            y_center = np.mean(self.y_data[start: start + window_size])
            dist_sq = (x_point - x_center) ** 2 + (y_point - y_center) ** 2
            if dist_sq < min_dist_sq:
                min_dist_sq = dist_sq
                best_start_index = start

        return best_start_index if best_start_index != -1 else None

    def find_projection(
            self,
            x_target: float,
            y_target: float,
            polynomial: np.poly1d,
            x_range: Tuple[float, float],
            search_points: int = 100,  # Number of points instead of step size
    ) -> Optional[Tuple[float, float, float]]:
        """Finds the approximate closest point on the polynomial within the x_range."""
        if x_range[1] <= x_range[0]: return None  # Invalid range

        x_values = np.linspace(x_range[0], x_range[1], search_points)
        y_values = polynomial(x_values)
        distances_sq = (x_target - x_values) ** 2 + (y_target - y_values) ** 2

        if len(distances_sq) == 0: return None

        min_idx = np.argmin(distances_sq)
        min_distance = np.sqrt(distances_sq[min_idx])

        return x_values[min_idx], y_values[min_idx], min_distance

    def fit_and_project(
            self, points: np.ndarray, window_size: int
    ) -> List[Dict[str, Any]]:
        """Fits polynomial and calculates curvature for each point in the input array."""
        if self.data is None or len(self.x_data) < window_size:
            print("Insufficient LaneMap data for fitting.")
            # Return default values for all points
            return [
                {
                    "projection": (np.nan, np.nan),
                    "lightMask": 0,
                    "curvHor": np.nan,
                    "curvHorDot": np.nan,
                    "laneOffset": np.nan,
                }
            ] * len(points)

        results = []
        if points.ndim != 2 or points.shape[1] != 2:
            raise ValueError("Input points must be a 2D numpy array with shape (n, 2).")

        for x_target, y_target in points:
            result = {  # Default result
                "projection": (np.nan, np.nan),
                "lightMask": 0,
                "curvHor": np.nan,
                "curvHorDot": np.nan,
                "laneOffset": np.nan,
            }

            best_start = self.find_best_window((x_target, y_target), window_size)
            if best_start is None:
                results.append(result)
                continue

            x_window = self.x_data[best_start: best_start + window_size]
            y_window = self.y_data[best_start: best_start + window_size]

            coefficients, polynomial = self.polynomial_fit(x_window, y_window)
            if coefficients is None or polynomial is None:
                results.append(result)
                continue

            x_min, x_max = np.min(x_window), np.max(x_window)
            projection_result = self.find_projection(
                x_target, y_target, polynomial, (x_min, x_max)
            )

            if projection_result is None:
                results.append(result)
                continue

            proj_x, proj_y, min_distance = projection_result

            curv_hor = self.curvature(coefficients, proj_x)
            curv_hor_dot = self.curvature_derivative(coefficients, proj_x)

            result = {
                "projection": (round(proj_x, 6), round(proj_y, 6)),
                "lightMask": 0,
                "curvHor": round(curv_hor, 6),
                "curvHorDot": round(curv_hor_dot, 6),
                "laneOffset": round(min_distance, 6),
            }
            results.append(result)

        return results


# --- Data Quality Analyzer (Optimized) ---
class DataQualityAnalyzer:
    """Analyzes data quality metrics, focusing on frame loss."""

    def __init__(self, df: Optional[pd.DataFrame] = None):
        self.df = df if df is not None and not df.empty else pd.DataFrame()  # Ensure df is DataFrame

    def analyze_frame_loss(self) -> Dict[str, Any]:
        """Analyzes frame loss characteristics."""
        metrics = {
            "total_frames_data": 0,
            "unique_frames_count": 0,
            "min_frame": np.nan,
            "max_frame": np.nan,
            "expected_frames": 0,
            "dropped_frames_count": 0,
            "loss_rate": np.nan,
            "max_consecutive_loss": 0,
            "max_loss_start_frame": np.nan,
            "max_loss_end_frame": np.nan,
            "loss_intervals_distribution": {},
            "valid": False,  # Indicate if analysis was possible
            "message": ""
        }

        if self.df.empty or 'simFrame' not in self.df.columns:
            metrics["message"] = "DataFrame is empty or 'simFrame' column is missing."
            return metrics

        # Drop rows with NaN simFrame and ensure integer type
        frames_series = self.df['simFrame'].dropna().astype(int)
        metrics["total_frames_data"] = len(frames_series)

        if frames_series.empty:
            metrics["message"] = "No valid 'simFrame' data found after dropping NaN."
            return metrics

        unique_frames = sorted(frames_series.unique())
        metrics["unique_frames_count"] = len(unique_frames)

        if metrics["unique_frames_count"] < 2:
            metrics["message"] = "Less than two unique frames; cannot analyze loss."
            metrics["valid"] = True  # Data exists, just not enough to analyze loss
            if metrics["unique_frames_count"] == 1:
                metrics["min_frame"] = unique_frames[0]
                metrics["max_frame"] = unique_frames[0]
                metrics["expected_frames"] = 1
            return metrics

        metrics["min_frame"] = unique_frames[0]
        metrics["max_frame"] = unique_frames[-1]
        metrics["expected_frames"] = metrics["max_frame"] - metrics["min_frame"] + 1

        # Calculate differences between consecutive unique frames
        frame_diffs = np.diff(unique_frames)
        # Gaps are where diff > 1. The number of lost frames in a gap is diff - 1.
        gaps = frame_diffs[frame_diffs > 1]
        lost_frames_in_gaps = gaps - 1

        metrics["dropped_frames_count"] = int(lost_frames_in_gaps.sum())

        if metrics["expected_frames"] > 0:
            metrics["loss_rate"] = round(metrics["dropped_frames_count"] / metrics["expected_frames"], 4)
        else:
            metrics["loss_rate"] = 0.0  # Avoid division by zero if min_frame == max_frame (already handled)

        if len(lost_frames_in_gaps) > 0:
            metrics["max_consecutive_loss"] = int(lost_frames_in_gaps.max())
            # Find where the max loss occurred
            max_loss_indices = np.where(frame_diffs == metrics["max_consecutive_loss"] + 1)[0]
            # Get the first occurrence start/end frames
            max_loss_idx = max_loss_indices[0]
            metrics["max_loss_start_frame"] = unique_frames[max_loss_idx]
            metrics["max_loss_end_frame"] = unique_frames[max_loss_idx + 1]

            # Count distribution of loss interval lengths
            loss_counts = Counter(lost_frames_in_gaps)
            metrics["loss_intervals_distribution"] = {int(k): int(v) for k, v in loss_counts.items()}
        else:
            metrics["max_consecutive_loss"] = 0

        metrics["valid"] = True
        metrics["message"] = "Frame loss analysis complete."
        return metrics


def get_all_csv_files(path: Path) -> List[Path]:
    """Gets all CSV files in path, excluding specific ones."""
    excluded_files = {OUTPUT_CSV_LANEMAP, ROADMARK_CSV}
    return [
        file_path
        for file_path in path.rglob("*.csv")  # Recursive search
        if file_path.is_file() and file_path.name not in excluded_files
    ]


def run_frame_loss_analysis_on_folder(path: Path) -> Dict[str, Dict[str, Any]]:
    """Runs frame loss analysis on all relevant CSV files in a folder."""
    analysis_results = {}
    csv_files = get_all_csv_files(path)

    if not csv_files:
        print(f"No relevant CSV files found in {path}")
        return analysis_results

    for file_path in csv_files:
        file_name = file_path.name
        if file_name in {OUTPUT_CSV_FUNCTION, OUTPUT_CSV_OBU}:  # Skip specific files if needed
            print(f"Skipping frame analysis for: {file_name}")
            continue

        print(f"Analyzing frame loss for: {file_name}")
        if file_path.stat().st_size == 0:
            print(f"File {file_name} is empty. Skipping analysis.")
            analysis_results[file_name] = {"valid": False, "message": "File is empty."}
            continue

        try:
            # Read only necessary column if possible, handle errors
            df = pd.read_csv(file_path, usecols=['simFrame'], index_col=False,
                             on_bad_lines='warn')  # 'warn' or 'skip'
            analyzer = DataQualityAnalyzer(df)
            metrics = analyzer.analyze_frame_loss()
            analysis_results[file_name] = metrics

            # Optionally print a summary here
            if metrics["valid"]:
                print(f"  Loss Rate: {metrics.get('loss_rate', np.nan) * 100:.2f}%, "
                      f"Dropped: {metrics.get('dropped_frames_count', 'N/A')}, "
                      f"Max Gap: {metrics.get('max_consecutive_loss', 'N/A')}")
            else:
                print(f"  Analysis failed: {metrics.get('message')}")


        except pd.errors.EmptyDataError:
            print(f"File {file_name} contains no data after reading.")
            analysis_results[file_name] = {"valid": False, "message": "Empty data after read."}
        except ValueError as ve:  # Handle case where simFrame might not be present
            print(f"ValueError processing file {file_name}: {ve}. Is 'simFrame' column present?")
            analysis_results[file_name] = {"valid": False, "message": f"ValueError: {ve}"}
        except Exception as e:
            print(f"Unexpected error processing file {file_name}: {e}")
            analysis_results[file_name] = {"valid": False, "message": f"Unexpected error: {e}"}

    return analysis_results


def data_precheck(output_dir: Path, max_allowed_loss_rate: float = 0.20) -> bool:
    """Checks data quality, focusing on frame loss rate."""
    print(f"--- Running Data Quality Precheck on: {output_dir} ---")
    if not output_dir.exists() or not output_dir.is_dir():
        print(f"Error: Output directory does not exist: {output_dir}")
        return False

    try:
        frame_loss_results = run_frame_loss_analysis_on_folder(output_dir)
    except Exception as e:
        print(f"Critical error during frame loss analysis: {e}")
        return False  # Treat critical error as failure

    if not frame_loss_results:
        print("Warning: No files were analyzed for frame loss.")
        # Decide if this is a failure or just a warning. Let's treat it as OK for now.
        return True

    all_checks_passed = True
    for file_name, metrics in frame_loss_results.items():
        if metrics.get("valid", False):
            loss_rate = metrics.get("loss_rate", np.nan)
            if pd.isna(loss_rate):
                print(f"  {file_name}: Loss rate could not be calculated.")
                # Decide if NaN loss rate is acceptable.
            elif loss_rate > max_allowed_loss_rate:
                print(
                    f"  FAIL: {file_name} - Frame loss rate ({loss_rate * 100:.2f}%) exceeds threshold ({max_allowed_loss_rate * 100:.1f}%).")
                all_checks_passed = False
            else:
                print(f"  PASS: {file_name} - Frame loss rate ({loss_rate * 100:.2f}%) is acceptable.")
        else:
            print(
                f"  WARN: {file_name} - Frame loss analysis could not be completed ({metrics.get('message', 'Unknown reason')}).")
            # Decide if inability to analyze is a failure. Let's allow it for now.

    print(f"--- Data Quality Precheck {'PASSED' if all_checks_passed else 'FAILED'} ---")
    return all_checks_passed


# --- Final Preprocessing Step ---
class FinalDataProcessor:
    """Merges processed CSVs, adds curvature, and handles traffic lights."""

    def __init__(self, config: Config):
        self.config = config
        self.output_dir = config.output_dir

    def _axis_to_ENU(self, speedX, speedY, speedZ, accelX, accelY, accelZ, posH, pitch, roll):
        posh_ENU = posH % 360
        posh_ENU = posh_ENU * np.pi / 180
        pitch = pitch * np.pi / 180
        roll = roll * np.pi / 180
        east_speedX, north_speedY, north_speedZ = [], [], []
        east_accelX, north_accelY, north_accelZ = [], [], []
        for i in range(len(posH)):
            sy = np.sin(posh_ENU[i])
            cy = np.cos(posh_ENU[i])
            cp = np.cos(pitch[i])
            sp = np.sin(pitch[i])
            cr = np.cos(roll[i])
            sr = np.sin(roll[i])
            trametrix = np.array([[sy * cp, sy * sp * sr - cy * cr, sy * sp * cr + cy * sr],
                                  [cy * cp, cy * sp * sr + sy * cr, cy * sp * cr - sy * sr], [-sp, cp * sr, cp * cr]])
            # trametrix = np.array([[sy, cy], [-cy, sy]])
            east_speedX_i, north_speedY_i, north_speedZ_i = np.linalg.pinv(trametrix) @ np.array(
                [speedX[i], speedY[i], speedZ[i]])
            # east_speedX_i, north_speedY_i = np.linalg.pinv(trametrix) @ np.array([speedX[i], speedY[i]])
            east_accelX_i, north_accelY_i, north_accelZ_i = np.linalg.pinv(trametrix) @ np.array(
                [accelX[i], accelY[i], accelZ[i]])
            # east_accelX_i, north_accelY_i = np.linalg.pinv(trametrix) @ np.array([accelX[i], accelY[i]])
            east_speedX.append(east_speedX_i)
            north_speedY.append(north_speedY_i)
            north_speedZ.append(north_speedZ_i)
            east_accelX.append(east_accelX_i)
            north_accelY.append(north_accelY_i)
            north_accelZ.append(north_accelZ_i)
        return east_speedX, north_speedY, speedZ, east_accelX, north_accelY, accelZ
        # return east_speedX, north_speedY, east_accelX, north_accelY

    def process(self) -> bool:
        """执行最终数据合并和处理步骤。"""
        print("--- Starting Final Data Processing ---")
        try:
            # 1. Load main object state data
            obj_state_path = self.output_dir / OUTPUT_CSV_OBJSTATE
            lane_map_path = self.output_dir / OUTPUT_CSV_LANEMAP

            if not obj_state_path.exists():
                print(f"Error: Required input file not found: {obj_state_path}")
                return False
            # 处理交通灯数据并保存

            df_traffic = self._process_trafficlight_data()
            if not df_traffic.empty:
                traffic_csv_path = self.output_dir / "Traffic.csv"
                df_traffic.to_csv(traffic_csv_path, index=False, float_format='%.6f')
                print(f"Successfully created traffic light data file: {traffic_csv_path}")
            # Load and process data
            df_object = pd.read_csv(obj_state_path, dtype={"simTime": float}, low_memory=False)
            # 坐标转换
            speedX = df_object['speedX']
            speedY = df_object['speedY']
            speedZ = df_object['speedZ']
            accelX = df_object['accelX']
            accelY = df_object['accelY']
            accelZ = df_object['accelZ']
            posH = df_object['posH']
            pitch = df_object['pitch']
            roll = df_object['roll']
            east_speedX, north_speedY, north_speedZ, east_accelX, north_accelY, north_accelZ = self._axis_to_ENU(speedX,
                                                                                                                 speedY,
                                                                                                                 speedZ,
                                                                                                                 accelX,
                                                                                                                 accelY,
                                                                                                                 accelZ,
                                                                                                                 posH,
                                                                                                                 pitch,
                                                                                                                 roll)
            # east_speedX, north_speedY, east_accelX, north_accelY = self._axis_to_ENU(speedX, speedY, speedZ, accelX, accelY, accelZ, posH, pitch, roll)
            df_object['speedX'] = east_speedX
            df_object['speedY'] = north_speedY
            df_object['speedZ'] = north_speedZ
            df_object['accelX'] = east_accelX
            df_object['accelY'] = north_accelY
            df_object['accelZ'] = north_accelZ

            df_ego = df_object[df_object["playerId"] == 1]
            points = df_ego[["posX", "posY"]].values
            window_size = 4
            fitting_instance = PolynomialCurvatureFitting(lane_map_path)
            result_list = fitting_instance.fit_and_project(points, window_size)

            curvHor_values = [result["curvHor"] for result in result_list]
            curvature_change_value = [result["curvHorDot"] for result in result_list]
            min_distance = [result["laneOffset"] for result in result_list]

            indices = df_object[df_object["playerId"] == 1].index
            if len(indices) == len(curvHor_values):
                df_object.loc[indices, "lightMask"] = 0
                df_object.loc[indices, "curvHor"] = curvHor_values
                df_object.loc[indices, "curvHorDot"] = curvature_change_value
                df_object.loc[indices, "laneOffset"] = min_distance
            else:
                print("计算值的长度与 playerId == 1 的行数不匹配！")
            # Process and merge data
            df_merged = self._merge_optional_data(df_object)
            # df_merged[['speedH', 'accelX']] = -df_merged[['speedH', 'accelX']]

            # Save final merged file directly to output directory
            merged_csv_path = self.output_dir / OUTPUT_CSV_MERGED
            print(f'merged_csv_path:{merged_csv_path}')
            df_merged.to_csv(merged_csv_path, index=False, float_format='%.6f')
            print(f"Successfully created final merged file: {merged_csv_path}")

            # Clean up intermediate files
            # if obj_state_path.exists():
            #     obj_state_path.unlink()

            print("--- Final Data Processing Finished ---")
            return True

        except Exception as e:
            print(f"An unexpected error occurred during final data processing: {e}")
            import traceback
            traceback.print_exc()
            return False

    def _merge_optional_data(self, df_object: pd.DataFrame) -> pd.DataFrame:
        """加载和合并可选数据"""
        df_merged = df_object.copy()

        # 检查并删除重复列的函数
        def clean_duplicate_columns(df):
            # 查找带有 _x 或 _y 后缀的列
            duplicate_cols = []
            base_cols = {}

            # 打印清理前的列名
            print(f"清理重复列前的列名: {df.columns.tolist()}")

            for col in df.columns:
                if col.endswith('_x') or col.endswith('_y'):
                    base_name = col[:-2]  # 去掉后缀
                    if base_name not in base_cols:
                        base_cols[base_name] = []
                    base_cols[base_name].append(col)

            # 对于每组重复列，检查数据是否相同，如果相同则只保留一个
            for base_name, cols in base_cols.items():
                if len(cols) > 1:
                    # 检查这些列的数据是否相同
                    is_identical = True
                    first_col = cols[0]
                    for col in cols[1:]:
                        if not df[first_col].equals(df[col]):
                            is_identical = False
                            break

                    if is_identical:
                        # 数据相同，保留第一列并重命名为基本名称
                        df = df.rename(columns={first_col: base_name})
                        # 删除其他重复列
                        for col in cols[1:]:
                            duplicate_cols.append(col)
                        print(f"列 {cols} 数据相同，保留为 {base_name}")
                    else:
                        print(f"列 {cols} 数据不同，保留所有列")
                        # 如果是 simTime 相关列，确保保留一个
                        if base_name == 'simTime' and 'simTime' not in df.columns:
                            df = df.rename(columns={cols[0]: 'simTime'})
                            print(f"将 {cols[0]} 重命名为 simTime")
                            # 删除其他 simTime 相关列
                            for col in cols[1:]:
                                duplicate_cols.append(col)

            # 删除重复列
            if duplicate_cols:
                # 确保不会删除 simTime 列
                if 'simTime' not in df.columns and any(col.startswith('simTime_') for col in duplicate_cols):
                    # 找到一个 simTime 相关列保留
                    for col in duplicate_cols[:]:
                        if col.startswith('simTime_'):
                            df = df.rename(columns={col: 'simTime'})
                            duplicate_cols.remove(col)
                            print(f"将 {col} 重命名为 simTime")
                            break

                df = df.drop(columns=duplicate_cols)
                print(f"删除了重复列: {duplicate_cols}")

            # 打印清理后的列名
            print(f"清理重复列后的列名: {df.columns.tolist()}")

            return df

        # --- 合并 EgoMap ---
        egomap_path = self.output_dir / OUTPUT_CSV_EGOMAP
        merged_egomap_path = self.output_dir / MERGED_CSV_EGOMAP
        if egomap_path.exists() and egomap_path.stat().st_size > 0:
            try:
                df_ego = pd.read_csv(egomap_path, dtype={"simTime": float})
                ego_column = ['posX', 'posY', 'posH']
                ego_new_column = ['posX_map', 'posY_map', 'posH_map']
                df_ego = df_ego.rename(columns=dict(zip(ego_column, ego_new_column)))
                # 删除 simFrame 列，因为使用主数据的 simFrame
                if 'simFrame' in df_ego.columns:
                    df_ego = df_ego.drop(columns=['simFrame'])

                # 打印合并前的列名
                print(f"合并 EgoMap 前 df_merged 的列: {df_merged.columns.tolist()}")
                print(f"df_ego 的列: {df_ego.columns.tolist()}")

                # 按时间和ID排序
                df_ego.sort_values(['simTime', 'playerId'], inplace=True)
                df_merged.sort_values(['simTime', 'playerId'], inplace=True)

                # 使用 merge_asof 进行就近合并，不包括 simFrame
                # df_merged = pd.merge_asof(
                #     df_merged,
                #     df_ego,
                #     on='simTime',
                #     by='playerId',
                #     direction='nearest',
                #     tolerance=0.01  # 10ms tolerance
                # )
                df_merged = pd.merge(
                    df_merged,
                    df_ego,
                    how='left',
                    on='simTime'
                )

                # 打印合并后的列名
                print(f"合并 EgoMap 后 df_merged 的列: {df_merged.columns.tolist()}")

                # 确保 simTime 列存在
                if 'simTime' not in df_merged.columns:
                    if 'simTime_x' in df_merged.columns:
                        df_merged.rename(columns={'simTime_x': 'simTime'}, inplace=True)
                        print("将 simTime_x 重命名为 simTime")
                    else:
                        print("警告: 合并 EgoMap 后找不到 simTime 列!")

                df_merged = df_merged.drop(columns=['posX_map', 'posY_map', 'posH_map', 'stateMask'])
                df_merged.to_csv(merged_egomap_path, index=False, float_format='%.6f')

                print("EgoMap data merged.")
            except Exception as e:
                print(f"Warning: Could not merge EgoMap data from {egomap_path}: {e}")
                import traceback
                traceback.print_exc()

        # 先处理可能的列名重复问题
        df_merged = clean_duplicate_columns(df_merged)

        # --- 合并hd_lane.csv，hd_road.csv ---
        current_file_path = os.path.abspath(__file__)

        # --- 合并 Traffic ---
        traffic_path = self.output_dir / "Traffic.csv"
        if traffic_path.exists() and traffic_path.stat().st_size > 0:
            try:
                df_traffic = pd.read_csv(traffic_path, dtype={"simTime": float}, low_memory=False).drop_duplicates()
                # 删除 simFrame 列
                if 'simFrame' in df_traffic.columns:
                    df_traffic = df_traffic.drop(columns=['simFrame'])

                # 根据车辆航向角确定行驶方向并筛选对应的红绿灯
                def get_direction_from_heading(heading):
                    # 将角度归一化到 -180 到 180 度范围
                    heading = heading % 360
                    if heading > 180:
                        heading -= 360

                    # 确定方向：北(N)、东(E)、南(S)、西(W)
                    if -45 <= heading <= 45:  # 北向
                        return 'N'
                    elif 45 < heading <= 135:  # 东向
                        return 'E'
                    elif -135 <= heading < -45:  # 西向
                        return 'W'
                    else:  # 南向 (135 < heading <= 180 或 -180 <= heading < -135)
                        return 'S'

                # 检查posH列是否存在，如果不存在但posH_x存在，则使用posH_x
                heading_col = 'posH'
                if heading_col not in df_merged.columns:
                    if 'posH_x' in df_merged.columns:
                        heading_col = 'posH_x'
                        print(f"使用 {heading_col} 替代 posH")
                    else:
                        print(f"警告: 找不到航向角列 posH 或 posH_x")
                        return df_merged

                # 添加方向列
                df_merged['vehicle_direction'] = df_merged[heading_col].apply(get_direction_from_heading)

                # 创建 phaseId 到方向的映射
                phase_to_direction = {
                    1: 'S',  # 南直行
                    2: 'W',  # 西直行
                    3: 'N',  # 北直行
                    4: 'E',  # 东直行
                    5: 'S',  # 南行人
                    6: 'W',  # 西行人
                    7: 'S',  # 南左转
                    8: 'W',  # 西左转
                    9: 'N',  # 北左转
                    10: 'E',  # 东左转
                    11: 'N',  # 北行人
                    12: 'E',  # 东行人
                    13: 'S',  # 南右转
                    14: 'W',  # 西右转
                    15: 'N',  # 北右转
                    16: 'E'  # 东右转
                }

                # 创建 trafficlight_id 到方向的映射
                trafficlight_to_direction = {
                    # 南向北方向的红绿灯
                    # 48100017: 'S',
                    # 48100038: 'S',
                    # 48100043: 'S',
                    # 48100030: 'S',
                    48100017: 'N',
                    48100038: 'N',
                    48100043: 'N',
                    48100030: 'N',
                    # 西向东方向的红绿灯
                    # 48100021: 'W',
                    # 48100039: 'W',
                    48100021: 'E',
                    48100039: 'E',
                    # 东向西方向的红绿灯
                    # 48100041: 'E',
                    # 48100019: 'E',
                    48100041: 'W',
                    48100019: 'W',
                    # 北向南方向的红绿灯
                    # 48100033: 'N',
                    # 48100018: 'N',
                    # 48100022: 'N'
                    48100033: 'S',
                    48100018: 'S',
                    48100022: 'S'
                }

                # 添加时间列用于合并
                df_traffic['time'] = df_traffic['simTime'].round(2).astype(float)

                # 检查 df_merged 中是否有 simTime 列
                if 'simTime' not in df_merged.columns:
                    print("警告: 合并 Traffic 前 df_merged 中找不到 simTime 列!")
                    # 尝试查找 simTime_x 或其他可能的列
                    if 'simTime_x' in df_merged.columns:
                        df_merged.rename(columns={'simTime_x': 'simTime'}, inplace=True)
                        print("将 simTime_x 重命名为 simTime")
                    else:
                        print("严重错误: 无法找到任何 simTime 相关列，无法继续合并!")
                        return df_merged

                df_merged['time'] = df_merged['simTime'].round(2).astype(float)
                tree = cKDTree(df_traffic[['simTime']])

                # 查询df1中每个时间戳的最近邻
                distances, indices = tree.query(df_merged[['simTime']], k=1)
                # df_merged['time1'] = df_merged['simTime'].round(0).astype(float)
                # df_traffic1 = df_traffic.rename(columns={'simTime' 'simTime1'})
                # df_traffic['time1'] = df_traffic['time'].round(0).astype(float)
                # 合并 Traffic 数据
                df_merged['matched_time'] = df_traffic.iloc[indices.flatten()]['simTime'].values

                # 合并DataFrame
                df_merged = pd.merge(df_merged, df_traffic, left_on='matched_time', right_on='simTime', how='left')
                # df_merged = pd.merge(df_merged, df_traffic, on=["time1"], how="left")
                # df_merged = df_merged.drop(columns = ['time1'])
                # 再次处理可能的列名重复问题
                df_merged = clean_duplicate_columns(df_merged)
                df_merged = df_merged.drop(columns=['time_x', 'time_y', 'matched_time'])

                # 检查trafficlight_id列是否存在
                trafficlight_col = 'trafficlight_id'
                if trafficlight_col not in df_merged.columns:
                    if 'trafficlight_id_x' in df_merged.columns:
                        trafficlight_col = 'trafficlight_id_x'
                        print(f"使用 {trafficlight_col} 替代 trafficlight_id")
                    else:
                        print(f"警告: 找不到红绿灯ID列 trafficlight_id 或 trafficlight_id_x")

                # 筛选与车辆行驶方向相关的红绿灯
                def filter_relevant_traffic_light(row):
                    if 'phaseId' not in row or pd.isna(row['phaseId']):
                        return np.nan

                    # 获取 phaseId 对应的方向
                    phase_id = int(row['phaseId']) if not pd.isna(row['phaseId']) else None
                    if phase_id is None:
                        return np.nan

                    phase_direction = phase_to_direction.get(phase_id, None)

                    # 如果 phaseId 方向与车辆方向匹配
                    if phase_direction == row['vehicle_direction']:
                        # 查找该方向的所有红绿灯 ID
                        relevant_ids = [tid for tid, direction in trafficlight_to_direction.items()
                                        if direction == phase_direction]

                        # 如果 trafficlight_id 在 EgoMap 中且方向匹配
                        # if trafficlight_col in row and not pd.isna(row[trafficlight_col]) and row[trafficlight_col] in relevant_ids:
                        if trafficlight_col in row:
                            if not pd.isna(row[trafficlight_col]):
                                if row[trafficlight_col] in relevant_ids:
                                    return row[trafficlight_col]

                    return np.nan

                # 应用筛选函数
                df_merged['filtered_trafficlight_id'] = df_merged.apply(filter_relevant_traffic_light, axis=1)

                # 清理临时列
                # print(f"删除 time 列前 df_merged 的列: {df_merged.columns.tolist()}")
                # df_merged.drop(columns=['time'], inplace=True)
                # print(f"删除 time 列后 df_merged 的列: {df_merged.columns.tolist()}")

                # 确保 simTime 列存在
                if 'simTime' not in df_merged.columns:
                    if 'simTime_x' in df_merged.columns:
                        df_merged.rename(columns={'simTime_x': 'simTime'}, inplace=True)
                        print("将 simTime_x 重命名为 simTime")
                    else:
                        print("警告: 处理 Traffic 数据后找不到 simTime 列!")

                print("Traffic light data merged and filtered.")
            except Exception as e:
                print(f"Warning: Could not merge Traffic data from {traffic_path}: {e}")
                import traceback
                traceback.print_exc()
        else:
            print("Traffic data not found or empty, skipping merge.")

        # --- Merge Function ---
        function_path = self.output_dir / OUTPUT_CSV_FUNCTION
        if function_path.exists() and function_path.stat().st_size > 0:
            try:
                # 添加调试信息
                print(f"正在读取 Function 数据: {function_path}")
                df_function = pd.read_csv(function_path, low_memory=False).drop_duplicates()
                print(f"Function 数据列名: {df_function.columns.tolist()}")

                # 删除 simFrame 列
                if 'simFrame' in df_function.columns:
                    df_function = df_function.drop(columns=['simFrame'])

                # 确保 simTime 列存在并且是浮点型
                if 'simTime' in df_function.columns:
                    # 安全地将 simTime 转换为浮点型
                    try:
                        df_function['simTime'] = pd.to_numeric(df_function['simTime'], errors='coerce')
                        df_function = df_function.dropna(subset=['simTime'])  # 删除无法转换的行
                        df_function['time'] = df_function['simTime'].round(2)

                        # 安全地处理 df_merged 的 simTime 列
                        if 'simTime' in df_merged.columns:
                            print(f"df_merged['simTime'] 的类型: {df_merged['simTime'].dtype}")
                            print(f"df_merged['simTime'] 的前5个值: {df_merged['simTime'].head().tolist()}")

                            df_merged['time'] = pd.to_numeric(df_merged['simTime'], errors='coerce').round(2)
                            # 删除 time 列中的 NaN 值
                            nan_count = df_merged['time'].isna().sum()
                            if nan_count > 0:
                                print(f"警告: 转换后有 {nan_count} 个 NaN 值，将删除这些行")
                                df_merged = df_merged.dropna(subset=['time'])

                            # 确保两个 DataFrame 的 time 列类型一致
                            df_function['time'] = df_function['time'].astype(float)
                            df_merged['time'] = df_merged['time'].astype(float)

                            common_cols = list(set(df_merged.columns) & set(df_function.columns) - {'time'})
                            df_function.drop(columns=common_cols, inplace=True, errors='ignore')

                            # 合并数据
                            df_merged = pd.merge(df_merged, df_function, on=["time"], how="left")
                            df_merged.drop(columns=['time'], inplace=True)
                            print("Function 数据合并成功。")
                        else:
                            print("警告: df_merged 中找不到 'simTime' 列，无法合并 Function 数据。")
                            # 打印所有列名以便调试
                            print(f"df_merged 的所有列: {df_merged.columns.tolist()}")
                    except Exception as e:
                        print(f"警告: 处理 Function.csv 中的 simTime 列时出错: {e}")
                        import traceback
                        traceback.print_exc()
                else:
                    print(f"警告: Function.csv 中找不到 'simTime' 列。可用的列: {df_function.columns.tolist()}")
            except Exception as e:
                print(f"警告: 无法合并 Function 数据: {e}")
                import traceback
                traceback.print_exc()
        else:
            print(f"Function 数据文件不存在或为空: {function_path}")

        # --- Merge OBU ---
        obu_path = self.output_dir / OUTPUT_CSV_OBU
        if obu_path.exists() and obu_path.stat().st_size > 0:
            try:
                # 添加调试信息
                print(f"正在读取 OBU 数据: {obu_path}")
                df_obu = pd.read_csv(obu_path, low_memory=False).drop_duplicates()
                print(f"OBU 数据列名: {df_obu.columns.tolist()}")

                # 删除 simFrame 列
                if 'simFrame' in df_obu.columns:
                    df_obu = df_obu.drop(columns=['simFrame'])

                # 确保 simTime 列存在并且是浮点型
                if 'simTime' in df_obu.columns:
                    # 安全地将 simTime 转换为浮点型
                    try:
                        df_obu['simTime'] = pd.to_numeric(df_obu['simTime'], errors='coerce')
                        df_obu = df_obu.dropna(subset=['simTime'])  # 删除无法转换的行
                        df_obu['time'] = df_obu['simTime'].round(2)

                        # 安全地处理 df_merged 的 simTime 列
                        if 'simTime' in df_merged.columns:
                            print(f"合并 OBU 前 df_merged['simTime'] 的类型: {df_merged['simTime'].dtype}")
                            print(f"合并 OBU 前 df_merged['simTime'] 的前5个值: {df_merged['simTime'].head().tolist()}")

                            df_merged['time'] = pd.to_numeric(df_merged['simTime'], errors='coerce').round(2)
                            # 删除 time 列中的 NaN 值
                            nan_count = df_merged['time'].isna().sum()
                            if nan_count > 0:
                                print(f"警告: 转换后有 {nan_count} 个 NaN 值，将删除这些行")
                                df_merged = df_merged.dropna(subset=['time'])

                            # 确保两个 DataFrame 的 time 列类型一致
                            df_obu['time'] = df_obu['time'].astype(float)
                            df_merged['time'] = df_merged['time'].astype(float)

                            common_cols = list(set(df_merged.columns) & set(df_obu.columns) - {'time'})
                            df_obu.drop(columns=common_cols, inplace=True, errors='ignore')

                            # 合并数据
                            df_merged = pd.merge(df_merged, df_obu, on=["time"], how="left")
                            df_merged.drop(columns=['time'], inplace=True)
                            print("OBU 数据合并成功。")
                        else:
                            print("警告: df_merged 中找不到 'simTime' 列，无法合并 OBU 数据。")
                            # 打印所有列名以便调试
                            print(f"df_merged 的所有列: {df_merged.columns.tolist()}")
                    except Exception as e:
                        print(f"警告: 处理 OBUdata.csv 中的 simTime 列时出错: {e}")
                        import traceback
                        traceback.print_exc()
                else:
                    print(f"警告: OBUdata.csv 中找不到 'simTime' 列。可用的列: {df_obu.columns.tolist()}")
            except Exception as e:
                print(f"警告: 无法合并 OBU 数据: {e}")
                import traceback
                traceback.print_exc()
        else:
            print(f"OBU 数据文件不存在或为空: {obu_path}")

        # 在所有合并完成后，再次清理重复列
        df_merged = clean_duplicate_columns(df_merged)

        return df_merged

    def _process_trafficlight_data(self) -> pd.DataFrame:
        """Processes traffic light JSON data if available."""
        # Check if json_path is provided and exists
        if not self.config.json_path:
            print("No traffic light JSON file provided. Skipping traffic light processing.")
            return pd.DataFrame()

        if not self.config.json_path.exists():
            print("Traffic light JSON file not found. Skipping traffic light processing.")
            return pd.DataFrame()

        print(f"Processing traffic light data from: {self.config.json_path}")
        valid_trafficlights = []
        try:
            with open(self.config.json_path, 'r', encoding='utf-8') as f:
                # Read the whole file, assuming it's a JSON array or JSON objects per line
                try:
                    # Attempt to read as a single JSON array
                    raw_data = json.load(f)
                    if not isinstance(raw_data, list):
                        raw_data = [raw_data]  # Handle case of single JSON object
                except json.JSONDecodeError:
                    # If fails, assume JSON objects per line
                    f.seek(0)  # Reset file pointer
                    raw_data = [json.loads(line) for line in f if line.strip()]

            for entry in raw_data:
                # Normalize entry if it's a string containing JSON
                if isinstance(entry, str):
                    try:
                        entry = json.loads(entry)
                    except json.JSONDecodeError:
                        print(f"Warning: Skipping invalid JSON string in traffic light data: {entry[:100]}...")
                        continue

                # Safely extract data using .get()
                intersections = entry.get('intersections', [])
                if not isinstance(intersections, list): continue  # Skip if not a list

                for intersection in intersections:
                    if not isinstance(intersection, dict): continue
                    timestamp_ms = intersection.get('intersectionTimestamp', 0)
                    sim_time = round(int(timestamp_ms) / 1000, 2)  # Convert ms to s and round

                    phases = intersection.get('phases', [])
                    if not isinstance(phases, list): continue

                    for phase in phases:
                        if not isinstance(phase, dict): continue
                        phase_id = phase.get('phaseId', 0)

                        phase_states = phase.get('phaseStates', [])
                        if not isinstance(phase_states, list): continue

                        for phase_state in phase_states:
                            if not isinstance(phase_state, dict): continue
                            # Check for startTime == 0 as per original logic
                            if phase_state.get('startTime') == 0:
                                light_state = phase_state.get('light', 0)  # Extract light state
                                data = {
                                    'simTime': sim_time,
                                    'phaseId': phase_id,
                                    'stateMask': light_state,
                                    # Add playerId for merging - assume applies to ego
                                    'playerId': PLAYER_ID_EGO
                                }
                                valid_trafficlights.append(data)

            if not valid_trafficlights:
                print("No valid traffic light states (with startTime=0) found in JSON.")
                return pd.DataFrame()

            df_trafficlights = pd.DataFrame(valid_trafficlights)
            # Drop duplicates based on relevant fields
            df_trafficlights.drop_duplicates(subset=['simTime', 'playerId', 'phaseId', 'stateMask'], keep='first',
                                             inplace=True)
            print(f"Processed {len(df_trafficlights)} unique traffic light state entries.")
            # 按时间升序排序 - 修复倒序问题
            df_trafficlights = df_trafficlights.sort_values('simTime', ascending=True)

            # 添加调试信息
            print(f"交通灯数据时间范围: {df_trafficlights['simTime'].min()} 到 {df_trafficlights['simTime'].max()}")
            print(f"交通灯数据前5行时间: {df_trafficlights['simTime'].head().tolist()}")
            print(f"交通灯数据后5行时间: {df_trafficlights['simTime'].tail().tolist()}")

            return df_trafficlights

        except json.JSONDecodeError as e:
            print(f"Error decoding traffic light JSON file {self.config.json_path}: {e}")
            return pd.DataFrame()
        except Exception as e:
            print(f"Unexpected error processing traffic light data: {e}")
            return pd.DataFrame()


# --- Rosbag Processing ---
class RosbagProcessor:
    """Extracts data from HMIdata files within a ZIP archive."""

    def __init__(self, config: Config):
        self.config = config
        self.output_dir = config.output_dir

    def process_zip_for_rosbags(self) -> None:
        """Finds, extracts, and processes rosbags from the ZIP file."""
        print(f"--- Processing HMIdata in {self.config.zip_path} ---")

        with tempfile.TemporaryDirectory() as tmp_dir_str:
            try:
                with zipfile.ZipFile(self.config.zip_path, 'r') as zip_ref:
                    for member in zip_ref.infolist():

                        # Extract HMIdata CSV files directly to output
                        if 'HMIdata/' in member.filename and member.filename.endswith('.csv'):
                            try:
                                target_path = self.output_dir / Path(member.filename).name
                                with zip_ref.open(member) as source, open(target_path, "wb") as target:
                                    shutil.copyfileobj(source, target)
                                print(f"Extracted HMI data: {target_path.name}")
                            except Exception as e:
                                print(f"Error extracting HMI data {member.filename}: {e}")

            except zipfile.BadZipFile:
                print(f"Error: Bad ZIP file provided: {self.config.zip_path}")
                return
            except FileNotFoundError:
                print(f"Error: ZIP file not found: {self.config.zip_path}")
                return

        print("--- HMIdata Processing Finished ---")


# --- Utility Functions ---
def get_base_path() -> Path:
    """Gets the base path of the script or executable."""
    if getattr(sys, 'frozen', False) and hasattr(sys, '_MEIPASS'):
        # Running in a PyInstaller bundle
        return Path(sys._MEIPASS)
    else:
        # Running as a normal script
        return Path(__file__).parent.resolve()


def run_cpp_engine(config: Config):
    """Runs the external C++ preprocessing engine."""
    if not config.engine_path or not config.map_path:
        print("C++ engine path or map path not configured. Skipping C++ engine execution.")
        return True  # Return True assuming it's optional or handled elsewhere

    engine_cmd = [
        str(config.engine_path),
        str(config.map_path),
        str(config.output_dir),
        str(config.x_offset),
        str(config.y_offset)
    ]

    print(f"--- Running C++ Preprocessing Engine ---")
    print(f"Command: {' '.join(engine_cmd)}")

    try:
        result = subprocess.run(
            engine_cmd,
            check=True,  # Raise exception on non-zero exit code
            capture_output=True,  # Capture stdout/stderr
            text=True,  # Decode output as text
            # cwd=config.engine_path.parent  # Run from the engine's directory? Or script's? Adjust if needed.
        )
        print("C++ Engine Output:")
        print(result.stdout)
        if result.stderr:
            print("C++ Engine Error Output:")
            print(result.stderr)
        print("--- C++ Engine Finished Successfully ---")
        return True
    except FileNotFoundError:
        print(f"Error: C++ engine executable not found at {config.engine_path}.")
        return False
    except subprocess.CalledProcessError as e:
        print(f"Error: C++ engine failed with exit code {e.returncode}.")
        print("C++ Engine Output (stdout):")
        print(e.stdout)
        print("C++ Engine Output (stderr):")
        print(e.stderr)
        return False
    except Exception as e:
        print(f"An unexpected error occurred while running the C++ engine: {e}")
        return False


if __name__ == "__main__":
    pass