Moving towards more OOP

decode_ft8.cpp

@@ -15,6 +15,9 @@
 
 #define LOG_LEVEL   LOG_INFO
 
+const int kFreq_osr = 2;
+const int kTime_osr = 2;
+
 const int kMin_score = 40;		// Minimum sync score threshold for candidates
 const int kMax_candidates = 120;
 const int kLDPC_iterations = 25;
@@ -22,11 +25,6 @@ const int kLDPC_iterations = 25;
 const int kMax_decoded_messages = 50;
 const int kMax_message_length = 25;
 
-const int kFreq_osr = 2;
-const int kTime_osr = 2;
-
-const float kFSK_dev = 6.25f;    // tone deviation in Hz and symbol rate
-
 void usage() {
     fprintf(stderr, "Decode a 15-second WAV file.\n");
 }
@@ -60,10 +58,63 @@ float blackman_i(int i, int N) {
     return a0 - a1*x1 + a2*x2;
 }
 
-static float max2(float a, float b) {
+class Monitor : public ft8::Monitor1Base {
-    return (a >= b) ? a : b;
+public:
+    Monitor(float sample_rate);
+protected:
+    virtual void fft_forward(const float *in, std::complex<float> *out) override;
+private:
+    kiss_fftr_cfg fft_cfg;
+};
+
+Monitor::Monitor(float sample_rate) : Monitor1Base(sample_rate)
+{
+    window_fn = new float[nfft];   // [nfft]
+    fft_frame = new float[nfft];   // [nfft]
+    last_frame = new float[nfft * 3/4];  // [nfft * 3/4]
+    freqdata = new std::complex<float>[nfft/2 + 1]; // [nfft/2 + 1]
+
+    fft_cfg = kiss_fftr_alloc(nfft, 0, NULL, NULL);
 }
 
+void Monitor::fft_forward(const float *in, std::complex<float> *out) {
+    kiss_fftr(fft_cfg, 
+        reinterpret_cast<const kiss_fft_scalar *>(in), 
+        reinterpret_cast<kiss_fft_cpx *>(out));
+}
+
+// #include <complex>
+
+// class FFT_r2c {
+// public:
+//     // [N] real --> [N/2 + 1] complex
+//     virtual void forward(const float *in, std::complex<float> *out) = 0;
+// };
+
+// class KissFFT_r2c : public FFT_r2c {
+// public:
+//     KissFFT_r2c(int N) {
+//         LOG(LOG_INFO, "N_FFT = %d\n", N);
+//         // size_t  fft_work_size;
+//         // kiss_fftr_alloc(N, 0, 0, &fft_work_size);
+//         // LOG(LOG_INFO, "FFT work area = %lu\n", fft_work_size);
+//         // fft_work = malloc(fft_work_size);
+//         // fft_cfg = kiss_fftr_alloc(N, 0, fft_work, &fft_work_size);
+//         fft_cfg = kiss_fftr_alloc(N, 0, NULL, NULL);
+//     }
+//     ~KissFFT_r2c() {
+//         // free(fft_work);
+//         kiss_fftr_free(fft_cfg);
+//     }
+//     virtual void forward(const float *in, std::complex<float> *out) override {
+//         kiss_fftr(fft_cfg, in, reinterpret_cast<kiss_fft_cpx *>(out));
+//     }
+
+// private:
+//     void        *fft_work;
+//     kiss_fftr_cfg fft_cfg;
+// };
+
 // Compute FFT magnitudes (log power) for each timeslot in the signal
 void extract_power(const float signal[], ft8::MagArray * power) {
     const int block_size = 2 * power->num_bins; // Average over 2 bins per FSK tone
@@ -106,17 +157,16 @@ void extract_power(const float signal[], ft8::MagArray * power) {
             // Compute log magnitude in decibels
             for (int j = 0; j < nfft/2 + 1; ++j) {
                 float mag2 = (freqdata[j].i * freqdata[j].i + freqdata[j].r * freqdata[j].r);
-                mag_db[j] = 10.0f * log10f(1E-10f + mag2 * fft_norm * fft_norm);
+                mag_db[j] = 10.0f * log10f(1E-12f + mag2 * fft_norm * fft_norm);
             }
 
             // Loop over two possible frequency bin offsets (for averaging)
             for (int freq_sub = 0; freq_sub < power->freq_osr; ++freq_sub) {                
                 for (int j = 0; j < power->num_bins; ++j) {
-                    float db1 = mag_db[j * power->freq_osr + freq_sub];
+                    //float db1 = mag_db[j * power->freq_osr + freq_sub];
                     //float db2 = mag_db[j * 2 + freq_sub + 1];
                     //float db = (db1 + db2) / 2;
-                    float db = db1;
+                    float db = mag_db[j * power->freq_osr + freq_sub];
-                    //float db = sqrtf(db1 * db2);
 
                     // Scale decibels to unsigned 8-bit range and clamp the value
                     int scaled = (int)(2 * (db + 120));
@@ -180,7 +230,7 @@ int main(int argc, char **argv) {
     normalize_signal(signal, num_samples);
 
     // Compute DSP parameters that depend on the sample rate
-    const int num_bins = (int)(sample_rate / (2 * kFSK_dev));
+    const int num_bins = (int)(sample_rate / (2 * ft8::FSK_dev));
     const int block_size = 2 * num_bins;
     const int subblock_size = block_size / kTime_osr;
     const int nfft = block_size * kFreq_osr;
@@ -212,8 +262,8 @@ int main(int argc, char **argv) {
         ft8::Candidate &cand = candidate_list[idx];
         if (cand.score < kMin_score) continue;
 
-        float freq_hz  = (cand.freq_offset + (float)cand.freq_sub / kFreq_osr) * kFSK_dev;
+        float freq_hz  = (cand.freq_offset + (float)cand.freq_sub / kFreq_osr) * ft8::FSK_dev;
-        float time_sec = (cand.time_offset + (float)cand.time_sub / kTime_osr) / kFSK_dev;
+        float time_sec = (cand.time_offset + (float)cand.time_sub / kTime_osr) / ft8::FSK_dev;
 
         float   log174[ft8::N];
         ft8::extract_likelihood(&power, cand, ft8::kGray_map, log174);
@@ -222,7 +272,9 @@ int main(int argc, char **argv) {
         uint8_t plain[ft8::N];
         int     n_errors = 0;
         ft8::bp_decode(log174, kLDPC_iterations, plain, &n_errors);
+        if (n_errors > 0) {
             // ft8::ldpc_decode(log174, kLDPC_iterations, plain, &n_errors);
+        }
 
         if (n_errors > 0) {
             LOG(LOG_DEBUG, "ldpc_decode() = %d (%.0f Hz)\n", n_errors, freq_hz);

ft8/constants.h

@@ -3,6 +3,7 @@
 #include <stdint.h>
 
 namespace ft8 {
+    constexpr float FSK_dev = 6.25f;    // tone deviation in Hz and symbol rate
 
     // Define FT8 symbol counts
     constexpr int ND = 58;      // Data symbols

ft8/decode.cpp

@@ -17,6 +17,57 @@ static int get_index(const MagArray *power, int block, int time_sub, int freq_su
     return ((((block * power->time_osr) + time_sub) * power->freq_osr + freq_sub) * power->num_bins) + bin; 
 }
 
+Monitor1Base::Monitor1Base(float sample_rate, int time_osr, int freq_osr, float fmin, float fmax) {
+    int block_size = (int)(0.5f + sample_rate / ft8::FSK_dev); // Samples per FSK tone
+    nfft = block_size * freq_osr; // FFT over symbols with frequency oversampling
+    int bin1 = (block_size * fmin) / sample_rate;
+    int bin2 = (block_size * fmax) / sample_rate;
+
+    power.time_osr = time_osr;
+    power.freq_osr = freq_osr;
+    power.num_bins = bin2 - bin1;
+    power.num_blocks = 0;
+}
+
+void Monitor1Base::feed(const float *frame) {
+    // Fill the first 3/4 of analysis frame
+    for (int i = 0; i < 3 * nfft / 4; ++i) {
+        fft_frame[i] = window_fn[i] * last_frame[i];
+    }
+
+    // Shift the frame history
+    for (int i = 0; i < nfft / 2; ++i) {
+        last_frame[i] = last_frame[i + nfft / 4];
+    }
+
+    // Now fill the last_frame array
+    for (int i = 0; i < nfft / 4; ++i) {
+        last_frame[i + nfft / 2] = frame[i];
+    }
+
+    // Fill the last 1/4 of analysis frame
+    for (int i = 3 * nfft / 4, j = nfft / 2; i < nfft; ++i, ++j) {
+        fft_frame[i] = window_fn[i] * last_frame[j];
+    }
+
+    fft_forward(fft_frame, freqdata);
+
+    for (int freq_sub = 0; freq_sub < power.freq_osr; ++freq_sub) {
+        for (int i = 0; i < power.num_bins; i += power.freq_osr) {
+            float mag2 = std::norm(freqdata[i]);    // re^2 + im^2
+            float mag_db = 10.0f * log10f(1E-12f + mag2);
+            int scaled = (int)(2 * (mag_db + 120));
+            power.mag[offset] = (scaled < 0) ? 0 : ((scaled > 255) ? 255 : scaled);
+            ++offset;
+        }
+    }
+
+    if (++time_sub >= power.time_osr) {
+        time_sub = 0;
+        ++power.num_blocks;
+    }
+}
+
 // Localize top N candidates in frequency and time according to their sync strength (looking at Costas symbols)
 // We treat and organize the candidate list as a min-heap (empty initially).
 int find_sync(const MagArray *power, const uint8_t *sync_map, int num_candidates, Candidate *heap, int min_score) {
@@ -35,13 +86,14 @@ int find_sync(const MagArray *power, const uint8_t *sync_map, int num_candidates
                     // Compute average score over sync symbols (m+k = 0-7, 36-43, 72-79)
                     int num_symbols = 0;
                     for (int m = 0; m <= 72; m += 36) {
+                        // Iterate over 7 Costas synchronisation symbols
                         for (int k = 0; k < 7; ++k) {
+                            int n = time_offset + k + m;
                             // Check for time boundaries
-                            if (time_offset + k + m < 0) continue;
+                            if (n < 0) continue;
-                            if (time_offset + k + m >= power->num_blocks) break;
+                            if (n >= power->num_blocks) break;
 
-                            // int offset = ((time_offset + k + m) * num_alt + alt) * power->num_bins + freq_offset;
+                            int offset = get_index(power, n, time_sub, freq_sub, freq_offset);
-                            int offset = get_index(power, time_offset + k + m, time_sub, freq_sub, freq_offset);
                             const uint8_t *p8 = power->mag + offset;
 
                             // Weighted difference between the expected and all other symbols
@@ -60,18 +112,18 @@ int find_sync(const MagArray *power, const uint8_t *sync_map, int num_candidates
                                 // look at one frequency bin higher
                                 score += p8[sm] - p8[sm + 1];
                             }
-                            if (k > 0) {
+                            if (k > 0 && n - 1 >= 0) {
                                 // look one symbol back in time
                                 score += p8[sm] - p8[sm - num_alt * power->num_bins];
                             }
-                            if (k < 6) {
+                            if (k < 6 && n + 1 < power->num_blocks) {
                                 // look one symbol forward in time
                                 score += p8[sm] - p8[sm + num_alt * power->num_bins];
                             }
-                        
                             ++num_symbols;                        
                         }
                     }
+                    if (num_symbols > 0)
                         score /= num_symbols;
 
                     if (score < min_score) continue;
@@ -121,10 +173,11 @@ void extract_likelihood(const MagArray *power, const Candidate & cand, const uin
         int sym_idx = (k < ft8::ND / 2) ? (k + 7) : (k + 14);
         int bit_idx = 3 * k;
 
-        // Pointer to 8 bins of the current symbol
+        // Index of the 8 bins of the current symbol
-        const uint8_t *ps = power->mag + (offset + sym_idx * num_alt * power->num_bins);
+        int sym_offset = offset + sym_idx * num_alt * power->num_bins;
 
-        decode_symbol(ps, code_map, bit_idx, log174);
+        decode_symbol(power->mag + sym_offset, code_map, bit_idx, log174);
+        // decode_multi_symbols(power->mag + sym_offset, power->num_bins, n_syms, code_map, bit_idx, log174);
     }
 
     // Compute the variance of log174

ft8/decode.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <stdint.h>
+#include <complex>
 
 namespace ft8 {
 
@@ -20,6 +21,28 @@ struct Candidate {
     uint8_t      freq_sub;
 };
 
+class Monitor1Base {
+public:
+    Monitor1Base(float sample_rate, int time_osr = 2, int freq_osr = 2, float fmin = 300, float fmax = 3000);
+
+    void feed(const float *frame);
+    void search();
+    void reset();
+protected:
+    float *window_fn;   // [nfft]
+    float *fft_frame;   // [nfft]
+    float *last_frame;  // [nfft * 3/4]
+    std::complex<float> *freqdata; // [nfft/2 + 1]
+    int nfft;
+
+    int offset;
+    int time_sub;
+    ft8::MagArray power;
+
+    // [N] real --> [N/2 + 1] log magnitudes (decibels)
+    // virtual void fft_forward_mag_db(const float *frame, uint8_t *mag_db) = 0;
+    virtual void fft_forward(const float *in, std::complex<float> *out) = 0;
+};
 
 // Localize top N candidates in frequency and time according to their sync strength (looking at Costas symbols)
 // We treat and organize the candidate list as a min-heap (empty initially).

ft8/ldpc.cpp

@@ -8,11 +8,11 @@
 // from Sarah Johnson's Iterative Error Correction book.
 // codeword[i] = log ( P(x=0) / P(x=1) )
 //
+#include "ldpc.h"
 
 #include <stdio.h>
 #include <math.h>
 #include <stdlib.h>
-#include "constants.h"
 
 namespace ft8 {
 
@@ -47,11 +47,13 @@ void pack_bits(const uint8_t plain[], int num_bits, uint8_t packed[]) {
 // codeword is 174 log-likelihoods.
 // plain is a return value, 174 ints, to be 0 or 1.
 // max_iters is how hard to try.
-// ok == 87 means success.
+// n_errors == 87 means success.
-void ldpc_decode(float codeword[], int max_iters, uint8_t plain[], int *ok) {
+void ldpc_decode(const float codeword[], int max_iters, uint8_t plain[], int *n_errors) {
     float m[ft8::M][ft8::N];       // ~60 kB
     float e[ft8::M][ft8::N];       // ~60 kB
     int min_errors = ft8::M;
+    int n_err_last = 0;
+    int n_cnt = 0;
 
     for (int j = 0; j < ft8::M; j++) {
         for (int i = 0; i < ft8::N; i++) {
@@ -93,6 +95,22 @@ void ldpc_decode(float codeword[], int max_iters, uint8_t plain[], int *ok) {
             }
         }
 
+        // Early stopping criterion
+        if (iter > 0) {
+            int nd = errors - n_err_last;
+            if (nd < 0) {
+                n_cnt = 0;
+            }
+            else {
+                ++n_cnt;
+            }
+            if (n_cnt >= 5 && iter >= 10 && errors >= 15) {
+                *n_errors = errors;
+                return;
+            }
+        }
+        n_err_last = errors;
+
         for (int i = 0; i < ft8::N; i++) {
             for (int ji1 = 0; ji1 < 3; ji1++) {
                 int j1 = kMn[i][ji1] - 1;
@@ -108,7 +126,7 @@ void ldpc_decode(float codeword[], int max_iters, uint8_t plain[], int *ok) {
         }
     }
 
-    *ok = min_errors;
+    *n_errors = min_errors;
 }
 
 
@@ -133,32 +151,19 @@ static int ldpc_check(uint8_t codeword[]) {
 }
 
 
-void bp_decode(float codeword[], int max_iters, uint8_t plain[], int *ok) {
+void BPDecoderState::reset() {
-    float tov[ft8::N][3];
-    float toc[ft8::M][7];
-
-    int min_errors = ft8::M;
-
-    int nclast = 0;
-    int ncnt = 0;
-
     // initialize messages to checks
-    for (int i = 0; i < ft8::M; ++i) {
-        for (int j = 0; j < kNrw[i]; ++j) {
-            toc[i][j] = codeword[kNm[i][j] - 1];
-        }
-    }
-
     for (int i = 0; i < ft8::N; ++i) {
         for (int j = 0; j < 3; ++j) {
             tov[i][j] = 0;
         }
     }
+}
 
-    for (int iter = 0; iter < max_iters; ++iter) {
-        float   zn[ft8::N];
 
+int BPDecoderState::iterate(const float codeword[], uint8_t plain[]) {
     // Update bit log likelihood ratios (tov=0 in iter 0)
+    float   zn[ft8::N];
     for (int i = 0; i < ft8::N; ++i) {
         zn[i] = codeword[i] + tov[i][0] + tov[i][1] + tov[i][2];
         plain[i] = (zn[i] > 0) ? 1 : 0;
@@ -167,13 +172,8 @@ void bp_decode(float codeword[], int max_iters, uint8_t plain[], int *ok) {
     // Check to see if we have a codeword (check before we do any iter)
     int errors = ldpc_check(plain);
 
-        if (errors < min_errors) {
-            // we have a better guess - update the result
-            min_errors = errors;
-
     if (errors == 0) {
-                break;  // Found a perfect answer
+        return 0;   // Found a perfect answer
-            }
     }
 
     // Send messages from bits to check nodes 
@@ -209,9 +209,44 @@ void bp_decode(float codeword[], int max_iters, uint8_t plain[], int *ok) {
             tov[i][j] = 2 * fast_atanh(-Tmn);
         }
     }
+
+    return errors;
 }
 
-    *ok = min_errors;
+
+
+void bp_decode(const float codeword[], int max_iters, uint8_t plain[], int *n_errors) {
+    BPDecoderState state;
+    
+    int n_err_last = 0;
+    int n_cnt = 0;
+
+    state.reset();
+    *n_errors = ft8::M;
+
+    for (int iter = 0; iter < max_iters; ++iter) {
+        int errors = state.iterate(codeword, plain);
+        if (errors < *n_errors) {
+            *n_errors = errors;
+        }
+        if (errors == 0) return;
+
+        // Early stopping criterion
+        if (iter > 0) {
+            int nd = errors - n_err_last;
+            if (nd < 0) {
+                n_cnt = 0;
+            }
+            else {
+                ++n_cnt;
+            }
+            if (n_cnt >= 5 && iter >= 10 && errors >= 15) {
+                *n_errors = errors;
+                return;
+            }
+        }
+        n_err_last = errors;
+    }
 }
 
 // https://varietyofsound.wordpress.com/2011/02/14/efficient-tanh-computation-using-lamberts-continued-fraction/

ft8/ldpc.h

@@ -1,14 +1,26 @@
 #pragma once
 
+#include "constants.h"
+
 namespace ft8 {
 
+    class BPDecoderState {
+    public:
+        void reset();
+        int iterate(const float codeword[], uint8_t plain[]);
+
+    private:
+        float tov[ft8::N][3];
+        float toc[ft8::M][7];
+    };
+
     // codeword is 174 log-likelihoods.
     // plain is a return value, 174 ints, to be 0 or 1.
     // iters is how hard to try.
-    // ok == 87 means success.
+    // n_errors == 0 means success.
-    void ldpc_decode(float codeword[], int max_iters, uint8_t plain[], int *ok);
+    void ldpc_decode(const float codeword[], int max_iters, uint8_t plain[], int *n_errors);
 
-    void bp_decode(float codeword[], int max_iters, uint8_t plain[], int *ok);
+    void bp_decode(const float codeword[], int max_iters, uint8_t plain[], int *n_errors);
 
     // Packs a string of bits each represented as a zero/non-zero byte in plain[], 
     // as a string of packed bits starting from the MSB of the first byte of packed[]

ft8/message77.cpp

@@ -0,0 +1,49 @@
+#include "message77.h"
+#include "unpack.h"
+
+#include <string.h>
+
+namespace ft8 {
+
+Message77::Message77() {
+    i3 = n3 = 0;
+    field1[0] = field2[0] = field3[0] = '\0';
+}
+
+int Message77::str(char *buf, int buf_sz) const {
+    // Calculate the available space sans the '\0' terminator
+    int rem_sz = buf_sz - 1;
+    int field1_sz = strlen(field1);
+    int field2_sz = strlen(field2);
+    int field3_sz = strlen(field3);
+    int msg_sz = field1_sz + (field2_sz > 0 ? 1 : 0) + 
+                 field2_sz + (field3_sz > 0 ? 1 : 0) + 
+                 field3_sz;
+
+    if (rem_sz < msg_sz) return -1;
+
+    char *dst = buf;
+
+    dst = stpcpy(dst, field1);
+    *dst++ = ' ';
+    dst = stpcpy(dst, field2);
+    *dst++ = ' ';
+    dst = stpcpy(dst, field3);
+    *dst = '\0';
+
+    return msg_sz;
+}
+
+int Message77::unpack(const uint8_t *a77) {
+    // Extract n3 (bits 71..73) and i3 (bits 74..76)
+    n3 = ((a77[8] << 2) & 0x04) | ((a77[9] >> 6) & 0x03);
+    i3 = (a77[9] >> 3) & 0x07;
+
+    int rc = unpack77_fields(a77, field1, field2, field3);
+    if (rc < 0) {
+        field1[0] = field2[0] = field3[0] = '\0';
+    }
+    return rc;
+}
+
+}   // namespace ft8

ft8/message77.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <stdint.h>
+
+namespace ft8 {
+
+class CallsignHasher {
+    virtual void save_callsign(void *obj, const char *callsign) = 0;
+    virtual bool hash10(void *obj, uint16_t hash, char *result) = 0;
+    virtual bool hash12(void *obj, uint16_t hash, char *result) = 0;
+    virtual bool hash22(void *obj, uint32_t hash, char *result) = 0;
+};
+
+class EmptyHasher : public CallsignHasher {
+    virtual void save_callsign(void *obj, const char *callsign) override {
+    }
+    virtual bool hash10(void *obj, uint16_t hash, char *result) override {
+        strcpy(result, "...");
+        return true;
+    }
+    virtual bool hash12(void *obj, uint16_t hash, char *result) override {
+        strcpy(result, "...");
+        return true;
+    }
+    virtual bool hash22(void *obj, uint32_t hash, char *result) override {
+        strcpy(result, "...");
+        return true;
+    }
+};
+
+struct Message77 {
+    uint8_t i3, n3;
+
+    // 11 chars nonstd call + 2 chars <...>
+    // 6 chars for grid/report/courtesy
+    char field1[13 + 1];
+    char field2[13 + 1];
+    char field3[6 + 1];
+
+    Message77();
+    int unpack(const uint8_t *packed77);
+    void pack(uint8_t *packed77);
+    int str(char *buf, int buf_sz) const;
+
+private:
+};
+
+} // namespace ft8

ft8/unpack.cpp

@@ -53,11 +53,11 @@ int unpack28(uint32_t n28, uint8_t ip, uint8_t i3, char *result) {
         // This is a 22-bit hash of a result
         //call hash22(n22,c13)     !Retrieve result from hash table
         // TODO: implement
-        // strcpy(result, "<...>");
+        strcpy(result, "<...>");
-        result[0] = '<';
+        // result[0] = '<';
-        int_to_dd(result + 1, n28, 7);
+        // int_to_dd(result + 1, n28, 7);
-        result[8] = '>';
+        // result[8] = '>';
-        result[9] = '\0';
+        // result[9] = '\0';
         return 0;
     }
 
@@ -181,7 +181,6 @@ int unpack_type1(const uint8_t *a77, uint8_t i3, char *field1, char *field2, cha
 
 
 int unpack_text(const uint8_t *a71, char *text) {
-    // TODO: test
     uint8_t b71[9];
 
     uint8_t carry = 0;
@@ -272,11 +271,11 @@ int unpack_nonstandard(const uint8_t *a77, char *field1, char *field2, char *fie
 
 	char call_3[15];
     // should replace with hash12(n12, call_3);
-    // strcpy(call_3, "<...>");
+    strcpy(call_3, "<...>");
-    call_3[0] = '<';
+    // call_3[0] = '<';
-    int_to_dd(call_3 + 1, n12, 4);
+    // int_to_dd(call_3 + 1, n12, 4);
-    call_3[5] = '>';
+    // call_3[5] = '>';
-    call_3[6] = '\0';
+    // call_3[6] = '\0';
 
 	char * call_1 = (iflip) ? c11 : call_3;
     char * call_2 = (iflip) ? call_3 : c11;
@@ -370,4 +369,4 @@ int unpack77(const uint8_t *a77, char *message) {
     return 0;
 }
 
-} // namespace
+} // namespace ft8

gen_ft8.cpp

@@ -4,12 +4,74 @@
 #include <cmath>
 
 #include "common/wave.h"
+#include "common/debug.h"
 //#include "ft8/v1/pack.h"
 //#include "ft8/v1/encode.h"
 #include "ft8/pack.h"
 #include "ft8/encode.h"
 #include "ft8/constants.h"
 
+#define LOG_LEVEL   LOG_INFO
+
+void gfsk_pulse(int n_spsym, float b, float *pulse) {
+    const float c = M_PI * sqrtf(2 / logf(2));
+
+    for (int i = 0; i < 3*n_spsym; ++i) {
+        float t = i/(float)n_spsym - 1.5f;
+        pulse[i] = (erff(c * b * (t + 0.5f)) - erff(c * b * (t - 0.5f))) / 2;
+    }
+}
+
+// Same as synth_fsk, but uses GFSK phase shaping
+void synth_gfsk(const uint8_t *symbols, int n_sym, float f0, int n_spsym, int signal_rate, float *signal) 
+{
+    LOG(LOG_DEBUG, "n_spsym = %d\n", n_spsym);
+    int n_wave = n_sym * n_spsym;
+    float hmod = 1.0f;
+
+    // Compute the smoothed frequency waveform.
+    // Length = (nsym+2)*nsps samples, first and last symbols extended 
+    float dphi_peak = 2 * M_PI * hmod / n_spsym;
+    float dphi[n_wave + 2*n_spsym];
+
+    // Shift frequency up by f0
+    for (int i = 0; i < n_wave + 2*n_spsym; ++i) {    
+        dphi[i] = 2 * M_PI * f0 / signal_rate;
+    }
+
+    float pulse[3 * n_spsym];
+    gfsk_pulse(n_spsym, 2.0f, pulse);
+
+    for (int i = 0; i < n_sym; ++i) {
+        int ib = i * n_spsym;
+        for (int j = 0; j < 3*n_spsym; ++j) {
+            dphi[j + ib] += dphi_peak*symbols[i]*pulse[j];
+        }
+    }
+
+    // Add dummy symbols at beginning and end with tone values equal to 1st and last symbol, respectively
+    for (int j = 0; j < 2*n_spsym; ++j) {
+        dphi[j] += dphi_peak*pulse[j + n_spsym]*symbols[0];
+        dphi[j + n_sym * n_spsym] += dphi_peak*pulse[j]*symbols[n_sym - 1];
+    }
+
+    // Calculate and insert the audio waveform
+    float phi = 0;
+    for (int k = 0; k < n_wave; ++k) { // Don't include dummy symbols
+        signal[k] = sinf(phi);
+        phi = fmodf(phi + dphi[k + n_spsym], 2*M_PI);
+    }
+
+    // Apply envelope shaping to the first and last symbols
+    int n_ramp = n_spsym / 8;
+    for (int i = 0; i < n_ramp; ++i) {
+        float env = (1 - cosf(2 * M_PI * i / (2 * n_ramp))) / 2;
+        signal[i] *= env;
+        signal[n_wave - 1 - i] *= env;
+    }
+}
+
+
 // Convert a sequence of symbols (tones) into a sinewave of continuous phase (FSK).
 // Symbol 0 gets encoded as a sine of frequency f0, the others are spaced in increasing
 // fashion.
@@ -23,8 +85,8 @@ void synth_fsk(const uint8_t *symbols, int num_symbols, float f0, float spacing,
     int i = 0;
     while (j < num_symbols) {
         float f = f0 + symbols[j] * spacing;
-        phase += 2 * M_PI * f / signal_rate;
+        phase = fmodf(phase + 2 * M_PI * f / signal_rate, 2 * M_PI);
-        signal[i] = sin(phase);
+        signal[i] = sinf(phase);
         t += dt;
         if (t >= dt_sym) {
             // Move to the next symbol
@@ -97,7 +159,8 @@ int main(int argc, char **argv) {
         signal[i] = 0;
     }
 
-    synth_fsk(tones, ft8::NN, frequency, symbol_rate, symbol_rate, sample_rate, signal + num_silence);
+    // synth_fsk(tones, ft8::NN, frequency, symbol_rate, symbol_rate, sample_rate, signal + num_silence);
+    synth_gfsk(tones, ft8::NN, frequency, sample_rate / symbol_rate, sample_rate, signal + num_silence); 
     save_wav(signal, num_silence + num_samples + num_silence, sample_rate, wav_path);
 
     return 0;