#include <iostream>
#include <vector>
#include <cmath>
#include <cassert>
#include <cstring>
#include "acoustic_analyzer/core/audio_buffer.h"
#include "acoustic_analyzer/core/feature_extractor.h"
#include "acoustic_analyzer/core/gcc_phat_localizer.h"
#include "acoustic_analyzer/core/distance_estimator.h"
#include "acoustic_analyzer/core/threat_tracker.h"

using namespace acoustic;

bool near(float a, float b, float eps = 1e-3f) {
    return std::abs(a - b) < eps;
}

void test_audio_buffer_wraparound() {
    AudioBuffer buf(100, 2);
    std::vector<float> data(200);
    for (int i = 0; i < 200; ++i) data[i] = static_cast<float>(i);
    buf.push_interleaved(data.data(), 200);
    assert(buf.available(0) == 100);
    float out[10];
    buf.get_latest_frame(out, 10, 0);
    // Latest 10 samples of channel 0 should be 190, 192, 194, ... (interleaved)
    assert(near(out[0], 190.0f));
    assert(near(out[9], 208.0f));
    std::cout << "[PASS] audio_buffer_wraparound" << std::endl;
}

void test_preemphasis() {
    float in[] = {1.0f, 0.5f, 0.25f};
    float out[3];
    apply_preemphasis(in, out, 3, 0.97f);
    assert(near(out[0], 1.0f));
    assert(near(out[1], 0.5f - 0.97f * 1.0f, 1e-4f));
    std::cout << "[PASS] preemphasis" << std::endl;
}

void test_gcc_phat_cross_array() {
    MicArrayGeometry geom;
    geom.num_mics = 4;
    geom.layout = "cross";
    geom.spacing = 0.15f;
    GccPhatLocalizer loc(geom, 16000, 4);
    loc.set_max_tdoa(0.00044f);

    // Simulate a signal arriving from 90 degrees (right), delay mic2 ahead
    int n = 512;
    std::vector<std::vector<float>> audio(4, std::vector<float>(n));
    for (int i = 0; i < n; ++i) {
        float s = std::sin(2.0f * M_PI * 1000.0f * i / 16000.0f);
        audio[0][i] = s; // front
        audio[1][i] = s; // right (lead)
        audio[2][i] = s; // back
        audio[3][i] = s; // left (lag)
    }
    // Add small delay to simulate direction
    float delay_samples = 0.15f / 343.0f * 16000.0f;
    for (int i = 0; i < n; ++i) {
        int idx = static_cast<int>(i + delay_samples);
        if (idx < n) audio[1][idx] = audio[0][i];
        idx = static_cast<int>(i - delay_samples);
        if (idx >= 0) audio[3][idx] = audio[0][i];
    }

    float azimuth, elevation;
    bool ok = loc.localize(audio, azimuth, elevation);
    assert(ok);
    // For this simplified test, just assert it returns a valid angle
    assert(azimuth >= 0.0f && azimuth < 360.0f);
    std::cout << "[PASS] gcc_phat_cross_array (azimuth=" << azimuth << ")" << std::endl;
}

void test_distance_estimator() {
    DistanceEstimator::Params p;
    p.reference_spl["gunshot"] = 150.0f;
    p.attenuation_alpha = 0.6f;
    DistanceEstimator est(p);
    float dist, conf;
    bool ok = est.estimate(120.0f, "gunshot", dist, conf);
    assert(ok);
    assert(dist > 0.0f);
    std::cout << "[PASS] distance_estimator (dist=" << dist << "m)" << std::endl;
}

void test_threat_tracker() {
    ThreatTracker tracker(15.0f, 5, 0.0f, 10.0f);
    Threat t1{"gunshot", 0.9f, 45.0f, 0.0f, 100.0f, 0.8f};
    std::vector<TrackedThreat> out;
    tracker.update({t1}, out);
    assert(!out.empty());
    assert(out[0].sound_type == "gunshot");
    assert(out[0].threat_id == "THREAT-0001");
    std::cout << "[PASS] threat_tracker" << std::endl;
}

void test_feature_extractor_shape() {
    FeatureExtractorParams p;
    p.sample_rate = 16000;
    p.n_mels = 64;
    p.n_fft = 2048;
    p.hop_length = 512;
    FeatureExtractor ext(p);
    std::vector<float> audio(16000 * 2, 0.0f); // 2 seconds
    for (int i = 0; i < 32000; ++i) {
        audio[i] = std::sin(2.0f * M_PI * 500.0f * i / 16000.0f) * 0.1f;
    }
    std::vector<float> mel;
    bool ok = ext.extract(audio.data(), audio.size(), mel);
    assert(ok);
    assert(mel.size() == static_cast<size_t>(64 * 63)); // n_mels * pad_frames
    std::cout << "[PASS] feature_extractor_shape (" << mel.size() << " floats)" << std::endl;
}

int main() {
    std::cout << "Running core library tests..." << std::endl;
    test_audio_buffer_wraparound();
    test_preemphasis();
    test_gcc_phat_cross_array();
    test_distance_estimator();
    test_threat_tracker();
    test_feature_extractor_shape();
    std::cout << "All tests passed!" << std::endl;
    return 0;
}
