// ----------------------------------------------------------------------------
//
//  Copyright (C) 2012..2021 Fons Adriaensen <fons@linuxaudio.org>
//    
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 3 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
// ----------------------------------------------------------------------------


#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "ambpan8.h"
#include "rspharm.h"


float Ambpan8::Csemi [12] =
{
    1.0f,         // 1, 2, 3
    0.8660254f,   // 4, 5, 7, 8    sqrt (3/4)
    0.5f,         // 6       
    0.7905694f,   //  9, 15        sqrt (5/8)
    1.9364917f,   // 10, 14        sqrt (15/4)
    0.6123724f,   // 11, 13        sqrt (3/8)
    0.5f,         // 12
    0.7395100f,   // 16, 24        sqrt (35/64)
    2.0916501f,   // 17, 23        sqrt (35/8)
    0.5590170f,   // 18, 22,       sqrt (5/16)
    0.7905694f,   // 19, 21        sqrt (5/8)
    0.125f        // 20
};
    

float Ambpan8::Cnorm [12] =
{   1.7320508f,   // 1, 2, 3       sqrt (3)
    1.9364917f,   // 4, 5, 7, 8    sqrt (15/4)
    1.1180340f,   // 6             sqrt (5/4)
    2.0916501f,   //  9, 15        sqrt (35/8)
    5.1234754f,   // 10, 14        sqrt (105/4)
    1.6201852f,   // 11, 13        sqrt (21/8)
    1.3228757f,   // 12            sqrt (7/4)
    2.2185300f,   // 16, 24        sqrt (315/64)
    6.2749502f,   // 17, 23        sqrt (315/8)
    1.6770510f,   // 18, 22,       sqrt (45/16)
    2.3717082f,   // 19, 21        sqrt (45/8)
    0.375f        // 20
};
    

    
Ambpan8::Ambpan8 (int fsamp, int degree, bool semi) :
    _fsamp (fsamp),
    _semi (semi),
    _touch0 (0),
    _touch1 (0),
    _nipol (0)
{
    if (degree < 0) degree = 0;
    if (degree > MAXDEGR) degree = MAXDEGR;
    _degree = degree;
    encode (0.0f, 0.0f, _G);
}


Ambpan8::~Ambpan8 (void)
{
}


void Ambpan8::set_direction (float az, float el, float dt)
{
    _az = az * M_PI / 180.0f;
    _el = el * M_PI / 180.0f;
    if (dt < 0.0f) dt = 0.0f;
    if (dt > 1.0f) dt = 1.0f;
    _dt = dt;
    _touch0++;
}


void Ambpan8::process (int nframes, float *inp, float *out[], bool add)
{
    int    i, k, k0, nf, nh;
    float  g, d;
    float  *q;
    
    if (_touch1 != _touch0) update ();
    q = out [0];
    if (add)
    {
        for (k = 0; k < nframes; k++) q [k] += inp [k];
    }
    else
    {
        memcpy (q, inp, nframes * sizeof (float));
    }
    k0 = 0;
    nh = (_degree + 1) * (_degree + 1);
    while (nframes)
    {
        nf = nframes;
        if (_nipol)
        {
            if (nf > _nipol) nf = _nipol;
            for (i = 1; i < nh; i++)
            {
                q = out [i] + k0;
                g = _G [i];
                d = (_T [i] - g) / _nipol;
                if (add)
                {
                    for (k = 0; k < nf; k++)
                    {
                        g += d;
                        q [k] += g * inp [k];
                    }
                }
                else
                {
                    for (k = 0; k < nf; k++)
                    {
                        g += d;
                        q [k] = g * inp [k];
                    }
                }
                _G [i] = g;
            }
            _nipol -= nf;
        }
        else
        {
            for (i = 1; i < nh; i++)
            {
                q = out [i] + k0;
                g = _G [i];
                if (add)
                {
                    for (k = 0; k < nf; k++)
                    {
                        q [k] += g * inp [k];
                    }
                }
                else
                {
                    for (k = 0; k < nf; k++)
                    {
                        q [k] = g * inp [k];
                    }
                }
            }
        }
        k0 += nf;
        inp += nf;
        nframes -= nf;
    }
}


void Ambpan8::update (void)
{
    int nh;
    
    _nipol = (int)(floorf (_dt * _fsamp + 0.5f));
    encode (_az, _el, _T);
    if (_nipol == 0)
    {
        nh = (_degree + 1) * (_degree + 1);
        memcpy (_G, _T, nh * sizeof (float));
    }
    _touch1 = _touch0;
}


void Ambpan8::encode (float azim, float elev, float *E)
{
    if (_degree < 5) encode4 (azim, elev, E);
    else realspharm (_degree, azim, elev, E, _semi);
}


void Ambpan8::encode4 (float azim, float elev, float *E)
{
    float t, x1, y1, z1, x2, y2, z2, c2, s2, c3, s3, x4, y4, z4;
    float *C;
    
    C = _semi ? Csemi : Cnorm;
    
    E [0] = 1.0f;
    t = cosf (elev);
    x1 = cosf (azim) * t;
    y1 = sinf (azim) * t;
    z1 = sinf (elev);
    t = C [0];
    E [1] = t * y1;
    E [2] = t * z1;
    E [3] = t * x1;
    if (_degree < 2) return;
    
    x2 = x1 * x1;
    y2 = y1 * y1;
    z2 = z1 * z1;
    c2 = x2 - y2;
    s2 = 2 * x1 * y1;
    t = C [1];
    E [8] = t * c2;
    E [4] = t * s2;
    t *= 2 * z1;
    E [7] = t * x1;
    E [5] = t * y1;
    E [6] = C [2] * (3 * z2 - 1);
    if (_degree < 3) return;
    
    c3 = x1 * (x2 - 3 * y2);
    s3 = y1 * (3 * x2 - y2);
    t = C [3];
    E [15] = t * c3;
    E [ 9] = t * s3;
    t = C [4] * z1;
    E [14] = t * c2;
    E [10] = t * s2;
    t = C [5] * (5 * z2 - 1);
    E [13] = t * x1; 
    E [11] = t * y1; 
    E [12] = C [6] * (5 * z2 - 3) * z1;
    if (_degree < 4) return;

    x4 = x2 * x2;
    y4 = y2 * y2;
    z4 = z2 * z2;
    t = C [7];
    E [24] = t * (x4 - 6 * x2 * y2 + y4);
    E [16] = t * 2 * s2 * c2;
    t = C [8] * z1;
    E [23] = t * c3;
    E [17] = t * s3;
    t = C [9] * (7 * z2 - 1);
    E [22] = t * c2;
    E [18] = t * s2;
    t = C [10] * z1 * (7 * z2 - 3);
    E [21] = t * x1;
    E [19] = t * y1;
    E [20] = C [11] * (35 * z4 - 30 * z2 + 3);
}


