@echo off
setlocal EnableDelayedExpansion
REM 3_pipeline_multiview.bat
REM Multi-camera ArUco detection and pose estimation pipeline
REM Parametr e.g.  
REM                  run_pipeline.bat ../data/simulation/Scene4




REM Wenn kein Argument übergeben wurde
if "%1"=="" (
    echo.
    echo [INFO] Aufruf fehlt!
    echo Beispiel: .\run_pipeline.bat ../data/simulation/Scene4
    echo.
    exit /b
)


set "IMAGES=%~1"

REM trailing slash entfernen
if "%IMAGES:~-1%"=="\" set "IMAGES=%IMAGES:~0,-1%"
if "%IMAGES:~-1%"=="/" set "IMAGES=%IMAGES:~0,-1%"

set ROBOT_JSON=..\data\robot\robot.json
set BASE_OUT_DIR=C:\Users\kech\SynologyDrive\2026-AppServer-AppRobot\appRobotRendering\data\evaluations

REM ✅ richtigen Namen extrahieren
for %%I in ("%IMAGES%") do set "SCENE_NAME=%%~nxI"

set OUT_DIR=%BASE_OUT_DIR%\%SCENE_NAME%



echo.
echo [STEP 1] Detect ArUco markers from all cameras in the folder %IMAGES%

for %%F in ("%IMAGES%\*.png" "%IMAGES%\*.PNG" "%IMAGES%\*.jpg" "%IMAGES%\*.jpeg" "%IMAGES%\*.JPG" "%IMAGES%\*.JPEG") do (   
    
    REM Dateiname ohne Pfad und ohne .png
    set "NAME=%%~nF"
    echo Bearbeite: !NAME!

    REM Split bei "_" → nehme 2. Teil (die ID)
    for /f "tokens=2 delims=_" %%A in ("%%~nF") do (
        set "CAMID=%%A"

        REM Takes files and detected arucos output to   render_c_aruco_detection.json
        python3 ../pipeline/1_detect_aruco_observations.py ^
            -i "%%F" ^
            -npz "%IMAGES%\render_a.npz" ^
            -outDir %OUT_DIR% ^
            -robot %ROBOT_JSON% ^
            -cameraId !CAMID!
    )
)

echo.
echo [STEP 2] Estimate camera poses from detections

for %%F in ("%OUT_DIR%\*_aruco_detection.json") do (   
    echo Bearbeite: %%F

    python3 ../pipeline/2_estimate_camera_from_observations.py ^
        -i  "%%F" ^
        -robot %ROBOT_JSON% ^
        -outDir %OUT_DIR%
)

echo.
echo [STEP 3] Triangulate marker positions from multi-view observations


REM Alle detection files sammeln
for %%F in ("%OUT_DIR%\*_aruco_detection.json") do (
    set DET_ARGS=!DET_ARGS! -det "%%F"
)

REM Alle pose files sammeln
for %%F in ("%OUT_DIR%\*_camera_pose.json") do (
    set POSE_ARGS=!POSE_ARGS! -pose "%%F"
)

python3 "..\pipeline\3_multiview_bundle_adjustment_v4.py" ^
    -robot "%ROBOT_JSON%" ^
    -lambdaWeight 100.0 ^
    !DET_ARGS! ^
    !POSE_ARGS!


echo.
echo [STEP 4] Robotics Pose calculation of angles and joint positions


python3 "..\pipeline\4_robotState_estimation_v6.py" ^
    "%OUT_DIR%\aruco_positions_initial.json" ^
    -robot "%ROBOT_JSON%" ^
    -out "%OUT_DIR%\robot_state.json"