Modified Costas loop; descramble sync data before unmapping

include/modulation/PSKModulator.h

@@ -3,6 +3,7 @@
 
 
 #include <algorithm>
+#include <array>
 #include <cmath>
 #include <complex>
 #include <cstdint>
@@ -10,9 +11,12 @@
 #include <stdexcept>
 #include <vector>
 #include <fftw3.h>
+#include <map>
+#include <tuple>
 
 #include "costasloop.h"
 #include "filters.h"
+#include "Scrambler.h"
 
 static constexpr double CARRIER_FREQ = 1800.0;
 static constexpr size_t SYMBOL_RATE = 2400;
@@ -77,9 +81,9 @@ public:
         return final_signal;
     }
 
-    std::vector<uint8_t> demodulate(const std::vector<int16_t> passband_signal) {
+    std::vector<uint8_t> demodulate(const std::vector<int16_t> passband_signal, size_t& baud_rate, size_t& interleave_setting, bool& is_voice) {
         // Carrier recovery. initialize the Costas loop.
-        CostasLoop costas_loop(CARRIER_FREQ, sample_rate, symbolMap, 5.0);
+        CostasLoop costas_loop(CARRIER_FREQ, sample_rate, symbolMap, 5.0, 0.05, 0.01);
 
         // Convert passband signal to doubles.
         std::vector<double> normalized_passband(passband_signal.size());
@@ -89,23 +93,30 @@ public:
 
         // Downmix passband to baseband
         std::vector<std::complex<double>> baseband_IQ = costas_loop.process(normalized_passband);
+        std::vector<uint8_t> detected_symbols;
 
         // Phase detection and symbol formation
-        std::vector<uint8_t> baseband_symbols;
         size_t samples_per_symbol = sample_rate / SYMBOL_RATE;
+        bool sync_found = false;
+        size_t sync_segments_detected;
 
+        size_t window_size = 32*15;
+        
         for (size_t i = 0; i < baseband_IQ.size(); i += samples_per_symbol) {
             std::complex<double> symbol_avg(0.0, 0.0);
-            for (size_t j = 0; j < samples_per_symbol; ++j) {
+            for (size_t j = 0; j < samples_per_symbol; j++) {
                 symbol_avg += baseband_IQ[i + j];
             }
             symbol_avg /= static_cast<double>(samples_per_symbol);
 
-            // Detect symbol from averaged signal
-            baseband_symbols.emplace_back(phase_detector.getSymbol(symbol_avg));
+            uint8_t detected_symbol = phase_detector.getSymbol(symbol_avg);
+            detected_symbols.push_back(detected_symbol);
+        }
+
+        if (processSyncSegments(detected_symbols, baud_rate, interleave_setting, is_voice)) {
+            return processDataSymbols(detected_symbols);
         }
 
-        return baseband_symbols;
     }
 
 private:
@@ -137,6 +148,220 @@ private:
             {gain * std::cos(2.0*M_PI*(7.0/8.0)), gain * std::sin(2.0*M_PI*(7.0/8.0))}  // 7 (111) corresponds to I = cos(315), Q = sin(315)
         };
     }
+
+    uint8_t extractBestTribit(const std::vector<uint8_t>& stream, const size_t start, const size_t window_size) const {
+        if (start + window_size > stream.size()) {
+            throw std::out_of_range("Window size exceeds symbol stream size.");
+        }
+
+        Scrambler scrambler;
+        std::vector<uint8_t> symbol(stream.begin() + start, stream.begin() + start + window_size);
+        std::vector<uint8_t> descrambled_symbol = scrambler.scrambleSyncPreamble(symbol);
+
+        const size_t split_len = window_size / 4;
+        std::array<uint8_t, 8> tribit_counts = {0};  // Counts for each channel symbol (000 to 111)
+
+        // Loop through each split segment (4 segments)
+        for (size_t i = 0; i < 4; ++i) {
+            // Extract the range for this split
+            size_t segment_start = start + i * split_len;
+            size_t segment_end = segment_start + split_len;
+
+            // Compare this segment to the predefined patterns from the table and map to a channel symbol
+            uint8_t tribit_value = mapSegmentToChannelSymbol(descrambled_symbol, segment_start, segment_end);
+
+            // Increment the corresponding channel symbol count
+            tribit_counts[tribit_value]++;
+        }
+
+        // Find the channel symbol with the highest count (majority vote)
+        uint8_t best_symbol = std::distance(tribit_counts.begin(), std::max_element(tribit_counts.begin(), tribit_counts.end()));
+
+        return best_symbol;
+    }
+
+    // Function to map a segment of the stream back to a channel symbol based on the repeating patterns
+    uint8_t mapSegmentToChannelSymbol(const std::vector<uint8_t>& segment, size_t start, size_t end) const {
+        std::vector<uint8_t> extracted_pattern(segment.begin() + start, segment.begin() + end);
+
+        // Compare the extracted pattern with known patterns from the table
+        if (matchesPattern(extracted_pattern, {0, 0, 0, 0, 0, 0, 0, 0})) return 0b000;
+        if (matchesPattern(extracted_pattern, {0, 4, 0, 4, 0, 4, 0, 4})) return 0b001;
+        if (matchesPattern(extracted_pattern, {0, 0, 4, 4, 0, 0, 4, 4})) return 0b010;
+        if (matchesPattern(extracted_pattern, {0, 4, 4, 0, 0, 4, 4, 0})) return 0b011;
+        if (matchesPattern(extracted_pattern, {0, 0, 0, 0, 4, 4, 4, 4})) return 0b100;
+        if (matchesPattern(extracted_pattern, {0, 4, 0, 4, 4, 0, 4, 0})) return 0b101;
+        if (matchesPattern(extracted_pattern, {0, 0, 4, 4, 4, 4, 0, 0})) return 0b110;
+        if (matchesPattern(extracted_pattern, {0, 4, 4, 0, 4, 0, 0, 4})) return 0b111;
+
+        throw std::invalid_argument("Invalid segment pattern");
+    }
+
+    // Helper function to compare two patterns
+    bool matchesPattern(const std::vector<uint8_t>& segment, const std::vector<uint8_t>& pattern) const {
+        return std::equal(segment.begin(), segment.end(), pattern.begin());
+    }
+
+    bool configureModem(uint8_t D1, uint8_t D2, size_t& baud_rate, size_t& interleave_setting, bool& is_voice) {
+        // Predefine all the valid combinations in a lookup map
+        static const std::map<std::pair<uint8_t, uint8_t>, std::tuple<size_t, size_t, bool>> modemConfig = {
+            {{7, 6}, {4800, 1, false}},  // 4800 bps
+            {{7, 7}, {2400, 1, true}},   // 2400 bps, voice
+            {{6, 4}, {2400, 1, false}},  // 2400 bps, data
+            {{6, 5}, {1200, 1, false}},  // 1200 bps
+            {{6, 6}, {600,  1, false}},  // 600 bps
+            {{6, 7}, {300,  1, false}},  // 300 bps
+            {{7, 4}, {150,  1, false}},  // 150 bps
+            {{7, 5}, {75,   1, false}},  // 75 bps
+            {{4, 4}, {2400, 2, false}},  // 2400 bps, long interleave
+            {{4, 5}, {1200, 2, false}},  // 1200 bps, long interleave
+            {{4, 6}, {600,  2, false}},  // 600 bps, long interleave
+            {{4, 7}, {300,  2, false}},  // 300 bps, long interleave
+            {{5, 4}, {150,  2, false}},  // 150 bps, long interleave
+            {{5, 5}, {75,   2, false}},  // 75 bps, long interleave
+        };
+
+        // Use D1 and D2 to look up the correct configuration
+        auto it = modemConfig.find({D1, D2});
+        if (it != modemConfig.end()) {
+            // Set the parameters if found
+            std::tie(baud_rate, interleave_setting, is_voice) = it->second;
+            return true;
+        } else {
+            return false;
+        }
+    }
+
+    uint8_t calculateSegmentCount(const uint8_t C1, const uint8_t C2, const uint8_t C3) {
+        uint8_t extracted_C1 = C1 & 0b11;
+        uint8_t extracted_C2 = C2 & 0b11;
+        uint8_t extracted_C3 = C3 & 0b11;
+        
+        uint8_t segment_count = (extracted_C1 << 4) | (extracted_C2 << 2) | extracted_C3;
+        return segment_count;
+    }
+
+    bool processSegment(const std::vector<uint8_t>& detected_symbols, size_t& start, size_t symbol_size, size_t& segment_count, uint8_t& D1, uint8_t& D2) {
+        size_t sync_pattern_length = 9;
+
+        if (start + symbol_size * sync_pattern_length > detected_symbols.size()) {
+            start = detected_symbols.size();
+            return false;
+        }
+
+        std::vector<uint8_t> window(detected_symbols.begin() + start, detected_symbols.begin() + start + sync_pattern_length * symbol_size);
+        std::vector<uint8_t> extracted_window;
+        for (size_t i = 0; i < sync_pattern_length; i++) {
+            extracted_window.push_back(extractBestTribit(window, i * symbol_size, symbol_size));
+        }
+
+        if (!matchesPattern(extracted_window, {0, 1, 3, 0, 1, 3, 1, 2, 0})) {
+            start += symbol_size;
+            return false;
+        }
+
+        start += sync_pattern_length * symbol_size;
+        size_t D1_index = start + symbol_size;
+        size_t D2_index = D1_index + symbol_size;
+
+        if (D2_index + symbol_size > detected_symbols.size()) {
+            start = detected_symbols.size();
+            return false;
+        }
+
+        D1 = extractBestTribit(detected_symbols, D1_index, symbol_size);
+        D2 = extractBestTribit(detected_symbols, D2_index, symbol_size);
+
+        // Process the count symbols (C1, C2, C3)
+        size_t C1_index = D2_index + symbol_size;
+        size_t C2_index = C1_index + symbol_size;
+        size_t C3_index = C2_index + symbol_size;
+
+        if (C3_index + symbol_size > detected_symbols.size()) {
+            start = detected_symbols.size();
+            return false;
+        }
+
+        uint8_t C1 = extractBestTribit(detected_symbols, C1_index, symbol_size);
+        uint8_t C2 = extractBestTribit(detected_symbols, C2_index, symbol_size);
+        uint8_t C3 = extractBestTribit(detected_symbols, C3_index, symbol_size);
+
+        segment_count = calculateSegmentCount(C1, C2, C3);
+
+        // Check for the constant zero pattern
+        size_t constant_zero_index = C3_index + symbol_size;
+
+        if (constant_zero_index + symbol_size > detected_symbols.size()) {
+            start = detected_symbols.size();
+            return false;
+        }
+        uint8_t constant_zero = extractBestTribit(detected_symbols, constant_zero_index, symbol_size);
+
+        if (constant_zero != 0) {
+            start = constant_zero_index + symbol_size;
+            return false; // Failed zero check, resync
+        }
+
+        // Successfully processed the segment
+        start = constant_zero_index + symbol_size; // Move start to next segment
+        return true;
+    }
+
+    bool processSyncSegments(const std::vector<uint8_t>& detected_symbols, size_t& baud_rate, size_t& interleave_setting, bool& is_voice) {
+        size_t symbol_size = 32;
+        size_t start = 0;
+        size_t segment_count = 0;
+        std::map<std::pair<uint8_t, uint8_t>, int> vote_map;
+        const int short_interleave_threshold = 2;
+        const int long_interleave_threshold = 5;
+
+        // Attempt to detect interleave setting dynamically
+        bool interleave_detected = false;
+        int current_threshold = short_interleave_threshold;  // Start by assuming short interleave
+
+        while (start + symbol_size * 15 < detected_symbols.size()) {
+            uint8_t D1 = 0, D2 = 0;
+            if (processSegment(detected_symbols, start, symbol_size, segment_count, D1, D2)) {
+                vote_map[{D1, D2}]++;
+
+                // Check if we have enough votes to make a decision based on current interleave assumption
+                if (vote_map.size() >= current_threshold) {
+                    auto majority_vote = std::max_element(vote_map.begin(), vote_map.end(), [](const auto& a, const auto& b) { return a.second < b.second; });
+
+                    if (configureModem(majority_vote->first.first, majority_vote->first.second, baud_rate, interleave_setting, is_voice)) {
+                        interleave_detected = true;
+                        break;  // Successfully configured modem, exit loop
+                    } else {
+                        // If configuration fails, retry with the other interleave type
+                        if (current_threshold == short_interleave_threshold) {
+                            current_threshold = long_interleave_threshold;  // Switch to long interleave
+                            vote_map.clear();  // Clear the vote map and start fresh
+                            start = 0;  // Restart segment processing
+                        } else {
+                            continue;  // Both short and long interleave attempts failed, signal is not usable
+                        }
+                    }
+                }
+
+                if (segment_count > 0) {
+                    while (segment_count > 0 && start < detected_symbols.size()) {
+                        uint8_t dummy_D1, dummy_D2;
+                        if (!processSegment(detected_symbols, start, symbol_size, segment_count, dummy_D1, dummy_D2)) {
+                            continue;
+                        }
+                    }
+                }
+            } else {
+                start += symbol_size;  // Move to the next segment
+            }
+        }
+
+        return interleave_detected;
+    }
+
+    std::vector<uint8_t> processDataSymbols(const std::vector<uint8_t>& detected_symbols) {
+        return std::vector<uint8_t>();
+    }
 };
 
 #endif