/*---------------------------------------------------------------------------*
  Project:  NintendoWare
  File:     snd_FxReverb.cpp

  Copyright (C)2009-2010 Nintendo Co., Ltd./HAL Laboratory, Inc.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Revision: $
 *---------------------------------------------------------------------------*/

#include "precompiled.h"

#include <nw/snd/snd_FxReverb.h>
#include <nw/snd/snd_HardwareManager.h>
#include <nw/ut/ut_Inlines.h>

namespace
{
const f32 MSEC_PER_FRAME = NN_SND_USECS_PER_FRAME / 1000.f;

inline f32 RoundUpToMsecPerFrame( u32 msec )
{
    return ( msec > MSEC_PER_FRAME ) ? msec : MSEC_PER_FRAME;
}

inline u32 ConvertMsecToSamples( f32 msec )
{
    return static_cast<u32>( msec / MSEC_PER_FRAME ) * NN_SND_SAMPLES_PER_FRAME;
}
} // anonymous namespace

namespace nw {
namespace snd {

FxReverb::FilterSize FxReverb::s_DefaultFilterSize;

FxReverb::FxReverb()
: m_pBuffer( NULL ),
  m_FilterSize( s_DefaultFilterSize ),
  m_EarlyGain( 0 ),
  m_FusedGain( 0 ),
  m_LpfCoef1( 0 ),
  m_LpfCoef2( 0 ),
  m_ProcessChannelCount( 4 ),
  m_IsActive( false )
{
    m_EarlyLength = NN_SND_SAMPLES_PER_FRAME;
    m_EarlyPos = 0;

    m_PreDelayLength = NN_SND_SAMPLES_PER_FRAME;
    m_PreDelayPos = 0;

    for ( int i = 0; i < 2; i++ )
    {
        m_CombFilterLength[i] = NN_SND_SAMPLES_PER_FRAME;
        m_CombFilterPos[i] = 0;
        m_CombFilterCoef[i] = 0;
    }

    m_AllPassFilterLength = NN_SND_SAMPLES_PER_FRAME;
    m_AllPassFilterPos = 0;
    m_AllPassFilterCoef = 0;

    for ( int ch = 0; ch < 4; ch++ )
    {
        m_WorkBuffer.m_EarlyReflection[ch] = NULL;
        m_WorkBuffer.m_PreDelay[ch] = NULL;

        for ( int i = 0; i < 2; i++ )
        {
            m_WorkBuffer.m_CombFilter[ch][i] = NULL;
        }
        m_WorkBuffer.m_AllPassFilter[ch] = NULL;
        m_WorkBuffer.m_Lpf[ch] = 0;
        m_LastLpfOut[ch] = 0;
    }
}

FxReverb::~FxReverb()
{
    if ( m_IsActive )
    {
        Finalize();
    }
    if ( m_pBuffer != NULL )
    {
        ReleaseWorkBuffer();
    }
}

bool FxReverb::Initialize()
{
    NW_ASSERT( ! m_IsActive );

    m_EarlyReflectionTimeAtInitialize = m_Param.m_EarlyReflectionTime;
    m_PreDelayTimeAtInitialize = m_Param.m_PreDelayTime;
    m_FilterSizeAtInitialize = m_FilterSize;
    m_IsEnableSurroundAtInitialize = m_Param.m_IsEnableSurround;

    AllocBuffer();
    InitializeParam();
    m_IsActive = true;

    return true;
}

void FxReverb::Finalize()
{
    if ( ! m_IsActive )
    {
        return;
    }
    m_IsActive = false;
    FreeBuffer();
}

bool FxReverb::SetParam( const FxReverb::Param& param )
{
    // 範囲チェック
    {
        NW_MINMAX_ASSERT( param.m_Coloration, 0.0f, 1.0f );
        if ( param.m_Coloration < 0.0f || param.m_Coloration > 1.f )
        {
            return false;
        }

        NW_MINMAX_ASSERT( param.m_Damping, 0.0f, 1.0f );
        if ( param.m_Damping < 0.0f || param.m_Damping > 1.f )
        {
            return false;
        }

        NW_MINMAX_ASSERT( param.m_EarlyGain, 0.0f, 1.0f );
        if ( param.m_EarlyGain < 0.0f || param.m_EarlyGain > 1.f )
        {
            return false;
        }

        NW_MINMAX_ASSERT( param.m_FusedGain, 0.0f, 1.0f );
        if ( param.m_FusedGain < 0.0f || param.m_FusedGain > 1.f )
        {
            return false;
        }
    }

    // バッファ長に影響するパラメータ
    {
        if ( m_IsActive == true )
        {
            if ( param.m_EarlyReflectionTime > m_EarlyReflectionTimeAtInitialize )
            {
                return false;
            }
            if ( param.m_PreDelayTime > m_PreDelayTimeAtInitialize )
            {
                return false;
            }
            if ( param.m_pFilterSize != NULL )
            {
                if ( param.m_pFilterSize->m_Comb0 > m_FilterSizeAtInitialize.m_Comb0 )
                {
                    return false;
                }
                if ( param.m_pFilterSize->m_Comb1 > m_FilterSizeAtInitialize.m_Comb1 )
                {
                    return false;
                }
                if ( param.m_pFilterSize->m_AllPass > m_FilterSizeAtInitialize.m_AllPass )
                {
                    return false;
                }
            }
            if ( m_IsEnableSurroundAtInitialize == false && param.m_IsEnableSurround == true )
            {
                return false;
            }
        }

        if ( param.m_IsEnableSurround == false )
        {
            m_ProcessChannelCount = 2;
        }
        else
        {
            m_ProcessChannelCount = 4;
        }

    }

    m_Param = param;   // 構造体コピー

    if ( m_Param.m_pFilterSize != NULL )
    {
        m_FilterSize = *m_Param.m_pFilterSize;  // 構造体コピー
        m_Param.m_pFilterSize = &m_FilterSize;
    }
    return true;
}

bool FxReverb::AssignWorkBuffer( uptr buffer, size_t size )
{
    NW_NULL_ASSERT( buffer );
    if ( buffer == NULL )
    {
        return false;
    }

    m_pBuffer = buffer;
    m_BufferSize = size;
    return true;
}

void FxReverb::ReleaseWorkBuffer()
{
    NW_NULL_ASSERT( m_pBuffer );
    m_pBuffer = NULL;
}

size_t FxReverb::GetRequiredMemSize()
{
    const size_t bufSizeForEarlyReflection = sizeof(s32)
        * ConvertMsecToSamples( RoundUpToMsecPerFrame( m_Param.m_EarlyReflectionTime ) );
    const size_t bufSizeForPreDelay = sizeof(s32)
        * ConvertMsecToSamples( RoundUpToMsecPerFrame( m_Param.m_PreDelayTime ) );
    const size_t bufSizeForFilterComp0 = sizeof(s32) * m_FilterSize.m_Comb0;
    const size_t bufSizeForFilterComp1 = sizeof(s32) * m_FilterSize.m_Comb1;
    const size_t bufSizeForFilterAllPass = sizeof(s32) * m_FilterSize.m_AllPass;

    size_t result = (
        bufSizeForEarlyReflection +
        bufSizeForPreDelay +
        bufSizeForFilterComp0 +
        bufSizeForFilterComp1 +
        bufSizeForFilterAllPass ) * m_ProcessChannelCount;

    result += 32;
        // 特に制約は無いはずだが、32 バイト境界に置くため、+32 しておく。
    return result;
}

void FxReverb::AllocBuffer()
{
    NW_NULL_ASSERT( m_pBuffer );

    const size_t bufSizeForEarlyReflection = sizeof(s32)
        * ConvertMsecToSamples( RoundUpToMsecPerFrame( m_Param.m_EarlyReflectionTime ) );
    const size_t bufSizeForPreDelay = sizeof(s32)
        * ConvertMsecToSamples( RoundUpToMsecPerFrame( m_Param.m_PreDelayTime ) );
    const size_t bufSizeForFilterComp0 = sizeof(s32) * m_FilterSize.m_Comb0;
    const size_t bufSizeForFilterComp1 = sizeof(s32) * m_FilterSize.m_Comb1;
    const size_t bufSizeForFilterAllPass = sizeof(s32) * m_FilterSize.m_AllPass;

    uptr ptr = ut::RoundUp( m_pBuffer, 32 );
    // NN_LOG("[%s] ptr(%p) m_pBuffer(%p) +bufSize(%p)\n",
    //         __FUNCTION__, ptr, m_pBuffer, m_pBuffer+m_BufferSize);
    for ( int ch = 0; ch < m_ProcessChannelCount; ch++ )
    {
        m_WorkBuffer.m_EarlyReflection[ch] = reinterpret_cast<s32*>(ptr);
        ptr += bufSizeForEarlyReflection;

        m_WorkBuffer.m_PreDelay[ch] = reinterpret_cast<s32*>(ptr);
        ptr += bufSizeForPreDelay;

        m_WorkBuffer.m_CombFilter[ch][0] = reinterpret_cast<s32*>(ptr);
        ptr += bufSizeForFilterComp0;

        m_WorkBuffer.m_CombFilter[ch][1] = reinterpret_cast<s32*>(ptr);
        ptr += bufSizeForFilterComp1;

        m_WorkBuffer.m_AllPassFilter[ch] = reinterpret_cast<s32*>(ptr);
        ptr += bufSizeForFilterAllPass;
    }
    // NN_LOG(" => ptr(%p)\n", ptr );
    NW_ASSERT( ptr <= m_pBuffer + m_BufferSize );
}

void FxReverb::FreeBuffer()
{
    for ( int ch = 0; ch < m_ProcessChannelCount; ch++ )
    {
        m_WorkBuffer.m_EarlyReflection[ch] = NULL;
        m_WorkBuffer.m_PreDelay[ch]        = NULL;
        m_WorkBuffer.m_CombFilter[ch][0]   = NULL;
        m_WorkBuffer.m_CombFilter[ch][1]   = NULL;
        m_WorkBuffer.m_AllPassFilter[ch]   = NULL;
    }
}

void FxReverb::InitializeParam()
{
    // 初期反射音（One-Shotディレイ）
    f32 early_time = RoundUpToMsecPerFrame( m_Param.m_EarlyReflectionTime );
    m_EarlyLength = ConvertMsecToSamples( early_time );
    m_EarlyPos = 0;
    
    // 後置残響音
    
    // プリディレイ
    f32 pre_delay_time = RoundUpToMsecPerFrame( m_Param.m_PreDelayTime );
    m_PreDelayLength = ConvertMsecToSamples( pre_delay_time );
    m_PreDelayPos = 0;
    
    // くし型フィルタ
    f32 fused_time_sec = static_cast<f32>( m_Param.m_FusedTime ) / 1000.f;
    NW_ASSERT( fused_time_sec != 0.f );
    
    m_CombFilterLength[0] = static_cast<s32>( m_FilterSize.m_Comb0 );
    m_CombFilterLength[1] = static_cast<s32>( m_FilterSize.m_Comb1 );
    
    for ( s32 i = 0; i < 2; i++ )
    {
        m_CombFilterPos[i] = 0;
        
        // 減衰時間から係数（フィードバックゲインへ変換）
        f32 comb_coef = std::powf( 10.f, 
                (-3.f * static_cast<f32>(m_CombFilterLength[i]) /
                 (fused_time_sec * internal::driver::HardwareManager::FX_SAMPLE_RATE) ) );
        m_CombFilterCoef[i] = static_cast<s32>( static_cast<f32>(0x80L) * comb_coef );
    }
    
    // 全域通過フィルタ
    m_AllPassFilterLength = static_cast<s32>( m_FilterSize.m_AllPass );
    m_AllPassFilterPos = 0;
    f32 all_pass_coef = m_Param.m_Coloration;
    m_AllPassFilterCoef = static_cast<s32>( static_cast<f32>(0x80L) * all_pass_coef );
    
    // 各係数
    m_EarlyGain = static_cast<s32>( static_cast<f32>(0x80L) * m_Param.m_EarlyGain );
    m_FusedGain = static_cast<s32>( static_cast<f32>(0x80L) * m_Param.m_FusedGain );
    
    f32 lpf_coef = m_Param.m_Damping;
    if ( lpf_coef > 0.95f ) lpf_coef = 0.95f;
    m_LpfCoef1 = static_cast<s32>( static_cast<s32>(0x80L) * ( 1.f - lpf_coef ) );
    m_LpfCoef2 = static_cast<s32>( static_cast<s32>(0x80L) * lpf_coef );
    
    
    // バッファを０クリア
    std::memset( reinterpret_cast<void*>(m_pBuffer), 0, m_BufferSize );
}

void FxReverb::UpdateBuffer(
        int numChannels,
        nn::snd::AuxBusData* data,
        s32 sampleLength,
        SampleFormat format,
        f32 sampleRate,
        OutputMode mode )
{
    NW_UNUSED_VARIABLE( sampleRate );
    NW_UNUSED_VARIABLE( format );
    NW_UNUSED_VARIABLE( mode );
    NW_UNUSED_VARIABLE( numChannels );

    if ( m_IsActive == false )
    {
        return;
    }
    NW_NULL_ASSERT( data );

    s32* input[nn::snd::CHANNEL_INDEX_NUM];
    input[nn::snd::CHANNEL_INDEX_FRONT_LEFT]  = data->frontLeft;
    input[nn::snd::CHANNEL_INDEX_FRONT_RIGHT] = data->frontRight;
    input[nn::snd::CHANNEL_INDEX_REAR_LEFT]   = data->rearLeft;
    input[nn::snd::CHANNEL_INDEX_REAR_RIGHT]  = data->rearRight;
    
    
    //-----------------------------------------------------------------
    // NOTE : 高速化のポイント
    // 
    //   ・固定小数計算に変更
    //     （浮動小数計算はレイテンシがあるので）
    //   ・メモリに置かれているデータを数回参照する箇所は、一度ローカル変数に
    //     入れて使う
    //     （バスアクセスが遅いので、レジスタに移してから使った方が速い。
    //       配列とメンバがこれに相当する）
    //   ・その他、処理の調整（式の変形など）
    //-----------------------------------------------------------------
    u32 early_pos           = 0;
    u32 pre_delay_pos       = 0;
    u32 comb_filter_pos0    = 0;
    u32 comb_filter_pos1    = 0;
    u32 allpass_filter_pos  = 0;

    for ( int ch = 0; ch < m_ProcessChannelCount; ch++ )
    {
        early_pos           = m_EarlyPos;
        pre_delay_pos       = m_PreDelayPos;
        comb_filter_pos0    = m_CombFilterPos[0];
        comb_filter_pos1    = m_CombFilterPos[1];
        allpass_filter_pos  = m_AllPassFilterPos;
        
        s32* early_reflection = m_WorkBuffer.m_EarlyReflection[ch];
        s32* pre_delay      = m_WorkBuffer.m_PreDelay[ch];
        
        for ( s32 samp = 0; samp < sampleLength; samp++ )
        {
            s32 indata = input[ch][samp];
            
            // 初期反射音
            s32 early_out = early_reflection[early_pos] * m_EarlyGain;
            // early_out >>= 7; // NOTE : 後で fused_out と纏めてシフトする
            early_reflection[early_pos] = indata;
            
            
            //
            // 以下、後置残響音
            //
            
            // プリディレイ
            s32 pre_delay_out = pre_delay[pre_delay_pos];
            pre_delay[pre_delay_pos] = indata;
            
            s32 filter_out = 0;
            

            // くし型フィルタ (1段目)
            s32* comb_line = m_WorkBuffer.m_CombFilter[ch][0];
            s32 out_tmp = comb_line[comb_filter_pos0];
            
            s32 comb_fb_0 = out_tmp;
            if( comb_fb_0 < 0 )
            {
                s32 tmp = -comb_fb_0;
                tmp = ( tmp * m_CombFilterCoef[0] ) >> 7;
                comb_fb_0 = -tmp;
            }
            else
            {
                comb_fb_0 = ( comb_fb_0 * m_CombFilterCoef[0] ) >> 7;
            }
            
            comb_line[comb_filter_pos0] = pre_delay_out + comb_fb_0;
            filter_out += out_tmp;
            
            
            // くし型フィルタ (2段目)
            comb_line = m_WorkBuffer.m_CombFilter[ch][1];
            out_tmp = comb_line[comb_filter_pos1];
            
            s32 comb_fb_1 = out_tmp;
            if( comb_fb_1 < 0 )
            {
                s32 tmp = -comb_fb_1;
                tmp = ( tmp * m_CombFilterCoef[1] ) >> 7;
                comb_fb_1 = -tmp;
            }
            else
            {
                comb_fb_1 = ( comb_fb_1 * m_CombFilterCoef[1] ) >> 7;
            }
            
            comb_line[comb_filter_pos1] = pre_delay_out + comb_fb_1;
            filter_out -= out_tmp;  // 逆相にして、出力のリプルを抑える
            
            
            // 全域通過フィルタ (1段)
            s32* allpass_line = m_WorkBuffer.m_AllPassFilter[ch];
            out_tmp = allpass_line[allpass_filter_pos];
            s32 allpass_coef = m_AllPassFilterCoef;
            
            s32 allpass_in = out_tmp;
            if( allpass_in < 0 ){
                s32 tmp = -allpass_in;
                tmp = ( tmp * allpass_coef ) >> 7;
                allpass_in = -tmp;
            }
            else
            {
                allpass_in = ( allpass_in * allpass_coef ) >> 7;
            }
            allpass_in += filter_out;
            
            allpass_line[allpass_filter_pos] = allpass_in;
            
            
            s32 fo_2 = allpass_in;
            if( fo_2 < 0 )
            {
                s32 tmp = -fo_2;
                tmp = ( tmp * allpass_coef ) >> 7;
                fo_2 = -tmp;
            }
            else
            {
                fo_2 = ( fo_2 * allpass_coef ) >> 7;
            }
            filter_out = out_tmp - fo_2;
            
            
            // 単極 LPF

            // NOTE : 以下の変形で乗算を１つ減らせます
            //         また、m_LpfCoef1 が不要になります
            //         （m_LpfCoef1 は残してありますので、以下の方が良ければ
            //           消して下さい）
            //
            // OUT = ( 1 - Coef ) * In + Coef * Histry
            //     = In - Coef * In + Coef * Histry
            //     = In - Coef * ( In + History )
            
            s32 tmp = m_LpfCoef2 * ( filter_out + m_LastLpfOut[ch] );
            tmp >>= 7;
            s32 fused_out = filter_out - tmp;
            m_LastLpfOut[ch] = fused_out;


            // 出力
            fused_out *= m_FusedGain;
            fused_out += early_out;
            fused_out >>= 7;
            input[ch][samp] = fused_out;
            
            ++early_pos;
            ++pre_delay_pos;
            ++comb_filter_pos0;
            ++comb_filter_pos1;
            ++allpass_filter_pos;
        }
    }
    
    // バッファ位置をメモリに書き戻す
    if ( early_pos >= m_EarlyLength )
    {
        m_EarlyPos = 0;
    }
    else
    {
        m_EarlyPos = early_pos;
    }
    
    if ( pre_delay_pos >= m_PreDelayLength )
    {
        m_PreDelayPos = 0;
    }
    else
    {
        m_PreDelayPos = pre_delay_pos;
    }
    
    if ( comb_filter_pos0 >= m_CombFilterLength[0] )
    {
        m_CombFilterPos[0] = 0;
    }
    else
    {
        m_CombFilterPos[0] = comb_filter_pos0;
    }
    
    if ( comb_filter_pos1 >= m_CombFilterLength[1] )
    {
        m_CombFilterPos[1] = 0;
    }
    else
    {
        m_CombFilterPos[1] = comb_filter_pos1;
    }
    
    if ( allpass_filter_pos >= m_AllPassFilterLength )
    {
        m_AllPassFilterPos = 0;
    }
    else
    {
        m_AllPassFilterPos = allpass_filter_pos;
    }
}

} // namespace nw::snd
} // namespace nw