#include <XCEngine/Audio/HRTF.h>
#include <algorithm>
#include <cmath>

namespace XCEngine {
namespace Audio {

HRTF::HRTF()
    : m_leftDelayLine(MaxDelaySamples, 0.0f)
    , m_rightDelayLine(MaxDelaySamples, 0.0f)
{
}

HRTF::~HRTF() {
}

void HRTF::ProcessAudio(float* buffer, uint32 sampleCount, uint32 channels,
                       const Math::Vector3& sourcePosition,
                       const Math::Vector3& listenerPosition,
                       const Math::Quaternion& listenerRotation) {
    if (!m_enabled || buffer == nullptr || sampleCount == 0) {
        return;
    }

    float azimuth = 0.0f;
    float elevation = 0.0f;

    ComputeDirection(sourcePosition, listenerPosition, listenerRotation, azimuth, elevation);
    ComputeITD(azimuth, elevation);
    ComputeILD(azimuth, elevation);

    m_params.azimuth = azimuth;
    m_params.elevation = elevation;

    if (m_hrtfEnabled) {
        ApplyHRTF(buffer, sampleCount, channels);
    } else {
        float pan = azimuth / 180.0f;
        ApplySimplePanning(buffer, sampleCount, channels, pan);
    }
}

void HRTF::SetQualityLevel(uint32 level) {
    m_qualityLevel = std::max(1u, std::min(3u, level));
}

void HRTF::SetCrossFeed(float crossFeed) {
    m_crossFeed = std::max(0.0f, std::min(1.0f, crossFeed));
}

void HRTF::SetSpeedOfSound(float speed) {
    m_speedOfSound = std::max(1.0f, speed);
}

void HRTF::ComputeDirection(const Math::Vector3& sourcePosition,
                           const Math::Vector3& listenerPosition,
                           const Math::Quaternion& listenerRotation,
                           float& azimuth, float& elevation) {
    Math::Vector3 direction = sourcePosition - listenerPosition;
    float distance = Math::Vector3::Magnitude(direction);

    if (distance < 0.001f) {
        azimuth = 0.0f;
        elevation = 0.0f;
        return;
    }

    direction = Math::Vector3::Normalize(direction);

    Math::Quaternion conjugateRotation = listenerRotation.Inverse();
    Math::Vector3 localDirection;
    localDirection.x = conjugateRotation.x * direction.x + conjugateRotation.y * direction.y + conjugateRotation.z * direction.z;
    localDirection.y = conjugateRotation.x * direction.y - conjugateRotation.y * direction.x + conjugateRotation.w * direction.z;
    localDirection.z = conjugateRotation.x * direction.z - conjugateRotation.y * direction.y - conjugateRotation.w * direction.x;

    Math::Vector3 forward(0.0f, 0.0f, 1.0f);
    Math::Vector3 up(0.0f, 1.0f, 0.0f);

    float dotForward = Math::Vector3::Dot(forward, localDirection);
    float dotUp = Math::Vector3::Dot(up, localDirection);

    azimuth = std::atan2(localDirection.x, localDirection.z) * 180.0f / 3.14159265f;
    elevation = std::asin(dotUp) * 180.0f / 3.14159265f;

    azimuth = std::max(-180.0f, std::min(180.0f, azimuth));
    elevation = std::max(-90.0f, std::min(90.0f, elevation));
}

void HRTF::ComputeITD(float azimuth, float elevation) {
    float headRadius = 0.075f;

    float cosAzimuth = std::cos(azimuth * 3.14159265f / 180.0f);

    float itd = (headRadius / m_speedOfSound) * (cosAzimuth - 1.0f);

    m_params.interauralTimeDelay = itd * m_sampleRate;
}

void HRTF::ComputeILD(float azimuth, float elevation) {
    float absAzimuth = std::abs(azimuth);

    float ild = (absAzimuth / 90.0f) * 20.0f;

    m_params.interauralLevelDifference = ild;
}

float HRTF::ComputePinnaEffect(float azimuth, float elevation) {
    float pinnaGain = 1.0f;

    if (elevation > 0.0f) {
        pinnaGain += elevation / 90.0f * 3.0f;
    } else if (elevation < -30.0f) {
        pinnaGain -= 3.0f;
    }

    pinnaGain = std::max(0.5f, std::min(2.0f, pinnaGain));

    return pinnaGain;
}

void HRTF::ApplyHRTF(float* buffer, uint32 sampleCount, uint32 channels) {
    if (channels < 2) {
        return;
    }

    float leftGain = 1.0f;
    float rightGain = 1.0f;

    if (m_params.azimuth < 0.0f) {
        rightGain *= (1.0f - std::abs(m_params.azimuth) / 90.0f);
    } else {
        leftGain *= (1.0f - std::abs(m_params.azimuth) / 90.0f);
    }

    float levelReduction = m_params.interauralLevelDifference / 20.0f;
    rightGain *= std::pow(10.0f, -levelReduction / 20.0f);

    for (uint32 i = 0; i < sampleCount; ++i) {
        uint32 sampleIndex = i * channels;

        float leftSample = buffer[sampleIndex];
        float rightSample = buffer[sampleIndex + 1];

        float delayedLeft = m_leftDelayLine[m_leftDelayIndex];
        float delayedRight = m_rightDelayLine[m_rightDelayIndex];

        buffer[sampleIndex] = delayedLeft * leftGain;
        buffer[sampleIndex + 1] = delayedRight * rightGain;

        m_leftDelayLine[m_leftDelayIndex] = leftSample;
        m_rightDelayLine[m_rightDelayIndex] = rightSample;

        m_leftDelayIndex = (m_leftDelayIndex + 1) % MaxDelaySamples;
        m_rightDelayIndex = (m_rightDelayIndex + 1) % MaxDelaySamples;
    }
}

void HRTF::ApplySimplePanning(float* buffer, uint32 sampleCount, uint32 channels, float pan) {
    if (channels < 2) {
        return;
    }

    pan = std::max(-1.0f, std::min(1.0f, pan));

    float leftGain = (1.0f - pan) / 2.0f;
    float rightGain = (1.0f + pan) / 2.0f;

    for (uint32 i = 0; i < sampleCount; ++i) {
        uint32 sampleIndex = i * channels;
        float sample = (buffer[sampleIndex] + buffer[sampleIndex + 1]) / 2.0f;

        buffer[sampleIndex] = sample * leftGain;
        buffer[sampleIndex + 1] = sample * rightGain;
    }
}

} // namespace Audio
} // namespace XCEngine