Minimal implementation of AT3P bitstream for residual signal.

* Write scale factor index, wordlen, codetable directly (without VLC)
* Use huffman tables for mantisas
* Some draft of the library to simplify bit allocation code
* Tonal encoding teporary disabled
* Allow c++14

15 files changed, 955 insertions, 34 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ac02187..eba4332 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,6 +1,6 @@
 CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
 
-set (CMAKE_CXX_STANDARD 11)
+set (CMAKE_CXX_STANDARD 14)
 set (CMAKE_C_STANDARD 11)
 
 #add_definitions( "-Wall -O2 -g -Rpass-analysis=loop-vectorize" )
@@ -99,6 +99,7 @@ set(SOURCE_ATRACDENC_IMPL
     atrac/at3p/at3p_mdct.cpp
     atrac/at3p/at3p_tables.cpp
     lib/mdct/mdct.cpp
+    lib/bs_encode/encode.cpp
 )
 
 add_library(pcm_io STATIC ${SOURCE_PCM_IO_LIB})
diff --git a/src/atrac/at3p/at3p.cpp b/src/atrac/at3p/at3p.cpp
index 88d0176..514fa96 100644
--- a/src/atrac/at3p/at3p.cpp
+++ b/src/atrac/at3p/at3p.cpp
@@ -22,6 +22,9 @@
 
 #include "at3p_bitstream.h"
 #include "at3p_gha.h"
+#include "at3p_mdct.h"
+#include "at3p_tables.h"
+#include <atrac/atrac_scale.h>
 
 #include <cassert>
 #include <vector>
@@ -43,6 +46,7 @@ private:
     struct TChannelCtx {
         TChannelCtx()
             : PqfCtx(at3plus_pqf_create_a_ctx())
+            , Specs(TAt3PEnc::NumSamples)
         {}
 
         ~TChannelCtx() {
@@ -55,8 +59,12 @@ private:
         float* CurBuf = nullptr;
         float Buf1[TAt3PEnc::NumSamples];
         float Buf2[TAt3PEnc::NumSamples];
+        TAt3pMDCT::THistBuf MdctBuf;
+        std::vector<float> Specs;
     };
 
+    TAt3pMDCT Mdct;
+    TScaler<NAt3p::TScaleTable> Scaler;
     TAt3PBitStream BitStream;
     vector<TChannelCtx> ChannelCtx;
     std::unique_ptr<IGhaProcessor> GhaProcessor;
@@ -65,7 +73,6 @@ private:
 TPCMEngine::EProcessResult TAt3PEnc::TImpl::
 EncodeFrame(const float* data, int channels)
 {
-
     int needMore = 0;
     for (int ch = 0; ch < channels; ch++) {
         float src[TAt3PEnc::NumSamples];
@@ -93,10 +100,27 @@ EncodeFrame(const float* data, int channels)
     const float* b2Cur = (channels == 2) ? ChannelCtx[1].CurBuf : nullptr;
     const float* b2Next = (channels == 2) ? ChannelCtx[1].NextBuf : nullptr;
 
-
     auto tonalBlock = GhaProcessor->DoAnalize({b1Cur, b1Next}, {b2Cur, b2Next});
 
-    BitStream.WriteFrame(channels, tonalBlock);
+    std::vector<std::vector<TScaledBlock>> scaledBlocks;
+    for (int ch = 0; ch < channels; ch++) {
+        auto& c = ChannelCtx[ch];
+        TAt3pMDCT::TPcmBandsData p;
+        float tmp[2048];
+        //TODO: scale window
+        for (size_t i = 0; i < 2048; i++) {
+            tmp[i] = c.CurBuf[i] / 32768.0;
+        }
+        for (size_t b = 0; b < 16; b++) {
+            p[b] = tmp + b * 128;
+        }
+        Mdct.Do(c.Specs.data(), p, c.MdctBuf);
+
+        const auto& block = Scaler.ScaleFrame(c.Specs, NAt3p::TScaleTable::TBlockSizeMod());
+        scaledBlocks.push_back(block);
+    }
+
+    BitStream.WriteFrame(channels, tonalBlock, scaledBlocks);
 
     for (int ch = 0; ch < channels; ch++) {
         std::swap(ChannelCtx[ch].NextBuf, ChannelCtx[ch].CurBuf);
diff --git a/src/atrac/at3p/at3p_bitstream.cpp b/src/atrac/at3p/at3p_bitstream.cpp
index a43ed18..93c29df 100644
--- a/src/atrac/at3p/at3p_bitstream.cpp
+++ b/src/atrac/at3p/at3p_bitstream.cpp
@@ -16,14 +16,17 @@
  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  */
 
+#include "at3p_bitstream_impl.h"
 #include "at3p_bitstream.h"
 #include "at3p_gha.h"
 #include "at3p_tables.h"
-#include <lib/bitstream/bitstream.h>
 #include <env.h>
 #include <util.h>
 
+#include "ff/atrac3plus_data.h"
+
 #include <iostream>
+#include <memory>
 
 namespace NAtracDEnc {
 
@@ -88,8 +91,251 @@ TTonePackResult CreateFreqBitPack(const TAt3PGhaData::TWaveParam* const param, i
     }
 }
 
+size_t TDumper::GetConsumption() const noexcept
+{
+    return std::accumulate(Buf.begin(), Buf.end(), 0,
+        [](size_t acc, const std::pair<uint8_t, uint8_t>& x) noexcept -> size_t { return acc + x.second; });
+}
+
+IBitStreamPartEncoder::EStatus TConfigure::Encode(void* frameData, TBitAllocHandler&)
+{
+    TSpecFrame* frame = TSpecFrame::Cast(frameData);
+
+    size_t numQuantUnits = 28;
+    frame->NumQuantUnits = numQuantUnits;
+    frame->WordLen.resize(numQuantUnits);
+
+    for (size_t i = 0; i < frame->WordLen.size(); i++) {
+        static uint8_t allocTable[32] = {
+            7, 7, 7, 7, 7, 6, 6, 6,
+            6, 6, 6, 5, 5, 5, 5, 5,
+            5, 5, 5, 5, 5, 5, 5, 4,
+            3, 2, 1, 1, 1, 1, 1, 1
+        };
+        frame->WordLen[i].first = allocTable[i];
+        frame->WordLen[i].second = allocTable[i];
+    }
+
+    frame->SfIdx.resize(numQuantUnits);
+
+    for (size_t i = 0; i < frame->SfIdx.size(); i++) {
+        frame->SfIdx[i].first = frame->Chs[0].ScaledBlocks.at(i).ScaleFactorIndex;
+        if (frame->Chs.size() > 1)
+            frame->SfIdx[i].second = frame->Chs[1].ScaledBlocks.at(i).ScaleFactorIndex;
+    }
+
+    frame->SpecTabIdx.resize(numQuantUnits);
+
+    for (size_t i = 0; i < frame->SpecTabIdx.size(); i++) {
+        frame->SpecTabIdx[i].first = 7;
+        frame->SpecTabIdx[i].second = 7;
+    }
+
+    Insert(numQuantUnits - 1, 5);
+    Insert(0, 1); //mute flag
+
+    frame->AllocatedBits = GetConsumption();
+
+    return EStatus::Ok;
+}
+
+IBitStreamPartEncoder::EStatus TWordLenEncoder::Encode(void* frameData, TBitAllocHandler&) {
+    auto specFrame = TSpecFrame::Cast(frameData);
+
+    ASSERT(specFrame->WordLen.size() > specFrame->NumQuantUnits);
+    for (size_t ch = 0; ch < specFrame->Chs.size(); ch++) {
+
+        Insert(0, 2); // 0 - constant number of bits
+
+        if (ch == 0) {
+            for (size_t i = 0; i < specFrame->NumQuantUnits; i++) {
+                Insert(specFrame->WordLen[i].first, 3);
+            }
+        } else {
+            for (size_t i = 0; i < specFrame->NumQuantUnits; i++) {
+                Insert(specFrame->WordLen[i].second, 3);
+            }
+        }
+    }
+
+    return EStatus::Ok;
+}
+
+IBitStreamPartEncoder::EStatus TSfIdxEncoder::Encode(void* frameData, TBitAllocHandler&) {
+    auto specFrame = TSpecFrame::Cast(frameData);
+
+    if (specFrame->SfIdx.empty()) {
+        return EStatus::Ok;
+    }
+
+    for (size_t ch = 0; ch < specFrame->Chs.size(); ch++) {
+
+        Insert(0, 2); // 0 - constant number of bits
+
+        if (ch == 0) {
+            for (size_t i = 0; i < specFrame->NumQuantUnits; i++) {
+                Insert(specFrame->SfIdx[i].first, 6);
+            }
+        } else {
+            for (size_t i = 0; i < specFrame->NumQuantUnits; i++) {
+                Insert(specFrame->SfIdx[i].second, 6);
+            }
+        }
+    }
+
+    return EStatus::Ok;
+}
+
+IBitStreamPartEncoder::EStatus TCodeTabEncoder::Encode(void* frameData, TBitAllocHandler&) {
+    auto specFrame = TSpecFrame::Cast(frameData);
+
+    const size_t useFullTable = 1;
+    Insert(useFullTable, 1); // use full table
+
+    for (size_t ch = 0; ch < specFrame->Chs.size(); ch++) {
+
+        Insert(0, 1); // table type
+
+        Insert(0, 2); // 0 - constant number of bits
+
+        Insert(0, 1); // num_coded_vals equal to used_quant_units
+
+        if (ch == 0) {
+            for (size_t i = 0; i < specFrame->NumQuantUnits; i++) {
+                Insert(specFrame->SpecTabIdx[i].first, useFullTable + 2);
+            }
+        } else {
+            for (size_t i = 0; i < specFrame->NumQuantUnits; i++) {
+                Insert(specFrame->SpecTabIdx[i].second, useFullTable + 2);
+            }
+        }
+    }
+
+    return EStatus::Ok;
+}
+
+void TQuantUnitsEncoder::EncodeQuSpectra(const int* qspec, const size_t num_spec, const size_t idx) {
+    const Atrac3pSpecCodeTab *tab = &atrac3p_spectra_tabs[idx];
+    const std::array<TVlcElement, 256>& vlcTab = HuffTabs.VlcSpecs[idx];
+
+    size_t groupSize = tab->group_size;
+    size_t numCoeffs = tab->num_coeffs;
+    size_t bitsCoeff = tab->bits;
+    bool isSigned  = tab->is_signed;
+
+    for (size_t pos = 0; pos < num_spec;) {
+        if (groupSize != 1) {
+            // TODO: Acording to FFmpeg it should be possible
+            // to skip group, if all rest of coeffs is zero
+            // but this should be checked with real AT3P decoder
+            Insert(1, 1);
+        }
+
+        for (size_t j = 0; j < groupSize; j++) {
+            uint32_t val = 0;
+            int8_t signs[4] = {0};
+            for (size_t i = 0; i < numCoeffs; i++) {
+                int16_t t = qspec[pos++];
+#ifndef NDEBUG
+                {
+                    uint16_t x = std::abs(t);
+                    x >>= (uint16_t)(bitsCoeff - (int)isSigned);
+                    ASSERT(x == 0);
+                }
+#endif
+                if (!isSigned && t != 0) {
+                    signs[i] = t > 0 ? 1 : -1;
+                    if (t < 0)
+                        t = -t;
+                } else {
+                    t = t & ((1u << (bitsCoeff)) - 1);
+                }
+                t <<= (bitsCoeff * i);
+                val |= t;
+            }
+
+            ASSERT(val > 255);
+
+            const TVlcElement& el = vlcTab.at(val);
+
+            Insert(el.Code, el.Len);
+            for (size_t i = 0; i < 4; i++) {
+                if (signs[i] != 0) {
+                    if (signs[i] > 0) {
+                        Insert(0, 1);
+                    } else {
+                        Insert(1, 1);
+                    }
+                }
+            }
+        }
+    }
+}
+
+IBitStreamPartEncoder::EStatus TQuantUnitsEncoder::Encode(void* frameData, TBitAllocHandler&) {
+    auto specFrame = TSpecFrame::Cast(frameData);
+    for (size_t ch = 0; ch < specFrame->Chs.size(); ch++) {
+        auto& chData = specFrame->Chs.at(ch);
+        auto scaledBlocks = chData.ScaledBlocks;
+
+        int * const mant = chData.Mant;
+        for (size_t qu = 0; qu < specFrame->NumQuantUnits; qu++) {
+            auto lenIdx = (ch == 0) ?
+                        specFrame->WordLen.at(qu).first :
+                        specFrame->WordLen.at(qu).second;
+            const uint32_t first = TScaleTable::BlockSizeTab[qu];
+            const uint32_t last = TScaleTable::BlockSizeTab[qu+1];
+            auto mul = 1/atrac3p_mant_tab[lenIdx];
+
+            const float* values = scaledBlocks.at(qu).Values.data();
+
+            QuantMantisas(values, first, last, mul, false, mant);
+        }
+    }
+
+    for (size_t ch = 0; ch < specFrame->Chs.size(); ch++) {
+        auto& chData = specFrame->Chs.at(ch);
+        int * const mant = chData.Mant;
+        for (size_t qu = 0; qu < specFrame->NumQuantUnits; qu++) {
+            const size_t numSpecs = TScaleTable::BlockSizeTab[qu + 1] -
+                TScaleTable::BlockSizeTab[qu];
+            size_t tabIndex = (ch == 0) ?
+                specFrame->SpecTabIdx.at(qu).first :
+                specFrame->SpecTabIdx.at(qu).second;
+            size_t wordLen = (ch == 0) ?
+                specFrame->WordLen.at(qu).first :
+                specFrame->WordLen.at(qu).second;
+
+            tabIndex = tabIndex * 7 + wordLen - 1;
+
+            EncodeQuSpectra(&mant[TScaleTable::BlockSizeTab[qu]], numSpecs, tabIndex);
+        }
+        if (true /*frame.NumUsedQuantUnits > 2*/) {
+            size_t numPwrSpec = atrac3p_subband_to_num_powgrps[atrac3p_qu_to_subband[specFrame->NumQuantUnits - 1]];
+            for (size_t i = 0; i < numPwrSpec; i++) {
+                Insert(15, 4);
+            }
+        }
+    }
+    return EStatus::Ok;
+
+}
+
+static std::vector<IBitStreamPartEncoder::TPtr> CreateEncParts()
+{
+    vector<IBitStreamPartEncoder::TPtr> parts;
+    parts.emplace_back(new TConfigure());
+    parts.emplace_back(new TWordLenEncoder());
+    parts.emplace_back(new TSfIdxEncoder());
+    parts.emplace_back(new TCodeTabEncoder());
+    parts.emplace_back(new TQuantUnitsEncoder());
+
+    return parts;
+}
+
 TAt3PBitStream::TAt3PBitStream(ICompressedOutput* container, uint16_t frameSz)
     : Container(container)
+    , Encoder(CreateEncParts())
     , FrameSz(frameSz)
 {
     NEnv::SetRoundFloat();
@@ -239,7 +485,7 @@ static void WriteTonalBlock(NBitStream::TBitStream& bs, int channels, const TAt3
     }
 }
 
-void TAt3PBitStream::WriteFrame(int channels, const TAt3PGhaData* tonalBlock)
+void TAt3PBitStream::WriteFrame(int channels, const TAt3PGhaData* /*tonalBlock*/, const std::vector<std::vector<TScaledBlock>>& scaledBlocks)
 {
     NBitStream::TBitStream bitStream;
     // First bit must be zero
@@ -250,34 +496,24 @@ 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 =  24;
-    bitStream.Write(num_quant_units - 1, 5);
+    TSpecFrame frame(FrameSz * 8, channels, scaledBlocks);
 
-    for (int ch = 0; ch < channels; ch++) {
-        bitStream.Write(0, 2); // channel wordlen mode
-        for (int j = 0; j < num_quant_units; j++) {
-            bitStream.Write(0, 3); // wordlen
-        }
-    }
+    Encoder.Do(&frame, bitStream);
 
-    // Skip some bits to produce correct zero bitstream
     if (channels == 2) {
-        bitStream.Write(0, 7);
-    } else {
-        bitStream.Write(0, 3);
+        bitStream.Write(0, 2); //swap_channels and negate_coeffs
     }
 
-    if (tonalBlock && tonalBlock->NumToneBands) {
-        // Bit indicate tonal block is used
-        bitStream.Write(1, 1);
-        WriteTonalBlock(bitStream, channels, tonalBlock);
-    } else {
-        bitStream.Write(0, 1);
+    for (size_t ch = 0; ch < frame.Chs.size(); ch++) {
+        bitStream.Write(0, 1); // window shape
     }
 
-    bitStream.Write(0, 1); // no noise info
+    for (size_t ch = 0; ch < frame.Chs.size(); ch++) {
+        bitStream.Write(0, 1); //gain comp
+    }
 
+    bitStream.Write(0, 1);
+    bitStream.Write(0, 1); // no noise info
     // Terminator
     bitStream.Write(3, 2);
 
diff --git a/src/atrac/at3p/at3p_bitstream.h b/src/atrac/at3p/at3p_bitstream.h
index f69256f..4baba4d 100644
--- a/src/atrac/at3p/at3p_bitstream.h
+++ b/src/atrac/at3p/at3p_bitstream.h
@@ -20,9 +20,12 @@
 
 #include "compressed_io.h"
 #include "at3p_gha.h"
+#include <lib/bs_encode/encode.h>
 
 namespace NAtracDEnc {
 
+class TScaledBlock;
+
 struct TAt3PGhaData;
 
 enum class ETonePackOrder : bool {
@@ -45,9 +48,10 @@ TTonePackResult CreateFreqBitPack(const TAt3PGhaData::TWaveParam* param, int len
 class TAt3PBitStream {
 public:
     TAt3PBitStream(ICompressedOutput* container, uint16_t frameSz);
-    void WriteFrame(int channels, const TAt3PGhaData* tonalData);
+    void WriteFrame(int channels, const TAt3PGhaData* tonalData, const std::vector<std::vector<TScaledBlock>>& scaledBlocks);
 private:
     ICompressedOutput* Container;
+    TBitStreamEncoder Encoder;
     const uint16_t FrameSz;
 };
 
diff --git a/src/atrac/at3p/at3p_bitstream_impl.h b/src/atrac/at3p/at3p_bitstream_impl.h
new file mode 100644
index 0000000..f0654b6
--- /dev/null
+++ b/src/atrac/at3p/at3p_bitstream_impl.h
@@ -0,0 +1,113 @@
+#pragma once
+
+/*
+ * This file is part of AtracDEnc.
+ *
+ * AtracDEnc is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * AtracDEnc is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with AtracDEnc; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include <lib/bitstream/bitstream.h>
+#include <lib/bs_encode/encode.h>
+#include <atrac/atrac_scale.h>
+#include <vector>
+
+namespace NAtracDEnc {
+
+struct TSpecFrame {
+    TSpecFrame(size_t sz, size_t channels,
+               const std::vector<std::vector<TScaledBlock>>& scaledBlocks)
+        : SizeBits(sz)
+        , AllocatedBits(0)
+    {
+        Chs.reserve(channels);
+        for (size_t i = 0; i < channels; i++) {
+            Chs.emplace_back(TChannel(scaledBlocks[i]));
+        }
+    }
+
+    const size_t SizeBits;
+    size_t NumQuantUnits;
+    std::vector<std::pair<uint8_t, uint8_t>> WordLen;
+    std::vector<std::pair<uint8_t, uint8_t>> SfIdx;
+    std::vector<std::pair<uint8_t, uint8_t>> SpecTabIdx;
+
+    struct TChannel {
+        TChannel(const std::vector<TScaledBlock>& scaledBlock)
+            : ScaledBlocks(scaledBlock)
+        {}
+        const std::vector<TScaledBlock>& ScaledBlocks;
+        int Mant[2048];
+    };
+
+    std::vector<TChannel> Chs;
+    size_t AllocatedBits;
+    static TSpecFrame* Cast(void* p) { return reinterpret_cast<TSpecFrame*>(p); }
+};
+
+
+class TDumper : public IBitStreamPartEncoder {
+public:
+    void Dump(NBitStream::TBitStream& bs) override {
+        for (const auto& pair : Buf) {
+            bs.Write(pair.first, pair.second);
+        }
+        Buf.clear();
+    }
+protected:
+    size_t GetConsumption() const noexcept;
+    // value, nbits
+    void Insert(uint16_t value, uint8_t nbits) { Buf.emplace_back(std::make_pair(value, nbits)); }
+    std::vector<std::pair<uint16_t, uint8_t>> Buf;
+};
+
+class TConfigure : public TDumper {
+public:
+    TConfigure() = default;
+    EStatus Encode(void* frameData, TBitAllocHandler& ba) override;
+};
+
+class TWordLenEncoder : public TDumper {
+public:
+    TWordLenEncoder() = default;
+    EStatus Encode(void* frameData, TBitAllocHandler& ba) override;
+private:
+};
+
+class TSfIdxEncoder : public TDumper {
+public:
+    TSfIdxEncoder() = default;
+    EStatus Encode(void* frameData, TBitAllocHandler& ba) override;
+private:
+};
+
+class TCodeTabEncoder : public TDumper {
+public:
+    TCodeTabEncoder() = default;
+    EStatus Encode(void* frameData, TBitAllocHandler& ba) override;
+private:
+};
+
+class TQuantUnitsEncoder : public TDumper {
+public:
+    TQuantUnitsEncoder() = default;
+    EStatus Encode(void* frameData, TBitAllocHandler& ba) override;
+private:
+    void EncodeQuSpectra(const int* qspec, const size_t num_spec, const size_t idx);
+};
+
+
+
+
+}
diff --git a/src/atrac/at3p/at3p_bitstream_ut.cpp b/src/atrac/at3p/at3p_bitstream_ut.cpp
index 3129673..ddc33f3 100644
--- a/src/atrac/at3p/at3p_bitstream_ut.cpp
+++ b/src/atrac/at3p/at3p_bitstream_ut.cpp
@@ -1,4 +1,5 @@
 #include "at3p_bitstream.h"
+#include "at3p_bitstream_impl.h"
 #include <gtest/gtest.h>
 #include <cmath>
 
@@ -108,3 +109,31 @@ TEST(AT3PBitstream, ToneFreqBitPack__1_2_1020_1021_1022) {
     EXPECT_EQ(r.Data[4].Bits, 2);
 }
 
+void FillFrameData(TSpecFrame& frame) {
+    frame.NumQuantUnits = 6;
+
+    for (size_t i = 0; i < frame.NumQuantUnits; i++) {
+        frame.WordLen.push_back({6, 6});
+    }
+}
+
+TEST(AT3PBitstream, Wordlen) {
+    std::vector<IBitStreamPartEncoder::TPtr> encoders;
+
+    encoders.emplace_back(new TWordLenEncoder());
+
+    NBitStream::TBitStream bs;
+    TBitStreamEncoder encoder(std::move(encoders));
+
+    std::vector<std::vector<TScaledBlock>> scaledBlocks;
+    scaledBlocks.resize(2);
+
+    TSpecFrame frame(444, 2, scaledBlocks);
+
+    FillFrameData(frame);
+
+    encoder.Do(&frame, bs);
+
+    EXPECT_EQ(bs.GetSizeInBits(), 40);
+}
+
diff --git a/src/atrac/at3p/at3p_gha_ut.cpp b/src/atrac/at3p/at3p_gha_ut.cpp
index b0cd123..e152ae3 100644
--- a/src/atrac/at3p/at3p_gha_ut.cpp
+++ b/src/atrac/at3p/at3p_gha_ut.cpp
@@ -81,7 +81,7 @@ public:
         TAt3PBitStream bs(out.get(), 2048);
 
         for (size_t i = 0; i < len; i++) {
-            bs.WriteFrame(channels, gha + i);
+        //    bs.WriteFrame(channels, gha + i);
         }
     }
 private:
diff --git a/src/atrac/at3p/at3p_tables.cpp b/src/atrac/at3p/at3p_tables.cpp
index 3bfccda..a4d6021 100644
--- a/src/atrac/at3p/at3p_tables.cpp
+++ b/src/atrac/at3p/at3p_tables.cpp
@@ -20,19 +20,68 @@
 #include "ff/atrac3plus_data.h"
 
 #include <iostream>
+#include <sstream>
 
 namespace NAtracDEnc {
 namespace NAt3p {
 
-static void __attribute__ ((noinline)) GenHuffmanEncTable(const uint8_t *cb, const uint8_t *xlat, uint16_t outLen, TVlcElement* out)
+static struct TInvMantTab {
+public:
+    TInvMantTab() {
+        Data[0] = 0.0; //unused (for zero lenght on the decoder)
+        for (size_t i = 1; i < 8; i++) {
+            Data[i] = 1.0 / atrac3p_mant_tab[i];
+        }
+    }
+    float Data[8];
+} InvMantTab_;
+
+float InvMantTab(size_t i) { return InvMantTab_.Data[i]; }
+
+static struct TScaleTableInitializer {
+public:
+    TScaleTableInitializer() {
+        const std::array<float, 64> src = {
+            0.027852058,  0.0350914,   0.044212341, 0.055704117,
+            0.0701828,    0.088424683, 0.11140823,  0.1403656,
+            0.17684937,   0.22281647,  0.2807312,   0.35369873,
+            0.44563293,   0.5614624,   0.70739746,  0.89126587,
+            1.1229248,    1.4147949,   1.7825317,   2.2458496,
+            2.8295898,    3.5650635,   4.4916992,   5.6591797,
+            7.130127,     8.9833984,   11.318359,   14.260254,
+            17.966797,    22.636719,   28.520508,   35.933594,
+            45.273438,    57.041016,   71.867188,   90.546875,
+            114.08203,    143.73438,   181.09375,   228.16406,
+            287.46875,    362.1875,    456.32812,   574.9375,
+            724.375,      912.65625,   1149.875,    1448.75,
+            1825.3125,    2299.75,     2897.5,      3650.625,
+            4599.5,       5795.0,      7301.25,     9199.0,
+            11590.0,      14602.5,     18398.0,     23180.0,
+            29205.0,      36796.0,     46360.0,     58410.0
+        };
+
+        for (size_t i = 0; i < src.size(); i++) {
+            TScaleTable::ScaleTable[i] = src[i] / src.back();
+        }
+    }
+} ScaleTableInitializer_;
+
+float TScaleTable::ScaleTable[64];
+constexpr uint32_t TScaleTable::BlocksPerBand[NumQMF + 1];
+constexpr uint32_t TScaleTable::SpecsPerBlock[MaxBfus];
+constexpr uint32_t TScaleTable::BlockSizeTab[MaxBfus + 1];
+
+static uint16_t __attribute__ ((noinline)) GenHuffmanEncTable(const uint8_t *cb, const uint8_t *xlat, uint16_t outLen, TVlcElement* const out)
 {
     uint16_t index = 0;
     uint16_t code = 0;
     for (int b = 1; b <= 12; b++) {
         for (int i = *cb++; i > 0; i--) {
             uint8_t val = xlat[index];
-            if (val > outLen) {
-                throw std::runtime_error("encoded value out of range, invalid haffman table");
+            if (val >= outLen) {
+                std::stringstream ss;
+                ss << "encoded value out of range, invalid haffman table, got: " << (int)val << " table len: " << (int)outLen;
+                throw std::runtime_error(ss.str());
             }
             TVlcElement* cur = out + val;
             cur->Code = code;
@@ -42,17 +91,33 @@ static void __attribute__ ((noinline)) GenHuffmanEncTable(const uint8_t *cb, con
         }
         code <<= 1;
     }
+    return index;
 }
 
 template<typename T>
-void GenHuffmanEncTable(const uint8_t *cb, const uint8_t *xlat, T& out)
+uint16_t GenHuffmanEncTable(const uint8_t *cb, const uint8_t *xlat, T& out)
 {
-    GenHuffmanEncTable(cb, xlat, out.size(), out.data());
+    return GenHuffmanEncTable(cb, xlat, out.size(), out.data());
 }
 
 THuffTables::THuffTables()
 {
     GenHuffmanEncTable(&atrac3p_tone_cbs[0][0], &atrac3p_tone_xlats[0], NumToneBands);
+
+    for (size_t i = 0; i < VlcSpecs.size(); i++) {
+        for (size_t j = 0; j < VlcSpecs[i].size(); j++) {
+            VlcSpecs[i][j].Code = 0;
+            VlcSpecs[i][j].Len = 0;
+        }
+    }
+
+    for (int i = 0, x = 0; i < 112; i++) {
+        if (atrac3p_spectra_cbs[i][0] >= 0) {
+            x += GenHuffmanEncTable((uint8_t*)&atrac3p_spectra_cbs[i][0], &atrac3p_spectra_xlats[x], VlcSpecs[i]);
+        } else {
+            VlcSpecs[i] = VlcSpecs[-atrac3p_spectra_cbs[i][0]];
+        }
+    }
 }
 
 } // namespace NAt3p
diff --git a/src/atrac/at3p/at3p_tables.h b/src/atrac/at3p/at3p_tables.h
index 34d924b..0840273 100644
--- a/src/atrac/at3p/at3p_tables.h
+++ b/src/atrac/at3p/at3p_tables.h
@@ -20,11 +20,14 @@
 
 #include <array>
 #include <cstdint>
+#include <cstddef>
 
 namespace NAtracDEnc {
 
 namespace NAt3p {
 
+float InvMantTab(size_t i);
+
 struct TVlcElement {
     int16_t Code;
     int16_t Len;
@@ -33,6 +36,42 @@ struct TVlcElement {
 struct THuffTables {
     THuffTables();
     std::array<TVlcElement, 16> NumToneBands;
+    std::array<std::array<TVlcElement, 256>, 112> VlcSpecs;
+};
+
+struct TScaleTable {
+
+    class TBlockSizeMod {
+    public:
+        constexpr bool ShortWin(uint8_t) const noexcept {
+            return false;
+        }
+    };
+
+    static constexpr uint32_t MaxBfus = 32;
+    static constexpr uint32_t NumQMF = 16;
+    static float ScaleTable[64];
+
+    static constexpr uint32_t BlocksPerBand[NumQMF + 1] =
+    {
+        0, 8, 12, 16, 18, 20, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32
+    };
+    static constexpr uint32_t SpecsPerBlock[MaxBfus] = {
+        16,   16,   16,   16,   16,   16,   16,   16,
+        32,   32,   32,   32,   32,   32,   32,   32,
+        64,   64,   64,   64,   64,   64,   128,  128,
+        128,  128,  128,  128,  128,  128,  128,  128
+    };
+    static constexpr uint32_t BlockSizeTab[MaxBfus + 1] = {
+        0,    16,   32,   48,   64,   80,   96,   112,
+        128,  160,  192,  224,  256,  288,  320,  352,
+        384,  448,  512,  576,  640,  704,  768,  896,
+        1024, 1152, 1280, 1408, 1536, 1664, 1792, 1920,
+        2048
+    };
+    static constexpr uint32_t const * const SpecsStartShort = &BlockSizeTab[0];
+
+    static constexpr uint32_t const * const SpecsStartLong = &BlockSizeTab[0];
 };
 
 }
diff --git a/src/atrac/at3p/ff/atrac3plus_data.h b/src/atrac/at3p/ff/atrac3plus_data.h
index ba16da3..3e64292 100644
--- a/src/atrac/at3p/ff/atrac3plus_data.h
+++ b/src/atrac/at3p/ff/atrac3plus_data.h
@@ -26,6 +26,19 @@
 #include <stddef.h>
 #include <stdint.h>
 
+/* Mantissa table. */
+/* pow(10, x * log10(2) + 0.05) / 2 / ([1,2,3,5,7,15,31] + 0.5) */
+static const float atrac3p_mant_tab[8] = {
+    0.0,
+    0.74801636,
+    0.44882202,
+    0.32058716,
+    0.20400238,
+    0.1496048,
+    0.07239151,
+    0.035619736
+};
+
 /** VLC tables for wordlen */
 static const uint8_t atrac3p_wl_cbs[][12] = {
     {  1,  2,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 },
diff --git a/src/atrac/at3p/ghasend_tool.cpp b/src/atrac/at3p/ghasend_tool.cpp
index e3f95a4..c6be7c5 100644
--- a/src/atrac/at3p/ghasend_tool.cpp
+++ b/src/atrac/at3p/ghasend_tool.cpp
@@ -63,7 +63,7 @@ void process(const string& in, NAtracDEnc::TAt3PBitStream* bs) {
     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);
+    //bs->WriteFrame(1, &frame);
 }
 
 int main(int argc, char** argv) {
diff --git a/src/atrac/atrac_scale.cpp b/src/atrac/atrac_scale.cpp
index 5a75bdd..b3cb324 100644
--- a/src/atrac/atrac_scale.cpp
+++ b/src/atrac/atrac_scale.cpp
@@ -19,6 +19,7 @@
 #include "atrac_scale.h"
 #include "atrac1.h"
 #include "atrac3.h"
+#include "atrac/at3p/at3p_tables.h"
 #include "util.h"
 #include <cmath>
 #include <iostream>
@@ -193,4 +194,7 @@ class TScaler<NAtrac1::TAtrac1Data>;
 template
 class TScaler<NAtrac3::TAtrac3Data>;
 
+template
+class TScaler<NAt3p::TScaleTable>;
+
 } //namespace NAtracDEnc
diff --git a/src/lib/bs_encode/encode.cpp b/src/lib/bs_encode/encode.cpp
new file mode 100644
index 0000000..4c17ce9
--- /dev/null
+++ b/src/lib/bs_encode/encode.cpp
@@ -0,0 +1,164 @@
+/*
+ * This file is part of AtracDEnc.
+ *
+ * AtracDEnc is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * AtracDEnc is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with AtracDEnc; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include "encode.h"
+
+#include <iostream>
+
+namespace NAtracDEnc {
+
+using NBitStream::TBitStream;
+
+class TBitStreamEncoder::TImpl : public TBitAllocHandler {
+public:
+    explicit TImpl(std::vector<IBitStreamPartEncoder::TPtr>&& encoders);
+    void DoStart(size_t targetBits, float minLambda, float maxLambda) noexcept;
+    float DoContinue() noexcept;
+    void DoSubmit(size_t gotBits) noexcept;
+    bool DoCheck(size_t gotBits) const noexcept;
+    void DoRun(void* frameData, TBitStream& bs);
+private:
+    std::vector<IBitStreamPartEncoder::TPtr> Encoders;
+    size_t CurEncPos;
+    size_t RepeatEncPos;
+
+    size_t TargetBits;
+    float MinLambda;
+    float MaxLambda;
+    float CurLambda;
+    float LastLambda;
+
+    bool NeedRepeat = false;
+};
+
+TBitStreamEncoder::TImpl::TImpl(std::vector<IBitStreamPartEncoder::TPtr>&& encoders)
+    : Encoders(std::move(encoders))
+    , CurEncPos(0)
+    , RepeatEncPos(0)
+{
+}
+
+void TBitStreamEncoder::TImpl::DoStart(size_t targetBits, float minLambda, float maxLambda) noexcept
+{
+    TargetBits = targetBits;
+    MinLambda = minLambda;
+    MaxLambda = maxLambda;
+}
+
+float TBitStreamEncoder::TImpl::DoContinue() noexcept
+{
+    if (MaxLambda <= MinLambda) {
+        return LastLambda;
+    }
+
+    CurLambda = (MaxLambda + MinLambda) / 2.0;
+    RepeatEncPos = CurEncPos;
+    return CurLambda;
+}
+
+void TBitStreamEncoder::TImpl::DoSubmit(size_t gotBits) noexcept
+{
+    if  (MaxLambda <= MinLambda) {
+        NeedRepeat = false;
+    } else {
+        if (gotBits < TargetBits) {
+            LastLambda = CurLambda;
+            MaxLambda = CurLambda - 0.01f;
+            NeedRepeat = true;
+        } else if (gotBits > TargetBits) {
+            MinLambda = CurLambda + 0.01f;
+            NeedRepeat = true;
+        } else {
+            NeedRepeat = false;
+        }
+    }
+}
+
+bool TBitStreamEncoder::TImpl::DoCheck(size_t gotBits) const noexcept
+{
+    return gotBits < TargetBits;
+}
+
+void TBitStreamEncoder::TImpl::DoRun(void* frameData, TBitStream& bs)
+{
+    bool cont = false;
+    do {
+        for (CurEncPos = RepeatEncPos; CurEncPos < Encoders.size(); CurEncPos++) {
+            auto status = Encoders[CurEncPos]->Encode(frameData, *this);
+            if (NeedRepeat) {
+                NeedRepeat = false;
+                cont = true;
+                break;
+            } else {
+                cont = false;
+            }
+            if (status == IBitStreamPartEncoder::EStatus::Repeat) {
+                cont = true;
+                RepeatEncPos = 0;
+                break;
+            }
+        }
+    } while (cont);
+
+    for (size_t i = 0; i < Encoders.size(); i++) {
+        Encoders[i]->Dump(bs);
+    }
+}
+
+/////
+
+TBitStreamEncoder::TBitStreamEncoder(std::vector<IBitStreamPartEncoder::TPtr>&& encoders)
+    : Impl(new TBitStreamEncoder::TImpl(std::move(encoders)))
+{}
+
+TBitStreamEncoder::~TBitStreamEncoder()
+{
+    delete Impl;
+}
+
+void TBitStreamEncoder::Do(void* frameData, TBitStream& bs)
+{
+    Impl->DoRun(frameData, bs);
+}
+
+/////
+
+void TBitAllocHandler::Start(size_t targetBits, float minLambda, float maxLambda) noexcept
+{
+    TBitStreamEncoder::TImpl* self = static_cast<TBitStreamEncoder::TImpl*>(this);
+    self->DoStart(targetBits, minLambda, maxLambda);
+}
+
+float TBitAllocHandler::Continue() noexcept
+{
+    TBitStreamEncoder::TImpl* self = static_cast<TBitStreamEncoder::TImpl*>(this);
+    return self->DoContinue();
+}
+
+void TBitAllocHandler::Submit(size_t gotBits) noexcept
+{
+    TBitStreamEncoder::TImpl* self = static_cast<TBitStreamEncoder::TImpl*>(this);
+    self->DoSubmit(gotBits);
+}
+
+bool TBitAllocHandler::Check(size_t gotBits) const noexcept
+{
+    const TBitStreamEncoder::TImpl* self = static_cast<const TBitStreamEncoder::TImpl*>(this);
+    return self->DoCheck(gotBits);
+}
+}
diff --git a/src/lib/bs_encode/encode.h b/src/lib/bs_encode/encode.h
new file mode 100644
index 0000000..9671e10
--- /dev/null
+++ b/src/lib/bs_encode/encode.h
@@ -0,0 +1,67 @@
+/*
+ * This file is part of AtracDEnc.
+ *
+ * AtracDEnc is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * AtracDEnc is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with AtracDEnc; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#pragma once
+
+#include <memory>
+#include <vector>
+#include <functional>
+
+namespace NBitStream {
+class TBitStream;
+}
+
+namespace NAtracDEnc {
+
+class TBitAllocHandler {
+public:
+    void Start(size_t targetBits, float minLambda, float maxLambda) noexcept;
+    float Continue() noexcept;
+    bool Check(size_t gitBits) const noexcept;
+    void Submit(size_t gotBits) noexcept;
+};
+
+class IBitStreamPartEncoder {
+public:
+    using TPtr = std::unique_ptr<IBitStreamPartEncoder>;
+    enum class EStatus {
+        Ok, // Ok, go to the next stage
+        Repeat, // Repeat from first stage
+    };
+
+    virtual ~IBitStreamPartEncoder() = default;
+    virtual EStatus Encode(void* frameData, TBitAllocHandler& ba) = 0;
+    virtual void Dump(NBitStream::TBitStream& bs) = 0;
+
+};
+
+class TBitStreamEncoder {
+public:
+    class TImpl;
+    explicit TBitStreamEncoder(std::vector<IBitStreamPartEncoder::TPtr>&& encoders);
+    ~TBitStreamEncoder();
+
+    void Do(void* frameData, NBitStream::TBitStream& bs);
+    TBitStreamEncoder(const TBitStreamEncoder&) = delete;
+    TBitStreamEncoder& operator=(const TBitStreamEncoder&) = delete;
+private:
+    std::vector<IBitStreamPartEncoder::TPtr> Encoders;
+    TImpl* Impl;
+};
+
+}
diff --git a/src/lib/bs_encode/encode_ut.cpp b/src/lib/bs_encode/encode_ut.cpp
new file mode 100644
index 0000000..e9293a2
--- /dev/null
+++ b/src/lib/bs_encode/encode_ut.cpp
@@ -0,0 +1,162 @@
+/*
+ * This file is part of AtracDEnc.
+ *
+ * AtracDEnc is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * AtracDEnc is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with AtracDEnc; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include "encode.h"
+#include <gtest/gtest.h>
+#include <cmath>
+#include <memory>
+#include <bitstream/bitstream.h>
+
+using namespace NAtracDEnc;
+
+static size_t SomeBitFn1(float lambda) {
+    return sqrt(lambda * (-1.0f)) * 300;
+}
+
+static size_t SomeBitFn2(float lambda) {
+    return 1 + (SomeBitFn1(lambda) & (~(size_t)7));
+}
+
+class TPartEncoder1 : public IBitStreamPartEncoder {
+public:
+    explicit TPartEncoder1(size_t expCalls)
+        : ExpCalls(expCalls)
+    {}
+
+    EStatus Encode(void* frameData, TBitAllocHandler& ba) override {
+        EncCalls++;
+        ba.Start(1000, -15, -1);
+        return EStatus::Ok;
+    }
+
+    void Dump(NBitStream::TBitStream& bs) override {
+         EXPECT_EQ(EncCalls, ExpCalls);
+    }
+private:
+    const size_t ExpCalls;
+    size_t EncCalls = 0;
+};
+
+template<size_t (*F)(float)>
+class TPartEncoder2 : public IBitStreamPartEncoder {
+public:
+    explicit TPartEncoder2(size_t expCalls)
+        : ExpCalls(expCalls)
+    {}
+
+    EStatus Encode(void* frameData, TBitAllocHandler& ba) override {
+        EncCalls++;
+        auto lambda = ba.Continue();
+        auto bits = F(lambda);
+        ba.Submit(bits);
+        Bits = bits;
+        return EStatus::Ok;
+    }
+
+    void Dump(NBitStream::TBitStream& bs) override {
+         EXPECT_EQ(EncCalls, ExpCalls);
+         for (size_t i = 0; i < Bits; i++) {
+             bs.Write(1, 1);
+         }
+    }
+private:
+    const size_t ExpCalls;
+    size_t EncCalls = 0;
+    size_t Bits = 0;
+};
+
+class TPartEncoder3 : public IBitStreamPartEncoder {
+public:
+    explicit TPartEncoder3(size_t expCalls)
+        : ExpCalls(expCalls)
+    {}
+
+    EStatus Encode(void* frameData, TBitAllocHandler& ba) override {
+        EncCalls++;
+        return EStatus::Ok;
+    }
+
+    void Dump(NBitStream::TBitStream& bs) override {
+         EXPECT_EQ(EncCalls, ExpCalls);
+    }
+private:
+    const size_t ExpCalls;
+    size_t EncCalls = 0;
+};
+
+class TPartEncoder4 : public IBitStreamPartEncoder {
+public:
+    TPartEncoder4() = default;
+
+    EStatus Encode(void* frameData, TBitAllocHandler& ba) override {
+        if (EncCalls == 0) {
+            EncCalls++;
+            return EStatus::Repeat;
+        }
+
+        return EStatus::Ok;
+    }
+
+    void Dump(NBitStream::TBitStream& bs) override {
+         EXPECT_EQ(EncCalls, 1);
+    }
+private:
+    size_t EncCalls = 0;
+};
+
+TEST(BsEncode, SimpleAlloc) {
+    std::vector<IBitStreamPartEncoder::TPtr> encoders;
+    encoders.emplace_back(std::make_unique<TPartEncoder1>(1));
+    encoders.emplace_back(std::make_unique<TPartEncoder2<SomeBitFn1>>(8));
+    encoders.emplace_back(std::make_unique<TPartEncoder3>(1));
+
+    NBitStream::TBitStream bs;
+    TBitStreamEncoder encoder(std::move(encoders));
+    encoder.Do(nullptr, bs);
+
+    EXPECT_EQ(bs.GetSizeInBits(), 1000);
+}
+
+TEST(BsEncode, AllocWithRepeat) {
+    std::vector<IBitStreamPartEncoder::TPtr> encoders;
+    encoders.emplace_back(std::make_unique<TPartEncoder1>(2));
+    encoders.emplace_back(std::make_unique<TPartEncoder2<SomeBitFn1>>(16));
+    encoders.emplace_back(std::make_unique<TPartEncoder3>(2));
+    encoders.emplace_back(std::make_unique<TPartEncoder4>());
+
+    NBitStream::TBitStream bs;
+    TBitStreamEncoder encoder(std::move(encoders));
+    encoder.Do(nullptr, bs);
+
+    EXPECT_EQ(bs.GetSizeInBits(), 1000);
+}
+
+TEST(BsEncode, NotExactAlloc) {
+    std::vector<IBitStreamPartEncoder::TPtr> encoders;
+    encoders.emplace_back(std::make_unique<TPartEncoder1>(1));
+    encoders.emplace_back(std::make_unique<TPartEncoder2<SomeBitFn2>>(11));
+    encoders.emplace_back(std::make_unique<TPartEncoder3>(1));
+
+    NBitStream::TBitStream bs;
+    TBitStreamEncoder encoder(std::move(encoders));
+    encoder.Do(nullptr, bs);
+
+    EXPECT_EQ(bs.GetSizeInBits(), 993);
+}
+
+