ContainerMPG.h

Go to the documentation of this file.

#pragma once
#include <string.h>
#include "AudioTools/AudioCodecs/AudioCodecsBase.h"
#include "AudioTools/AudioCodecs/AudioFormat.h"
#include "AudioTools/AudioCodecs/ContainerCommon.h"
#include "AudioTools/CoreAudio/AudioBasic/Collections/Vector.h"
#include "AudioTools/Video/Video.h"
 
namespace audio_tools {
 
// packet_start_code / stream_id values used by the ISO/IEC 11172-1 (MPEG-1)
// Program Stream (also reused, unchanged, by MPEG-2 Program Streams)
static const uint8_t MPG_PACK_START_CODE = 0xBA;
static const uint8_t MPG_SYSTEM_HEADER_START_CODE = 0xBB;
static const uint8_t MPG_PROGRAM_END_CODE = 0xB9;
static const uint8_t MPG_PROGRAM_STREAM_MAP = 0xBC;
static const uint8_t MPG_PRIVATE_STREAM_1 = 0xBD;
static const uint8_t MPG_PADDING_STREAM = 0xBE;
static const uint8_t MPG_PRIVATE_STREAM_2 = 0xBF;
static const uint8_t MPG_VIDEO_STREAM_ID = 0xE0;
static const uint8_t MPG_AUDIO_STREAM_ID = 0xC0;
static const uint32_t MPG_CLOCK_HZ = 90000;
 
class MPGParseBuffer {
 public:
  void resize(size_t size) { vec.resize(size + 4); }
  size_t writeArray(const uint8_t *data, size_t len) {
    size_t to_write = min(availableToWrite(), len);
    memmove(vec.data() + count, data, to_write);
    count += to_write;
    return to_write;
  }
  void consume(size_t len) {
    if (len > count) len = count;
    memmove(vec.data(), vec.data() + len, count - len);
    count -= len;
  }
  uint8_t *data() { return vec.data(); }
  size_t available() { return count; }
  size_t availableToWrite() { return vec.size() - count; }
  void clear() { count = 0; }
 
  long indexOfStartCode(size_t from = 0) {
    if (count < 3 || from + 3 > count) return -1;
    for (size_t i = from; i + 2 < count; i++) {
      if (vec[i] == 0 && vec[i + 1] == 0 && vec[i + 2] == 1) return (long)i;
    }
    return -1;
  }
 
 protected:
  Vector<uint8_t> vec{0};
  size_t count = 0;
};
 
class DemuxerMPG : public Demuxer {
 public:
  DemuxerMPG(int bufferSize = 1024) { parse_buffer.resize(bufferSize); }
 
  const char *mimeVideo() override { return "video/mpeg"; }
  const char *mime() override {
    if (audio_layer == 2) return "audio/mpeg; codecs=\"mpeg1-layer2\"";
    return toMime(AudioFormat::MP3);
  }
 
  bool isValid(const uint8_t *data, size_t len) override {
    return len >= 4 && data[0] == 0x00 && data[1] == 0x00 &&
           data[2] == 0x01 && data[3] == MPG_PACK_START_CODE;
  }
 
  bool begin() override {
    parse_buffer.clear();
    is_parsing_active = true;
    unit_open = false;
    is_skip_unit = false;
    unbounded = false;
    video_frame_open = false;
    audio_frame_open = false;
    video_unit_len = 0;
    audio_unit_len = 0;
    video_unit_seen_picture = false;
    video_header_parsed = false;
    audio_header_parsed = false;
    video_probe.clear();
    audio_probe.clear();
    video_width = 0;
    video_height = 0;
    video_fps = 0;
    audio_sample_rate = 0;
    audio_channels = 0;
    audio_layer = 0;
    total_bytes = 0;
    return true;
  }
 
  void end() override {
    if (video_frame_open) flushVideoUnit();
    if (audio_frame_open) flushAudioUnit();
    video_frame_open = false;
    audio_frame_open = false;
    is_parsing_active = false;
  }
 
  operator bool() override { return is_parsing_active; }
 
  void setOutputAudio(Print &out) override { p_output_audio = &out; }
 
  void setOutput(Print &out) override { setOutputAudio(out); }
 
  void setOutputVideo(Print &out) override { p_output_video = &out; }
  void setOutputVideo(VideoOutput &out) override { p_output_video_video = &out; }
 
  void setSendWavHeader(bool flag) override { (void)flag; }
 
  VideoInfo getVideoInfo() override {
    VideoInfo result;
    result.width = video_width;
    result.height = video_height;
    result.fps = video_fps;
    result.format = VideoFormat::MPEG1;
    result.frame_size = 0;  // compressed: size varies per frame
    result.total_file_size = total_bytes;
    return result;
  }
 
  AudioInfoFormat getAudioInfo() override {
    AudioInfoFormat result;
    result.sample_rate = audio_sample_rate > 0 ? audio_sample_rate : 44100;
    result.channels = audio_channels > 0 ? audio_channels : 2;
    result.bits_per_sample = 16;  // nominal - the codec is compressed
    result.format = AudioFormat::MP3;
    return result;
  }
 
  size_t write(const uint8_t *data, size_t len) override {
    size_t result = parse_buffer.writeArray(data, len);
    total_bytes += result;
    if (is_parsing_active) {
      while (parse()) {
      }
    }
    return result;
  }
 
 protected:
  bool is_parsing_active = true;
  MPGParseBuffer parse_buffer;
  Print *p_output_audio = nullptr;
  Print *p_output_video = nullptr;
  VideoOutput *p_output_video_video = nullptr;
  uint32_t total_bytes = 0;
 
  // current streamed unit (PES payload, or a skipped block's payload)
  bool unit_open = false;
  bool is_skip_unit = false;
  bool is_video_unit = false;
  bool unbounded = false;  // PES_packet_length == 0 (video only)
  size_t remaining = 0;
 
  // video access-unit framing: a new PES with a PTS starts a new picture
  bool video_frame_open = false;
  // audio access-unit framing: a new PES with a PTS starts a new audio frame
  bool audio_frame_open = false;
 
  // Accumulates one complete access unit (a whole picture, or a whole
  // audio frame) across every PES fragment and every parse_buffer-sized
  // write() chunk that contributes to it, so the decoder downstream gets
  // one single write() call per unit instead of an arbitrary number of
  // partial ones - see deliver()/flushVideoUnit()/flushAudioUnit(). A
  // container-demuxed stream's chunk boundaries don't line up with
  // picture/frame boundaries at all (the parse buffer above is a small,
  // fixed size; a real access unit is routinely several times larger),
  // and feeding a video decoder like TinyMPGDecoder a boundary-split
  // buffer under its default whole-buffer contract can silently
  // reconstruct a picture from stale/wrong macroblocks rather than
  // failing cleanly - see that project's own TinyMPGDecoder::write() doc
  // comment, which calls this exact demuxer out by name.
  //
  // Separate buffers for audio/video rather than one shared one: a real
  // encoder (ffmpeg's MPEG-PS muxer among them, confirmed by testing
  // against files it produced) routinely interleaves audio PES packets
  // in the middle of a video picture's own PES fragments, before that
  // picture's bytes are complete - unlike MuxerMPG (this project's own
  // encoder), which happens to write one access unit's fragments back to
  // back. A single shared buffer can't survive that: whichever unit was
  // mid-accumulation would have to be flushed the moment the other type's
  // bytes arrived, truncating it and permanently losing/misaligning
  // whatever hadn't arrived yet - confirmed by testing (a truncated video
  // unit's trailing bytes reappear, unprefixed by their own start code,
  // corrupting the *next* unit too). Two independent buffers mean
  // real interleaving no longer needs any special-casing at all - each
  // side just keeps accumulating exactly where it left off.
  //
  // *_unit_len is tracked separately from each Vector's own size(), which
  // is deliberately over-allocated for amortized growth - Vector::resize()
  // always allocates exactly the requested size with no spare capacity
  // (AudioBasic/Collections/Vector.h), so resizing it to the exact new
  // length on every single delivered fragment - one picture can need a
  // dozen of these - would reallocate and copy everything accumulated so
  // far *every single call*, the same O(N^2) cost this project's
  // ChunkedSampleTableStore (SampleTableStore.h) exists to avoid for
  // MP4's sample tables.
  Vector<uint8_t> video_unit_buffer;
  size_t video_unit_len = 0;
  Vector<uint8_t> audio_unit_buffer;
  size_t audio_unit_len = 0;
  // Whether the current (not yet flushed) video unit has already seen its
  // first internal picture_start_code - see deliverVideo().
  bool video_unit_seen_picture = false;
 
  // lazily parsed from the elementary streams themselves
  bool video_header_parsed = false;
  bool audio_header_parsed = false;
  uint16_t video_width = 0;
  uint16_t video_height = 0;
  float video_fps = 0;
  uint32_t audio_sample_rate = 0;
  uint8_t audio_channels = 0;
  uint8_t audio_layer = 0;  
  Vector<uint8_t> video_probe;
  Vector<uint8_t> audio_probe;
  static const size_t VIDEO_PROBE_MAX = 64;
 
  bool parse() {
    if (unit_open) return continuePayload();
 
    if (parse_buffer.available() < 4) return false;
    uint8_t *buf = parse_buffer.data();
    if (!(buf[0] == 0 && buf[1] == 0 && buf[2] == 1)) {
      long idx = parse_buffer.indexOfStartCode();
      if (idx < 0) {
        size_t avail = parse_buffer.available();
        if (avail > 2) parse_buffer.consume(avail - 2);
        return false;
      }
      parse_buffer.consume((size_t)idx);
      if (parse_buffer.available() < 4) return false;
      buf = parse_buffer.data();
    }
 
    uint8_t id = buf[3];
    if (id == MPG_PACK_START_CODE) return parsePack();
    if (id == MPG_SYSTEM_HEADER_START_CODE) return parseSystemHeader();
    if (id == MPG_PROGRAM_END_CODE) {
      parse_buffer.consume(4);
      if (video_frame_open) flushVideoUnit();
      if (audio_frame_open) flushAudioUnit();
      video_frame_open = false;
      audio_frame_open = false;
      LOGI("MPEG_program_end_code");
      return true;
    }
    if (id == MPG_PROGRAM_STREAM_MAP || id == MPG_PRIVATE_STREAM_1 ||
        id == MPG_PADDING_STREAM || id == MPG_PRIVATE_STREAM_2)
      return parseSkip();
    if (id >= 0xC0 && id <= 0xDF) return parsePes(id, false);
    if (id >= 0xE0 && id <= 0xEF) return parsePes(id, true);
 
    // reserved/unrecognized id: drop this byte and resync on the next
    // start code
    parse_buffer.consume(1);
    return true;
  }
 
  bool parsePack() {
    if (parse_buffer.available() < 12) return false;
    parse_buffer.consume(12);
    return true;
  }
 
  bool parseSystemHeader() {
    if (parse_buffer.available() < 6) return false;
    uint8_t *buf = parse_buffer.data();
    uint16_t header_length = ((uint16_t)buf[4] << 8) | buf[5];
    size_t total = 6 + header_length;
    if (parse_buffer.available() < total) return false;
    parse_buffer.consume(total);
    return true;
  }
 
  bool parseSkip() {
    if (parse_buffer.available() < 6) return false;
    uint8_t *buf = parse_buffer.data();
    uint16_t len = ((uint16_t)buf[4] << 8) | buf[5];
    parse_buffer.consume(6);
    if (len == 0) return true;
    unit_open = true;
    is_skip_unit = true;
    is_video_unit = false;
    unbounded = false;
    remaining = len;
    return continuePayload();
  }
 
  bool parsePes(uint8_t id, bool isVideo) {
    // need the fixed 6-byte PES header plus 1 byte to peek the optional
    // header's shape
    if (parse_buffer.available() < 7) return false;
    uint8_t *buf = parse_buffer.data();
    uint16_t pes_len = ((uint16_t)buf[4] << 8) | buf[5];
    uint8_t peek = buf[6];
 
    size_t optional_len;
    bool has_pts = false;
    if (peek == 0x0F) {
      optional_len = 1;
    } else if ((peek >> 4) == 0x02) {
      optional_len = 5;
      has_pts = true;
    } else if ((peek >> 4) == 0x03) {
      optional_len = 10;
      has_pts = true;
    } else {
      LOGW("DemuxerMPG: unsupported PES optional header 0x%02X - treating as no-timestamp",
           (int)peek);
      optional_len = 1;
    }
    if (parse_buffer.available() < 6 + optional_len) return false;
 
    parse_buffer.consume(6 + optional_len);
    (void)id;
 
    bool is_unbounded = false;
    size_t payload_len = 0;
    if (pes_len == 0) {
      // PES_packet_length == 0 ("unbounded") is only meaningful for video
      is_unbounded = isVideo;
    } else if (pes_len < optional_len) {
      payload_len = 0;  // malformed - be defensive rather than underflow
    } else {
      payload_len = (size_t)pes_len - optional_len;
    }
 
    if (!isVideo && has_pts) {
      // a PTS marks the first fragment of a new audio access unit - close
      // out the previous one (see audio_unit_buffer's comment)
      if (audio_frame_open) flushAudioUnit();
      audio_frame_open = true;
    }
 
    if (isVideo && has_pts) {
      // a PTS marks the first fragment of a new access unit - close out
      // the previous one
      if (video_frame_open) flushVideoUnit();
      video_frame_open = true;
      // No wall-clock scheduling or skip decision here anymore - every
      // picture is always decoded and forwarded immediately, in full.
      // Pacing is the caller's responsibility now - see PacedVideoOutput
      // (Video/PacedVideoOutput.h), which schedules frames from its own
      // background task instead of blocking this dispatch loop (and, with
      // it, audio delivery) the way an inline wait here used to.
    }
 
    unit_open = true;
    is_skip_unit = false;
    is_video_unit = isVideo;
    unbounded = is_unbounded;
    remaining = payload_len;
    return continuePayload();
  }
 
  static bool isContainerLevelStartCode(uint8_t id) {
    return id == MPG_PACK_START_CODE || id == MPG_SYSTEM_HEADER_START_CODE ||
           id == MPG_PROGRAM_END_CODE || id == MPG_PROGRAM_STREAM_MAP ||
           id == MPG_PRIVATE_STREAM_1 || id == MPG_PADDING_STREAM ||
           id == MPG_PRIVATE_STREAM_2 || (id >= 0xC0 && id <= 0xEF);
  }
 
  long indexOfContainerStartCode(size_t from = 0) {
    long idx = parse_buffer.indexOfStartCode(from);
    while (idx >= 0) {
      size_t idPos = (size_t)idx + 3;
      if (idPos >= parse_buffer.available()) return -1;  // id byte not buffered yet
      if (isContainerLevelStartCode(parse_buffer.data()[idPos])) return idx;
      idx = parse_buffer.indexOfStartCode((size_t)idx + 3);
    }
    return -1;
  }
 
  bool continuePayload() {
    if (unbounded) {
      long idx = indexOfContainerStartCode();
      size_t avail = parse_buffer.available();
      if (idx >= 0) {
        deliver((size_t)idx);
        parse_buffer.consume((size_t)idx);
        unit_open = false;
        return true;
      }
      if (avail > 2) {
        deliver(avail - 2);
        parse_buffer.consume(avail - 2);
      }
      return false;
    }
    size_t avail = parse_buffer.available();
    size_t to_write = min(avail, remaining);
    deliver(to_write);
    parse_buffer.consume(to_write);
    remaining -= to_write;
    if (remaining == 0) {
      unit_open = false;
      return true;
    }
    return false;
  }
 
  void deliver(size_t len) {
    if (len == 0 || is_skip_unit) return;
    uint8_t *data = parse_buffer.data();
    if (is_video_unit) {
      if (!video_header_parsed) probeVideoHeader(data, len);
      deliverVideo(data, len);
    } else {
      if (!audio_header_parsed) probeAudioHeader(data, len);
      appendToAudio(data, len);
    }
  }
 
  void appendToVideo(const uint8_t *data, size_t len) {
    size_t needed = video_unit_len + len;
    if (needed > (size_t)video_unit_buffer.size()) {
      size_t newCap = video_unit_buffer.size() == 0 ? 8192 : (size_t)video_unit_buffer.size() * 2;
      if (newCap < needed) newCap = needed;
      video_unit_buffer.resize(newCap);
    }
    memcpy(video_unit_buffer.data() + video_unit_len, data, len);
    video_unit_len += len;
  }
 
  void appendToAudio(const uint8_t *data, size_t len) {
    size_t needed = audio_unit_len + len;
    if (needed > (size_t)audio_unit_buffer.size()) {
      size_t newCap = audio_unit_buffer.size() == 0 ? 4096 : (size_t)audio_unit_buffer.size() * 2;
      if (newCap < needed) newCap = needed;
      audio_unit_buffer.resize(newCap);
    }
    memcpy(audio_unit_buffer.data() + audio_unit_len, data, len);
    audio_unit_len += len;
  }
 
  void deliverVideo(const uint8_t *data, size_t len) {
    size_t old_len = video_unit_len;
    appendToVideo(data, len);
    // 3-byte lookback so a start code split across the previously
    // buffered tail and this call's newly appended bytes is still found -
    // video_unit_buffer now holds both contiguously, no separate boundary
    // handling needed.
    size_t search_from = old_len > 3 ? old_len - 3 : 0;
    uint8_t *buf = video_unit_buffer.data();
    while (true) {
      long idx = -1;
      for (size_t i = search_from; i + 3 < video_unit_len; i++) {
        if (buf[i] == 0 && buf[i + 1] == 0 && buf[i + 2] == 1 && buf[i + 3] == 0) {
          idx = (long)i;
          break;
        }
      }
      if (idx < 0) break;
      if (!video_unit_seen_picture) {
        video_unit_seen_picture = true;
        search_from = (size_t)idx + 4;
        continue;
      }
      size_t remaining = video_unit_len - (size_t)idx;
      video_unit_len = (size_t)idx;
      flushVideoUnit();  // writes [0, idx), resets video_unit_len to 0
      memmove(buf, buf + idx, remaining);
      video_unit_len = remaining;
      video_unit_seen_picture = true;  // the retained remainder already starts with one
      search_from = 4;  // skip the start code now sitting at offset 0
    }
  }
 
  void flushVideoUnit() {
    if (video_unit_len > 0) {
      if (p_output_video != nullptr)
        p_output_video->write(video_unit_buffer.data(), video_unit_len);
      if (p_output_video_video != nullptr)
        p_output_video_video->write(video_unit_buffer.data(), video_unit_len);
      video_unit_len = 0;
    }
    video_unit_seen_picture = false;
    if (p_output_video != nullptr) p_output_video->flush();
    if (p_output_video_video != nullptr) p_output_video_video->flush();
  }
 
  void flushAudioUnit() {
    if (audio_unit_len == 0) return;
    if (p_output_audio != nullptr)
      p_output_audio->write(audio_unit_buffer.data(), audio_unit_len);
    audio_unit_len = 0;
  }
 
  void probeVideoHeader(const uint8_t *data, size_t len) {
    size_t old_size = (size_t)video_probe.size();
    size_t take = old_size < VIDEO_PROBE_MAX ? VIDEO_PROBE_MAX - old_size : 0;
    if (take > len) take = len;
    for (size_t i = 0; i < take; i++) {
      uint8_t b = data[i];
      video_probe.push_back(b);
    }
 
    size_t n = (size_t)video_probe.size();
    for (size_t i = 0; i + 7 < n; i++) {
      if (video_probe[i] == 0 && video_probe[i + 1] == 0 &&
          video_probe[i + 2] == 1 && video_probe[i + 3] == 0xB3) {
        uint8_t b0 = video_probe[i + 4], b1 = video_probe[i + 5],
                b2 = video_probe[i + 6], b3 = video_probe[i + 7];
        video_width = (uint16_t)(((uint16_t)b0 << 4) | (b1 >> 4));
        video_height = (uint16_t)(((uint16_t)(b1 & 0x0F) << 8) | b2);
        static const float rates[16] = {0,       23.976f, 24.0f, 25.0f,
                                         29.97f,  30.0f,   50.0f, 59.94f,
                                         60.0f,   0,       0,     0,
                                         0,       0,       0,     0};
        video_fps = rates[b3 & 0x0F];
        video_header_parsed = true;
        break;
      }
    }
    if (video_header_parsed || n >= VIDEO_PROBE_MAX) {
      video_probe.clear();
      video_probe.shrink_to_fit();
    }
  }
 
  void probeAudioHeader(const uint8_t *data, size_t len) {
    size_t old_size = (size_t)audio_probe.size();
    size_t take = old_size < 4 ? 4 - old_size : 0;
    if (take > len) take = len;
    for (size_t i = 0; i < take; i++) {
      uint8_t b = data[i];
      audio_probe.push_back(b);
    }
    if ((size_t)audio_probe.size() < 4) return;
 
    uint8_t b0 = audio_probe[0], b1 = audio_probe[1], b2 = audio_probe[2],
            b3 = audio_probe[3];
    if (b0 == 0xFF && (b1 & 0xE0) == 0xE0) {
      uint8_t version = (b1 >> 3) & 0x03;  // 11=MPEG1, 10=MPEG2, 00=MPEG2.5
      uint8_t layer_bits = (b1 >> 1) & 0x03;  // 01=Layer III, 10=Layer II, 11=Layer I
      uint8_t sr_index = (b2 >> 2) & 0x03;
      uint8_t mode = (b3 >> 6) & 0x03;  // 11=mono
      static const uint32_t rates_v1[4] = {44100, 48000, 32000, 0};
      static const uint32_t rates_v2[4] = {22050, 24000, 16000, 0};
      static const uint32_t rates_v25[4] = {11025, 12000, 8000, 0};
      uint32_t rate = 0;
      if (version == 0x03)
        rate = rates_v1[sr_index];
      else if (version == 0x02)
        rate = rates_v2[sr_index];
      else if (version == 0x00)
        rate = rates_v25[sr_index];
      if (rate > 0) {
        audio_sample_rate = rate;
        audio_channels = (mode == 0x03) ? 1 : 2;
        // layer_bits -> standard layer number (I/II/III), 0 if reserved
        static const uint8_t layers[4] = {0, 3, 2, 1};
        audio_layer = layers[layer_bits];
        audio_header_parsed = true;
      }
    } else {
      // not a frame sync where we expected one - stop probing so we don't
      // keep re-checking every write(); getAudioInfo() falls back to
      // 44100/stereo
      audio_header_parsed = true;
    }
    audio_probe.clear();
    audio_probe.shrink_to_fit();
  }
};
 
class MuxerMPG : public Muxer {
 public:
  MuxerMPG() {
    video_cfg.format = VideoFormat::MPEG1;
    audio_info.format = AudioFormat::MP3;
  }
  MuxerMPG(Print &out) : MuxerMPG() { setOutput(out); }
 
  const char *mimeVideo() override { return "video/mpeg"; }
 
  void setOutput(Print &out) override { p_out = &out; }
 
  void setVideoInfo(MuxerVideoConfig config) override { video_cfg = config; }
  MuxerVideoConfig getVideoInfo() override { return video_cfg; }
 
  void setAudioInfo(AudioInfoFormat info) override {
    audio_info = info;
    if (audio_info.format == AudioFormat::UNKNOWN)
      audio_info.format = AudioFormat::MP3;
    has_audio = true;
  }
  AudioInfoFormat &audioInfo() override { return audio_info; }
 
  bool begin() override {
    if (p_out == nullptr) {
      LOGE("output not defined");
      return false;
    }
    if (video_cfg.width == 0 || video_cfg.height == 0) {
      LOGE("invalid video size: %d x %d", (int)video_cfg.width,
           (int)video_cfg.height);
      return false;
    }
    video_frame_count = 0;
    audio_frame_count = 0;
    last_scr = 0;
    writePackHeader(0);
    writeSystemHeader();
    is_open = true;
    return true;
  }
 
  void setStreamType(StreamContentType type) override {
    write_stream_type = type;
  }
  StreamContentType streamType() override { return write_stream_type; }
 
  void end() override {
    if (is_open && p_out != nullptr) {
      static const uint8_t end_code[4] = {0x00, 0x00, 0x01,
                                           MPG_PROGRAM_END_CODE};
      p_out->write(end_code, 4);
    }
    is_open = false;
  }
 
  operator bool() override { return is_open; }
 
  size_t write(const uint8_t *data, size_t len) override {
    if (write_stream_type == StreamContentType::Audio)
      return addAudioFrame(data, len);
    return addVideoFrame(data, len);
  }
 
  size_t addVideoFrame(const uint8_t *data, size_t len,
                       bool isKeyFrame = true) override {
    (void)isKeyFrame;
    if (!is_open || data == nullptr || len == 0) return 0;
    size_t written = writeAccessUnit(MPG_VIDEO_STREAM_ID, data, len, videoPts());
    video_frame_count++;
    return written;
  }
 
  size_t addJpegFrame(const uint8_t *data, size_t len) override {
    LOGW("MuxerMPG: MJPEG is not supported by MPEG-1 Program Stream");
    return addVideoFrame(data, len);
  }
  size_t addYUV422Frame(const uint8_t *data, size_t len) override {
    LOGW("MuxerMPG: raw YUV422 is not supported by MPEG-1 Program Stream");
    return addVideoFrame(data, len);
  }
  size_t addRGB565Frame(const uint8_t *data, size_t len) override {
    LOGW("MuxerMPG: raw RGB565 is not supported by MPEG-1 Program Stream");
    return addVideoFrame(data, len);
  }
  size_t addI420Frame(const uint8_t *data, size_t len) override {
    LOGW("MuxerMPG: raw I420 is not supported by MPEG-1 Program Stream");
    return addVideoFrame(data, len);
  }
 
  size_t addAudioFrame(const uint8_t *data, size_t len) override {
    if (!is_open || !has_audio || data == nullptr || len == 0) return 0;
    size_t written = writeAccessUnit(MPG_AUDIO_STREAM_ID, data, len, audioPts());
    audio_frame_count++;
    return written;
  }
 
  uint32_t videoFrameCount() { return video_frame_count; }
  uint32_t audioFrameCount() { return audio_frame_count; }
 
 protected:
  static const size_t MAX_PES_PAYLOAD = 4096;
  static const uint32_t AUDIO_SAMPLES_PER_FRAME = 1152;
 
  Print *p_out = nullptr;
  MuxerVideoConfig video_cfg;
  AudioInfoFormat audio_info;
  bool has_audio = false;
  bool is_open = false;
  StreamContentType write_stream_type = StreamContentType::Video;
  uint32_t video_frame_count = 0;
  uint32_t audio_frame_count = 0;
  uint64_t last_scr = 0;
  static const uint32_t MUX_RATE = 0x3FFFFF;
 
  uint64_t videoPts() {
    float fps = video_cfg.fps > 0 ? video_cfg.fps : 25.0f;
    return (uint64_t)((double)video_frame_count * MPG_CLOCK_HZ / fps);
  }
  uint64_t audioPts() {
    uint32_t sr = audio_info.sample_rate > 0 ? audio_info.sample_rate : 44100;
    return (uint64_t)((double)audio_frame_count * AUDIO_SAMPLES_PER_FRAME *
                       MPG_CLOCK_HZ / sr);
  }
 
  uint64_t clampScr(uint64_t scr) {
    if (scr < last_scr) scr = last_scr;
    last_scr = scr;
    return scr;
  }
 
  void writePackHeader(uint64_t scr_in) {
    uint64_t scr = clampScr(scr_in);
    static const uint8_t start[4] = {0x00, 0x00, 0x01, MPG_PACK_START_CODE};
    p_out->write(start, 4);
    uint8_t b[8];
    b[0] = (uint8_t)(0x20 | (((scr >> 30) & 0x07) << 1) | 0x01);
    b[1] = (uint8_t)((scr >> 22) & 0xFF);
    b[2] = (uint8_t)((((scr >> 15) & 0x7F) << 1) | 0x01);
    b[3] = (uint8_t)((scr >> 7) & 0xFF);
    b[4] = (uint8_t)(((scr & 0x7F) << 1) | 0x01);
    b[5] = (uint8_t)(0x80 | ((MUX_RATE >> 15) & 0x7F));
    b[6] = (uint8_t)((MUX_RATE >> 7) & 0xFF);
    b[7] = (uint8_t)(((MUX_RATE & 0x7F) << 1) | 0x01);
    p_out->write(b, 8);
  }
 
  void writeStreamEntry(uint8_t id, bool isVideo) {
    // P-STD buffer bound: informational only - 46 (x1024 = ~46KB) for
    // video, 4 (x128 = 512 bytes, ~1 audio frame) for audio
    uint16_t bound = isVideo ? 46 : 4;
    uint8_t b[3];
    b[0] = id;
    b[1] = (uint8_t)(0xC0 | (isVideo ? 0x20 : 0x00) | ((bound >> 8) & 0x1F));
    b[2] = (uint8_t)(bound & 0xFF);
    p_out->write(b, 3);
  }
 
  void writeSystemHeader() {
    int num_streams = 1 + (has_audio ? 1 : 0);
    uint16_t header_length = (uint16_t)(6 + 3 * num_streams);
    static const uint8_t start[4] = {0x00, 0x00, 0x01,
                                      MPG_SYSTEM_HEADER_START_CODE};
    p_out->write(start, 4);
    uint8_t len_b[2] = {(uint8_t)(header_length >> 8),
                         (uint8_t)header_length};
    p_out->write(len_b, 2);
 
    uint16_t audio_bound = has_audio ? 1 : 0;
    uint16_t video_bound = 1;
    uint8_t b[6];
    // marker(1) + mux_rate(22) + marker(1)
    b[0] = (uint8_t)(0x80 | ((MUX_RATE >> 15) & 0x7F));
    b[1] = (uint8_t)((MUX_RATE >> 7) & 0xFF);
    b[2] = (uint8_t)(((MUX_RATE & 0x7F) << 1) | 0x01);
    // audio_bound(6) + fixed_flag(1)=0 + CSPS_flag(1)=0
    b[3] = (uint8_t)((audio_bound & 0x3F) << 2);
    // audio_locked(1)=0 + video_locked(1)=0 + marker(1)=1 + video_bound(5)
    b[4] = (uint8_t)(0x20 | (video_bound & 0x1F));
    b[5] = 0xFF;  // reserved_byte
    p_out->write(b, 6);
 
    writeStreamEntry(MPG_VIDEO_STREAM_ID, true);
    if (has_audio) writeStreamEntry(MPG_AUDIO_STREAM_ID, false);
  }
 
  void writeTimestampField(uint8_t id4, uint64_t ts) {
    uint8_t b[5];
    b[0] = (uint8_t)((id4 << 4) | (((ts >> 30) & 0x07) << 1) | 0x01);
    uint16_t mid = (uint16_t)((((ts >> 15) & 0x7FFF) << 1) | 0x01);
    uint16_t low = (uint16_t)(((ts & 0x7FFF) << 1) | 0x01);
    b[1] = (uint8_t)(mid >> 8);
    b[2] = (uint8_t)mid;
    b[3] = (uint8_t)(low >> 8);
    b[4] = (uint8_t)low;
    p_out->write(b, 5);
  }
 
  size_t writeElementaryChunk(uint8_t stream_id, const uint8_t *data,
                              size_t len, bool withPts, uint64_t pts) {
    writePackHeader(pts);
    uint16_t optional_len = withPts ? 5 : 1;
    uint16_t pes_len = (uint16_t)(len + optional_len);
    uint8_t hdr[6] = {0,
                       0,
                       1,
                       stream_id,
                       (uint8_t)(pes_len >> 8),
                       (uint8_t)pes_len};
    p_out->write(hdr, 6);
    if (withPts) {
      writeTimestampField(0x02, pts);
    } else {
      // avoid Print::write(uint8_t): some Print/Stream implementations
      // (e.g. BaseStream-derived ones) buffer single-byte writes
      // separately from the write(data, len) path below, which would
      // reorder this marker byte after the payload - write it as a
      // one-element array instead so it goes through the same path.
      static const uint8_t no_ts = 0x0F;
      p_out->write(&no_ts, 1);
    }
    return p_out->write(data, len);
  }
 
  size_t writeAccessUnit(uint8_t stream_id, const uint8_t *data, size_t len,
                        uint64_t pts) {
    size_t offset = 0;
    size_t written = 0;
    bool first = true;
    do {
      size_t chunk = len - offset;
      if (chunk > MAX_PES_PAYLOAD) chunk = MAX_PES_PAYLOAD;
      written +=
          writeElementaryChunk(stream_id, data + offset, chunk, first, pts);
      offset += chunk;
      first = false;
    } while (offset < len);
    return written;
  }
};
 
}  // namespace audio_tools