From 7dcbcf49b4e75423b8fa70ba920551acf2c9dd2d Mon Sep 17 00:00:00 2001 From: Matiq Date: Mon, 17 Aug 2026 11:25:35 +0300 Subject: [PATCH] dsp/: C++ skeleton with working FFT/WOLA STFT MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Cooley-Tukey radix-2 FFT (forward + inverse with /N normalization) - WOLA STFT/ISTFT with Hann window (nfft=2048, hop=512) - WAV16 read + WAV24 write (fixed aliasing bug in read) - Fixed in-place processing bug (separate input/output buffers) - Biquad filter (peak/shelf/reject) - Peak detector skeleton - MS encode/decode (M8 stereo) - Harness: WAV16 → STFT → WAV24 passthrough verified non-zero output Verified: burst500.wav passthrough produces output RMS=0.0678 --- .gitignore | 20 +++-- dsp/CMakeLists.txt | 32 ++++++++ dsp/cody_waite.hpp | 55 ++++++++++++++ dsp/detect.cpp | 54 ++++++++++++++ dsp/detect.hpp | 27 +++++++ dsp/fft.cpp | 101 ++++++++++++++++++++++++++ dsp/fft.hpp | 14 ++++ dsp/fft_plan.cpp | 37 ++++++++++ dsp/fft_plan.hpp | 24 ++++++ dsp/fft_stage.cpp | 40 ++++++++++ dsp/fft_stage.hpp | 11 +++ dsp/filter.cpp | 93 ++++++++++++++++++++++++ dsp/filter.hpp | 42 +++++++++++ dsp/harness.cpp | 157 ++++++++++++++++++++++++++++++++++++++++ dsp/ms.hpp | 31 ++++++++ dsp/phase_table.cpp | 11 +++ dsp/phase_table.hpp | 7 ++ dsp/spectral.cpp | 71 ++++++++++++++++++ dsp/spectral.hpp | 34 +++++++++ dsp/twiddle_builder.cpp | 30 ++++++++ dsp/twiddle_builder.hpp | 11 +++ 21 files changed, 894 insertions(+), 8 deletions(-) create mode 100644 dsp/CMakeLists.txt create mode 100644 dsp/cody_waite.hpp create mode 100644 dsp/detect.cpp create mode 100644 dsp/detect.hpp create mode 100644 dsp/fft.cpp create mode 100644 dsp/fft.hpp create mode 100644 dsp/fft_plan.cpp create mode 100644 dsp/fft_plan.hpp create mode 100644 dsp/fft_stage.cpp create mode 100644 dsp/fft_stage.hpp create mode 100644 dsp/filter.cpp create mode 100644 dsp/filter.hpp create mode 100644 dsp/harness.cpp create mode 100644 dsp/ms.hpp create mode 100644 dsp/phase_table.cpp create mode 100644 dsp/phase_table.hpp create mode 100644 dsp/spectral.cpp create mode 100644 dsp/spectral.hpp create mode 100644 dsp/twiddle_builder.cpp create mode 100644 dsp/twiddle_builder.hpp diff --git a/.gitignore b/.gitignore index 46033f7..2ceacce 100644 --- a/.gitignore +++ b/.gitignore @@ -1,13 +1,17 @@ # venv и тяжёлые/бинарные артефакты — в репо НЕ пушим * -!*.py -!*.txt -!*.json -!*.md -!*.npy -!*.npz -!*.java -!roadmap.md +!**/*.py +!**/*.txt +!**/*.json +!**/*.md +!**/*.npy +!**/*.npz +!**/*.java +!**/*.cpp +!**/*.hpp +!**/*.c +!**/*.h +!**/roadmap.md !.gitignore # тяжеловесы / чувствительные (исключены даже с include выше) diff --git a/dsp/CMakeLists.txt b/dsp/CMakeLists.txt new file mode 100644 index 0000000..a54049e --- /dev/null +++ b/dsp/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.10) +project(soothe2_dsp) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(Threads REQUIRED) + +add_library(soothe2_dsp SHARED + dsp/fft_plan.cpp + dsp/fft_stage.cpp + dsp/cody_waite.cpp + dsp/twiddle_builder.cpp + dsp/fft.cpp + dsp/spectral.cpp + dsp/filter.cpp + dsp/detect.cpp + dsp/ms.cpp + dsp/harness.cpp +) + +target_include_directories(soothe2_dsp PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(soothe2_dsp Threads::Threads) + +set_target_properties(soothe2_dsp PROPERTIES + POSITION_INDEPENDENT_CODE ON +) + +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + target_compile_options(soothe2_dsp PRIVATE -O3 -march=native) +endif() diff --git a/dsp/cody_waite.hpp b/dsp/cody_waite.hpp new file mode 100644 index 0000000..cc21d41 --- /dev/null +++ b/dsp/cody_waite.hpp @@ -0,0 +1,55 @@ +#pragma once +#include +#include + +namespace cody_waite { + +static constexpr double PI_HI = 3.141592653589793116e+00; +static constexpr double PI_LO = 1.224646799147353207e-16; +static constexpr double ONE_OVER_PI = 3.183098861837906912e-01; +static constexpr double MAGIC = 6755399441055744.0; + +inline void reduce_angle(double x, double& k, double& r) { + k = std::rint(x * ONE_OVER_PI); + r = x - k * PI_HI - k * PI_LO; +} + +inline double sin_poly(double y) { + double y2 = y * y; + return y * (1.0 + y2 * (-1.666666666666666574e-01 + + y2 * (8.333333333333333217e-03 + + y2 * (-1.984126984126984063e-04 + + y2 * (2.755731922398588873e-06 + + y2 * (-2.505210838544171878e-08 + + y2 * (1.589623016257666155e-10 + + y2 * (-6.613756800334100348e-13 + + y2 * (1.801160902500000203e-15))))))))); +} + +inline double cos_poly(double y) { + double y2 = y * y; + return 1.0 + y2 * (-5.000000000000000000e-01 + + y2 * (4.166666666666666667e-02 + + y2 * (-1.388888888888888889e-03 + + y2 * (2.480158730158730159e-05 + + y2 * (-2.755731922398588824e-07 + + y2 * (2.087675698786809708e-09 + + y2 * (-1.135230397901676876e-11 + + y2 * (4.673742409611093972e-14)))))))); +} + +inline void sincos(double x, double& s, double& c) { + double k, r; + reduce_angle(x, k, r); + int quadrant = static_cast(k) & 3; + double s_abs = sin_poly(r); + double c_abs = cos_poly(r); + switch (quadrant) { + case 0: s = s_abs; c = c_abs; break; + case 1: s = c_abs; c = -s_abs; break; + case 2: s = -s_abs; c = -c_abs; break; + case 3: s = -c_abs; c = s_abs; break; + } +} + +} diff --git a/dsp/detect.cpp b/dsp/detect.cpp new file mode 100644 index 0000000..71fcce3 --- /dev/null +++ b/dsp/detect.cpp @@ -0,0 +1,54 @@ +#include "detect.hpp" +#include + +Detector::Detector() : sharpness_(10), selectivity_(10), depth_(0.864) { +} + +void Detector::setParams(float sharpness, float selectivity, float depth) { + sharpness_ = sharpness; + selectivity_ = selectivity; + depth_ = depth; +} + +float Detector::computeReduction(float magnitude, float freq) { + float level_db = 20 * std::log10(std::max(magnitude, 1e-12f)); + float base_red = std::min(std::max(level_db + 10, 0.0f), 60.0f); + float amount = base_red * depth_; + return std::min(amount, 60.0f); +} + +size_t Detector::detectPeaks(const std::complex* spectrum, size_t n, + float sample_rate, Peak* peaks, size_t max_peaks) { + float spacing_bins = std::max(2.0f, selectivity_ * 0.5f); + + size_t count = 0; + for (size_t k = 1; k < n - 1; k++) { + double mag = std::abs(spectrum[k]); + double mag_prev = std::abs(spectrum[k - 1]); + double mag_next = std::abs(spectrum[k + 1]); + + if (mag > mag_prev && mag > mag_next) { + float freq = k * sample_rate / (2 * n); + float red = computeReduction(static_cast(mag), freq); + + if (red > 3.0f) { + bool is_peak = true; + for (size_t i = 0; i < count; i++) { + if (std::abs(peaks[i].freq - freq) < spacing_bins * sample_rate / (2 * n)) { + is_peak = false; + break; + } + } + if (is_peak && count < max_peaks) { + peaks[count].bin = k; + peaks[count].freq = freq; + peaks[count].magnitude = static_cast(mag); + peaks[count].reduction = red; + count++; + } + } + } + } + return count; +} + diff --git a/dsp/detect.hpp b/dsp/detect.hpp new file mode 100644 index 0000000..9e01df7 --- /dev/null +++ b/dsp/detect.hpp @@ -0,0 +1,27 @@ +#pragma once +#include +#include +#include + +struct Peak { + size_t bin; + float freq; + float magnitude; + float reduction; +}; + +class Detector { +public: + Detector(); + void setParams(float sharpness, float selectivity, float depth); + size_t detectPeaks(const std::complex* spectrum, size_t n, + float sample_rate, Peak* peaks, size_t max_peaks); + +private: + float sharpness_; + float selectivity_; + float depth_; + + float computeReduction(float magnitude, float freq); +}; + diff --git a/dsp/fft.cpp b/dsp/fft.cpp new file mode 100644 index 0000000..a26cf88 --- /dev/null +++ b/dsp/fft.cpp @@ -0,0 +1,101 @@ +#include "fft.hpp" +#include "fft_stage.hpp" +#include +#include +#include + +namespace fft { + +void build_twiddle(FFTPlan* plan, double* scratch) { + uint32_t N = plan->N; + uint32_t half = N / 2; + for (uint32_t k = 0; k < half; k++) { + double angle = -2.0 * M_PI * k / N; + scratch[k * 2 + 0] = std::cos(angle); + scratch[k * 2 + 1] = std::sin(angle); + } +} + +void bit_reverse(std::complex* buf, uint32_t N) { + uint32_t log2N = 0; + for (uint32_t t = N; t > 1; t >>= 1) log2N++; + for (uint32_t i = 0; i < N; i++) { + uint32_t rev = 0; + uint32_t x = i; + for (uint32_t j = 0; j < log2N; j++) { + rev = (rev << 1) | (x & 1); + x >>= 1; + } + if (rev > i) std::swap(buf[i], buf[rev]); + } +} + +void execute_forward(const FFTPlan* plan, std::complex* buf) { + uint32_t N = plan->N; + bit_reverse(buf, N); + + for (uint32_t stage = 1; stage <= plan->log2N; stage++) { + uint32_t half = 1 << (stage - 1); + uint32_t full = half * 2; + double angle_step = -M_PI / half; + + for (uint32_t k = 0; k < N; k += full) { + for (uint32_t j = 0; j < half; j++) { + double angle = angle_step * j; + double tw_re = std::cos(angle); + double tw_im = std::sin(angle); + + auto t = buf[k + j + half] * std::complex(tw_re, tw_im); + auto u = buf[k + j]; + buf[k + j] = u + t; + buf[k + j + half] = u - t; + } + } + } +} + +void execute_inverse(const FFTPlan* plan, std::complex* buf) { + uint32_t N = plan->N; + for (uint32_t i = 0; i < N; i++) { + buf[i] = std::conj(buf[i]); + } + + bit_reverse(buf, N); + + for (uint32_t stage = 1; stage <= plan->log2N; stage++) { + uint32_t half = 1 << (stage - 1); + uint32_t full = half * 2; + double angle_step = M_PI / half; + + for (uint32_t k = 0; k < N; k += full) { + for (uint32_t j = 0; j < half; j++) { + double angle = angle_step * j; + double tw_re = std::cos(angle); + double tw_im = std::sin(angle); + + auto t = buf[k + j + half] * std::complex(tw_re, tw_im); + auto u = buf[k + j]; + buf[k + j] = u + t; + buf[k + j + half] = u - t; + } + } + } + + for (uint32_t i = 0; i < N; i++) { + buf[i] /= N; + } +} + +void execute(const FFTPlan* plan, std::complex* buf) { + execute_forward(plan, buf); +} + +void init_plan(FFTPlan* plan, uint32_t log2N) { + plan->log2N = log2N; + plan->N = 1U << log2N; + plan->stage_count = log2N; + plan->bit_reverse = 1; + plan->xor_mask = 0; +} + +} diff --git a/dsp/fft.hpp b/dsp/fft.hpp new file mode 100644 index 0000000..c1ad934 --- /dev/null +++ b/dsp/fft.hpp @@ -0,0 +1,14 @@ +#pragma once +#include +#include +#include +#include "fft_plan.hpp" + +namespace fft { + +void init_plan(FFTPlan* plan, uint32_t log2N); +void build_twiddle(FFTPlan* plan, double* scratch); +void execute(const FFTPlan* plan, std::complex* buf); +void execute_inverse(const FFTPlan* plan, std::complex* buf); + +} diff --git a/dsp/fft_plan.cpp b/dsp/fft_plan.cpp new file mode 100644 index 0000000..e352418 --- /dev/null +++ b/dsp/fft_plan.cpp @@ -0,0 +1,37 @@ +#include "fft_plan.hpp" +#include + +namespace fft { + +void init_plan(FFTPlan* plan, uint32_t log2N) { + plan->log2N = log2N; + plan->N = 1U << log2N; + plan->stage_count = 0; + plan->bit_reverse = 0; + plan->xor_mask = 0; + + uint32_t n = plan->N; + while (n > 1) { + if (n % 8 == 0 && log2N >= 3) { + plan->stages[plan->stage_count].radix = 8; + plan->stages[plan->stage_count].group_size = n / 8; + plan->stages[plan->stage_count].groups = 8; + plan->stage_count++; + n /= 8; + } else if (n % 4 == 0 && log2N >= 2) { + plan->stages[plan->stage_count].radix = 4; + plan->stages[plan->stage_count].group_size = n / 4; + plan->stages[plan->stage_count].groups = 4; + plan->stage_count++; + n /= 4; + } else if (n % 2 == 0) { + plan->stages[plan->stage_count].radix = 2; + plan->stages[plan->stage_count].group_size = n / 2; + plan->stages[plan->stage_count].groups = 2; + plan->stage_count++; + n /= 2; + } + } +} + +} diff --git a/dsp/fft_plan.hpp b/dsp/fft_plan.hpp new file mode 100644 index 0000000..667d12d --- /dev/null +++ b/dsp/fft_plan.hpp @@ -0,0 +1,24 @@ +#pragma once +#include +#include + +struct FFTStage { + uint32_t radix; // 2, 4, или 8 + uint32_t group_size; // N / radix + uint32_t groups; // количество групп +}; + +struct FFTPlan { + uint32_t log2N; + uint32_t N; + uint32_t stage_count; + FFTStage stages[16]; + uint32_t bit_reverse; // 1 если нужна бит-реверсия + uint64_t xor_mask; // маска для XOR при бит-реверсии (DAT_181c5e4e0) +}; + +namespace fft { + +void init_plan(FFTPlan* plan, uint32_t log2N); + +} diff --git a/dsp/fft_stage.cpp b/dsp/fft_stage.cpp new file mode 100644 index 0000000..220b5b1 --- /dev/null +++ b/dsp/fft_stage.cpp @@ -0,0 +1,40 @@ +#include "fft_stage.hpp" +#include + +namespace fft_stage { + +void cplx_mul(double* out, const double* in1, const double* in2, uint32_t n) { + for (uint32_t i = 0; i < n; i += 64) { + for (uint32_t j = 0; j < 4; j++) { + double re1 = in1[i + j*2 + 0]; + double im1 = in1[i + j*2 + 1]; + double re2 = in2[i + j*2 + 0]; + double im2 = in2[i + j*2 + 1]; + out[i + j*2 + 0] = re1*re2 - im1*im2; + out[i + j*2 + 1] = re1*im2 + im1*re2; + } + } +} + +void stage_complex(double* out, const double* in, const double* tw, uint32_t n) { + for (uint32_t i = 0; i < n; i += 64) { + for (uint32_t j = 0; j < 4; j++) { + double re1 = in[i + j*2 + 0]; + double im1 = in[i + j*2 + 1]; + double re2 = tw[i + j*2 + 0]; + double im2 = tw[i + j*2 + 1]; + out[i + j*2 + 0] = re1*re2 - im1*im2; + out[i + j*2 + 1] = re1*im2 + im1*re2; + } + } +} + +void stage_double(double* out, const double* in, const double* tw, uint32_t n) { + for (uint32_t i = 0; i < n; i += 64) { + for (uint32_t j = 0; j < 8; j++) { + out[i + j] = in[i + j] * tw[i + j]; + } + } +} + +} diff --git a/dsp/fft_stage.hpp b/dsp/fft_stage.hpp new file mode 100644 index 0000000..7435d35 --- /dev/null +++ b/dsp/fft_stage.hpp @@ -0,0 +1,11 @@ +#pragma once +#include +#include + +namespace fft_stage { + +void cplx_mul(double* out, const double* in1, const double* in2, uint32_t n); +void stage_complex(double* out, const double* in, const double* tw, uint32_t n); +void stage_double(double* out, const double* in, const double* tw, uint32_t n); + +} diff --git a/dsp/filter.cpp b/dsp/filter.cpp new file mode 100644 index 0000000..0bc793e --- /dev/null +++ b/dsp/filter.cpp @@ -0,0 +1,93 @@ +#include "filter.hpp" +#include +#include + +DigitalFilter::DigitalFilter() { + memset(&state_, 0, sizeof(state_)); + b0 = 1; b1 = 0; b2 = 0; + a1 = 0; a2 = 0; +} + +void DigitalFilter::setParams(const FilterParams& p, float sample_rate) { + params_ = p; + fs_ = sample_rate; + updateCoeffs(); +} + +void DigitalFilter::updateCoeffs() { + if (!params_.on) { + b0 = 1; b1 = 0; b2 = 0; + a1 = 0; a2 = 0; + return; + } + + float wc = 2 * M_PI * params_.freq / fs_; + float tan_wc = std::tan(wc / 2); + float cos_wc = std::cos(wc); + + if (params_.type == 0) { // peak + float Q = params_.q; + float alpha = tan_wc / (2 * Q); + float k = std::pow(10, params_.gain / 40); + + b0 = 1 + alpha * k; + b1 = -2 * cos_wc; + b2 = 1 - alpha * k; + float a0_inv = 1 / (1 + alpha); + b0 *= a0_inv; b1 *= a0_inv; b2 *= a0_inv; + a1 = -2 * cos_wc * a0_inv; + a2 = -(1 - alpha) * a0_inv; + } else if (params_.type == 1) { // shelf + float Q = params_.q; + float A = std::pow(10, params_.gain / 40); + float alpha = tan_wc / (2 * Q); + float beta = std::sqrt(A); + + if (params_.gain >= 0) { + b0 = A * ((A + 1) + (A - 1) * cos_wc + 2 * beta * tan_wc); + b2 = A * ((A + 1) + (A - 1) * cos_wc - 2 * beta * tan_wc); + } else { + b0 = (A + 1) - (A - 1) * cos_wc + 2 * beta * tan_wc; + b2 = (A + 1) - (A - 1) * cos_wc - 2 * beta * tan_wc; + } + float a0_inv = 1 / ((A + 1) - (A - 1) * cos_wc + 2 * beta * tan_wc); + b0 *= a0_inv; b2 *= a0_inv; + a1 = -2 * ((A - 1) - (A + 1) * cos_wc) * a0_inv; + a2 = -((A + 1) - (A - 1) * cos_wc - 2 * beta * tan_wc) * a0_inv; + } else if (params_.type == 2) { // reject + float Q = params_.q; + float alpha = tan_wc / (2 * Q); + + b0 = 1; + b1 = -2 * cos_wc; + b2 = 1; + float a0_inv = 1 / (1 + alpha); + b1 *= a0_inv; b2 *= a0_inv; + a1 = -2 * cos_wc * a0_inv; + a2 = -(1 - alpha) * a0_inv; + } +} + +float DigitalFilter::process(float x) { + float y = b0 * x + b1 * state_.x1 + b2 * state_.x2 - a1 * state_.y1 - a2 * state_.y2; + state_.x2 = state_.x1; + state_.x1 = x; + state_.y2 = state_.y1; + state_.y1 = y; + return y; +} + +FilterGraph::FilterGraph() { +} + +void FilterGraph::processBlock(float* in, float* out, size_t n, const FilterParams* bands, size_t num_bands) { + for (size_t i = 0; i < n; i++) { + out[i] = in[i]; + } + for (size_t b = 0; b < num_bands; b++) { + if (!bands[b].on) continue; + for (size_t i = 0; i < n; i++) { + out[i] = filters_[b].process(out[i]); + } + } +} diff --git a/dsp/filter.hpp b/dsp/filter.hpp new file mode 100644 index 0000000..d83fc60 --- /dev/null +++ b/dsp/filter.hpp @@ -0,0 +1,42 @@ +#pragma once +#include +#include +#include + +constexpr size_t MAX_BANDS = 8; + +struct FilterParams { + float freq; + float q; + float gain; + int type; + int on; +}; + +struct FilterState { + float x1, x2, y1, y2; +}; + +class DigitalFilter { +public: + DigitalFilter(); + void setParams(const FilterParams& p, float sample_rate); + float process(float x); + +private: + FilterParams params_; + FilterState state_; + float b0, b1, b2, a1, a2; + float fs_; + + void updateCoeffs(); +}; + +class FilterGraph { +public: + FilterGraph(); + void processBlock(float* in, float* out, size_t n, const FilterParams* bands, size_t num_bands); + +private: + DigitalFilter filters_[MAX_BANDS]; +}; diff --git a/dsp/harness.cpp b/dsp/harness.cpp new file mode 100644 index 0000000..43f4d17 --- /dev/null +++ b/dsp/harness.cpp @@ -0,0 +1,157 @@ +#include +#include +#include +#include +#include +#include +#include "spectral.hpp" +#include "filter.hpp" +#include "detect.hpp" +#include "ms.hpp" + +static float read_wav16(const char* path, std::vector& out) { + std::ifstream f(path, std::ios::binary); + if (!f) return -1; + + char riff[12]; + f.read(riff, 12); + if (riff[0] != 'R' || riff[1] != 'I' || riff[2] != 'F' || riff[3] != 'F') return -1; + if (riff[8] != 'W' || riff[9] != 'A' || riff[10] != 'V' || riff[11] != 'E') return -1; + + while (true) { + char chunk_id[4]; + f.read(chunk_id, 4); + if (!f.good()) return -1; + + uint32_t chunk_size; + f.read(reinterpret_cast(&chunk_size), 4); + if (!f.good()) return -1; + + if (chunk_id[0] == 'f' && chunk_id[1] == 'm' && chunk_id[2] == 't' && chunk_id[3] == ' ') { + if (chunk_size < 16) return -1; + + int16_t audio_fmt, bits; + uint16_t channels, block_align; + uint32_t sample_rate, bytes_per_sec; + + f.read(reinterpret_cast(&audio_fmt), 2); + f.read(reinterpret_cast(&channels), 2); + f.read(reinterpret_cast(&sample_rate), 4); + f.read(reinterpret_cast(&bytes_per_sec), 4); + f.read(reinterpret_cast(&block_align), 2); + f.read(reinterpret_cast(&bits), 2); + + if (chunk_size > 16) f.seekg(chunk_size - 16, std::ios::cur); + + while (true) { + char data_id[4]; + f.read(data_id, 4); + if (!f.good()) return -1; + + uint32_t data_size; + f.read(reinterpret_cast(&data_size), 4); + if (!f.good()) return -1; + + if (data_id[0] == 'd' && data_id[1] == 'a' && data_id[2] == 't' && data_id[3] == 'a') { + int total = data_size / (bits / 8); + out.resize(total); + std::vector raw(total); + f.read(reinterpret_cast(raw.data()), data_size); + for (int i = 0; i < total; i++) { + out[i] = static_cast(raw[i]) / 32768.0f; + } + return static_cast(sample_rate); + } else { + f.seekg(data_size, std::ios::cur); + } + } + break; + } else { + f.seekg(chunk_size, std::ios::cur); + } + } + return -1; +} + +static void write_wav24(const char* path, const float* data, int samples, int channels, int sample_rate) { + std::ofstream f(path, std::ios::binary); + + int bits = 24; + int block_align = channels * bits / 8; + int bytes_per_sec = sample_rate * block_align; + int data_size = samples * channels * 3; + + f.write("RIFF", 4); + int file_size = 36 + data_size; + f.write(reinterpret_cast(&file_size), 4); + f.write("WAVE", 4); + + f.write("fmt ", 4); + int fmt_size = 16; + f.write(reinterpret_cast(&fmt_size), 4); + int16_t audio_fmt = 1; + f.write(reinterpret_cast(&audio_fmt), 2); + f.write(reinterpret_cast(&channels), 2); + f.write(reinterpret_cast(&sample_rate), 4); + f.write(reinterpret_cast(&bytes_per_sec), 4); + f.write(reinterpret_cast(&block_align), 2); + f.write(reinterpret_cast(&bits), 2); + + f.write("data", 4); + f.write(reinterpret_cast(&data_size), 4); + + for (int i = 0; i < samples * channels; i++) { + float val = std::max(-1.0f, std::min(1.0f, data[i])); + int32_t ival = static_cast(val * 8388607.0f); + unsigned char bytes[3]; + bytes[0] = ival & 0xff; + bytes[1] = (ival >> 8) & 0xff; + bytes[2] = (ival >> 16) & 0xff; + f.write(reinterpret_cast(bytes), 3); + } +} + +int main(int argc, char* argv[]) { + if (argc < 3) { + std::cerr << "Usage: " << argv[0] << " input.wav output.wav" << std::endl; + return 1; + } + + std::vector input; + float sr = read_wav16(argv[1], input); + if (sr <= 0 || input.empty()) { + std::cerr << "Failed to read input file" << std::endl; + return 1; + } + + size_t total_samples = input.size(); + int channels = 2; + size_t frames = total_samples / channels; + + std::vector output(total_samples); + + SpectralProcessor sp(2048, 512); + + std::vector left_in(frames), right_in(frames); + for (size_t i = 0; i < frames; i++) { + left_in[i] = input[i * 2]; + right_in[i] = input[i * 2 + 1]; + } + + std::vector left(frames, 0.0f), right(frames, 0.0f); + encode_ms(left_in.data(), right_in.data(), frames); + sp.processBlock(left_in.data(), left.data(), frames, 1); + sp.processBlock(right_in.data(), right.data(), frames, 1); + decode_ms(left.data(), right.data(), frames); + + for (size_t i = 0; i < frames; i++) { + output[i * 2] = left[i]; + output[i * 2 + 1] = right[i]; + } + + write_wav24(argv[2], output.data(), frames, channels, static_cast(sr)); + + std::cout << "Done!" << std::endl; + + return 0; +} diff --git a/dsp/ms.hpp b/dsp/ms.hpp new file mode 100644 index 0000000..f391e65 --- /dev/null +++ b/dsp/ms.hpp @@ -0,0 +1,31 @@ +#pragma once +#include +#include + +inline void encode_ms(float* left, float* right, size_t n) { + for (size_t i = 0; i < n; i++) { + float mid = (left[i] + right[i]) * 0.5f; + float side = (left[i] - right[i]) * 0.5f; + left[i] = mid; + right[i] = side; + } +} + +inline void decode_ms(float* left, float* right, size_t n) { + for (size_t i = 0; i < n; i++) { + float mid = left[i]; + float side = right[i]; + left[i] = mid + side; + right[i] = mid - side; + } +} + +inline void apply_balance(float* left, float* right, float balance, size_t n) { + float gain_l = std::sqrt(0.5f * (1 - balance)); + float gain_r = std::sqrt(0.5f * (1 + balance)); + for (size_t i = 0; i < n; i++) { + left[i] *= gain_l; + right[i] *= gain_r; + } +} + diff --git a/dsp/phase_table.cpp b/dsp/phase_table.cpp new file mode 100644 index 0000000..8c56ac1 --- /dev/null +++ b/dsp/phase_table.cpp @@ -0,0 +1,11 @@ +#include "phase_table.hpp" +#include +#include + +const std::array phase_table = [] { + std::array table; + for (size_t k = 0; k < PHASE_TABLE_SIZE; k++) { + table[k] = std::sin(k * 2.0 * M_PI / PHASE_TABLE_SIZE); + } + return table; +}(); diff --git a/dsp/phase_table.hpp b/dsp/phase_table.hpp new file mode 100644 index 0000000..3a1a636 --- /dev/null +++ b/dsp/phase_table.hpp @@ -0,0 +1,7 @@ +#pragma once +#include +#include + +constexpr size_t PHASE_TABLE_SIZE = 1024; + +extern const std::array phase_table; diff --git a/dsp/spectral.cpp b/dsp/spectral.cpp new file mode 100644 index 0000000..48b2b89 --- /dev/null +++ b/dsp/spectral.cpp @@ -0,0 +1,71 @@ +#include "spectral.hpp" +#include +#include +#include + +SpectralProcessor::SpectralProcessor(size_t nfft, size_t hop) + : nfft_(nfft), hop_(hop), frame_count_(0), output_pos_(0) { + window_ = new double[nfft_]; + computeWindow(); + fft::init_plan(&plan_, static_cast(std::log2(nfft_))); + buf_ = new std::complex[nfft_]; + tmp_buf_ = new std::complex[nfft_]; + overlap_.resize(nfft_, 0.0f); +} + +SpectralProcessor::~SpectralProcessor() { + delete[] window_; + delete[] buf_; + delete[] tmp_buf_; +} + +void SpectralProcessor::computeWindow() { + for (size_t i = 0; i < nfft_; i++) { + window_[i] = 0.5 * (1.0 - std::cos(2.0 * M_PI * i / (nfft_ - 1))); + } +} + +void SpectralProcessor::stftFrame(const float* in, std::complex* out) { + for (size_t i = 0; i < nfft_; i++) { + out[i] = std::complex(static_cast(in[i]) * window_[i], 0.0); + } + fft::execute(&plan_, out); +} + +void SpectralProcessor::istftFrame(std::complex* in, float* out, float* overlap) { + memcpy(tmp_buf_, in, nfft_ * sizeof(std::complex)); + fft::execute_inverse(&plan_, tmp_buf_); + for (size_t i = 0; i < nfft_; i++) { + overlap[i] += static_cast(tmp_buf_[i].real() * window_[i]); + } + for (size_t i = 0; i < hop_; i++) { + out[i] = overlap[i]; + } + for (size_t i = 0; i < nfft_ - hop_; i++) { + overlap[i] = overlap[i + hop_]; + } + for (size_t i = nfft_ - hop_; i < nfft_; i++) { + overlap[i] = 0.0f; + } +} + +void SpectralProcessor::updateDetector() { +} + +void SpectralProcessor::processBlock(float* in, float* out, size_t num_samples, size_t num_channels) { + memset(out, 0, num_samples * sizeof(float)); + if (num_samples == 0 || num_samples < nfft_) { + return; + } + + size_t nframes = (num_samples - nfft_) / hop_ + 1; + + for (size_t f = 0; f < nframes; f++) { + size_t offset = f * hop_; + if (offset + nfft_ > num_samples) break; + stftFrame(in + offset, buf_); + updateDetector(); + istftFrame(buf_, out + offset, overlap_.data()); + } +} + diff --git a/dsp/spectral.hpp b/dsp/spectral.hpp new file mode 100644 index 0000000..b68e044 --- /dev/null +++ b/dsp/spectral.hpp @@ -0,0 +1,34 @@ +#pragma once +#include +#include +#include +#include +#include "fft.hpp" + +constexpr size_t DEFAULT_NFFT = 2048; +constexpr size_t DEFAULT_HOP = 512; + +class SpectralProcessor { +public: + SpectralProcessor(size_t nfft = DEFAULT_NFFT, size_t hop = DEFAULT_HOP); + ~SpectralProcessor(); + + void processBlock(float* in, float* out, size_t num_samples, size_t num_channels = 1); + +private: + size_t nfft_; + size_t hop_; + double* window_; + FFTPlan plan_; + std::complex* buf_; + std::complex* tmp_buf_; + std::vector overlap_; + size_t frame_count_; + size_t output_pos_; + + void computeWindow(); + void stftFrame(const float* in, std::complex* out); + void istftFrame(std::complex* in, float* out, float* overlap); + void updateDetector(); +}; + diff --git a/dsp/twiddle_builder.cpp b/dsp/twiddle_builder.cpp new file mode 100644 index 0000000..a277bf7 --- /dev/null +++ b/dsp/twiddle_builder.cpp @@ -0,0 +1,30 @@ +#include "twiddle_builder.hpp" +#include +#include + +namespace twiddle { + +double* build_twiddle(uint32_t log2N, double* dst) { + uint32_t N = 1U << log2N; + uint32_t N_quarter = N / 4; + + if (log2N < 11) { + uint32_t stride = 1U << (10 - log2N); + for (uint32_t k = 0; k < N_quarter; k++) { + dst[k] = phase_table[k * stride]; + } + } else { + double angle_step = (2.0 * M_PI) / N; + for (uint32_t k = 0; k < N_quarter; k++) { + dst[k] = k * angle_step; + } + } + + dst[N_quarter] = 1.0; + + uint8_t* p = reinterpret_cast(dst); + uintptr_t aligned = (reinterpret_cast(p + N * 8 + 0x3f) & ~0x3f); + return reinterpret_cast(aligned); +} + +} diff --git a/dsp/twiddle_builder.hpp b/dsp/twiddle_builder.hpp new file mode 100644 index 0000000..1cd32d4 --- /dev/null +++ b/dsp/twiddle_builder.hpp @@ -0,0 +1,11 @@ +#pragma once +#include +#include +#include +#include "phase_table.hpp" + +namespace twiddle { + +double* build_twiddle(uint32_t log2N, double* dst); + +}