// ----------------------------------------------------------------------------
//
//  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 "ambrot8.h"


RotMagics::RotMagics (int degree)
{
    int i, d = degree;

    _R = new float [4 * (d + 1)];
    _U = _R + d + 1;
    _V = _U + d + 1;
    _W = _V + d + 1;
    for (i = 0; i <= d; i++)
    {
	if (i < d)
	{
	    _R [i] = sqrtf (d * d - i * i);
	    _U [i] = _R [i];
	}
	else
	{
	    _R [i] = sqrtf (2 * d * (2 * d - 1));
	    _U [i] = 0.0f;
	}
	if (i > 0)
	{
  	    _V [i] = sqrtf ((d + i) * (d + i - 1) / 4.0f);
 	    _W [i] = sqrtf ((d - i) * (d - i - 1) / 4.0f);
	}
	else
	{
  	    _V [i] = -sqrtf (d * (d - 1) / 2.0f);
 	    _W [i] = 0.0f;
	}
    }
}


RotMagics::~RotMagics (void)
{
    delete[] _R;
}


Ambrot8::Ambrot8 (int fsamp, int degree) :
    _fsamp (fsamp),
    _touch0 (0),
    _touch1 (0),
    _nipol (0)
{
    if (degree < 0) degree = 0;
    if (degree > MAXDEGR) degree = MAXDEGR;
    _degree = degree;
    
    _M [0] = 0;
    _C [0] = 0;
    _Q [0] = _Q [1] = 0;
    for (int i = 1; i <= _degree; i++)
    {
        _M [i] = new RotMatrix (i);
        _C [i] = new RotMatrix (i);
        if (i > 1) _Q [i] = new RotMagics (i);
    }
}


Ambrot8::~Ambrot8 (void)
{
    for (int i = 0; i <= _degree; i++)
    {
        delete _M [i];
        delete _C [i];
        delete _Q [i];
    }
}


void Ambrot8::set_quaternion (float w, float x, float y, float z, float t)
{
    float m;

    m = sqrtf (w * w + x * x + y * y + z * z);
    if (!isnormal (m)) return;
    _mutex.lock ();
    _w = w / m;
    _x = x / m;
    _y = y / m;
    _z = z / m;
    if (t < 0.0f) t = 0.0f;
    if (t > 1.0f) t = 1.0f;
    _t = t;
    _touch0++;
    _mutex.unlock ();

#ifdef TEST
// Allows to test this class without calling proces ().
    update (); 
#endif        
}


void Ambrot8::set_rotation (float a, float x, float y, float z, float t)
{
    float m;

    m = sinf (a / 2) / sqrtf (x * x + y * y + z * z);
    if (!isnormal (m)) return;
    _mutex.lock ();
    _w = cosf (a / 2);
    _x = x * m;
    _y = y * m;
    _z = z * m;
    if (t < 0.0f) t = 0.0f;
    if (t > 1.0f) t = 1.0f;
    _t = t;
    _touch0++;
    _mutex.unlock ();
#ifdef TEST
// Allows to test this class without calling proces ().
    update (); 
#endif        
}


void Ambrot8::process (int nframes, float *inp[], float *out[])
{
    int  k0, nf, ni;

    if (_touch1 != _touch0) update ();
    memcpy (out [0], inp [0], nframes * sizeof (float));
    k0 = 0;
    ni = _nipol;
    while (nframes)
    {
	nf = nframes;
	if (ni)
	{
	    if (nf > ni) nf = ni;
	    process1 (inp, out, k0, nf, ni);
	    ni -= nf;
        }
	else
	{
	    process0 (inp, out, k0, nf);
	}
	nframes -= nf;
	k0 += nf;
    }
    _nipol = ni;
}


void Ambrot8::process0 (float *inp[], float *out[], int k0, int nf)
{
    RotMatrix  *C;
    int        d, i, j, k, i0;
    float      c, *p, *q;
    
    i0 = 0;
    for (d = 1; d <= _degree; d++)
    {
        i0 += 2 * d;
        C = _C [d];
        for (i = -d; i <= d; i++)
        {
  	    q = out [i0 + i] + k0;
	    p = inp [i0 - d] + k0;
	    c = C->get (i, -d);
	    for (k = 0; k < nf; k++) q [k] = c * p [k];
	    for (j = 1 - d ; j <= d ; j++)
	    {
	        p = inp [i0 + j] + k0;
	        c = C ->get (i, j);
	        for (k = 0; k < nf; k++) q [k] += c * p [k];
	    }
        }
    }
}


void Ambrot8::process1 (float *inp[], float *out[], int k0, int nf, int nc)
{
    RotMatrix  *C, *M;
    int        d, i, j, k, i0;
    float      c, c0, dc, *p, *q;

    i0 = 0;
    for (d = 1; d <= _degree; d++)
    {
        i0 += 2 * d;
        C = _C [d];
        M = _M [d];
        for (i = -d; i <= d; i++)
        {
  	    q = out [i0 + i] + k0;
  	    memset (q, 0, nf * sizeof (float));
	    for (j = -d ; j <= d ; j++)
	    {
	        p = inp [i0 + j] + k0;
	        c0 = c = C ->get (i, j);
  	        dc = (M ->get (i, j) - c) / nc;
	        for (k = 0; k < nf; k++)
                {
                    c += dc;
                    q [k] += c * p [k];
                }
                C->set (i, j, c0 + nf * dc);
	    }
        }
    }
}


void Ambrot8::update (void)
{
    if (_mutex.trylock ()) return;
    newmatrix1 ();
    _nipol = (int)(floorf (_t * _fsamp + 0.5f));
    _touch1 = _touch0;
    _mutex.unlock ();

    if (!_nipol) _C [1]->copy (_M [1]);
    for (int i = 2; i <= _degree; i++)
    {
        newmatrixd (i);
        if (!_nipol) _C [i]->copy (_M [i]);
    }
}


void Ambrot8::newmatrix1 (void)
{
    RotMatrix  *M = _M [1];
    float       xx, yy, zz, wx, wy, wz, xy, xz, yz;

    xx = _x * _x;
    yy = _y * _y;
    zz = _z * _z;
    wx = _w * _x;
    wy = _w * _y;
    wz = _w * _z;
    xy = _x * _y;
    xz = _x * _z;
    yz = _y * _z;

    M->set (-1, -1, 1 - 2 * (xx + zz));
    M->set (-1,  0, 2 * (yz - wx));
    M->set (-1,  1, 2 * (xy + wz));
    M->set ( 0, -1, 2 * (yz + wx));
    M->set ( 0,  0, 1 - 2 * (xx + yy));
    M->set ( 0,  1, 2 * (xz - wy));
    M->set ( 1, -1, 2 * (xy - wz));
    M->set ( 1,  0, 2 * (xz + wy));
    M->set ( 1,  1, 1 - 2 * (yy + zz));
}


void Ambrot8::newmatrixd (int d)
{
    int        k, m, n;
    float      s, u, v, w;
    RotMatrix  *M = _M [d];
    RotMagics  *Q = _Q [d];

    for (m = -d; m <= d; m++)
    {
	k = abs (m);
        u = Q->U (k);
	v = Q->V (k);
	w = Q->W (k);
        for (n = -d; n <= d; n++)
	{
   	    s = v * funcV (d, m, n);
	    if (u != 0) s += u * funcP (d, m, n, 0);
	    if (w != 0) s -= w * funcW (d, m, n);
	    M->set (m, n, s / Q->R (abs (n)));
	}
    }
}


float Ambrot8::funcV (int d, int m, int n)
{
    float p;
    
    if (m > 0)
    {
	m -= 1;
	p = funcP (d, m, n, 1);
        if (m) return p - funcP (d, -m, n, -1);
        else   return sqrtf (2.0f) * p;
    }
    if (m < 0)
    {
	m += 1;
	p = funcP (d, -m, n, -1);        
        if (m) return p + funcP (d, m, n, 1);
        else   return sqrtf (2.0f) * p;
    }
    return funcP (d, 1, n, 1) + funcP (d, -1, n, -1);
}


float Ambrot8::funcW (int d, int m, int n)
{
    if (m > 0)
    {
        m += 1;
        return funcP (d, m, n, 1) + funcP (d, -m, n, -1);       
    }
    if (m < 0)
    {
        m -= 1;
        return funcP (d, m, n, 1) - funcP (d, -m, n, -1);       
    }
    return 0;
}


float Ambrot8::funcP (int d, int m, int n, int i)
{
    RotMatrix *M1 = _M [1];
    RotMatrix *Mp = _M [d-1];

    if (n == -d) return M1->get (i, -1) * Mp->get (m, d-1) + M1->get (i,  1) * Mp->get (m, 1-d);
    if (n ==  d) return M1->get (i,  1) * Mp->get (m, d-1) - M1->get (i, -1) * Mp->get (m, 1-d);
    return M1->get (i, 0) * Mp->get (m, n);
}


#ifdef TEST
#include <stdio.h>

// Print matrix for degree d.
//
void Ambrot8::printmatrix (int d)
{
    int i, j;
    
    RotMatrix *M = _M [d];
    for (i = -d; i <= d; i++)
    {
        for (j = -d; j <= d; j++)
        {
            printf (" %9.6lf", M->get (i, j));
        }
        printf ("\n");
    }
    printf ("\n");
}


#endif
