[AT3P] GHA development:

 * Multidimensional gha, rework residual check
 * Possibility to look into the next frame during GHA
 * Possibility to pass envelope into bitstream
 * Tool to create oma file from tsv gha description (to test envelope processing)

9 files changed, 728 insertions, 219 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c82b5dc..75d75bc 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -4,7 +4,7 @@ set (CMAKE_CXX_STANDARD 11)
 set (CMAKE_C_STANDARD 11)
 
 #add_definitions( "-Wall -O2 -g -Rpass-analysis=loop-vectorize" )
-#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -fsanitize=address -fno-omit-frame-pointer")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -fsanitize=address -fno-omit-frame-pointer")
 
 if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose Release or Debug" FORCE)
@@ -125,3 +125,10 @@ set(SOURCE_EXE
 add_executable(atracdenc ${SOURCE_EXE})
 target_link_libraries(atracdenc pcm_io oma atracdenc_impl ${SNDFILE_LIBRARIES})
 install(TARGETS atracdenc)
+
+#DEVTOOL
+set(GHASENDTOOL_EXE
+    atrac/at3p/ghasend_tool.cpp
+)
+add_executable(ghasendtool ${GHASENDTOOL_EXE})
+target_link_libraries(ghasendtool pcm_io oma atracdenc_impl ${SNDFILE_LIBRARIES})
diff --git a/src/atrac/at3p/at3p.cpp b/src/atrac/at3p/at3p.cpp
index 62e51ae..110f522 100644
--- a/src/atrac/at3p/at3p.cpp
+++ b/src/atrac/at3p/at3p.cpp
@@ -88,10 +88,13 @@ EncodeFrame(const TFloat* data, int channels)
 
     assert(needMore == 0);
 
-    const float* b1 = ChannelCtx[0].CurBuf;
-    const float* b2 = (channels == 2) ? ChannelCtx[1].CurBuf : nullptr;
+    const float* b1Cur = ChannelCtx[0].CurBuf;
+    const float* b1Next = ChannelCtx[0].NextBuf;
+    const float* b2Cur = (channels == 2) ? ChannelCtx[1].CurBuf : nullptr;
+    const float* b2Next = (channels == 2) ? ChannelCtx[1].NextBuf : nullptr;
 
-    auto tonalBlock = GhaProcessor->DoAnalize(b1, b2);
+
+    auto tonalBlock = GhaProcessor->DoAnalize({b1Cur, b1Next}, {b2Cur, b2Next});
 
     BitStream.WriteFrame(channels, tonalBlock);
 
diff --git a/src/atrac/at3p/at3p_bitstream.cpp b/src/atrac/at3p/at3p_bitstream.cpp
index 26ebf41..a43ed18 100644
--- a/src/atrac/at3p/at3p_bitstream.cpp
+++ b/src/atrac/at3p/at3p_bitstream.cpp
@@ -143,11 +143,23 @@ static void WriteTonalBlock(NBitStream::TBitStream& bs, int channels, const TAt3
                 continue;
             }
 
-            //TODO: Add actual Envelope
-            // start point present
-            bs.Write(0, 1);
-            // stop point present
-            bs.Write(0, 1);
+            const auto envelope = tonalBlock->GetEnvelope(ch, i);
+
+            if (envelope.first != TAt3PGhaData::EMPTY_POINT) {
+                // start point present
+                bs.Write(1, 1);
+                bs.Write(envelope.first, 5);
+            } else {
+                bs.Write(0, 1);
+            }
+
+            if (envelope.second != TAt3PGhaData::EMPTY_POINT) {
+                // stop point present
+                bs.Write(1, 1);
+                bs.Write(envelope.second, 5);
+            } else {
+                bs.Write(0, 1);
+            }
         }
 
         // Num waves
@@ -238,7 +250,8 @@ void TAt3PBitStream::WriteFrame(int channels, const TAt3PGhaData* tonalBlock)
     // 2 - Nobody know
     bitStream.Write(channels - 1, 2);
 
-    int num_quant_units =  14;
+    //int num_quant_units =  14;
+    int num_quant_units =  24;
     bitStream.Write(num_quant_units - 1, 5);
 
     for (int ch = 0; ch < channels; ch++) {
diff --git a/src/atrac/at3p/at3p_gha.cpp b/src/atrac/at3p/at3p_gha.cpp
index 0a45f61..7e367e8 100644
--- a/src/atrac/at3p/at3p_gha.cpp
+++ b/src/atrac/at3p/at3p_gha.cpp
@@ -30,7 +30,6 @@
 #include <map>
 #include <vector>
 
-
 using std::map;
 using std::vector;
 using std::isnan;
@@ -49,14 +48,17 @@ uint32_t GhaFreqToIndex(float f, uint32_t sb)
 
 uint32_t GhaPhaseToIndex(float p)
 {
-    return static_cast<uint32_t>(lrintf(32.0 * (p / (2 * M_PI))) & 31);
+    return static_cast<uint32_t>(lrintf(31.0 * ((p) / (2 * M_PI))) & 31);
 }
 
 class TGhaProcessor : public IGhaProcessor {
     // Number of subbands to process;
     // No need to process all subbands.
-    static const size_t SUBBANDS = 13;
-    static const size_t CHANNEL_BUF_SZ = SUBBANDS * 128;;
+    static constexpr size_t SUBBANDS = 8;
+    static constexpr size_t SAMPLES_PER_SUBBAND = 128;
+    static constexpr size_t LOOK_AHEAD = 32;
+    static constexpr size_t GHA_SUBBAND_BUF_SZ = SAMPLES_PER_SUBBAND + LOOK_AHEAD;
+    static constexpr size_t CHANNEL_BUF_SZ = SUBBANDS * GHA_SUBBAND_BUF_SZ;
 
     using TGhaInfoMap = map<uint32_t, struct gha_info>;
     using TWavesChannel = TAt3PGhaData::TWavesChannel;
@@ -68,6 +70,9 @@ class TGhaProcessor : public IGhaProcessor {
         pair<uint32_t, uint32_t> Envelopes[SUBBANDS] = {{0,0}};
         uint8_t SubbandDone[SUBBANDS] = {0};
         TGhaInfoMap GhaInfos;
+        float MaxToneMagnitude[SUBBANDS] = {0}; // Max magnitude of sine in the band. Used to stop processing when next extracted sine become significant less then max one
+        float LastResuidalEnergy[SUBBANDS] = {0}; // Resuidal energy on the last round for subband. It is the second criteria to stop processing, we expect resuidal becaming less on the each round
+        uint16_t LastAddedFreqIdx[SUBBANDS];
 
         void MarkSubbandDone(size_t sb) {
             SubbandDone[sb] = 16;
@@ -78,13 +83,35 @@ class TGhaProcessor : public IGhaProcessor {
         }
     };
 
+    struct TChannelGhaCbCtx {
+        TChannelGhaCbCtx(TChannelData* data, size_t sb)
+            : Data(data)
+            , Sb(sb)
+        {}
+        TChannelData* Data;
+        size_t Sb;
+        struct {
+            bool Ok;
+        } Result;
+    };
+
 public:
     TGhaProcessor(bool stereo)
         : LibGhaCtx(gha_create_ctx(128))
-        , AmpSfTab(CreateAmpSfTab())
         , Stereo(stereo)
     {
-        FillSubbandAth(&SubbandAth[0]);
+        gha_set_max_magnitude(LibGhaCtx, 32768);
+
+        if (!StaticInited) {
+            FillSubbandAth(&SubbandAth[0]);
+
+            AmpSfTab = CreateAmpSfTab();
+            for (int i = 0; i < 2048; i++) {
+                SineTab[i] = sin(2 * M_PI * i / 2048);
+            }
+
+            StaticInited = true;
+        }
     }
 
     ~TGhaProcessor()
@@ -92,54 +119,154 @@ public:
         gha_free_ctx(LibGhaCtx);
     }
 
-    const TAt3PGhaData* DoAnalize(const float* b1, const float* b2) override;
+    const TAt3PGhaData* DoAnalize(TBufPtr b1, TBufPtr b2) override;
 private:
-    static void FillSubbandAth(float* out) {
-        const auto ath = CalcATH(16 * 1024, 44100);
-        #pragma GCC nounroll
-        for (size_t sb = 0; sb < SUBBANDS; sb++) {
-            float m = 999.;
-            for (size_t f = sb * 1024, i = 0; i < 1024; f++, i++) {
-                m = fmin(m, ath[f]);
-            }
-            m += 26; //Some gap to not encode too much
-            out[sb] = pow(10, 0.1 * (m + 90)); //adjust to 0db level = 32768, convert to power
-        }
-    }
-
-    static TAmpSfTab CreateAmpSfTab() {
-        TAmpSfTab AmpSfTab;
-        for (int i = 0; i < (int)AmpSfTab.size(); i++) {
-            AmpSfTab[i] = exp2f((i - 3) / 4.0f);
-        }
-        return AmpSfTab;
-    }
+    static void FillSubbandAth(float* out);
+    static TAmpSfTab CreateAmpSfTab();
+    static void CheckResuidalAndApply(float* resuidal, size_t size, void* self) noexcept;
 
     uint32_t FillFolowerRes(const TGhaInfoMap& lGha, const TGhaInfoMap& fGha, TGhaInfoMap::const_iterator& it, uint32_t leaderSb);
 
     uint32_t AmplitudeToSf(float amp);
 
     bool DoRound(TChannelData& data, size_t& totalTones) const;
-    bool PsyPreCheck(size_t sb, const struct gha_info& gha) const;
-    pair<uint32_t, uint32_t> FindPos(const float* src, const float* analized, float magn, uint32_t freqIdx) const;
+    bool PsyPreCheck(size_t sb, const struct gha_info& gha, const TChannelData& data) const;
     void FillResultBuf(const vector<TChannelData>& data);
 
     gha_ctx_t LibGhaCtx;
-    const TAmpSfTab AmpSfTab;
-    float SubbandAth[SUBBANDS];
     TAt3PGhaData ResultBuf;
     const bool Stereo;
+
+    static float SubbandAth[SUBBANDS];
+    static float SineTab[2048];
+    static bool StaticInited;
+    static TAmpSfTab AmpSfTab;
 };
 
-const TAt3PGhaData* TGhaProcessor::DoAnalize(const float* b1, const float* b2)
+bool TGhaProcessor::StaticInited = false;
+float TGhaProcessor::SubbandAth[SUBBANDS];
+float TGhaProcessor::SineTab[2048];
+TGhaProcessor::TAmpSfTab TGhaProcessor::AmpSfTab;
+
+void TGhaProcessor::FillSubbandAth(float* out)
+{
+    const auto ath = CalcATH(16 * 1024, 44100);
+    #pragma GCC nounroll
+    for (size_t sb = 0; sb < SUBBANDS; sb++) {
+        float m = 999.;
+        for (size_t f = sb * 1024, i = 0; i < 1024; f++, i++) {
+            m = fmin(m, ath[f]);
+        }
+        //m += 26; //Some gap to not encode too much
+        out[sb] = pow(10, 0.1 * (m + 90)); //adjust to 0db level = 32768, convert to power
+    }
+}
+
+TGhaProcessor::TAmpSfTab TGhaProcessor::CreateAmpSfTab()
+{
+    TAmpSfTab AmpSfTab;
+    for (int i = 0; i < (int)AmpSfTab.size(); i++) {
+        AmpSfTab[i] = exp2f((i - 3) / 4.0f);
+    }
+    return AmpSfTab;
+}
+
+void TGhaProcessor::CheckResuidalAndApply(float* resuidal, size_t size, void* d) noexcept
+{
+    TChannelGhaCbCtx* ctx = (TChannelGhaCbCtx*)(d);
+    const float* srcBuf = ctx->Data->SrcBuf + (ctx->Sb * SAMPLES_PER_SUBBAND);
+    //std::cerr << "TGhaProcessor::CheckResuidal " << srcBuf[0] << " " << srcBuf[1] << " " << srcBuf[2] << " " << srcBuf[3] <<  std::endl;
+
+    float resuidalEnergy = 0;
+
+    uint32_t start = 0;
+    uint32_t curStart = 0;
+    uint32_t count = 0;
+    uint32_t len = 0;
+    bool found = false;
+
+    if (size != SAMPLES_PER_SUBBAND)
+        abort();
+
+    for (size_t i = 0; i < SAMPLES_PER_SUBBAND; i += 4) {
+        float energyIn = 0.0;
+        float energyOut = 0.0;
+        for (size_t j = 0; j < 4; j++) {
+            energyIn += srcBuf[i + j] * srcBuf[i + j];
+            energyOut += resuidal[i + j] * resuidal[i + j];
+        }
+
+        energyIn = sqrt(energyIn/4);
+        energyOut = sqrt(energyOut/4);
+        resuidalEnergy += energyOut;
+
+        if (energyIn / energyOut < 1) {
+            count = 0;
+            found = false;
+            curStart = i + 4;
+        } else {
+            count++;
+            if (count > len) {
+                len = count;
+                if (!found) {
+                    start = curStart;
+                    found = true;
+                }
+            }
+        }
+
+        //std::cerr << " " << i << " rms : " << energyIn << " " << energyOut  << "\t\t\t" << ((energyOut < energyIn) ? "+" : "-")  << std::endl;
+    }
+
+    const auto sb = ctx->Sb;
+    // Do not encode too short frame
+    if (len < 4) {
+        ctx->Result.Ok = false;
+        return;
+    }
+
+    const uint32_t end = start + len * 4;
+
+    const float threshold = 1.4; //TODO: tune it
+    if (static_cast<bool>(ctx->Data->LastResuidalEnergy[sb]) == false) {
+        ctx->Data->LastResuidalEnergy[sb] = resuidalEnergy;
+    } else if (ctx->Data->LastResuidalEnergy[sb] < resuidalEnergy * threshold) {
+        ctx->Result.Ok = false;
+        return;
+    } else {
+        ctx->Data->LastResuidalEnergy[sb] = resuidalEnergy;
+    }
+
+    auto& envelope = ctx->Data->Envelopes[sb];
+    envelope.first = start;
+    envelope.second = end;
+
+    ctx->Result.Ok = true;
+
+    float* b = &ctx->Data->Buf[sb * GHA_SUBBAND_BUF_SZ];
+
+    memcpy(b, resuidal, sizeof(float) * SAMPLES_PER_SUBBAND);
+}
+
+const TAt3PGhaData* TGhaProcessor::DoAnalize(TBufPtr b1, TBufPtr b2)
 {
     vector<TChannelData> data((size_t)Stereo + 1);
     bool progress[2] = {false};
 
     for (size_t ch = 0; ch < data.size(); ch++) {
-        const float* b = (ch == 0) ? b1 : b2;
-        data[ch].SrcBuf = b;
-        memcpy(&data[ch].Buf[0], b, sizeof(float) * CHANNEL_BUF_SZ);
+        const float* bCur = (ch == 0) ? b1[0] : b2[0];
+        const float* bNext = (ch == 0) ? b1[1] : b2[1];
+        data[ch].SrcBuf = bCur;
+
+        for (size_t sb = 0; sb < SUBBANDS; sb++, bCur += SAMPLES_PER_SUBBAND, bNext += SAMPLES_PER_SUBBAND) {
+            constexpr auto copyCurSz = sizeof(float) * SAMPLES_PER_SUBBAND;
+            constexpr auto copyNextSz = sizeof(float) * LOOK_AHEAD;
+            memcpy(&data[ch].Buf[0] + sb * GHA_SUBBAND_BUF_SZ                      , bCur, copyCurSz);
+            memcpy(&data[ch].Buf[0] + sb * GHA_SUBBAND_BUF_SZ + SAMPLES_PER_SUBBAND, bNext, copyNextSz);
+        }
+        //for (int i = 0; i < SAMPLES_PER_SUBBAND + LOOK_AHEAD; i++) {
+            //std::cerr << i << " " << data[0].Buf[i] << std::endl;
+        //}
     }
 
     size_t totalTones = 0;
@@ -170,182 +297,136 @@ bool TGhaProcessor::DoRound(TChannelData& data, size_t& totalTones) const
             return false;
         }
 
-        float* b = &data.Buf[sb * 128];
-        struct gha_info res;
-
-        gha_analyze_one(b, &res, LibGhaCtx);
-
-#if 0
+        const float* srcB = data.SrcBuf + (sb * SAMPLES_PER_SUBBAND);
         {
-            const float* srcB = data.SrcBuf + (sb * 128);
-            auto it = data.GhaInfos.lower_bound(sb << 10);
+            auto cit = data.GhaInfos.lower_bound(sb << 10);
             vector<gha_info> tmp;
-            for(; it != data.GhaInfos.end() && it->first < (sb + 1) << 10; it++) {
+            for(auto it = cit; it != data.GhaInfos.end() && it->first < (sb + 1) << 10; it++) {
                 tmp.push_back(it->second);
             }
-            if (tmp.size() > 1) {
-                for (const auto& x : tmp) {
-                    std::cerr << "to adjust: " << x.frequency << " " << x.magnitude << std::endl;
-                }
-                int ar = gha_adjust_info(srcB, tmp.data(), tmp.size(), LibGhaCtx);
-                for (const auto& x : tmp) {
-                    std::cerr << "after adjust: " << x.frequency << " " << x.magnitude << " ar: " << ar << std::endl;
-                }
+            if (tmp.size() > 0) {
+                TChannelGhaCbCtx ctx(&data, sb);
+                int ar = gha_adjust_info(srcB, tmp.data(), tmp.size(), LibGhaCtx, CheckResuidalAndApply, &ctx);
+                if (ar == 0 && ctx.Result.Ok) {
+                    std::sort(tmp.begin(), tmp.end(), [](const gha_info& a, const gha_info& b) {return a.frequency < b.frequency;});
+
+                    bool dupFound = false;
+                    {
+                        auto idx1 = GhaFreqToIndex(tmp[0].frequency, sb);
+                        for (size_t i = 1; i < tmp.size(); i++) {
+                            auto idx2 = GhaFreqToIndex(tmp[i].frequency, sb);
+                            if (idx2 == idx1) {
+                                dupFound = true;
+                                break;
+                            } else {
+                                idx1 = idx2;
+                            }
+                        }
+                    }
 
+                    if (!dupFound) {
+                        auto it = cit;
+                        for (const auto& x : tmp) {
+                            data.MaxToneMagnitude[sb] = std::max(data.MaxToneMagnitude[sb], x.magnitude);
+                            const auto newIndex = GhaFreqToIndex(x.frequency, sb);
+                            //std ::cerr << "after adjust, idx: " << it->first << " -> " << newIndex << " info: "  << it->second.frequency << " -> " << x.frequency << " " << it->second.magnitude << " -> " << x.magnitude << " phase: " << x.phase << std::endl;
+                            if (newIndex != it->first) {
+                                bool skip = newIndex > it->first;
+                                //std::cerr << "erase: " << it->first << "skip: " << skip << std::endl;
+                                data.GhaInfos.insert(it, {newIndex, x});
+                                it = data.GhaInfos.erase(it);
+                                if (skip && it != data.GhaInfos.end()) {
+                                    it++;
+                                }
+                            } else {
+                               //std::cerr << "replace" << std::endl;
+                               it->second = x;
+                               it++;
+                            }
+                        }
+                    } else {
+                        std::cerr << "jackpot! same freq index after adjust call, sb: " << sb << " " << std::endl;
+                        data.GhaInfos.erase(data.LastAddedFreqIdx[sb]);
+                        totalTones--;
+                        data.MarkSubbandDone(sb);
+                        continue;
+                    }
+                } else {
+                    data.GhaInfos.erase(data.LastAddedFreqIdx[sb]);
+                    totalTones--;
+                    data.MarkSubbandDone(sb);
+                    continue;
+                }
             }
         }
-#endif
+
+        float* b = &data.Buf[sb * GHA_SUBBAND_BUF_SZ];
+        struct gha_info res;
+
+        gha_analyze_one(b, &res, LibGhaCtx);
+
         auto freqIndex = GhaFreqToIndex(res.frequency, sb);
         //std::cerr << "sb: " << sb << " findex: " << freqIndex << " magn " << res.magnitude <<  std::endl;
-        if (PsyPreCheck(sb, res) == false) {
+        if (PsyPreCheck(sb, res, data) == false) {
             data.MarkSubbandDone(sb);
-            //std::cerr << "PreCheck failed" << std::endl;
         } else {
-            const float* tone = gha_get_analyzed(LibGhaCtx);
-            auto pos = FindPos(b, tone, res.magnitude, freqIndex);
-            if (pos.second == 0) {
-                data.MarkSubbandDone(sb);
+            if (data.SubbandDone[sb] == 0) {
+                bool ins = data.GhaInfos.insert({freqIndex, res}).second;
+                data.LastAddedFreqIdx[sb] = freqIndex;
+                assert(ins);
             } else {
-                auto& envelope = data.Envelopes[sb];
-                // Envelope for whole subband not initialized
-                if (envelope.second == 0) {
-                    assert(data.SubbandDone[sb] == 0);
-                    envelope = pos;
-                    bool ins = data.GhaInfos.insert({freqIndex, res}).second;
-                    assert(ins);
-                } else {
-                    //TODO:
-                    // Nth sine for subband was out of existed envelope. This case requires more
-                    // complicated analisys - just skip it for a while
-                    if ((pos.first < envelope.first && pos.second <= envelope.first) ||
-                        (pos.first >= envelope.second)) {
+                const auto it = data.GhaInfos.lower_bound(freqIndex);
+                const size_t minFreqDistanse = 10; // Now we unable to handle tones with close frequency
+                if (it != data.GhaInfos.end()) {
+                    if (it->first == freqIndex) {
                         data.MarkSubbandDone(sb);
                         continue;
                     }
 
-                    const auto it = data.GhaInfos.lower_bound(freqIndex);
-
-                    const size_t minFreqDistanse = 40; // Now we unable to handle tones with close frequency
-                    if (it != data.GhaInfos.end()) {
-                        if (it->first == freqIndex) {
-                            //std::cerr << "already added: " << freqIndex << std::endl;
-                            data.MarkSubbandDone(sb);
-                            continue;
-                        }
-                        //std::cerr << "right: " << it->first << " " << freqIndex << std::endl;
-                        if (it->first - freqIndex < minFreqDistanse) {
-                            data.MarkSubbandDone(sb);
-                            continue;
-                        }
-                    }
-                    if (it != data.GhaInfos.begin()) {
-                        auto prev = it;
-                        prev--;
-                        //std::cerr << "left: " << prev->first << " " << freqIndex << std::endl;
-                        if (freqIndex - prev->first < minFreqDistanse) {
-                            data.MarkSubbandDone(sb);
-                            continue;
-                        }
+                    if (it->first - freqIndex < minFreqDistanse) {
+                        data.MarkSubbandDone(sb);
+                        continue;
                     }
-                    data.GhaInfos.insert(it, {freqIndex, res});
-                    envelope.first = max(envelope.first, pos.first);
-                    envelope.second = min(envelope.second, pos.second);
                 }
-                // Here we have updated envelope, we are ready to add extracted tone
-
-                for (size_t j = envelope.first; j < envelope.second; j++) {
-                    b[j] -= tone[j] * res.magnitude;
+                if (it != data.GhaInfos.begin()) {
+                    auto prev = it;
+                    prev--;
+                    if (freqIndex - prev->first < minFreqDistanse) {
+                        data.MarkSubbandDone(sb);
+                        continue;
+                    }
                 }
-
-                data.SubbandDone[sb]++;
-                totalTones++;
-                progress = true;
-
-                //std::cerr << "envelop " << envelope.first << " " << envelope.second << std::endl;
+                data.GhaInfos.insert(it, {freqIndex, res});
+                data.LastAddedFreqIdx[sb] = freqIndex;
             }
+
+            data.SubbandDone[sb]++;
+            totalTones++;
+            progress = true;
         }
 
     }
     return progress;
 }
 
-bool TGhaProcessor::PsyPreCheck(size_t sb, const struct gha_info& gha) const
+bool TGhaProcessor::PsyPreCheck(size_t sb, const struct gha_info& gha, const TChannelData& data) const
 {
     if (isnan(gha.magnitude)) {
         return false;
     }
 
-    //std::cerr << "sb: " << sb << " " << gha.magnitude << " ath: " << SubbandAth[sb] << std::endl;
+    //std::cerr << "sb: " << sb << " " << gha.magnitude << " ath: " << SubbandAth[sb] << " max: " << data.MaxToneMagnitude[sb] << std::endl;
     // TODO: improve it
     // Just to start. Actualy we need to consider spectral leakage during MDCT
     if ((gha.magnitude * gha.magnitude) > SubbandAth[sb]) {
-        return true;
-    } else {
-        return false;
-    }
-    return true;
-}
-
-pair<uint32_t, uint32_t> TGhaProcessor::FindPos(const float* src, const float* tone, float magn, uint32_t freqIdx) const
-{
-    freqIdx &= 1023; // clear subband part
-    size_t windowSz = 64;
-    // TODO: place to make experiments
-    // we should compare rms on some window. Optimal window sile depends on frequency so right now
-    // here just some heuristic to match window size to freq index
-
-    // Ideas:
-    // - generate FIR filter for each frequncy index and use it fo filter source signal before compare level
-    // - may be just perform search all sinusoid components, run N dimension GHA optimization, and then find positions
-    if (freqIdx > 256) {
-        windowSz = 8;
-    } else if (freqIdx > 128) {
-        windowSz = 16;
-    } else if (freqIdx > 64) {
-        windowSz = 32;
-    } else {
-        windowSz = 64;
-    }
-
-    uint32_t start = 0;
-    uint32_t curStart = 0;
-    uint32_t count = 0;
-    uint32_t len = 0;
-    bool found = false;
-
-    for (uint32_t i = 0; i < 128; i += windowSz) {
-        float rmsIn = 0.0;
-        float rmsOut = 0.0;
-        for (size_t j = 0; j < windowSz; j++) {
-            rmsIn += src[i + j] * src[i + j];
-            float r = src[i + j] - tone[i + j] * magn;
-            rmsOut += r * r;
-        }
-        rmsIn /= windowSz;
-        rmsOut /= windowSz;
-        rmsIn = sqrt(rmsIn);
-        rmsOut = sqrt(rmsOut);
-
-        //std::cerr << i << " rms : " << rmsIn << " " << rmsOut  << "\t\t\t" << ((rmsOut < rmsIn) ? "+" : "-")  << std::endl;
-        if (rmsIn / rmsOut < 1) {
-            count = 0;
-            found = false;
-            curStart = i;
-        } else {
-            count++;
-            if (count > len) {
-                len = count;
-                if (!found) {
-                    start = curStart;
-                    found = true;
-                }
-            }
+        // Stop processing for sb if next extracted tone 23db less then maximal one
+        // TODO: tune
+        if (gha.magnitude > data.MaxToneMagnitude[sb] / 10) {
+            return true;
         }
     }
 
-    auto end = start + len * windowSz;
-
-    return {start, end};
+    return false;
 }
 
 void TGhaProcessor::FillResultBuf(const vector<TChannelData>& data)
@@ -373,6 +454,11 @@ void TGhaProcessor::FillResultBuf(const vector<TChannelData>& data)
 
     bool leader = usedContiguousSb[1] > usedContiguousSb[0];
 
+    std::vector<TWavesChannel> history;
+    history.reserve(data.size());
+
+    history.push_back(ResultBuf.Waves[0]);
+
     ResultBuf.SecondIsLeader = leader;
     ResultBuf.NumToneBands = usedContiguousSb[leader];
 
@@ -380,6 +466,9 @@ void TGhaProcessor::FillResultBuf(const vector<TChannelData>& data)
     TGhaInfoMap::const_iterator folowerIt;
     if (data.size() == 2) {
         TWavesChannel& fWaves = ResultBuf.Waves[1];
+
+        history.push_back(fWaves);
+
         fWaves.WaveParams.clear();
         fWaves.WaveSbInfos.clear();
         // Yes, see bitstream code
@@ -394,10 +483,13 @@ void TGhaProcessor::FillResultBuf(const vector<TChannelData>& data)
     waves.WaveSbInfos.clear();
     waves.WaveSbInfos.resize(usedContiguousSb[leader]);
     auto it = ghaInfos.begin();
-    uint32_t nextSb = 0;
+    uint32_t prevSb = 0;
     uint32_t index = 0;
 
-    uint32_t nextFolowerSb = 1;
+    if (usedContiguousSb[leader] == 0) {
+        return;
+    }
+
     while (it != ghaInfos.end()) {
         const auto sb = ((it->first) >> 10);
         if (sb >= usedContiguousSb[leader]) {
@@ -409,26 +501,31 @@ void TGhaProcessor::FillResultBuf(const vector<TChannelData>& data)
         const auto ampSf = AmplitudeToSf(it->second.magnitude);
 
         waves.WaveSbInfos[sb].WaveNums++;
-        if (sb != nextSb) {
-            // cur subband done
+        if (sb != prevSb) {
+            waves.WaveSbInfos[prevSb].Envelope.first = TAt3PGhaData::EMPTY_POINT;
+            waves.WaveSbInfos[prevSb].Envelope.second = TAt3PGhaData::EMPTY_POINT;
+
             // update index sb -> wave position index
             waves.WaveSbInfos[sb].WaveIndex = index;
 
             // process folower if present
             if (data.size() == 2) {
-                nextFolowerSb = FillFolowerRes(data[leader].GhaInfos, data[!leader].GhaInfos, folowerIt, nextSb);
+                FillFolowerRes(data[leader].GhaInfos, data[!leader].GhaInfos, folowerIt, prevSb);
                 leaderStartIt = it;
             }
 
-            nextSb = sb;
+            prevSb = sb;
         }
         waves.WaveParams.push_back(TAt3PGhaData::TWaveParam{freqIndex, ampSf, 1, phaseIndex});
         it++;
         index++;
     }
 
-    if (data.size() == 2 && nextFolowerSb <= usedContiguousSb[leader]) {
-        FillFolowerRes(data[leader].GhaInfos, data[!leader].GhaInfos, folowerIt, nextSb);
+    waves.WaveSbInfos[prevSb].Envelope.first = TAt3PGhaData::EMPTY_POINT;
+    waves.WaveSbInfos[prevSb].Envelope.second = TAt3PGhaData::EMPTY_POINT;
+
+    if (data.size() == 2) {
+        FillFolowerRes(data[leader].GhaInfos, data[!leader].GhaInfos, folowerIt, prevSb);
     }
 }
 
diff --git a/src/atrac/at3p/at3p_gha.h b/src/atrac/at3p/at3p_gha.h
index 5fef68d..86da845 100644
--- a/src/atrac/at3p/at3p_gha.h
+++ b/src/atrac/at3p/at3p_gha.h
@@ -27,6 +27,7 @@ static_assert(sizeof(TFloat) == sizeof(float), "TFloat must be float32");
 namespace NAtracDEnc {
 
 struct TAt3PGhaData {
+    static constexpr uint32_t EMPTY_POINT = static_cast<uint32_t>(-1);
     struct TWaveParam {
         uint32_t FreqIndex;
         uint32_t AmpSf;
@@ -36,6 +37,7 @@ struct TAt3PGhaData {
     struct TWaveSbInfo {
         size_t WaveIndex;
         size_t WaveNums;
+        std::pair<uint32_t, uint32_t> Envelope = {EMPTY_POINT, EMPTY_POINT};
     };
     struct TWavesChannel {
         std::vector<TWaveSbInfo> WaveSbInfos;
@@ -52,6 +54,10 @@ struct TAt3PGhaData {
         return Waves[ch].WaveSbInfos.at(sb).WaveNums;
     }
 
+    std::pair<uint32_t, uint32_t> GetEnvelope(size_t ch, size_t sb) const {
+        return Waves[ch].WaveSbInfos.at(sb).Envelope;
+    }
+
     std::pair<const TWaveParam*, size_t> GetWaves(size_t ch, size_t sb) const {
         const auto& sbInfo = Waves[ch].WaveSbInfos.at(sb);
         return { &Waves[ch].WaveParams[sbInfo.WaveIndex], sbInfo.WaveNums };
@@ -60,8 +66,9 @@ struct TAt3PGhaData {
 
 class IGhaProcessor {
 public:
+    using TBufPtr = std::array<const float*, 2>;
     virtual ~IGhaProcessor() {}
-    virtual const TAt3PGhaData* DoAnalize(const float* b1, const float* b2) = 0;
+    virtual const TAt3PGhaData* DoAnalize(TBufPtr b1, TBufPtr b2) = 0;
 };
 
 std::unique_ptr<IGhaProcessor> MakeGhaProcessor0(bool stereo);
diff --git a/src/atrac/at3p/at3p_gha_ut.cpp b/src/atrac/at3p/at3p_gha_ut.cpp
index 0da0393..68c4300 100644
--- a/src/atrac/at3p/at3p_gha_ut.cpp
+++ b/src/atrac/at3p/at3p_gha_ut.cpp
@@ -12,6 +12,7 @@ using namespace NAtracDEnc;
 
 struct TTestParam {
     float freq;
+    float phase;
     uint16_t amplitude;
     uint16_t start;
     uint16_t end;
@@ -22,14 +23,15 @@ static void __attribute__ ((noinline)) Gen(const TTestParam& p, vector<float>& o
     float freq = p.freq / (44100.0 / 16.0);
     float a = p.amplitude;
     int end = p.end;
-    for (int i = p.start; i < end; i++) {
-        out[i] += sin(freq * (float)(i % 128) * 2 * M_PI) * a;
+    int j = 0;
+    for (int i = p.start; i < end; i++, j++) {
+        out[i] += sin(freq * (float)j * 2.0 * M_PI + p.phase) * a;
     }
 }
 
 static const TAt3PGhaData __attribute__ ((noinline)) GenAndRunGha(vector<TTestParam> p1, vector<TTestParam> p2)
 {
-    vector<float> buf1(2048);
+    vector<float> buf1(2048 * 2);
 
     for (const auto& p : p1) {
         Gen(p, buf1);
@@ -38,7 +40,7 @@ static const TAt3PGhaData __attribute__ ((noinline)) GenAndRunGha(vector<TTestPa
     vector<float> buf2;
 
     if (!p2.empty()) {
-        buf2.resize(2048);
+        buf2.resize(2048 * 2);
 
         for (const auto& p : p2) {
             Gen(p, buf2);
@@ -49,7 +51,7 @@ static const TAt3PGhaData __attribute__ ((noinline)) GenAndRunGha(vector<TTestPa
     const float* b1 = buf1.data();
     const float* b2 = buf2.empty() ? nullptr : buf2.data();
 
-    return *(processor->DoAnalize(b1, b2));
+    return *(processor->DoAnalize({b1, b1 + 2048}, {b2, b2 + 2048}));
 }
 
 static class TDumper {
@@ -88,7 +90,7 @@ private:
 // Single channel simple cases
 
 TEST(AT3PGHA, 689hz0625__full_frame_mono) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 0, 128}}, {});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 0, 128}}, {});
     EXPECT_EQ(res.NumToneBands, 1);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
@@ -96,12 +98,26 @@ TEST(AT3PGHA, 689hz0625__full_frame_mono) {
     EXPECT_EQ(res.GetWaves(0, 0).second, 1);
     EXPECT_EQ(res.GetWaves(0, 0).first->FreqIndex, 512);
     EXPECT_EQ(res.GetWaves(0, 0).first->AmpSf, 63);
+    //EXPECT_EQ(res.GetWaves(0, 0).first->PhaseIndex, 0);
+    Dumper.Dump(&res, 1, 1);
+}
+
+TEST(AT3PGHA, 0__full_frame_mono) {
+    auto res = GenAndRunGha({{0.0f, M_PI/2, 32768, 0, 128}}, {});
+    EXPECT_EQ(res.NumToneBands, 1);
+    EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
+    EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+    EXPECT_EQ(res.GetNumWaves(0, 0), 1);
+    EXPECT_EQ(res.GetWaves(0, 0).second, 1);
+    EXPECT_EQ(res.GetWaves(0, 0).first->FreqIndex, 0);
+    EXPECT_EQ(res.GetWaves(0, 0).first->AmpSf, 63);
+    //EXPECT_EQ(res.GetWaves(0, 0).first->PhaseIndex, 0);
     Dumper.Dump(&res, 1, 1);
 }
 
 
 TEST(AT3PGHA, 689hz0625__partial_frame_mono) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 32, 128}}, {});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 32, 128}}, {});
     EXPECT_EQ(res.NumToneBands, 1);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
@@ -111,19 +127,19 @@ TEST(AT3PGHA, 689hz0625__partial_frame_mono) {
 }
 
 TEST(AT3PGHA, 689hz0625_900hz__full_frame_mono) {
-    auto res = GenAndRunGha({{689.0625f, 16384, 0, 128}, {900.0, 8192, 0, 128}}, {});
+    auto res = GenAndRunGha({{689.0625f, 0, 16384, 0, 128}, {900.0, 0, 8192, 0, 128}}, {});
     EXPECT_EQ(res.NumToneBands, 1);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 2);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
     EXPECT_EQ(res.GetNumWaves(0, 0), 2);
     EXPECT_EQ(res.GetWaves(0, 0).second, 2);
-    EXPECT_EQ(res.GetWaves(0, 0).first[0].FreqIndex, 512);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].FreqIndex, 511);
     EXPECT_EQ(res.GetWaves(0, 0).first[1].FreqIndex, 668);
     Dumper.Dump(&res, 1, 1);
 }
 
 TEST(AT3PGHA, 400hz_800hz__full_frame_mono) {
-    auto res = GenAndRunGha({{400.0, 16384, 0, 128}, {800.0, 4096, 0, 128}}, {});
+    auto res = GenAndRunGha({{400.0, 0, 16384, 0, 128}, {800.0, 0, 4096, 0, 128}}, {});
     EXPECT_EQ(res.NumToneBands, 1);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 2);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
@@ -135,7 +151,7 @@ TEST(AT3PGHA, 400hz_800hz__full_frame_mono) {
 }
 
 TEST(AT3PGHA, 689hz0625_2067hz1875__full_frame_mono) {
-    auto res = GenAndRunGha({{689.0625f, 16384, 0, 128}, {689.0625f, 16384, 128, 256}}, {});
+    auto res = GenAndRunGha({{689.0625f, 0, 16384, 0, 128}, {689.0625f, 0, 16384, 128, 256}}, {});
     EXPECT_EQ(res.NumToneBands, 2);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 2);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 2);
@@ -151,7 +167,7 @@ TEST(AT3PGHA, 689hz0625_2067hz1875__full_frame_mono) {
 }
 
 TEST(AT3PGHA, 689hz0625_4823hz4375__full_frame_mono) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 0, 128}, {689.0625f, 16384, 256, 384}}, {});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 0, 128}, {689.0625f, 0, 16384, 256, 384}}, {});
     EXPECT_EQ(res.NumToneBands, 1);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
@@ -164,7 +180,7 @@ TEST(AT3PGHA, 689hz0625_4823hz4375__full_frame_mono) {
 // Two channels simple cases
 
 TEST(AT3PGHA, 689hz0625__full_frame_stereo_shared) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 0, 128}}, {{689.0625f, 32768, 0, 128}});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 0, 128}}, {{689.0625f, 0, 32768, 0, 128}});
     EXPECT_EQ(res.NumToneBands, 1);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
@@ -178,7 +194,7 @@ TEST(AT3PGHA, 689hz0625__full_frame_stereo_shared) {
 }
 
 TEST(AT3PGHA, 689hz0625__full_frame_stereo_own) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 0, 128}}, {{1000.0625f, 32768, 0, 128}});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 0, 128}}, {{1000.0625f, 0, 32768, 0, 128}});
     EXPECT_EQ(res.NumToneBands, 1);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
@@ -198,7 +214,7 @@ TEST(AT3PGHA, 689hz0625__full_frame_stereo_own) {
 }
 
 TEST(AT3PGHA, 689hz0625__full_frame_stereo_multiple_second) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 0, 128}}, {{689.0625f, 16384, 0, 128}, {900.0, 8192, 0, 128}});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 0, 128}}, {{689.0625f, 0, 16384, 0, 128}, {900.0, 0, 8192, 0, 128}});
     EXPECT_EQ(res.NumToneBands, 1);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
@@ -211,14 +227,14 @@ TEST(AT3PGHA, 689hz0625__full_frame_stereo_multiple_second) {
     EXPECT_EQ(res.Waves[1].WaveSbInfos.size(), 1);
     EXPECT_EQ(res.GetNumWaves(1, 0), 2);
     EXPECT_EQ(res.GetWaves(1, 0).second, 2);
-    EXPECT_EQ(res.GetWaves(1, 0).first[0].FreqIndex, 512);
+    EXPECT_EQ(res.GetWaves(1, 0).first[0].FreqIndex, 511);
     EXPECT_EQ(res.GetWaves(1, 0).first[1].FreqIndex, 668);
     Dumper.Dump(&res, 2, 1);
 }
 
 TEST(AT3PGHA, 689hz0625_2067hz1875__full_frame_stereo_first_is_leader) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 0, 128}, {689.0625f, 16384, 128, 256}},
-                            {{689.0625f, 32768, 0, 128}});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 0, 128}, {689.0625f, 0, 16384, 128, 256}},
+                            {{689.0625f, 0, 32768, 0, 128}});
     EXPECT_EQ(res.NumToneBands, 2);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 2);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 2);
@@ -239,8 +255,8 @@ TEST(AT3PGHA, 689hz0625_2067hz1875__full_frame_stereo_first_is_leader) {
 }
 
 TEST(AT3PGHA, 689hz0625_2067hz1875__full_frame_stereo_second_is_leader) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 0, 128}},
-                            {{689.0625f, 32768, 0, 128}, {689.0625f, 16384, 128, 256}});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 0, 128}},
+                            {{689.0625f, 0, 32768, 0, 128}, {689.0625f, 0, 16384, 128, 256}});
     EXPECT_EQ(res.NumToneBands, 2);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 2);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 2);
@@ -262,8 +278,8 @@ TEST(AT3PGHA, 689hz0625_2067hz1875__full_frame_stereo_second_is_leader) {
 }
 
 TEST(AT3PGHA, 689hz0625_2067hz1875_3445hz3125__full_frame_stereo_sharing_0_2) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 0, 128}, {689.0625f, 32768, 128, 256}, {689.0625f, 16384, 256, 384}},
-                            {{689.0625f, 32768, 0, 128}, {689.0625f, 16384, 256, 384}});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 0, 128}, {689.0625f, 0, 32768, 128, 256}, {689.0625f, 0, 16384, 256, 384}},
+                            {{689.0625f, 0, 32768, 0, 128}, {689.0625f, 0, 16384, 256, 384}});
     EXPECT_EQ(res.NumToneBands, 3);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 3);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 3);
@@ -286,8 +302,8 @@ TEST(AT3PGHA, 689hz0625_2067hz1875_3445hz3125__full_frame_stereo_sharing_0_2) {
 }
 
 TEST(AT3PGHA, 689hz0625_2067hz1875_3445hz3125__full_frame_stereo_folower_sharing_2) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 0, 128}, {689.0625f, 32768, 128, 256}, {689.0625f, 16384, 256, 384}},
-                            {{689.0625f, 16384, 256, 384}});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 0, 128}, {689.0625f, 0, 32768, 128, 256}, {689.0625f, 0, 16384, 256, 384}},
+                            {{689.0625f, 0, 16384, 256, 384}});
     EXPECT_EQ(res.NumToneBands, 3);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 3);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 3);
@@ -311,8 +327,8 @@ TEST(AT3PGHA, 689hz0625_2067hz1875_3445hz3125__full_frame_stereo_folower_sharing
 }
 
 TEST(AT3PGHA, 689hz0625_2067hz1875_3445hz3125__full_frame_stereo_folower_sharing_1) {
-    auto res = GenAndRunGha({{689.0625f, 32768, 0, 128}, {689.0625f, 32768, 128, 256}, {689.0625f, 16384, 256, 384}},
-                            {{689.0625f, 16384, 128, 256}});
+    auto res = GenAndRunGha({{689.0625f, 0, 32768, 0, 128}, {689.0625f, 0, 32768, 128, 256}, {689.0625f, 0, 16384, 256, 384}},
+                            {{689.0625f, 0, 16384, 128, 256}});
     EXPECT_EQ(res.NumToneBands, 3);
     EXPECT_EQ(res.Waves[0].WaveParams.size(), 3);
     EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 3);
@@ -441,3 +457,278 @@ TEST(AT3PGHA, max_tones_multiple_bands_full_frame_stereo) {
 }
 
 */
+
+TEST(AT3PGHA, 100hz__two_frames_mono) {
+    vector<float> buf(2048 * 2);
+
+    Gen({100.0f, 0, 32768, 0, 256}, buf);
+
+    memcpy(&buf[2048], &buf[128], sizeof(float) * 128);
+    memset(&buf[128], 0, sizeof(float) * 128);
+
+    std::vector<TAt3PGhaData> resBuf;
+    auto processor = MakeGhaProcessor0(false);
+
+    {
+        const auto& res = *processor->DoAnalize({&buf[0], &buf[2048]}, {nullptr, nullptr});
+
+        EXPECT_EQ(res.NumToneBands, 1);
+        EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
+        EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+        EXPECT_EQ(res.GetNumWaves(0, 0), 1);
+        EXPECT_EQ(res.GetWaves(0, 0).second, 1);
+        EXPECT_EQ(res.GetWaves(0, 0).first->FreqIndex, 74);
+        EXPECT_EQ(res.GetWaves(0, 0).first->AmpSf, 62);
+        EXPECT_EQ(res.GetWaves(0, 0).first->PhaseIndex, 0);
+
+        resBuf.push_back(res);
+    }
+
+    {
+        memset(&buf[0], 0, sizeof(float) * 128);
+        const auto& res = *processor->DoAnalize({&buf[2048], &buf[0]}, {nullptr, nullptr});
+
+        EXPECT_EQ(res.NumToneBands, 1);
+        EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
+        EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+        EXPECT_EQ(res.GetNumWaves(0, 0), 1);
+        EXPECT_EQ(res.GetWaves(0, 0).second, 1);
+        EXPECT_EQ(res.GetWaves(0, 0).first->FreqIndex, 74);
+        EXPECT_EQ(res.GetWaves(0, 0).first->AmpSf, 62);
+        EXPECT_EQ(res.GetWaves(0, 0).first->PhaseIndex, 20);
+
+        resBuf.push_back(res);
+    }
+    Dumper.Dump(resBuf.data(), 1, 2);
+}
+
+TEST(AT3PGHA, 100hz_than_500hz_than_100hz__3_frames_mono) {
+    vector<float> buf(2048 * 2);
+
+    Gen({100.0f, 0, 32768, 0, 128}, buf);
+    Gen({500.0f, 0, 32768, 128, 256}, buf);
+
+    memcpy(&buf[2048], &buf[128], sizeof(float) * 128);
+    memset(&buf[128], 0, sizeof(float) * 128);
+
+    std::vector<TAt3PGhaData> resBuf;
+    auto processor = MakeGhaProcessor0(false);
+
+    {
+        const auto& res = *processor->DoAnalize({&buf[0], &buf[2048]}, {nullptr, nullptr});
+
+        EXPECT_EQ(res.NumToneBands, 1);
+        EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
+        EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+        EXPECT_EQ(res.GetNumWaves(0, 0), 1);
+        EXPECT_EQ(res.GetWaves(0, 0).second, 1);
+        EXPECT_EQ(res.GetWaves(0, 0).first->FreqIndex, 74);
+        EXPECT_EQ(res.GetWaves(0, 0).first->AmpSf, 62);
+        EXPECT_EQ(res.GetWaves(0, 0).first->PhaseIndex, 0);
+
+        resBuf.push_back(res);
+    }
+
+    {
+        memset(&buf[0], 0, sizeof(float) * 128);
+        Gen({100.0f, 0, 32768, 0, 128}, buf);
+        const auto& res = *processor->DoAnalize({&buf[2048], &buf[0]}, {nullptr, nullptr});
+
+        EXPECT_EQ(res.NumToneBands, 1);
+        EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
+        EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+        EXPECT_EQ(res.GetNumWaves(0, 0), 1);
+        EXPECT_EQ(res.GetWaves(0, 0).second, 1);
+        EXPECT_EQ(res.GetWaves(0, 0).first->FreqIndex, 371);
+        EXPECT_EQ(res.GetWaves(0, 0).first->AmpSf, 62);
+        EXPECT_EQ(res.GetWaves(0, 0).first->PhaseIndex, 0);
+
+        resBuf.push_back(res);
+    }
+    {
+        memset(&buf[2048], 0, sizeof(float) * 128);
+        const auto& res = *processor->DoAnalize({&buf[0], &buf[2048]}, {nullptr, nullptr});
+
+        EXPECT_EQ(res.NumToneBands, 1);
+        EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
+        EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+        EXPECT_EQ(res.GetNumWaves(0, 0), 1);
+        EXPECT_EQ(res.GetWaves(0, 0).second, 1);
+        EXPECT_EQ(res.GetWaves(0, 0).first->FreqIndex, 74);
+        EXPECT_EQ(res.GetWaves(0, 0).first->AmpSf, 62);
+        EXPECT_EQ(res.GetWaves(0, 0).first->PhaseIndex, 0);
+
+        resBuf.push_back(res);
+    }
+
+    Dumper.Dump(resBuf.data(), 1, 3);
+}
+
+
+TEST(AT3PGHA, 100hz__phase_two_frames_mono) {
+    vector<float> buf(2048 * 2);
+
+    Gen({100.0f, M_PI * 0.25, 32768, 0, 256}, buf);
+
+    memcpy(&buf[2048], &buf[128], sizeof(float) * 128);
+    memset(&buf[128], 0, sizeof(float) * 128);
+
+    auto processor = MakeGhaProcessor0(false);
+    const auto& res = *processor->DoAnalize({&buf[0], &buf[2048]}, {nullptr, nullptr});
+
+    EXPECT_EQ(res.NumToneBands, 1);
+    EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
+    EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+    EXPECT_EQ(res.GetNumWaves(0, 0), 1);
+    EXPECT_EQ(res.GetWaves(0, 0).second, 1);
+    EXPECT_EQ(res.GetWaves(0, 0).first->FreqIndex, 74);
+    EXPECT_EQ(res.GetWaves(0, 0).first->AmpSf, 62);
+    EXPECT_EQ(res.GetWaves(0, 0).first->PhaseIndex, 4);
+}
+
+
+TEST(AT3PGHA, 689hz0625__two_frames_mono) {
+    vector<float> buf(2048 * 2);
+
+    Gen({689.0625f, 0, 32768, 0, 256}, buf);
+
+    memcpy(&buf[2048], &buf[128], sizeof(float) * 128);
+    memset(&buf[128], 0, sizeof(float) * 128);
+
+    auto processor = MakeGhaProcessor0(false);
+    const auto& res = *processor->DoAnalize({&buf[0], &buf[2048]}, {nullptr, nullptr});
+
+    EXPECT_EQ(res.NumToneBands, 1);
+    EXPECT_EQ(res.Waves[0].WaveParams.size(), 1);
+    EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+    EXPECT_EQ(res.GetNumWaves(0, 0), 1);
+    EXPECT_EQ(res.GetWaves(0, 0).second, 1);
+    EXPECT_EQ(res.GetWaves(0, 0).first->FreqIndex, 512);
+    EXPECT_EQ(res.GetWaves(0, 0).first->AmpSf, 63);
+}
+
+TEST(AT3PGHA, 689hz0625_1000hz__two_frames_mono) {
+    vector<float> buf(2048 * 2);
+
+    Gen({689.0625f, 0, 16384, 0, 256}, buf);
+    Gen({1000.0f, 0, 16384, 0, 256}, buf);
+
+    memcpy(&buf[2048], &buf[128], sizeof(float) * 128);
+    memset(&buf[128], 0, sizeof(float) * 128);
+
+    auto processor = MakeGhaProcessor0(false);
+    const auto& res = *processor->DoAnalize({&buf[0], &buf[2048]}, {nullptr, nullptr});
+
+    EXPECT_EQ(res.NumToneBands, 1);
+    EXPECT_EQ(res.Waves[0].WaveParams.size(), 2);
+    EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+    EXPECT_EQ(res.GetNumWaves(0, 0), 2);
+    EXPECT_EQ(res.GetWaves(0, 0).second, 2);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].FreqIndex, 511);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].AmpSf, 58);
+    EXPECT_EQ(res.GetWaves(0, 0).first[1].FreqIndex, 742); //TODO: should be 743
+    EXPECT_EQ(res.GetWaves(0, 0).first[1].AmpSf, 58);
+}
+
+TEST(AT3PGHA, 500hz_1000hz__two_frames_mono) {
+    vector<float> buf(2048 * 2);
+
+    Gen({500.0f, 0, 16384, 0, 256}, buf);
+    Gen({1000.0f, 0, 2048, 0, 256}, buf);
+
+    memcpy(&buf[2048], &buf[128], sizeof(float) * 128);
+    memset(&buf[128], 0, sizeof(float) * 128);
+
+    auto processor = MakeGhaProcessor0(false);
+    const auto& res = *processor->DoAnalize({&buf[0], &buf[2048]}, {nullptr, nullptr});
+
+    EXPECT_EQ(res.NumToneBands, 1);
+    EXPECT_EQ(res.Waves[0].WaveParams.size(), 2);
+    EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+    EXPECT_EQ(res.GetNumWaves(0, 0), 2);
+    EXPECT_EQ(res.GetWaves(0, 0).second, 2);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].FreqIndex, 371);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].AmpSf, 58);
+    EXPECT_EQ(res.GetWaves(0, 0).first[1].FreqIndex, 742); //TODO: should be 743
+    EXPECT_EQ(res.GetWaves(0, 0).first[1].AmpSf, 46);
+}
+
+TEST(AT3PGHA, 500hz_1000hz__phase_two_frames_mono) {
+    vector<float> buf(2048 * 2);
+
+    Gen({500.0f, M_PI * 0.5, 16384, 0, 256}, buf);
+    Gen({1000.0f, M_PI * 0.25, 2048, 0, 256}, buf);
+
+    memcpy(&buf[2048], &buf[128], sizeof(float) * 128);
+    memset(&buf[128], 0, sizeof(float) * 128);
+
+    auto processor = MakeGhaProcessor0(false);
+    const auto& res = *processor->DoAnalize({&buf[0], &buf[2048]}, {nullptr, nullptr});
+
+    EXPECT_EQ(res.NumToneBands, 1);
+    EXPECT_EQ(res.Waves[0].WaveParams.size(), 2);
+    EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+    EXPECT_EQ(res.GetNumWaves(0, 0), 2);
+    EXPECT_EQ(res.GetWaves(0, 0).second, 2);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].FreqIndex, 371);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].AmpSf, 59);
+    EXPECT_EQ(res.GetWaves(0, 0).first[1].FreqIndex, 742); //TODO: should be 743
+    EXPECT_EQ(res.GetWaves(0, 0).first[1].AmpSf, 46);
+    Dumper.Dump(&res, 1, 1);
+}
+
+
+TEST(AT3PGHA, 250hz_500hz_1000hz__two_frames_mono) {
+    vector<float> buf(2048 * 2);
+
+    Gen({250.0f, 0, 16384, 0, 256}, buf);
+    Gen({500.0f, 0, 4096, 0, 256}, buf);
+    Gen({1000.0f, 0, 2048, 0, 256}, buf);
+
+    memcpy(&buf[2048], &buf[128], sizeof(float) * 128);
+    memset(&buf[128], 0, sizeof(float) * 128);
+
+    auto processor = MakeGhaProcessor0(false);
+    const auto& res = *processor->DoAnalize({&buf[0], &buf[2048]}, {nullptr, nullptr});
+
+    EXPECT_EQ(res.NumToneBands, 1);
+    EXPECT_EQ(res.Waves[0].WaveParams.size(), 3);
+    EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+    EXPECT_EQ(res.GetNumWaves(0, 0), 3);
+    EXPECT_EQ(res.GetWaves(0, 0).second, 3);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].FreqIndex, 186);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].AmpSf, 58);
+    EXPECT_EQ(res.GetWaves(0, 0).first[1].FreqIndex, 371);
+    EXPECT_EQ(res.GetWaves(0, 0).first[1].AmpSf, 50);
+    EXPECT_EQ(res.GetWaves(0, 0).first[2].FreqIndex, 742);
+    EXPECT_EQ(res.GetWaves(0, 0).first[2].AmpSf, 46);
+}
+
+TEST(AT3PGHA, 250hz_500hz_1000hz_1200hz__two_frames_mono) {
+    vector<float> buf(2048 * 2);
+
+    Gen({250.0f, 0, 16384, 0, 256}, buf);
+    Gen({500.0f, 0, 8000, 0, 256}, buf);
+    Gen({1000.0f, 0, 4096, 0, 256}, buf);
+    Gen({1200.0f, 0, 2048, 0, 256}, buf);
+
+    memcpy(&buf[2048], &buf[128], sizeof(float) * 128);
+    memset(&buf[128], 0, sizeof(float) * 128);
+
+    auto processor = MakeGhaProcessor0(false);
+    const auto& res = *processor->DoAnalize({&buf[0], &buf[2048]}, {nullptr, nullptr});
+
+    EXPECT_EQ(res.NumToneBands, 1);
+    EXPECT_EQ(res.Waves[0].WaveParams.size(), 4);
+    EXPECT_EQ(res.Waves[0].WaveSbInfos.size(), 1);
+    EXPECT_EQ(res.GetNumWaves(0, 0), 4);
+    EXPECT_EQ(res.GetWaves(0, 0).second, 4);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].FreqIndex, 186);
+    EXPECT_EQ(res.GetWaves(0, 0).first[0].AmpSf, 58);
+    EXPECT_EQ(res.GetWaves(0, 0).first[1].FreqIndex, 371);
+    EXPECT_EQ(res.GetWaves(0, 0).first[1].AmpSf, 54);
+    EXPECT_EQ(res.GetWaves(0, 0).first[2].FreqIndex, 742);
+    EXPECT_EQ(res.GetWaves(0, 0).first[2].AmpSf, 50);
+    EXPECT_EQ(res.GetWaves(0, 0).first[3].FreqIndex, 891);
+    EXPECT_EQ(res.GetWaves(0, 0).first[3].AmpSf, 46);
+}
diff --git a/src/atrac/at3p/ghasend_tool.cpp b/src/atrac/at3p/ghasend_tool.cpp
new file mode 100644
index 0000000..e3f95a4
--- /dev/null
+++ b/src/atrac/at3p/ghasend_tool.cpp
@@ -0,0 +1,90 @@
+#include "at3p_bitstream.h"
+#include "oma.h"
+
+#include <string>
+#include <iostream>
+
+using namespace std;
+
+void process(const string& in, NAtracDEnc::TAt3PBitStream* bs) {
+    size_t pos = 0;
+    int cur_num_pos = 0;
+    enum {
+        TAB,
+        NUM,
+        ERR,
+    } state = NUM;
+
+    std::vector<int> nums;
+
+    while (pos < in.size()) {
+        switch (state) {
+            case TAB:
+                if (in[pos] == '\t') {
+                    break;
+                } else if (('0' <= in[pos] && in[pos] <= '9') || in[pos] == '-') {
+                    state = NUM;
+                    cur_num_pos = pos;
+                    break;
+                } else {
+                    fprintf(stderr, "TAB state: %s\n", &in[pos]);
+                    abort();
+                }
+            case NUM:
+                if (in[pos] == '\t') {
+                    nums.push_back(stoi(in.substr(cur_num_pos, pos - cur_num_pos)));
+                    state = TAB;
+                    break;
+                } else if (pos == in.size() - 1) {
+                    nums.push_back(stoi(in.substr(cur_num_pos)));
+                    break;
+                } else if (('0' <= in[pos] && in[pos] <= '9') || in[pos] == '-') {
+                    break;
+                } else {
+                    fprintf(stderr, "NUM state: %s\n", &in[pos]);
+                    abort();
+                }
+            case ERR:
+                abort();
+        }
+        pos++;
+    }
+
+    if (nums.size() != 3)
+        return;
+
+    std::cerr << "gen: " << nums[0] << '\t' << nums[1] << '\t' << nums[2] << std::endl;
+
+    NAtracDEnc::TAt3PGhaData frame;
+    frame.NumToneBands = 1;
+    frame.Waves[0].WaveParams.push_back(NAtracDEnc::TAt3PGhaData::TWaveParam{(uint32_t)nums[1], 53, 0, 0});
+    frame.Waves[0].WaveSbInfos.resize(1);
+    frame.Waves[0].WaveSbInfos[0].WaveIndex = 0;
+    frame.Waves[0].WaveSbInfos[0].WaveNums = 1;
+    frame.Waves[0].WaveSbInfos[0].Envelope =  {(uint32_t)nums[0], (uint32_t)nums[2]};
+
+    bs->WriteFrame(1, &frame);
+}
+
+int main(int argc, char** argv) {
+    if (argc != 2)
+        return -1;
+
+    string path(argv[1]);
+
+    std::unique_ptr<TOma> out(new TOma(path,
+            "test",
+            1,
+            1, OMAC_ID_ATRAC3PLUS,
+            2048,
+            false));
+
+    NAtracDEnc::TAt3PBitStream bs(out.get(), 2048);
+
+    cout << "output: " << path << endl;
+    string textline;
+    while (getline(cin, textline)) {
+        process(textline, &bs);
+    }
+    return 0;
+}
diff --git a/src/lib/libgha b/src/lib/libgha
-Subproject dc14436986b7b1561ca5a521c3b8b5a95c6b052
+Subproject a9cefceaf856df82c6867e853a357b0298d566c
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index f6d1827..2cd1a7b 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -3,6 +3,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
 set (CMAKE_CXX_STANDARD 11)
 set (CMAKE_C_STANDARD 11)
 
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -fsanitize=address -fno-omit-frame-pointer")
 include_directories(
     "../src/lib"
 )