// Minimal ZED SDK-only reproduction of the area-map export abort.
// usage: sdk_area_repro <in.svo2> <out.area> [max_frames] [mode=GEN_3|GEN_1] [save_every_n=0]
#include <sl/Camera.hpp>
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <string>
#include <thread>

static double now_s() {
    return std::chrono::duration<double>(std::chrono::steady_clock::now().time_since_epoch()).count();
}

int main(int argc, char** argv) {
    if (argc < 3) { std::fprintf(stderr, "usage: %s in.svo2 out.area [max_frames] [GEN_3|GEN_1]\n", argv[0]); return 2; }
    const long max_frames = argc > 3 ? std::atol(argv[3]) : 0;
    const std::string mode = argc > 4 ? argv[4] : "GEN_3";

    sl::Camera zed;
    sl::InitParameters init;
    init.input.setFromSVOFile(argv[1]);
    init.depth_mode = sl::DEPTH_MODE::NEURAL_LIGHT;       // zed_init_parameters.json
    init.coordinate_units = sl::UNIT::METER;
    init.camera_resolution = sl::RESOLUTION::HD1200;
    init.camera_fps = 30;
    init.depth_stabilization = 30;
    init.depth_maximum_distance = 8.0f;                   // config.json max_dist
    init.svo_real_time_mode = false;
    sl::ERROR_CODE err = zed.open(init);
    if (err > sl::ERROR_CODE::SUCCESS) { std::printf("open failed: %s\n", sl::toString(err).c_str()); return 3; }

    sl::PositionalTrackingParameters pt;                  // zed_positional_tracking_parameters.json + config
    pt.set_as_static = false;
    pt.enable_imu_fusion = true;
    pt.set_gravity_as_origin = true;
    sl::Transform world = sl::Transform::identity();
    world.setTranslation(sl::Translation(-0.025f, 0.03275f, 0.0525f));
    world.setRotationVector(sl::float3(-1.57079632679f, 0.f, 0.f));
    pt.initial_world_transform = world;
    pt.mode = mode == "GEN_1" ? sl::POSITIONAL_TRACKING_MODE::GEN_1 : sl::POSITIONAL_TRACKING_MODE::GEN_3;
    pt.enable_area_memory = true;
    err = zed.enablePositionalTracking(pt);
    if (err > sl::ERROR_CODE::SUCCESS) { std::printf("tracking failed: %s\n", sl::toString(err).c_str()); return 4; }

    sl::RuntimeParameters rt;                             // zed_runtime_parameters.json
    rt.confidence_threshold = 50;
    rt.remove_saturated_areas = true;
    sl::Pose pose;
    long frames = 0;
    const double t0 = now_s();
    while (max_frames == 0 || frames < max_frames) {
        err = zed.grab(rt);
        if (err == sl::ERROR_CODE::END_OF_SVOFILE_REACHED) break;
        if (err > sl::ERROR_CODE::SUCCESS) continue;
        zed.getPosition(pose, sl::REFERENCE_FRAME::WORLD);
        (void)zed.getPositionalTrackingStatus();
        if (++frames % 1000 == 0) {
            const auto st = zed.getPositionalTrackingStatus();
            std::printf("frames=%ld t=%.1f spatial=%s\n", frames, now_s() - t0,
                        sl::toString(st.spatial_memory_status).c_str());
            std::fflush(stdout);
        }
    }
    std::printf("GRAB_DONE frames=%ld t=%.1f\n", frames, now_s() - t0);
    std::fflush(stdout);

    const double ts = now_s();
    std::printf("SAVE_START\n"); std::fflush(stdout);
    err = zed.saveAreaMap(argv[2]);
    std::printf("SAVE_RETURNED err=%s dt=%.3f\n", sl::toString(err).c_str(), now_s() - ts); std::fflush(stdout);
    auto state = sl::AREA_EXPORTING_STATE::RUNNING;
    while (state == sl::AREA_EXPORTING_STATE::RUNNING && now_s() - ts < 120) {
        state = zed.getAreaExportState();
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    std::printf("EXPORT_STATE %s dt=%.3f\n", sl::toString(state).c_str(), now_s() - ts); std::fflush(stdout);
    zed.close();
    std::printf("CLOSED\n");
    return state == sl::AREA_EXPORTING_STATE::SUCCESS ? 0 : 1;
}