TaskScheduler* scheduler = BasicTaskScheduler::createNew( );
    if ( scheduler == nullptr ) {
        std::cerr << "Failed to create task scheduler" << std::endl;
        delete v4l2Device;
        return 1;
    }

    UsageEnvironment* env = BasicUsageEnvironment::createNew( *scheduler );
    if ( env == nullptr ) {
        std::cerr << "Failed to create usage environment" << std::endl;
        delete scheduler;
        delete v4l2Device;
        return 1;
    }

    // Increase buffer size for high-resolution MJPEG
    OutPacketBuffer::maxSize = 1000000;  // 1MB buffer

    // Create V4l2MJPEGSource
    V4l2MJPEGSource* videoSource = V4l2MJPEGSource::createNew( *env );
    if ( videoSource == nullptr ) {
        std::cerr << "Failed to create V4l2MJPEGSource" << std::endl;
        env->reclaim( );
        delete scheduler;
        delete v4l2Device;
        return 1;
    }
    videoSource->setDevice( v4l2Device );

    // // Start streaming before creating RTSP server
    // if ( !videoSource->start( ) ) {
    //     std::cerr << "Failed to start V4l2Device" << std::endl;
    //     V4l2MJPEGSource::close( videoSource );
    //     env->reclaim( );
    //     delete scheduler;
    //     delete v4l2Device;
    //     return 1;
    // }

    // Set up RTSP server with videoSource
    RTSPServer* rtspServer = RTSPServer::createNew( *env, port, NULL );
    if ( rtspServer == nullptr ) {
        std::cerr << "Failed to create RTSP server" << std::endl;
        V4l2MJPEGSource::close( videoSource );
        env->reclaim( );
        delete scheduler;
        delete v4l2Device;
        return 1;
    }

    ServerMediaSession* sms = ServerMediaSession::createNew(
        *env, streamUrl.c_str( ), streamUrl.c_str( ), "GRALmarine global shutter camera" );
    if ( sms == nullptr ) {
        std::cerr << "Failed to create server media session" << std::endl;
        Medium::close( rtspServer );
        V4l2MJPEGSource::close( videoSource );
        env->reclaim( );
        delete scheduler;
        delete v4l2Device;
        return 1;
    }

    // Create and add subsession
    ServerMediaSubsession* subsession = LiveServerMediaSubsession::createNew( *env );
    if ( subsession == nullptr ) {
        std::cerr << "Failed to create media subsession" << std::endl;
        Medium::close( sms );
        Medium::close( rtspServer );
        V4l2MJPEGSource::close( videoSource );
        env->reclaim( );
        delete scheduler;
        delete v4l2Device;
        return 1;
    }

    sms->addSubsession( subsession );
    rtspServer->addServerMediaSession( sms );

    std::cout << "RTSP server started on port " << port << std::endl;
    std::cout << "Stream available at rtsp://<ip>:" << port << "/" << streamUrl << std::endl;

    // Main event loop
    env->taskScheduler( ).doEventLoop( );