/*############################################################################*/
/*#                                                                          #*/
/*#  Ambisonic C++ Library                                                   #*/
/*#  AmbisonicProcessor - Ambisonic Processor                               #*/
/*#  Copyright © 2007 Aristotel Digenis                                      #*/
/*#  Copyright © 2017 Videolabs                                              #*/
/*#                                                                          #*/
/*#  Filename:      AmbisonicProcessor.cpp                                   #*/
/*#  Version:       0.2                                                      #*/
/*#  Date:          19/05/2007                                               #*/
/*#  Author(s):     Aristotel Digenis, Peter Stitt                           #*/
/*#  Licence:       LGPL                                                     #*/
/*#                                                                          #*/
/*############################################################################*/


#include "AmbisonicProcessor.h"
#include <iostream>

namespace spaudio {

    AmbisonicProcessor::AmbisonicProcessor()
        : m_orientation(0, 0, 0)
    {
        m_pfTempSample = nullptr;
    }

    AmbisonicProcessor::~AmbisonicProcessor()
    {
        if (m_pfTempSample)
            delete[] m_pfTempSample;
    }

    bool AmbisonicProcessor::Configure(unsigned nOrder, bool b3D, unsigned nMisc)
    {
        bool success = AmbisonicBase::Configure(nOrder, b3D, nMisc);
        if (!success)
            return false;
        if (m_pfTempSample)
            delete[] m_pfTempSample;
        m_pfTempSample = new float[m_nChannelCount];
        memset(m_pfTempSample, 0, m_nChannelCount * sizeof(float));

        return true;
    }

    bool AmbisonicProcessor::Configure(unsigned nOrder, bool b3D, unsigned /*nBlockSize*/, unsigned nMisc)
    {
        return Configure(nOrder, b3D, nMisc);
    }

    void AmbisonicProcessor::Reset()
    {
    }

    void AmbisonicProcessor::Refresh()
    {
        // Trig terms used multiple times in rotation equations
        m_fCosAlpha = cosf(m_orientation.fAlpha);
        m_fSinAlpha = sinf(m_orientation.fAlpha);
        m_fCosBeta = cosf(m_orientation.fBeta);
        m_fSinBeta = sinf(m_orientation.fBeta);
        m_fCosGamma = cosf(m_orientation.fGamma);
        m_fSinGamma = sinf(m_orientation.fGamma);
        m_fCos2Alpha = cosf(2.f * m_orientation.fAlpha);
        m_fSin2Alpha = sinf(2.f * m_orientation.fAlpha);
        m_fCos2Beta = cosf(2.f * m_orientation.fBeta);
        m_fSin2Beta = sinf(2.f * m_orientation.fBeta);
        m_fCos2Gamma = cosf(2.f * m_orientation.fGamma);
        m_fSin2Gamma = sinf(2.f * m_orientation.fGamma);
        m_fCos3Alpha = cosf(3.f * m_orientation.fAlpha);
        m_fSin3Alpha = sinf(3.f * m_orientation.fAlpha);
        m_fCos3Beta = cosf(3.f * m_orientation.fBeta);
        m_fSin3Beta = sinf(3.f * m_orientation.fBeta);
        m_fCos3Gamma = cosf(3.f * m_orientation.fGamma);
        m_fSin3Gamma = sinf(3.f * m_orientation.fGamma);
    }

    void AmbisonicProcessor::SetOrientation(Orientation orientation)
    {
        m_orientation = orientation;
    }

    Orientation AmbisonicProcessor::GetOrientation()
    {
        return m_orientation;
    }

    void AmbisonicProcessor::Process(BFormat* pBFSrcDst, unsigned nSamples)
    {
        /* 3D Ambisonics input expected so perform 3D rotations */
        if (m_nOrder >= 1)
            ProcessOrder1_3D(pBFSrcDst, nSamples);
        if (m_nOrder >= 2)
            ProcessOrder2_3D(pBFSrcDst, nSamples);
        if (m_nOrder >= 3)
            ProcessOrder3_3D(pBFSrcDst, nSamples);
    }

    void AmbisonicProcessor::ProcessOrder1_3D(BFormat* pBFSrcDst, unsigned nSamples)
    {
        /* Rotations are performed in the following order:
            1 - rotation around the z-axis
            2 - rotation around the *new* y-axis (y')
            3 - rotation around the new z-axis (z'')
        This is different to the rotations obtained from the video, which are around z, y' then x''.
        The rotation equations used here work for third order. However, for higher orders a recursive algorithm
        should be considered.*/
        for (unsigned niSample = 0; niSample < nSamples; niSample++)
        {
            // Alpha rotation
            m_pfTempSample[kY] = -pBFSrcDst->m_ppfChannels[kX][niSample] * m_fSinAlpha
                + pBFSrcDst->m_ppfChannels[kY][niSample] * m_fCosAlpha;
            m_pfTempSample[kZ] = pBFSrcDst->m_ppfChannels[kZ][niSample];
            m_pfTempSample[kX] = pBFSrcDst->m_ppfChannels[kX][niSample] * m_fCosAlpha
                + pBFSrcDst->m_ppfChannels[kY][niSample] * m_fSinAlpha;

            // Beta rotation
            pBFSrcDst->m_ppfChannels[kY][niSample] = m_pfTempSample[kY];
            pBFSrcDst->m_ppfChannels[kZ][niSample] = m_pfTempSample[kZ] * m_fCosBeta
                + m_pfTempSample[kX] * m_fSinBeta;
            pBFSrcDst->m_ppfChannels[kX][niSample] = m_pfTempSample[kX] * m_fCosBeta
                - m_pfTempSample[kZ] * m_fSinBeta;

            // Gamma rotation
            m_pfTempSample[kY] = -pBFSrcDst->m_ppfChannels[kX][niSample] * m_fSinGamma
                + pBFSrcDst->m_ppfChannels[kY][niSample] * m_fCosGamma;
            m_pfTempSample[kZ] = pBFSrcDst->m_ppfChannels[kZ][niSample];
            m_pfTempSample[kX] = pBFSrcDst->m_ppfChannels[kX][niSample] * m_fCosGamma
                + pBFSrcDst->m_ppfChannels[kY][niSample] * m_fSinGamma;

            pBFSrcDst->m_ppfChannels[kX][niSample] = m_pfTempSample[kX];
            pBFSrcDst->m_ppfChannels[kY][niSample] = m_pfTempSample[kY];
            pBFSrcDst->m_ppfChannels[kZ][niSample] = m_pfTempSample[kZ];
        }
    }

    void AmbisonicProcessor::ProcessOrder2_3D(BFormat* pBFSrcDst, unsigned nSamples)
    {
        float fSqrt3 = sqrtf(3.f);

        for (unsigned niSample = 0; niSample < nSamples; niSample++)
        {
            // Alpha rotation
            m_pfTempSample[kV] = -pBFSrcDst->m_ppfChannels[kU][niSample] * m_fSin2Alpha
                + pBFSrcDst->m_ppfChannels[kV][niSample] * m_fCos2Alpha;
            m_pfTempSample[kT] = -pBFSrcDst->m_ppfChannels[kS][niSample] * m_fSinAlpha
                + pBFSrcDst->m_ppfChannels[kT][niSample] * m_fCosAlpha;
            m_pfTempSample[kR] = pBFSrcDst->m_ppfChannels[kR][niSample];
            m_pfTempSample[kS] = pBFSrcDst->m_ppfChannels[kS][niSample] * m_fCosAlpha
                + pBFSrcDst->m_ppfChannels[kT][niSample] * m_fSinAlpha;
            m_pfTempSample[kU] = pBFSrcDst->m_ppfChannels[kU][niSample] * m_fCos2Alpha
                + pBFSrcDst->m_ppfChannels[kV][niSample] * m_fSin2Alpha;

            // Beta rotation
            pBFSrcDst->m_ppfChannels[kV][niSample] = -m_fSinBeta * m_pfTempSample[kT]
                + m_fCosBeta * m_pfTempSample[kV];
            pBFSrcDst->m_ppfChannels[kT][niSample] = -m_fCosBeta * m_pfTempSample[kT]
                + m_fSinBeta * m_pfTempSample[kV];
            pBFSrcDst->m_ppfChannels[kR][niSample] = (0.75f * m_fCos2Beta + 0.25f) * m_pfTempSample[kR]
                + (0.5f * fSqrt3 * powf(m_fSinBeta, 2.0f)) * m_pfTempSample[kU]
                + (fSqrt3 * m_fSinBeta * m_fCosBeta) * m_pfTempSample[kS];
            pBFSrcDst->m_ppfChannels[kS][niSample] = m_fCos2Beta * m_pfTempSample[kS]
                - fSqrt3 * m_fCosBeta * m_fSinBeta * m_pfTempSample[kR]
                + m_fCosBeta * m_fSinBeta * m_pfTempSample[kU];
            pBFSrcDst->m_ppfChannels[kU][niSample] = (0.25f * m_fCos2Beta + 0.75f) * m_pfTempSample[kU]
                - m_fCosBeta * m_fSinBeta * m_pfTempSample[kS]
                + 0.5f * fSqrt3 * powf(m_fSinBeta, 2.0f) * m_pfTempSample[kR];

            // Gamma rotation
            m_pfTempSample[kV] = -pBFSrcDst->m_ppfChannels[kU][niSample] * m_fSin2Gamma
                + pBFSrcDst->m_ppfChannels[kV][niSample] * m_fCos2Gamma;
            m_pfTempSample[kT] = -pBFSrcDst->m_ppfChannels[kS][niSample] * m_fSinGamma
                + pBFSrcDst->m_ppfChannels[kT][niSample] * m_fCosGamma;

            m_pfTempSample[kR] = pBFSrcDst->m_ppfChannels[kR][niSample];
            m_pfTempSample[kS] = pBFSrcDst->m_ppfChannels[kS][niSample] * m_fCosGamma
                + pBFSrcDst->m_ppfChannels[kT][niSample] * m_fSinGamma;
            m_pfTempSample[kU] = pBFSrcDst->m_ppfChannels[kU][niSample] * m_fCos2Gamma
                + pBFSrcDst->m_ppfChannels[kV][niSample] * m_fSin2Gamma;

            pBFSrcDst->m_ppfChannels[kR][niSample] = m_pfTempSample[kR];
            pBFSrcDst->m_ppfChannels[kS][niSample] = m_pfTempSample[kS];
            pBFSrcDst->m_ppfChannels[kT][niSample] = m_pfTempSample[kT];
            pBFSrcDst->m_ppfChannels[kU][niSample] = m_pfTempSample[kU];
            pBFSrcDst->m_ppfChannels[kV][niSample] = m_pfTempSample[kV];
        }
    }

    void AmbisonicProcessor::ProcessOrder3_3D(BFormat* pBFSrcDst, unsigned nSamples)
    {
        /* (should move these somewhere that does recompute each time) */
        float fSqrt3_2 = sqrtf(3.f / 2.f);
        float fSqrt15 = sqrtf(15.f);
        float fSqrt5_2 = sqrtf(5.f / 2.f);

        for (unsigned niSample = 0; niSample < nSamples; niSample++)
        {
            // Alpha rotation
            m_pfTempSample[kQ] = -pBFSrcDst->m_ppfChannels[kP][niSample] * m_fSin3Alpha
                + pBFSrcDst->m_ppfChannels[kQ][niSample] * m_fCos3Alpha;
            m_pfTempSample[kO] = -pBFSrcDst->m_ppfChannels[kN][niSample] * m_fSin2Alpha
                + pBFSrcDst->m_ppfChannels[kO][niSample] * m_fCos2Alpha;
            m_pfTempSample[kM] = -pBFSrcDst->m_ppfChannels[kL][niSample] * m_fSinAlpha
                + pBFSrcDst->m_ppfChannels[kM][niSample] * m_fCosAlpha;
            m_pfTempSample[kK] = pBFSrcDst->m_ppfChannels[kK][niSample];
            m_pfTempSample[kL] = pBFSrcDst->m_ppfChannels[kL][niSample] * m_fCosAlpha
                + pBFSrcDst->m_ppfChannels[kM][niSample] * m_fSinAlpha;
            m_pfTempSample[kN] = pBFSrcDst->m_ppfChannels[kN][niSample] * m_fCos2Alpha
                + pBFSrcDst->m_ppfChannels[kO][niSample] * m_fSin2Alpha;
            m_pfTempSample[kP] = pBFSrcDst->m_ppfChannels[kP][niSample] * m_fCos3Alpha
                + pBFSrcDst->m_ppfChannels[kQ][niSample] * m_fSin3Alpha;

            // Beta rotation
            pBFSrcDst->m_ppfChannels[kQ][niSample] = 0.125f * m_pfTempSample[kQ] * (5.f + 3.f * m_fCos2Beta)
                - fSqrt3_2 * m_pfTempSample[kO] * m_fCosBeta * m_fSinBeta
                + 0.25f * fSqrt15 * m_pfTempSample[kM] * powf(m_fSinBeta, 2.0f);
            pBFSrcDst->m_ppfChannels[kO][niSample] = m_pfTempSample[kO] * m_fCos2Beta
                - fSqrt5_2 * m_pfTempSample[kM] * m_fCosBeta * m_fSinBeta
                + fSqrt3_2 * m_pfTempSample[kQ] * m_fCosBeta * m_fSinBeta;
            pBFSrcDst->m_ppfChannels[kM][niSample] = 0.125f * m_pfTempSample[kM] * (3.f + 5.f * m_fCos2Beta)
                - fSqrt5_2 * m_pfTempSample[kO] * m_fCosBeta * m_fSinBeta
                + 0.25f * fSqrt15 * m_pfTempSample[kQ] * powf(m_fSinBeta, 2.0f);
            pBFSrcDst->m_ppfChannels[kK][niSample] = 0.25f * m_pfTempSample[kK] * m_fCosBeta * (-1.f + 15.f * m_fCos2Beta)
                + 0.5f * fSqrt15 * m_pfTempSample[kN] * m_fCosBeta * powf(m_fSinBeta, 2.f)
                + 0.5f * fSqrt5_2 * m_pfTempSample[kP] * powf(m_fSinBeta, 3.f)
                + 0.125f * fSqrt3_2 * m_pfTempSample[kL] * (m_fSinBeta + 5.f * m_fSin3Beta);
            pBFSrcDst->m_ppfChannels[kL][niSample] = 0.0625f * m_pfTempSample[kL] * (m_fCosBeta + 15.f * m_fCos3Beta)
                + 0.25f * fSqrt5_2 * m_pfTempSample[kN] * (1.f + 3.f * m_fCos2Beta) * m_fSinBeta
                + 0.25f * fSqrt15 * m_pfTempSample[kP] * m_fCosBeta * powf(m_fSinBeta, 2.f)
                - 0.125f * fSqrt3_2 * m_pfTempSample[kK] * (m_fSinBeta + 5.f * m_fSin3Beta);
            pBFSrcDst->m_ppfChannels[kN][niSample] = 0.125f * m_pfTempSample[kN] * (5.f * m_fCosBeta + 3.f * m_fCos3Beta)
                + 0.25f * fSqrt3_2 * m_pfTempSample[kP] * (3.f + m_fCos2Beta) * m_fSinBeta
                + 0.5f * fSqrt15 * m_pfTempSample[kK] * m_fCosBeta * powf(m_fSinBeta, 2.f)
                + 0.125f * fSqrt5_2 * m_pfTempSample[kL] * (m_fSinBeta - 3.f * m_fSin3Beta);
            pBFSrcDst->m_ppfChannels[kP][niSample] = 0.0625f * m_pfTempSample[kP] * (15.f * m_fCosBeta + m_fCos3Beta)
                - 0.25f * fSqrt3_2 * m_pfTempSample[kN] * (3.f + m_fCos2Beta) * m_fSinBeta
                + 0.25f * fSqrt15 * m_pfTempSample[kL] * m_fCosBeta * powf(m_fSinBeta, 2.f)
                - 0.5f * fSqrt5_2 * m_pfTempSample[kK] * powf(m_fSinBeta, 3.f);

            // Gamma rotation
            m_pfTempSample[kQ] = -pBFSrcDst->m_ppfChannels[kP][niSample] * m_fSin3Gamma
                + pBFSrcDst->m_ppfChannels[kQ][niSample] * m_fCos3Gamma;
            m_pfTempSample[kO] = -pBFSrcDst->m_ppfChannels[kN][niSample] * m_fSin2Gamma
                + pBFSrcDst->m_ppfChannels[kO][niSample] * m_fCos2Gamma;
            m_pfTempSample[kM] = -pBFSrcDst->m_ppfChannels[kL][niSample] * m_fSinGamma
                + pBFSrcDst->m_ppfChannels[kM][niSample] * m_fCosGamma;
            m_pfTempSample[kK] = pBFSrcDst->m_ppfChannels[kK][niSample];
            m_pfTempSample[kL] = pBFSrcDst->m_ppfChannels[kL][niSample] * m_fCosGamma
                + pBFSrcDst->m_ppfChannels[kM][niSample] * m_fSinGamma;
            m_pfTempSample[kN] = pBFSrcDst->m_ppfChannels[kN][niSample] * m_fCos2Gamma
                + pBFSrcDst->m_ppfChannels[kO][niSample] * m_fSin2Gamma;
            m_pfTempSample[kP] = pBFSrcDst->m_ppfChannels[kP][niSample] * m_fCos3Gamma
                + pBFSrcDst->m_ppfChannels[kQ][niSample] * m_fSin3Gamma;

            pBFSrcDst->m_ppfChannels[kQ][niSample] = m_pfTempSample[kQ];
            pBFSrcDst->m_ppfChannels[kO][niSample] = m_pfTempSample[kO];
            pBFSrcDst->m_ppfChannels[kM][niSample] = m_pfTempSample[kM];
            pBFSrcDst->m_ppfChannels[kK][niSample] = m_pfTempSample[kK];
            pBFSrcDst->m_ppfChannels[kL][niSample] = m_pfTempSample[kL];
            pBFSrcDst->m_ppfChannels[kN][niSample] = m_pfTempSample[kN];
            pBFSrcDst->m_ppfChannels[kP][niSample] = m_pfTempSample[kP];
        }
    }

} // namespace spaudio
