Rework frequency information representation

2 files changed, 227 insertions, 161 deletions

diff --git a/src/fig/FIG0_21.cpp b/src/fig/FIG0_21.cpp
index 8f4f95b..5855fb1 100644
--- a/src/fig/FIG0_21.cpp
+++ b/src/fig/FIG0_21.cpp
@@ -83,6 +83,34 @@ FIG0_21::FIG0_21(FIGRuntimeInformation *rti) :
 {
 }
 
+static size_t get_num_frequencies(
+        std::vector<FrequencyInformation>::iterator fi)
+{
+    switch (fi->rm) {
+        case RangeModulation::dab_ensemble:
+            if (fi->fi_dab.frequencies.empty()) {
+                throw std::logic_error("Empty DAB FI");
+            }
+            return fi->fi_dab.frequencies.size();
+        case RangeModulation::fm_with_rds:
+            if (fi->fi_fm.frequencies.empty()) {
+                throw std::logic_error("Empty FM FI");
+            }
+            return fi->fi_fm.frequencies.size();
+        case RangeModulation::amss:
+            if (fi->fi_amss.frequencies.empty()) {
+                throw std::logic_error("Empty AMSS FI");
+            }
+            return fi->fi_amss.frequencies.size();
+        case RangeModulation::drm:
+            if (fi->fi_drm.frequencies.empty()) {
+                throw std::logic_error("Empty DRM FI");
+            }
+            return fi->fi_drm.frequencies.size();
+    }
+    throw logic_error("Unhandled get_num_frequencies");
+}
+
 FillStatus FIG0_21::fill(uint8_t *buf, size_t max_size)
 {
 #define FIG0_21_TRACE discard
@@ -90,75 +118,77 @@ FillStatus FIG0_21::fill(uint8_t *buf, size_t max_size)
     size_t remaining = max_size;
     auto ensemble = m_rti->ensemble;
 
-    etiLog.level(FIG0_21_TRACE) << "FIG0_21::fill init " <<
-        (m_initialised ? 1 : 0) << " ********************************";
-
     if (not m_initialised) {
         freqInfoFIG0_21 = ensemble->frequency_information.begin();
+        fi_frequency_index = 0;
 
         if (freqInfoFIG0_21 != ensemble->frequency_information.end()) {
-            m_last_oe = (*freqInfoFIG0_21)->other_ensemble;
+            m_last_oe = freqInfoFIG0_21->other_ensemble;
         }
         m_initialised = true;
     }
 
     FIGtype0* fig0 = nullptr;
 
-    for (; freqInfoFIG0_21 != ensemble->frequency_information.end();
-            ++freqInfoFIG0_21) {
+    auto advance_loop = [&](void){
+            if (fi_frequency_index == get_num_frequencies(freqInfoFIG0_21)) {
+                ++freqInfoFIG0_21;
+                fi_frequency_index = 0;
+            }
+        };
 
+    for (; freqInfoFIG0_21 != ensemble->frequency_information.end();
+            advance_loop()) {
+        /* For better usage of FIC capacity, we want to transmit
+         * frequency lists with
+         * 2 DAB frequencies,
+         * 7 FM frequencies,
+         * 3 AM/DRM frequencies
+         */
+
+        // Check we have space for one frequency
         size_t required_fi_size = 2; // RegionId + length of FI list
-        for (const auto& fle : (*freqInfoFIG0_21)->frequency_information) {
-            size_t list_entry_size = sizeof(struct FIGtype0_21_fi_list_header);
-            switch (fle.rm) {
-                case RangeModulation::dab_ensemble:
-                    list_entry_size += fle.fi_dab.frequencies.size() * 3;
-                    break;
-                case RangeModulation::fm_with_rds:
-                    list_entry_size += fle.fi_fm.frequencies.size() * 1;
-                    break;
-                case RangeModulation::amss:
-                    list_entry_size += 1; // Id field 2
-                    list_entry_size += fle.fi_amss.frequencies.size() * 2;
-                    break;
-                case RangeModulation::drm:
-                    list_entry_size += 1; // Id field 2
-                    list_entry_size += fle.fi_drm.frequencies.size() * 2;
-                    break;
-            }
-            required_fi_size += list_entry_size;
+        size_t list_entry_size = sizeof(struct FIGtype0_21_fi_list_header);
+        switch (freqInfoFIG0_21->rm) {
+            case RangeModulation::dab_ensemble:
+                list_entry_size += 3;
+                break;
+            case RangeModulation::fm_with_rds:
+                list_entry_size += 1;
+                break;
+            case RangeModulation::amss:
+                list_entry_size += 1; // Id field 2
+                list_entry_size += 2;
+                break;
+            case RangeModulation::drm:
+                list_entry_size += 1; // Id field 2
+                list_entry_size += 2;
+                break;
         }
+        required_fi_size += list_entry_size;
 
-        const size_t required_size = sizeof(struct FIGtype0_21_header) + required_fi_size;
+        const size_t required_size =
+            sizeof(struct FIGtype0_21_header) + required_fi_size;
 
-        etiLog.level(FIG0_21_TRACE) << "FIG0_21::loop " << (*freqInfoFIG0_21)->uid;
-
-        if (m_last_oe != (*freqInfoFIG0_21)->other_ensemble) {
+        if (m_last_oe != freqInfoFIG0_21->other_ensemble) {
             // Trigger resend of FIG0 when OE changes
             fig0 = nullptr;
-            m_last_oe = (*freqInfoFIG0_21)->other_ensemble;
+            m_last_oe = freqInfoFIG0_21->other_ensemble;
             etiLog.level(FIG0_21_TRACE) << "FIG0_21::switch OE to " <<
-                (*freqInfoFIG0_21)->other_ensemble;
+                freqInfoFIG0_21->other_ensemble;
         }
 
         if (fig0 == nullptr) {
             if (remaining < 2 + required_size) {
-                etiLog.level(FIG0_21_TRACE) << "FIG0_21::no space for fig0: remain " <<
-                    remaining << " require " << 2 + required_size;
                 break;
             }
-            else {
-                etiLog.level(FIG0_21_TRACE) << "FIG0_21::inserting fig0: remain " <<
-                    remaining << " require " << 2 + required_size;
-            }
             fig0 = (FIGtype0*)buf;
             fig0->FIGtypeNumber = 0;
             fig0->Length = 1;
 
             // Database start or continuation flag, EN 300 401 Clause 5.2.2.1 part b)
-            fig0->CN =
-                (freqInfoFIG0_21 == ensemble->frequency_information.begin() ? 0 : 1);
-            fig0->OE = (*freqInfoFIG0_21)->other_ensemble ? 1 : 0;
+            fig0->CN = (fi_frequency_index == 0 ? 0 : 1);
+            fig0->OE = freqInfoFIG0_21->other_ensemble ? 1 : 0;
             fig0->PD = false;
             fig0->Extension = 21;
 
@@ -166,149 +196,185 @@ FillStatus FIG0_21::fill(uint8_t *buf, size_t max_size)
             remaining -= 2;
         }
         else if (remaining < required_size) {
-            etiLog.level(FIG0_21_TRACE) << "FIG0_21::no space";
             break;
         }
 
+        etiLog.level(FIG0_21_TRACE) << "FIG0_21::loop " << freqInfoFIG0_21->uid << " " <<
+            std::distance(ensemble->frequency_information.begin(), freqInfoFIG0_21) <<
+            " freq entry " << fi_frequency_index;
+
         auto *fig0_21_header = (FIGtype0_21_header*)buf;
         fig0_21_header->rfaHigh = 0;
         fig0_21_header->rfaLow = 0;
-        fig0_21_header->length_fi = sizeof(struct FIGtype0_21_fi_list_header);
-
-        fig0->Length += sizeof(struct FIGtype0_21_header);
-        buf += sizeof(struct FIGtype0_21_header);
-        remaining -= sizeof(struct FIGtype0_21_header);
-
-        for (const auto& fle : (*freqInfoFIG0_21)->frequency_information) {
-            switch (fle.rm) {
-                case RangeModulation::dab_ensemble:
-                    fig0_21_header->length_fi += 3 * fle.fi_dab.frequencies.size();
-                    etiLog.level(FIG0_21_TRACE) << "FIG0_21::fle hdr DAB " <<
-                        fle.fi_dab.frequencies.size();
-                    break;
-                case RangeModulation::fm_with_rds:
-                    fig0_21_header->length_fi += fle.fi_fm.frequencies.size();
-                    etiLog.level(FIG0_21_TRACE) << "FIG0_21::fle hdr FM " <<
-                        fle.fi_fm.frequencies.size();
-                    break;
-                case RangeModulation::drm:
-                    fig0_21_header->length_fi += 1; // ID field 2
-                    fig0_21_header->length_fi += 2*fle.fi_drm.frequencies.size();
-                    etiLog.level(FIG0_21_TRACE) << "FIG0_21::fle hdr DRM " <<
-                        fle.fi_drm.frequencies.size();
-                    break;
-                case RangeModulation::amss:
-                    fig0_21_header->length_fi += 1; // ID field 2
-                    fig0_21_header->length_fi += 2*fle.fi_amss.frequencies.size();
-                    etiLog.level(FIG0_21_TRACE) << "FIG0_21::fle hdr AMSS " <<
-                        fle.fi_amss.frequencies.size();
-                    break;
-            }
-
-            auto *fi_list_header = (FIGtype0_21_fi_list_header*)buf;
-            fig0->Length += sizeof(struct FIGtype0_21_fi_list_header);
-            buf += sizeof(struct FIGtype0_21_fi_list_header);
-            remaining -= sizeof(struct FIGtype0_21_fi_list_header);
-
-            fi_list_header->continuity = fle.continuity;
-            fi_list_header->length_freq_list = 0;
-            fi_list_header->range_modulation = static_cast<uint8_t>(fle.rm);
-
-            switch (fle.rm) {
-                case RangeModulation::dab_ensemble:
-                    fi_list_header->setId(fle.fi_dab.eid);
-                    assert(fle.fi_dab.frequencies.size() < 8);
-
-                    for (const auto& freq : fle.fi_dab.frequencies) {
-                        auto *field = (FIGtype0_21_fi_dab_entry*)buf;
-                        field->control_field = static_cast<uint8_t>(freq.control_field);
-                        field->setFreq(static_cast<uint32_t>(
-                                    freq.frequency * 1000.0f / 16.0f));
-
-                        fi_list_header->addToLength(3);
-                        fig0->Length += 3;
-                        buf += 3;
-                        remaining -= 3;
-                        etiLog.level(FIG0_21_TRACE) << "FIG0_21::freq " <<
-                            freq.frequency << " rem " << remaining;
+        fig0_21_header->length_fi = sizeof(FIGtype0_21_fi_list_header);
+
+        fig0->Length += sizeof(FIGtype0_21_header);
+        buf          += sizeof(FIGtype0_21_header);
+        remaining    -= sizeof(FIGtype0_21_header);
+
+        auto *fi_list_header = (FIGtype0_21_fi_list_header*)buf;
+        fig0->Length += sizeof(FIGtype0_21_fi_list_header);
+        buf          += sizeof(FIGtype0_21_fi_list_header);
+        remaining    -= sizeof(FIGtype0_21_fi_list_header);
+
+        fi_list_header->continuity = freqInfoFIG0_21->continuity;
+        fi_list_header->length_freq_list = 0;
+        fi_list_header->range_modulation = static_cast<uint8_t>(freqInfoFIG0_21->rm);
+
+        bool continue_loop = true;
+
+        switch (freqInfoFIG0_21->rm) {
+            case RangeModulation::dab_ensemble:
+                fi_list_header->setId(freqInfoFIG0_21->fi_dab.eid);
+
+                for (size_t num_inserted = 0, i = fi_frequency_index;
+                        num_inserted < 2 and
+                        i < freqInfoFIG0_21->fi_dab.frequencies.size();
+                        num_inserted++, i++) {
+                    if (remaining < 3) {
+                        continue_loop = false;
+                        break;
                     }
-                    break;
-                case RangeModulation::fm_with_rds:
-                    fi_list_header->setId(fle.fi_fm.pi_code);
-
-                    for (const auto& freq : fle.fi_fm.frequencies) {
-                        // RealFreq = 87.5 MHz + (F * 100kHz)
-                        // => F = (RealFreq - 87.5 MHz) / 100kHz
-                        // Do the whole calculation in kHz:
-                        *buf = (freq * 1000.0f - 87500.0f) / 100.0f;
-
-                        fi_list_header->addToLength(1);
-                        fig0->Length += 1;
-                        buf += 1;
-                        remaining -= 1;
-                        etiLog.level(FIG0_21_TRACE) << "FIG0_21::freq " <<
-                            freq;
+
+                    const auto& freq = freqInfoFIG0_21->fi_dab.frequencies.at(i);
+
+                    auto *field = (FIGtype0_21_fi_dab_entry*)buf;
+                    field->control_field = static_cast<uint8_t>(freq.control_field);
+                    field->setFreq(static_cast<uint32_t>(
+                                freq.frequency * 1000.0f / 16.0f));
+
+                    fig0_21_header->length_fi += 3;
+                    fi_list_header->addToLength(3);
+                    fig0->Length += 3;
+                    buf += 3;
+                    remaining -= 3;
+                    etiLog.level(FIG0_21_TRACE) << "FIG0_21::freq " <<
+                        fi_frequency_index << " " << freq.frequency << " rem " << remaining;
+                    fi_frequency_index++;
+                }
+                break;
+            case RangeModulation::fm_with_rds:
+                fi_list_header->setId(freqInfoFIG0_21->fi_fm.pi_code);
+
+                for (size_t num_inserted = 0, i = fi_frequency_index;
+                        num_inserted < 7 and
+                        i < freqInfoFIG0_21->fi_fm.frequencies.size();
+                        num_inserted++, i++) {
+                    if (remaining < 1) {
+                        continue_loop = false;
+                        break;
                     }
-                    break;
-                case RangeModulation::drm:
-                    fi_list_header->setId((fle.fi_drm.drm_service_id) & 0xFFFF);
 
-                    // Id field 2
-                    *buf = (fle.fi_drm.drm_service_id >> 16) & 0xFF;
+                    const auto& freq = freqInfoFIG0_21->fi_fm.frequencies.at(i);
+                    // RealFreq = 87.5 MHz + (F * 100kHz)
+                    // => F = (RealFreq - 87.5 MHz) / 100kHz
+                    // Do the whole calculation in kHz:
+                    *buf = (freq * 1000.0f - 87500.0f) / 100.0f;
 
+                    fig0_21_header->length_fi += 1;
                     fi_list_header->addToLength(1);
                     fig0->Length += 1;
                     buf += 1;
                     remaining -= 1;
-
-                    for (const auto& freq : fle.fi_drm.frequencies) {
-                        uint16_t freq_field = static_cast<uint16_t>(freq * 1000.0f);
-                        buf[0] = freq_field >> 8;
-                        buf[1] = freq_field & 0xFF;
-
-                        fi_list_header->addToLength(2);
-                        fig0->Length += 2;
-                        buf += 2;
-                        remaining -= 2;
-                        etiLog.level(FIG0_21_TRACE) << "FIG0_21::freq_field " <<
-                            freq_field;
+                    fi_frequency_index++;
+                }
+                break;
+            case RangeModulation::drm:
+                fi_list_header->setId((freqInfoFIG0_21->fi_drm.drm_service_id) & 0xFFFF);
+
+                if (remaining < 3) {
+                    throw logic_error("Incorrect DRM FI size calculation");
+                }
+
+                // Id field 2
+                *buf = (freqInfoFIG0_21->fi_drm.drm_service_id >> 16) & 0xFF;
+                fig0_21_header->length_fi += 1;
+
+                fi_list_header->addToLength(1);
+                fig0->Length += 1;
+                buf += 1;
+                remaining -= 1;
+
+                for (size_t num_inserted = 0, i = fi_frequency_index;
+                        num_inserted < 3 and
+                        i < freqInfoFIG0_21->fi_drm.frequencies.size();
+                        num_inserted++, i++) {
+                    if (remaining < 2) {
+                        continue_loop = false;
+                        break;
                     }
-                    break;
-                case RangeModulation::amss:
-                    fi_list_header->setId((fle.fi_amss.amss_service_id) & 0xFFFF);
 
-                    // Id field 2
-                    *buf = (fle.fi_amss.amss_service_id >> 16) & 0xFF;
+                    const auto& freq = freqInfoFIG0_21->fi_drm.frequencies.at(i);
+                    uint16_t freq_field = static_cast<uint16_t>(freq * 1000.0f);
+                    buf[0] = freq_field >> 8;
+                    buf[1] = freq_field & 0xFF;
+
+                    fi_list_header->addToLength(2);
+                    fig0_21_header->length_fi += 2;
+                    fig0->Length += 2;
+                    buf += 2;
+                    remaining -= 2;
+                    fi_frequency_index++;
+                }
+                break;
+            case RangeModulation::amss:
+                fi_list_header->setId((freqInfoFIG0_21->fi_amss.amss_service_id) & 0xFFFF);
+
+                if (remaining < 3) {
+                    throw logic_error("Incorrect AMSS FI size calculation");
+                }
+
+                // Id field 2
+                *buf = (freqInfoFIG0_21->fi_amss.amss_service_id >> 16) & 0xFF;
+                fig0_21_header->length_fi += 1;
+
+                fi_list_header->addToLength(1);
+                fig0->Length += 1;
+                buf += 1;
+                remaining -= 1;
+
+                for (size_t num_inserted = 0, i = fi_frequency_index;
+                        num_inserted < 3 and
+                        i < freqInfoFIG0_21->fi_amss.frequencies.size();
+                        num_inserted++, i++) {
+                    if (remaining < 2) {
+                        continue_loop = false;
+                        break;
+                    }
 
-                    fi_list_header->addToLength(1);
-                    fig0->Length += 1;
-                    buf += 1;
-                    remaining -= 1;
+                    const auto& freq = freqInfoFIG0_21->fi_amss.frequencies.at(i);
+                    uint16_t freq_field = static_cast<uint16_t>(freq * 1000.0f);
+                    buf[0] = freq_field >> 8;
+                    buf[1] = freq_field & 0xFF;
+
+                    fi_list_header->addToLength(2);
+                    fig0_21_header->length_fi += 2;
+                    fig0->Length += 2;
+                    buf += 2;
+                    remaining -= 2;
+                    fi_frequency_index++;
+                }
+                break;
+        } // switch (RM)
 
-                    for (const auto& freq : fle.fi_amss.frequencies) {
-                        uint16_t freq_field = static_cast<uint16_t>(freq * 1000.0f);
-                        buf[0] = freq_field >> 8;
-                        buf[1] = freq_field & 0xFF;
-
-                        fi_list_header->addToLength(2);
-                        fig0->Length += 2;
-                        buf += 2;
-                        remaining -= 2;
-                        etiLog.level(FIG0_21_TRACE) << "FIG0_21::freq_field " <<
-                            freq_field;
-                    }
-                    break;
-            } // switch (RM)
-            etiLog.level(FIG0_21_TRACE) << "FIG0_21::fle end len=" <<
-                static_cast<size_t>(fig0->Length) << " rem=" << remaining;
-        } // for over fle
-        etiLog.level(FIG0_21_TRACE) << "FIG0_21::next FI "
+        etiLog.level(FIG0_21_TRACE) << "FIG0_21::end " <<
+            (continue_loop ? "same " : "next ") << " len=" <<
+            static_cast<size_t>(fig0->Length) << " rem=" << remaining <<
             " ********************************";
+
+        if (not continue_loop) {
+            // There is no more space left. The freqInfoFIG0_21 iterator
+            // and fi_frequency_index save the position how to continue
+            // next time we are called.
+            break;
+        }
     } // for over FI
 
     if (freqInfoFIG0_21 == ensemble->frequency_information.end()) {
         fs.complete_fig_transmitted = true;
+
         freqInfoFIG0_21 = ensemble->frequency_information.begin();
+        fi_frequency_index = 0;
     }
 
     fs.num_bytes_written = max_size - remaining;
diff --git a/src/fig/FIG0_21.h b/src/fig/FIG0_21.h
index 018727a..0aabf3b 100644
--- a/src/fig/FIG0_21.h
+++ b/src/fig/FIG0_21.h
@@ -49,8 +49,8 @@ class FIG0_21 : public IFIG
         bool m_initialised = false;
         bool m_last_oe = false;
 
-        std::vector<std::shared_ptr<FrequencyInformation> >::iterator
-            freqInfoFIG0_21;
+        std::vector<FrequencyInformation>::iterator freqInfoFIG0_21;
+        size_t fi_frequency_index = 0;
 };
 
 }