@echo off
setlocal EnableDelayedExpansion
REM run_pipeline_v8.bat
REM v8 pipeline: adds elbow angle estimation + camera z refinement
REM
REM Steps:
REM   1  ArUco detection (same as v4)
REM   2  Camera pose from board markers (same as v4)
REM   3a First bundle adjustment with board-calibrated cameras
REM   3b Elbow angle estimation (robust to z-error, uses relative marker vectors)
REM   4  Camera z refinement using FK-predicted Ellbow z
REM   5  Second bundle adjustment with z-corrected cameras -> v8/ subfolder
REM   6  Robot state estimation from v8 positions
REM
REM Usage:  .\run_pipeline_v8.bat ..\data\simulation\Scene9

if "%1"=="" (
    echo.
    echo [INFO] Aufruf fehlt!
    echo Beispiel: .\run_pipeline_v8.bat ..\data\simulation\Scene9
    echo.
    exit /b
)

set "IMAGES=%~1"
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

for %%I in ("%IMAGES%") do set "SCENE_NAME=%%~nxI"

set OUT_DIR=%BASE_OUT_DIR%\%SCENE_NAME%
set OUT_DIR_V8=%OUT_DIR%\v8

REM ── STEP 1: ArUco detection ────────────────────────────────────
echo.
echo [STEP 1] Detect ArUco markers from %IMAGES%

for %%F in ("%IMAGES%\*.png" "%IMAGES%\*.PNG" "%IMAGES%\*.jpg" "%IMAGES%\*.jpeg" "%IMAGES%\*.JPG" "%IMAGES%\*.JPEG") do (
    set "NAME=%%~nF"
    echo Bearbeite: !NAME!
    for /f "tokens=2 delims=_" %%A in ("%%~nF") do (
        set "CAMID=%%A"
        python3 ../pipeline/1_detect_aruco_observations.py ^
            -i "%%F" ^
            -npz "%IMAGES%\render_a.npz" ^
            -outDir %OUT_DIR% ^
            -robot %ROBOT_JSON% ^
            -cameraId !CAMID!
    )
)

REM ── STEP 2: Camera pose from board markers ─────────────────────
echo.
echo [STEP 2] Estimate camera poses from board markers

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%
)

REM ── Collect file lists ─────────────────────────────────────────
set DET_ARGS=
set POSE_ARGS=
set POSE_ARGS_V8=

for %%F in ("%OUT_DIR%\*_aruco_detection.json") do (
    set DET_ARGS=!DET_ARGS! -det "%%F"
)
for %%F in ("%OUT_DIR%\*_camera_pose.json") do (
    set POSE_ARGS=!POSE_ARGS! -pose "%%F"
)

REM ── STEP 3a: First bundle adjustment (board cameras) ──────────
echo.
echo [STEP 3a] Initial bundle adjustment with board-calibrated cameras

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

REM ── STEP 3b: Elbow angle (robust to z-error) ──────────────────
echo.
echo [STEP 3b-v8] Estimate elbow angle

python3 "..\pipeline\4b_revolute_angle.py" ^
    --robot "%ROBOT_JSON%" ^
    --aruco "%OUT_DIR%\aruco_positions_optimized.json" ^
    --link  Ellbow ^
    --output "%OUT_DIR%\v8_ellbow_angle.json"

if errorlevel 1 (
    echo [ERROR] Elbow angle estimation failed. Aborting v8 pipeline.
    exit /b 1
)

REM ── STEP 4: Camera z refinement ───────────────────────────────
echo.
echo [STEP 4-v8] Refine camera z using FK prediction

for %%F in ("%OUT_DIR%\*_camera_pose.json") do (
    set POSE_ARGS_V8=!POSE_ARGS_V8! -pose "%%F"
)

python3 "..\pipeline\5_camera_z_refine.py" ^
    --angle  "%OUT_DIR%\v8_ellbow_angle.json" ^
    --robot  "%ROBOT_JSON%" ^
    --aruco  "%OUT_DIR%\aruco_positions_optimized.json" ^
    !POSE_ARGS_V8! ^
    --outDir "%OUT_DIR%"

if errorlevel 1 (
    echo [ERROR] Camera z refinement failed. Aborting v8 pipeline.
    exit /b 1
)

REM ── STEP 5: Second bundle adjustment (z-corrected cameras) ────
echo.
echo [STEP 5-v8] Second bundle adjustment with z-corrected cameras

set POSE_ARGS_REFINED=
for %%F in ("%OUT_DIR%\*_camera_pose_v8.json") do (
    set POSE_ARGS_REFINED=!POSE_ARGS_REFINED! -pose "%%F"
)

python3 "..\pipeline\3_multiview_bundle_adjustment_v5_board_anchor_patch.py" ^
    -robot "%ROBOT_JSON%" ^
    -lambdaWeight 100.0 ^
    --outDir "%OUT_DIR_V8%" ^
    !DET_ARGS! ^
    !POSE_ARGS_REFINED!

REM ── STEP 6: Robot state estimation from v8 positions ──────────
echo.
echo [STEP 6-v8] Robot state estimation

python3 "..\pipeline\4_robotState_estimation_v6.py" ^
    "%OUT_DIR_V8%\aruco_positions_optimized.json" ^
    -robot "%ROBOT_JSON%" ^
    -out   "%OUT_DIR_V8%\robot_state_v8.json"

echo.
echo [DONE] v8 results in: %OUT_DIR_V8%
echo        z-correction summary: %OUT_DIR%\v8_z_correction.json