soothe2-re/dsp/harness.cpp

#include <iostream>
#include <fstream>
#include <vector>
#include <cstring>
#include <cmath>
#include <algorithm>
#include "spectral.hpp"
#include "filter.hpp"
#include "detect.hpp"
#include "ms.hpp"

static float read_wav16(const char* path, std::vector<float>& 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<char*>(&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<char*>(&audio_fmt), 2);
            f.read(reinterpret_cast<char*>(&channels), 2);
            f.read(reinterpret_cast<char*>(&sample_rate), 4);
            f.read(reinterpret_cast<char*>(&bytes_per_sec), 4);
            f.read(reinterpret_cast<char*>(&block_align), 2);
            f.read(reinterpret_cast<char*>(&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<char*>(&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<int16_t> raw(total);
                    f.read(reinterpret_cast<char*>(raw.data()), data_size);
                    for (int i = 0; i < total; i++) {
                        out[i] = static_cast<float>(raw[i]) / 32768.0f;
                    }
                    return static_cast<float>(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<const char*>(&file_size), 4);
    f.write("WAVE", 4);
    
    f.write("fmt ", 4);
    int fmt_size = 16;
    f.write(reinterpret_cast<const char*>(&fmt_size), 4);
    int16_t audio_fmt = 1;
    f.write(reinterpret_cast<const char*>(&audio_fmt), 2);
    f.write(reinterpret_cast<const char*>(&channels), 2);
    f.write(reinterpret_cast<const char*>(&sample_rate), 4);
    f.write(reinterpret_cast<const char*>(&bytes_per_sec), 4);
    f.write(reinterpret_cast<const char*>(&block_align), 2);
    f.write(reinterpret_cast<const char*>(&bits), 2);
    
    f.write("data", 4);
    f.write(reinterpret_cast<const char*>(&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<int32_t>(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<const char*>(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<float> 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<float> output(total_samples);
    
    SpectralProcessor sp(2048, 512);
    sp.setDetectorParams(10.0f, 10.0f, 0.864f);
    
    std::vector<float> 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<float> 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<int>(sr));
    
    std::cout << "Done!" << std::endl;
    
    return 0;
}