// Copyright (c) 2024 D-Robotics.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <argp.h>
#include <errno.h>
#include <inttypes.h>
#include <pthread.h>
#include <signal.h>
#include <stdarg.h>
#include <stdatomic.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#include "sp_codec.h"
#include "sp_display.h"
#include "sp_sys.h"
#include "sp_vio.h"

#define CAMERA_COUNT 2
#define STREAM_FRAME_SIZE 2097152
#define LOG_TS_BUF_SIZE 64

typedef enum
{
    STOP_REASON_NONE = 0,
    STOP_REASON_TIMEOUT,
    STOP_REASON_SIGNAL,
    STOP_REASON_CAM0_ERROR,
    STOP_REASON_CAM1_ERROR,
    STOP_REASON_MAIN_ERROR,
} stop_reason_t;

typedef enum
{
    EXIT_REASON_NONE = 0,
    EXIT_REASON_TIMEOUT,
    EXIT_REASON_SIGNAL,
    EXIT_REASON_PEER_ERROR,
    EXIT_REASON_START_FAILURE,
    EXIT_REASON_STREAM_FAILURE,
    EXIT_REASON_WRITE_FAILURE,
    EXIT_REASON_STALL,
    EXIT_REASON_MAIN_ERROR,
} exit_reason_t;

enum
{
    KEY_DURATION_SEC = 0x100,
    KEY_HEARTBEAT_SEC,
    KEY_STALL_TIMEOUT_SEC,
    KEY_CAM0_VIDEO_INDEX,
    KEY_CAM0_OUTPUT,
    KEY_CAM0_IWIDTH,
    KEY_CAM0_IHEIGHT,
    KEY_CAM0_OWIDTH,
    KEY_CAM0_OHEIGHT,
    KEY_CAM0_BITRATE,
    KEY_CAM0_FPS,
    KEY_CAM1_VIDEO_INDEX,
    KEY_CAM1_OUTPUT,
    KEY_CAM1_IWIDTH,
    KEY_CAM1_IHEIGHT,
    KEY_CAM1_OWIDTH,
    KEY_CAM1_OHEIGHT,
    KEY_CAM1_BITRATE,
    KEY_CAM1_FPS,
};

static char doc[] = "vio2encode step5 sample -- stability-oriented dual camera worker model";
static atomic_bool is_stop = 0;
static atomic_int stop_reason = STOP_REASON_NONE;

typedef struct
{
    const char *output_path;
    int output_height;
    int output_width;
    int input_height;
    int input_width;
    int pipe_id;
    int video_index;
    int chn_num;
    int encode_chn;
    int bitrate;
    int fps;
} camera_config_t;

typedef struct
{
    camera_config_t cameras[CAMERA_COUNT];
    int duration_sec;
    int heartbeat_interval_sec;
    int stall_timeout_sec;
} program_config_t;

typedef struct
{
    camera_config_t cfg;
    const program_config_t *program_cfg;
    void *vio_object;
    void *encoder;
    FILE *stream;
    char *stream_buffer;
    int actual_width;
    int actual_height;
    int camera_opened;
    int encode_started;
    int bind_done;
    int result;
    int thread_index;
    atomic_bool *stop_flag;
    uint64_t start_ms;
    uint64_t first_frame_ms;
    uint64_t last_frame_ms;
    uint64_t end_ms;
    uint64_t frame_count;
    uint64_t bytes_written;
    uint64_t stream_fail_count;
    uint64_t write_fail_count;
    uint64_t heartbeat_count;
    uint64_t max_frame_gap_ms;
    exit_reason_t exit_reason;
} camera_context_t;

typedef struct
{
    int duration_sec;
} timer_context_t;

static struct argp_option options[] = {
    {"duration-sec", KEY_DURATION_SEC, "seconds", 0, "auto stop after the given number of seconds"},
    {"heartbeat-sec", KEY_HEARTBEAT_SEC, "seconds", 0, "heartbeat log interval, 0 to disable"},
    {"stall-timeout-sec", KEY_STALL_TIMEOUT_SEC, "seconds", 0, "max allowed frame gap before marking stall, 0 to disable"},
    {"cam0-video-index", KEY_CAM0_VIDEO_INDEX, "index", 0, "camera0 host index"},
    {"cam0-output", KEY_CAM0_OUTPUT, "path", 0, "camera0 output file"},
    {"cam0-iwidth", KEY_CAM0_IWIDTH, "width", 0, "camera0 sensor width"},
    {"cam0-iheight", KEY_CAM0_IHEIGHT, "height", 0, "camera0 sensor height"},
    {"cam0-owidth", KEY_CAM0_OWIDTH, "width", 0, "camera0 output width"},
    {"cam0-oheight", KEY_CAM0_OHEIGHT, "height", 0, "camera0 output height"},
    {"cam0-bitrate", KEY_CAM0_BITRATE, "bits", 0, "camera0 encode bitrate"},
    {"cam0-fps", KEY_CAM0_FPS, "fps", 0, "camera0 sensor fps"},
    {"cam1-video-index", KEY_CAM1_VIDEO_INDEX, "index", 0, "camera1 host index"},
    {"cam1-output", KEY_CAM1_OUTPUT, "path", 0, "camera1 output file"},
    {"cam1-iwidth", KEY_CAM1_IWIDTH, "width", 0, "camera1 sensor width"},
    {"cam1-iheight", KEY_CAM1_IHEIGHT, "height", 0, "camera1 sensor height"},
    {"cam1-owidth", KEY_CAM1_OWIDTH, "width", 0, "camera1 output width"},
    {"cam1-oheight", KEY_CAM1_OHEIGHT, "height", 0, "camera1 output height"},
    {"cam1-bitrate", KEY_CAM1_BITRATE, "bits", 0, "camera1 encode bitrate"},
    {"cam1-fps", KEY_CAM1_FPS, "fps", 0, "camera1 sensor fps"},
    {0}};

static uint64_t get_monotonic_ms(void)
{
    struct timespec ts;

    clock_gettime(CLOCK_MONOTONIC, &ts);
    return (uint64_t)ts.tv_sec * 1000ULL + (uint64_t)ts.tv_nsec / 1000000ULL;
}

static void format_wallclock_timestamp(char *buffer, size_t buffer_size)
{
    struct timespec ts;
    struct tm local_tm;
    time_t seconds;
    size_t used = 0;

    clock_gettime(CLOCK_REALTIME, &ts);
    seconds = ts.tv_sec;
    localtime_r(&seconds, &local_tm);
    used = strftime(buffer, buffer_size, "%Y-%m-%d %H:%M:%S", &local_tm);
    if (used == 0)
    {
        if (buffer_size > 0)
        {
            buffer[0] = '\0';
        }
        return;
    }

    snprintf(buffer + used, buffer_size - used, ".%03ld", ts.tv_nsec / 1000000L);
}

static void log_message(int cam_index, const char *level, const char *fmt, ...)
{
    char timestamp[LOG_TS_BUF_SIZE];
    va_list args;

    format_wallclock_timestamp(timestamp, sizeof(timestamp));
    if (cam_index >= 0)
    {
        printf("[%s] [cam%d] [%s] ", timestamp, cam_index, level);
    }
    else
    {
        printf("[%s] [main] [%s] ", timestamp, level);
    }

    va_start(args, fmt);
    vprintf(fmt, args);
    va_end(args);
    printf("\n");
    fflush(stdout);
}

static const char *stop_reason_to_string(stop_reason_t reason)
{
    switch (reason)
    {
    case STOP_REASON_TIMEOUT:
        return "timeout";
    case STOP_REASON_SIGNAL:
        return "signal";
    case STOP_REASON_CAM0_ERROR:
        return "cam0_error";
    case STOP_REASON_CAM1_ERROR:
        return "cam1_error";
    case STOP_REASON_MAIN_ERROR:
        return "main_error";
    case STOP_REASON_NONE:
    default:
        return "none";
    }
}

static const char *exit_reason_to_string(exit_reason_t reason)
{
    switch (reason)
    {
    case EXIT_REASON_TIMEOUT:
        return "timeout";
    case EXIT_REASON_SIGNAL:
        return "signal";
    case EXIT_REASON_PEER_ERROR:
        return "peer_error";
    case EXIT_REASON_START_FAILURE:
        return "start_failure";
    case EXIT_REASON_STREAM_FAILURE:
        return "stream_failure";
    case EXIT_REASON_WRITE_FAILURE:
        return "write_failure";
    case EXIT_REASON_STALL:
        return "stall";
    case EXIT_REASON_MAIN_ERROR:
        return "main_error";
    case EXIT_REASON_NONE:
    default:
        return "none";
    }
}

static stop_reason_t camera_error_stop_reason(int thread_index)
{
    return thread_index == 0 ? STOP_REASON_CAM0_ERROR : STOP_REASON_CAM1_ERROR;
}

static stop_reason_t get_stop_reason(void)
{
    return (stop_reason_t)atomic_load(&stop_reason);
}

static int get_stop_flag(const atomic_bool *stop_flag)
{
    return atomic_load(stop_flag);
}

static void request_stop(stop_reason_t reason)
{
    int expected = STOP_REASON_NONE;

    if (reason != STOP_REASON_NONE)
    {
        atomic_compare_exchange_strong(&stop_reason, &expected, reason);
    }
    atomic_store(&is_stop, 1);
}

static void init_default_camera_config(camera_config_t *cfg, int camera_index)
{
    memset(cfg, 0, sizeof(*cfg));
    cfg->pipe_id = 0;
    cfg->video_index = camera_index;
    cfg->chn_num = 1;
    cfg->encode_chn = camera_index;
    cfg->bitrate = 8000;
    cfg->fps = -1;
    cfg->output_path = camera_index == 0 ? "stream0.h264" : "stream1.h264";
}

static void init_default_program_config(program_config_t *cfg)
{
    int i = 0;

    memset(cfg, 0, sizeof(*cfg));
    cfg->duration_sec = 120;
    cfg->heartbeat_interval_sec = 5;
    cfg->stall_timeout_sec = 0;

    for (i = 0; i < CAMERA_COUNT; ++i)
    {
        init_default_camera_config(&cfg->cameras[i], i);
    }
}

static int validate_camera_config(const camera_config_t *cfg, int camera_index)
{
    if (cfg->output_path == NULL || cfg->output_path[0] == '\0')
    {
        log_message(-1, "ERROR", "camera%d output path is empty", camera_index);
        return -1;
    }

    if (cfg->input_width <= 0 || cfg->input_height <= 0)
    {
        log_message(-1, "ERROR", "camera%d input size is invalid", camera_index);
        return -1;
    }

    if (cfg->output_width <= 0 || cfg->output_height <= 0)
    {
        log_message(-1, "ERROR", "camera%d output size is invalid", camera_index);
        return -1;
    }

    if (cfg->bitrate <= 0)
    {
        log_message(-1, "ERROR", "camera%d bitrate is invalid", camera_index);
        return -1;
    }

    return 0;
}

static int validate_program_config(struct argp_state *state, const program_config_t *cfg)
{
    int i = 0;

    if (cfg->duration_sec <= 0)
    {
        log_message(-1, "ERROR", "duration-sec must be greater than 0");
        argp_state_help(state, stdout, ARGP_HELP_STD_HELP);
        return -1;
    }

    if (cfg->heartbeat_interval_sec < 0)
    {
        log_message(-1, "ERROR", "heartbeat-sec must be >= 0");
        argp_state_help(state, stdout, ARGP_HELP_STD_HELP);
        return -1;
    }

    if (cfg->stall_timeout_sec < 0)
    {
        log_message(-1, "ERROR", "stall-timeout-sec must be >= 0");
        argp_state_help(state, stdout, ARGP_HELP_STD_HELP);
        return -1;
    }

    for (i = 0; i < CAMERA_COUNT; ++i)
    {
        if (validate_camera_config(&cfg->cameras[i], i) != 0)
        {
            argp_state_help(state, stdout, ARGP_HELP_STD_HELP);
            return -1;
        }
    }

    if (cfg->cameras[0].video_index == cfg->cameras[1].video_index)
    {
        log_message(-1, "ERROR", "camera0 and camera1 must use different video_index");
        argp_state_help(state, stdout, ARGP_HELP_STD_HELP);
        return -1;
    }

    if (cfg->cameras[0].encode_chn == cfg->cameras[1].encode_chn)
    {
        log_message(-1, "ERROR", "camera0 and camera1 must use different encode channel");
        argp_state_help(state, stdout, ARGP_HELP_STD_HELP);
        return -1;
    }

    return 0;
}

static error_t parse_opt(int key, char *arg, struct argp_state *state)
{
    program_config_t *cfg = state->input;

    switch (key)
    {
    case KEY_DURATION_SEC:
        cfg->duration_sec = atoi(arg);
        break;
    case KEY_HEARTBEAT_SEC:
        cfg->heartbeat_interval_sec = atoi(arg);
        break;
    case KEY_STALL_TIMEOUT_SEC:
        cfg->stall_timeout_sec = atoi(arg);
        break;
    case KEY_CAM0_VIDEO_INDEX:
        cfg->cameras[0].video_index = atoi(arg);
        break;
    case KEY_CAM0_OUTPUT:
        cfg->cameras[0].output_path = arg;
        break;
    case KEY_CAM0_IWIDTH:
        cfg->cameras[0].input_width = atoi(arg);
        break;
    case KEY_CAM0_IHEIGHT:
        cfg->cameras[0].input_height = atoi(arg);
        break;
    case KEY_CAM0_OWIDTH:
        cfg->cameras[0].output_width = atoi(arg);
        break;
    case KEY_CAM0_OHEIGHT:
        cfg->cameras[0].output_height = atoi(arg);
        break;
    case KEY_CAM0_BITRATE:
        cfg->cameras[0].bitrate = atoi(arg);
        break;
    case KEY_CAM0_FPS:
        cfg->cameras[0].fps = atoi(arg);
        break;
    case KEY_CAM1_VIDEO_INDEX:
        cfg->cameras[1].video_index = atoi(arg);
        break;
    case KEY_CAM1_OUTPUT:
        cfg->cameras[1].output_path = arg;
        break;
    case KEY_CAM1_IWIDTH:
        cfg->cameras[1].input_width = atoi(arg);
        break;
    case KEY_CAM1_IHEIGHT:
        cfg->cameras[1].input_height = atoi(arg);
        break;
    case KEY_CAM1_OWIDTH:
        cfg->cameras[1].output_width = atoi(arg);
        break;
    case KEY_CAM1_OHEIGHT:
        cfg->cameras[1].output_height = atoi(arg);
        break;
    case KEY_CAM1_BITRATE:
        cfg->cameras[1].bitrate = atoi(arg);
        break;
    case KEY_CAM1_FPS:
        cfg->cameras[1].fps = atoi(arg);
        break;
    case ARGP_KEY_END:
        if (validate_program_config(state, cfg) != 0)
        {
            return EINVAL;
        }
        break;
    default:
        return ARGP_ERR_UNKNOWN;
    }

    return 0;
}

static struct argp argp = {options, parse_opt, 0, doc};

static void signal_handler_func(int signum)
{
    (void)signum;
    request_stop(STOP_REASON_SIGNAL);
}

static exit_reason_t derive_exit_reason_from_global(const camera_context_t *ctx)
{
    stop_reason_t reason = get_stop_reason();

    switch (reason)
    {
    case STOP_REASON_TIMEOUT:
        return EXIT_REASON_TIMEOUT;
    case STOP_REASON_SIGNAL:
        return EXIT_REASON_SIGNAL;
    case STOP_REASON_CAM0_ERROR:
        return ctx->thread_index == 0 ? EXIT_REASON_STREAM_FAILURE : EXIT_REASON_PEER_ERROR;
    case STOP_REASON_CAM1_ERROR:
        return ctx->thread_index == 1 ? EXIT_REASON_STREAM_FAILURE : EXIT_REASON_PEER_ERROR;
    case STOP_REASON_MAIN_ERROR:
        return EXIT_REASON_MAIN_ERROR;
    case STOP_REASON_NONE:
    default:
        return EXIT_REASON_NONE;
    }
}

static void init_camera_context(camera_context_t *ctx, const program_config_t *program_cfg, int thread_index,
                                atomic_bool *stop_flag)
{
    memset(ctx, 0, sizeof(*ctx));
    ctx->cfg = program_cfg->cameras[thread_index];
    ctx->program_cfg = program_cfg;
    ctx->actual_width = ctx->cfg.output_width;
    ctx->actual_height = ctx->cfg.output_height;
    ctx->thread_index = thread_index;
    ctx->stop_flag = stop_flag;
    ctx->exit_reason = EXIT_REASON_NONE;
}

static void log_camera_summary(const camera_context_t *ctx)
{
    uint64_t run_ms = 0;
    uint64_t first_frame_latency_ms = 0;
    uint64_t avg_bitrate_kbps = 0;
    uint64_t avg_fps_x1000 = 0;

    if (ctx->end_ms > ctx->start_ms)
    {
        run_ms = ctx->end_ms - ctx->start_ms;
    }

    if (ctx->first_frame_ms > ctx->start_ms)
    {
        first_frame_latency_ms = ctx->first_frame_ms - ctx->start_ms;
    }

    if (run_ms > 0)
    {
        avg_bitrate_kbps = (ctx->bytes_written * 8ULL) / run_ms;
        avg_fps_x1000 = (ctx->frame_count * 1000000ULL) / run_ms;
    }

    log_message(
        ctx->thread_index, "INFO",
        "SUMMARY result=%d exit_reason=%s stop_reason=%s frames=%" PRIu64 " bytes=%" PRIu64
        " run_ms=%" PRIu64 " first_frame_latency_ms=%" PRIu64 " avg_bitrate_kbps=%" PRIu64
        " avg_fps_x1000=%" PRIu64 " max_frame_gap_ms=%" PRIu64 " heartbeats=%" PRIu64
        " stream_failures=%" PRIu64 " write_failures=%" PRIu64,
        ctx->result, exit_reason_to_string(ctx->exit_reason), stop_reason_to_string(get_stop_reason()),
        ctx->frame_count, ctx->bytes_written, run_ms, first_frame_latency_ms, avg_bitrate_kbps, avg_fps_x1000,
        ctx->max_frame_gap_ms, ctx->heartbeat_count, ctx->stream_fail_count, ctx->write_fail_count);
}

static int camera_pipeline_start(camera_context_t *ctx)
{
    int ret = 0;
    sp_sensors_parameters parms;

    memset(&parms, 0, sizeof(parms));
    parms.fps = ctx->cfg.fps;
    parms.raw_height = ctx->cfg.input_height;
    parms.raw_width = ctx->cfg.input_width;

    ctx->start_ms = get_monotonic_ms();
    ctx->last_frame_ms = 0;

    log_message(ctx->thread_index, "INFO",
                "thread start video_index=%d encode_chn=%d input=%dx%d output=%dx%d bitrate=%d fps=%d output=%s",
                ctx->cfg.video_index, ctx->cfg.encode_chn, ctx->cfg.input_width, ctx->cfg.input_height,
                ctx->cfg.output_width, ctx->cfg.output_height, ctx->cfg.bitrate, ctx->cfg.fps,
                ctx->cfg.output_path);

    ctx->vio_object = sp_init_vio_module();
    if (ctx->vio_object == NULL)
    {
        log_message(ctx->thread_index, "ERROR", "sp_init_vio_module failed");
        ctx->exit_reason = EXIT_REASON_START_FAILURE;
        return -1;
    }

    ctx->encoder = sp_init_encoder_module();
    if (ctx->encoder == NULL)
    {
        log_message(ctx->thread_index, "ERROR", "sp_init_encoder_module failed");
        ctx->exit_reason = EXIT_REASON_START_FAILURE;
        return -1;
    }

    ctx->stream_buffer = malloc(sizeof(char) * STREAM_FRAME_SIZE);
    if (ctx->stream_buffer == NULL)
    {
        log_message(ctx->thread_index, "ERROR", "malloc stream buffer failed");
        ctx->exit_reason = EXIT_REASON_START_FAILURE;
        return -1;
    }

    ctx->stream = fopen(ctx->cfg.output_path, "wb");
    if (ctx->stream == NULL)
    {
        log_message(ctx->thread_index, "ERROR", "fopen failed path=%s", ctx->cfg.output_path);
        ctx->exit_reason = EXIT_REASON_START_FAILURE;
        return -1;
    }

    ret = sp_open_camera_v2(ctx->vio_object, ctx->cfg.pipe_id, ctx->cfg.video_index, ctx->cfg.chn_num, &parms,
                            &ctx->actual_width, &ctx->actual_height);
    if (ret != 0)
    {
        log_message(ctx->thread_index, "ERROR", "sp_open_camera_v2 failed video_index=%d ret=%d",
                    ctx->cfg.video_index, ret);
        ctx->exit_reason = EXIT_REASON_START_FAILURE;
        return -1;
    }
    ctx->camera_opened = 1;
    log_message(ctx->thread_index, "INFO", "sp_open_camera_v2 success video_index=%d size=%dx%d",
                ctx->cfg.video_index, ctx->actual_width, ctx->actual_height);

    ret = sp_start_encode(ctx->encoder, ctx->cfg.encode_chn, SP_ENCODER_H264, ctx->actual_width,
                          ctx->actual_height, ctx->cfg.bitrate);
    if (ret != 0)
    {
        log_message(ctx->thread_index, "ERROR", "sp_start_encode failed chn=%d ret=%d", ctx->cfg.encode_chn, ret);
        ctx->exit_reason = EXIT_REASON_START_FAILURE;
        return -1;
    }
    ctx->encode_started = 1;
    log_message(ctx->thread_index, "INFO", "sp_start_encode success chn=%d bitrate=%d", ctx->cfg.encode_chn,
                ctx->cfg.bitrate);

    ret = sp_module_bind(ctx->vio_object, SP_MTYPE_VIO, ctx->encoder, SP_MTYPE_ENCODER);
    if (ret != 0)
    {
        log_message(ctx->thread_index, "ERROR", "sp_module_bind(vio->encoder) failed ret=%d", ret);
        ctx->exit_reason = EXIT_REASON_START_FAILURE;
        return -1;
    }
    ctx->bind_done = 1;
    log_message(ctx->thread_index, "INFO", "sp_module_bind(vio->encoder) success");

    return 0;
}

static int camera_pipeline_run(camera_context_t *ctx)
{
    uint64_t next_heartbeat_ms = 0;

    if (ctx->program_cfg->heartbeat_interval_sec > 0)
    {
        next_heartbeat_ms = ctx->start_ms + (uint64_t)ctx->program_cfg->heartbeat_interval_sec * 1000ULL;
    }

    while (!get_stop_flag(ctx->stop_flag))
    {
        int stream_frame_size = 0;
        size_t write_size = 0;
        uint64_t now_ms = 0;
        uint64_t frame_gap_ms = 0;

        memset(ctx->stream_buffer, 0, STREAM_FRAME_SIZE);
        stream_frame_size = sp_encoder_get_stream(ctx->encoder, ctx->stream_buffer);
        now_ms = get_monotonic_ms();

        if (stream_frame_size == -1)
        {
            ctx->stream_fail_count++;
            log_message(ctx->thread_index, "ERROR", "sp_encoder_get_stream failed count=%" PRIu64,
                        ctx->stream_fail_count);
            ctx->exit_reason = EXIT_REASON_STREAM_FAILURE;
            request_stop(camera_error_stop_reason(ctx->thread_index));
            return -1;
        }

        if (ctx->first_frame_ms == 0)
        {
            ctx->first_frame_ms = now_ms;
            log_message(ctx->thread_index, "INFO", "first frame captured size=%d", stream_frame_size);
        }

        if (ctx->last_frame_ms != 0 && now_ms > ctx->last_frame_ms)
        {
            frame_gap_ms = now_ms - ctx->last_frame_ms;
            if (frame_gap_ms > ctx->max_frame_gap_ms)
            {
                ctx->max_frame_gap_ms = frame_gap_ms;
            }
        }
        ctx->last_frame_ms = now_ms;

        if (ctx->program_cfg->stall_timeout_sec > 0 &&
            frame_gap_ms > (uint64_t)ctx->program_cfg->stall_timeout_sec * 1000ULL)
        {
            log_message(ctx->thread_index, "ERROR", "frame gap exceeded threshold gap_ms=%" PRIu64
                                               " threshold_sec=%d",
                        frame_gap_ms, ctx->program_cfg->stall_timeout_sec);
            ctx->exit_reason = EXIT_REASON_STALL;
            request_stop(camera_error_stop_reason(ctx->thread_index));
            return -1;
        }

        write_size = fwrite(ctx->stream_buffer, sizeof(char), (size_t)stream_frame_size, ctx->stream);
        if (write_size != (size_t)stream_frame_size)
        {
            ctx->write_fail_count++;
            log_message(ctx->thread_index, "ERROR", "fwrite failed expect=%d actual=%zu count=%" PRIu64,
                        stream_frame_size, write_size, ctx->write_fail_count);
            ctx->exit_reason = EXIT_REASON_WRITE_FAILURE;
            request_stop(camera_error_stop_reason(ctx->thread_index));
            return -1;
        }

        ctx->frame_count++;
        ctx->bytes_written += (uint64_t)stream_frame_size;

        if (next_heartbeat_ms > 0 && now_ms >= next_heartbeat_ms)
        {
            uint64_t run_ms = now_ms > ctx->start_ms ? now_ms - ctx->start_ms : 0;
            uint64_t avg_fps_x1000 = run_ms > 0 ? (ctx->frame_count * 1000000ULL) / run_ms : 0;

            ctx->heartbeat_count++;
            log_message(ctx->thread_index, "INFO",
                        "heartbeat frames=%" PRIu64 " bytes=%" PRIu64 " run_ms=%" PRIu64
                        " avg_fps_x1000=%" PRIu64 " max_frame_gap_ms=%" PRIu64,
                        ctx->frame_count, ctx->bytes_written, run_ms, avg_fps_x1000, ctx->max_frame_gap_ms);
            next_heartbeat_ms += (uint64_t)ctx->program_cfg->heartbeat_interval_sec * 1000ULL;
        }
    }

    if (ctx->exit_reason == EXIT_REASON_NONE)
    {
        ctx->exit_reason = derive_exit_reason_from_global(ctx);
    }
    return 0;
}

static void camera_pipeline_stop(camera_context_t *ctx)
{
    if (ctx->bind_done)
    {
        sp_module_unbind(ctx->vio_object, SP_MTYPE_VIO, ctx->encoder, SP_MTYPE_ENCODER);
        ctx->bind_done = 0;
    }

    if (ctx->stream != NULL)
    {
        fclose(ctx->stream);
        ctx->stream = NULL;
    }

    if (ctx->stream_buffer != NULL)
    {
        free(ctx->stream_buffer);
        ctx->stream_buffer = NULL;
    }

    if (ctx->encode_started)
    {
        sp_stop_encode(ctx->encoder);
        ctx->encode_started = 0;
    }

    if (ctx->camera_opened)
    {
        sp_vio_close(ctx->vio_object);
        ctx->camera_opened = 0;
    }

    if (ctx->encoder != NULL)
    {
        sp_release_encoder_module(ctx->encoder);
        ctx->encoder = NULL;
    }

    if (ctx->vio_object != NULL)
    {
        sp_release_vio_module(ctx->vio_object);
        ctx->vio_object = NULL;
    }

    ctx->end_ms = get_monotonic_ms();
}

static void *duration_timer_worker(void *arg)
{
    const timer_context_t *timer_ctx = (const timer_context_t *)arg;
    uint64_t deadline_ms = get_monotonic_ms() + (uint64_t)timer_ctx->duration_sec * 1000ULL;

    while (!get_stop_flag(&is_stop))
    {
        uint64_t now_ms = get_monotonic_ms();
        struct timespec sleep_ts;

        if (now_ms >= deadline_ms)
        {
            log_message(-1, "INFO", "duration reached duration_sec=%d", timer_ctx->duration_sec);
            request_stop(STOP_REASON_TIMEOUT);
            break;
        }

        sleep_ts.tv_sec = 0;
        sleep_ts.tv_nsec = 200000000L;
        nanosleep(&sleep_ts, NULL);
    }

    return NULL;
}

static void *camera_worker(void *arg)
{
    camera_context_t *ctx = (camera_context_t *)arg;

    ctx->result = camera_pipeline_start(ctx);
    if (ctx->result == 0)
    {
        ctx->result = camera_pipeline_run(ctx);
    }
    else
    {
        request_stop(camera_error_stop_reason(ctx->thread_index));
    }

    camera_pipeline_stop(ctx);
    if (ctx->exit_reason == EXIT_REASON_NONE)
    {
        ctx->exit_reason = derive_exit_reason_from_global(ctx);
    }

    log_camera_summary(ctx);
    log_message(ctx->thread_index, "INFO", "thread exit result=%d exit_reason=%s", ctx->result,
                exit_reason_to_string(ctx->exit_reason));
    return NULL;
}

int main(int argc, char **argv)
{
    int i = 0;
    int ret = 0;
    int final_ret = 0;
    int timer_created = 0;
    pthread_t worker_tids[CAMERA_COUNT];
    pthread_t timer_tid;
    program_config_t cfg;
    camera_context_t contexts[CAMERA_COUNT];
    int thread_created[CAMERA_COUNT] = {0};
    uint64_t main_start_ms = 0;
    uint64_t main_end_ms = 0;
    timer_context_t timer_ctx;

    setvbuf(stdout, NULL, _IONBF, 0);
    signal(SIGINT, signal_handler_func);
    signal(SIGTERM, signal_handler_func);

    init_default_program_config(&cfg);
    argp_parse(&argp, argc, argv, 0, 0, &cfg);
    atomic_store(&is_stop, 0);
    atomic_store(&stop_reason, STOP_REASON_NONE);
    main_start_ms = get_monotonic_ms();

    log_message(-1, "INFO", "config duration_sec=%d heartbeat_sec=%d stall_timeout_sec=%d", cfg.duration_sec,
                cfg.heartbeat_interval_sec, cfg.stall_timeout_sec);

    for (i = 0; i < CAMERA_COUNT; ++i)
    {
        init_camera_context(&contexts[i], &cfg, i, &is_stop);
    }

    timer_ctx.duration_sec = cfg.duration_sec;
    ret = pthread_create(&timer_tid, NULL, duration_timer_worker, &timer_ctx);
    if (ret != 0)
    {
        log_message(-1, "ERROR", "pthread_create failed for timer ret=%d", ret);
        request_stop(STOP_REASON_MAIN_ERROR);
        return -1;
    }
    timer_created = 1;

    for (i = 0; i < CAMERA_COUNT; ++i)
    {
        ret = pthread_create(&worker_tids[i], NULL, camera_worker, &contexts[i]);
        if (ret != 0)
        {
            log_message(-1, "ERROR", "pthread_create failed cam=%d ret=%d", i, ret);
            request_stop(STOP_REASON_MAIN_ERROR);
            final_ret = -1;
            break;
        }
        thread_created[i] = 1;
    }

    for (i = 0; i < CAMERA_COUNT; ++i)
    {
        if (!thread_created[i])
        {
            continue;
        }

        ret = pthread_join(worker_tids[i], NULL);
        if (ret != 0)
        {
            log_message(-1, "ERROR", "pthread_join failed cam=%d ret=%d", i, ret);
            final_ret = -1;
            request_stop(STOP_REASON_MAIN_ERROR);
        }
        else if (contexts[i].result != 0)
        {
            final_ret = -1;
        }
    }

    if (timer_created)
    {
        ret = pthread_join(timer_tid, NULL);
        if (ret != 0)
        {
            log_message(-1, "ERROR", "pthread_join failed for timer ret=%d", ret);
            final_ret = -1;
        }
    }

    main_end_ms = get_monotonic_ms();
    log_message(-1, "INFO", "PROGRAM_SUMMARY result=%d stop_reason=%s total_run_ms=%" PRIu64, final_ret,
                stop_reason_to_string(get_stop_reason()), main_end_ms - main_start_ms);

    return final_ret == 0 ? 0 : -1;
}