/* lmath.c
This file is part of a program that implements a Software-Defined Radio.
Copyright (C) 2015, 2016, 2023 Warren Pratt, NR0V
Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel
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 2
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, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
The author can be reached by email at
warren@wpratt.com
*/
#include "comm.hpp"
#include "bldr.hpp"
namespace WDSP {
BLDR::BLDR(int points, int ints)
{
// for the create function, 'points' and 'ints' are the MAXIMUM values that will be encountered
catxy = new double[2 * points];
sx.resize(points);
sy.resize(points);
h .resize(ints);
p.resize(ints);
np.resize(ints);
taa.resize(ints);
tab.resize(ints);
tag.resize(ints);
tad.resize(ints);
tbb.resize(ints);
tbg.resize(ints);
tbd.resize(ints);
tgg.resize(ints);
tgd.resize(ints);
tdd.resize(ints);
int nsize = 3 * ints + 1;
int intp1 = ints + 1;
int intm1 = ints - 1;
A .resize(intp1 * intp1);
B .resize(intp1 * intp1);
C .resize(intp1 * intp1);
D .resize(intp1);
E .resize(intp1 * intp1);
F .resize(intm1 * intp1);
G .resize(intp1);
MAT.resize(nsize * nsize);
RHS.resize(nsize);
SLN.resize(nsize);
z .resize(intp1);
zp.resize(intp1);
wrk.resize(nsize);
ipiv.resize(nsize);
}
BLDR::~BLDR()
{
delete[]catxy;
}
void BLDR::flush(int points)
{
memset(catxy, 0, 2 * points * sizeof(double));
std::fill(sx.begin(), sx.end(), 0);
std::fill(sy.begin(), sy.end(), 0);
std::fill(h.begin(), h.end(), 0);
std::fill(p.begin(), p.end(), 0);
std::fill(np.begin(), np.end(), 0);
std::fill(taa.begin(), taa.end(), 0);
std::fill(tab.begin(), tab.end(), 0);
std::fill(tag.begin(), tag.end(), 0);
std::fill(tad.begin(), tad.end(), 0);
std::fill(tbb.begin(), tbb.end(), 0);
std::fill(tbg.begin(), tbg.end(), 0);
std::fill(tbd.begin(), tbd.end(), 0);
std::fill(tgg.begin(), tgg.end(), 0);
std::fill(tgd.begin(), tgd.end(), 0);
std::fill(tdd.begin(), tdd.end(), 0);
std::fill(A.begin(), A.end(), 0);
std::fill(B.begin(), B.end(), 0);
std::fill(C.begin(), C.end(), 0);
std::fill(D.begin(), D.end(), 0);
std::fill(E.begin(), E.end(), 0);
std::fill(F.begin(), F.end(), 0);
std::fill(G.begin(), G.end(), 0);
std::fill(MAT.begin(), MAT.end(), 0);
std::fill(RHS.begin(), RHS.end(), 0);
std::fill(SLN.begin(), SLN.end(), 0);
std::fill(z.begin(), z.end(), 0);
std::fill(zp.begin(), zp.end(), 0);
std::fill(wrk.begin(), wrk.end(), 0);
std::fill(ipiv.begin(), ipiv.end(), 0);
}
int BLDR::fcompare(const void* a, const void* b)
{
if (*(double*)a < *(double*)b)
return -1;
else if (*(double*)a == *(double*)b)
return 0;
else
return 1;
}
void BLDR::decomp(int n, std::vector<double>& a, std::vector<int>& piv, int* info, std::vector<double>& wrk)
{
int i;
int j;
int t_piv;
double m_row;
double mt_row;
double m_col;
double mt_col;
*info = 0;
for (i = 0; i < n; i++)
{
piv[i] = i;
m_row = 0.0;
for (j = 0; j < n; j++)
{
mt_row = a[n * i + j];
if (mt_row < 0.0) mt_row = -mt_row;
if (mt_row > m_row) m_row = mt_row;
}
if (m_row == 0.0)
{
*info = i;
goto cleanup;
}
wrk[i] = m_row;
}
for (int k = 0; k < n - 1; k++)
{
j = k;
m_col = a[n * piv[k] + k] / wrk[piv[k]];
if (m_col < 0) m_col = -m_col;
for (i = k + 1; i < n; i++)
{
mt_col = a[n * piv[i] + k] / wrk[piv[k]];
if (mt_col < 0.0) mt_col = -mt_col;
if (mt_col > m_col)
{
m_col = mt_col;
j = i;
}
}
if (m_col == 0)
{
*info = -k;
goto cleanup;
}
t_piv = piv[k];
piv[k] = piv[j];
piv[j] = t_piv;
for (i = k + 1; i < n; i++)
{
a[n * piv[i] + k] /= a[n * piv[k] + k];
for (j = k + 1; j < n; j++)
a[n * piv[i] + j] -= a[n * piv[i] + k] * a[n * piv[k] + j];
}
}
if (a[n * n - 1] == 0.0)
*info = -n;
cleanup:
return;
}
void BLDR::dsolve(int n, std::vector<double>& a, std::vector<int>& piv, std::vector<double>& b, std::vector<double>& x)
{
int j;
int k;
double sum;
for (k = 0; k < n; k++)
{
sum = 0.0;
for (j = 0; j < k; j++)
sum += a[n * piv[k] + j] * x[j];
x[k] = b[piv[k]] - sum;
}
for (k = n - 1; k >= 0; k--)
{
sum = 0.0;
for (j = k + 1; j < n; j++)
sum += a[n * piv[k] + j] * x[j];
x[k] = (x[k] - sum) / a[n * piv[k] + k];
}
}
void BLDR::cull(int* n, int ints, std::vector<double>& x, const double* t, double ptol)
{
int k = 0;
int i = *n;
int ntopint;
int npx;
while (x[i - 1] > t[ints - 1])
i--;
ntopint = *n - i;
npx = (int)(ntopint * (1.0 - ptol));
i = *n;
while ((k < npx) && (x[--i] > t[ints]))
k++;
*n -= k;
}
void BLDR::execute(int points, const double* x, const double* y, int ints, const double* t, int* info, double* c, double ptol)
{
double u;
double v;
double alpha;
double beta;
double gamma;
double delta;
int nsize = 3 * ints + 1;
int intp1 = ints + 1;
int intm1 = ints - 1;
int i;
int j;
int k;
int m;
int dinfo;
flush(points);
for (i = 0; i < points; i++)
{
catxy[2 * i + 0] = x[i];
catxy[2 * i + 1] = y[i];
}
qsort(catxy, points, 2 * sizeof(double), fcompare);
for (i = 0; i < points; i++)
{
sx[i] = catxy[2 * i + 0];
sy[i] = catxy[2 * i + 1];
}
cull(&points, ints, sx, t, ptol);
if (points <= 0 || sx[points - 1] > t[ints])
{
*info = -1000;
goto cleanup;
}
else *info = 0;
for (j = 0; j < ints; j++)
h[j] = t[j + 1] - t[j];
p[0] = 0;
j = 0;
for (i = 0; i < points; i++)
{
if (sx[i] <= t[j + 1])
np[j]++;
else
{
p[++j] = i;
while (sx[i] > t[j + 1])
p[++j] = i;
np[j] = 1;
}
}
for (i = 0; i < ints; i++)
for (j = p[i]; j < p[i] + np[i]; j++)
{
u = (sx[j] - t[i]) / h[i];
v = u - 1.0;
alpha = (2.0 * u + 1.0) * v * v;
beta = u * u * (1.0 - 2.0 * v);
gamma = h[i] * u * v * v;
delta = h[i] * u * u * v;
taa[i] += alpha * alpha;
tab[i] += alpha * beta;
tag[i] += alpha * gamma;
tad[i] += alpha * delta;
tbb[i] += beta * beta;
tbg[i] += beta * gamma;
tbd[i] += beta * delta;
tgg[i] += gamma * gamma;
tgd[i] += gamma * delta;
tdd[i] += delta * delta;
D[i + 0] += 2.0 * sy[j] * alpha;
D[i + 1] += 2.0 * sy[j] * beta;
G[i + 0] += 2.0 * sy[j] * gamma;
G[i + 1] += 2.0 * sy[j] * delta;
}
for (i = 0; i < ints; i++)
{
A[(i + 0) * intp1 + (i + 0)] += 2.0 * taa[i];
A[(i + 1) * intp1 + (i + 1)] = 2.0 * tbb[i];
A[(i + 0) * intp1 + (i + 1)] = 2.0 * tab[i];
A[(i + 1) * intp1 + (i + 0)] = 2.0 * tab[i];
B[(i + 0) * intp1 + (i + 0)] += 2.0 * tag[i];
B[(i + 1) * intp1 + (i + 1)] = 2.0 * tbd[i];
B[(i + 0) * intp1 + (i + 1)] = 2.0 * tbg[i];
B[(i + 1) * intp1 + (i + 0)] = 2.0 * tad[i];
E[(i + 0) * intp1 + (i + 0)] += 2.0 * tgg[i];
E[(i + 1) * intp1 + (i + 1)] = 2.0 * tdd[i];
E[(i + 0) * intp1 + (i + 1)] = 2.0 * tgd[i];
E[(i + 1) * intp1 + (i + 0)] = 2.0 * tgd[i];
}
for (i = 0; i < intm1; i++)
{
C[i * intp1 + (i + 0)] = +3.0 * h[i + 1] / h[i];
C[i * intp1 + (i + 2)] = -3.0 * h[i] / h[i + 1];
C[i * intp1 + (i + 1)] = -C[i * intp1 + (i + 0)] - C[i * intp1 + (i + 2)];
F[i * intp1 + (i + 0)] = h[i + 1];
F[i * intp1 + (i + 1)] = 2.0 * (h[i] + h[i + 1]);
F[i * intp1 + (i + 2)] = h[i];
}
for (i = 0, k = 0; i < intp1; i++, k++)
{
for (j = 0, m = 0; j < intp1; j++, m++)
MAT[k * nsize + m] = A[i * intp1 + j];
for (j = 0, m = intp1; j < intp1; j++, m++)
MAT[k * nsize + m] = B[j * intp1 + i];
for (j = 0, m = 2 * intp1; j < intm1; j++, m++)
MAT[k * nsize + m] = C[j * intp1 + i];
RHS[k] = D[i];
}
for (i = 0, k = intp1; i < intp1; i++, k++)
{
for (j = 0, m = 0; j < intp1; j++, m++)
MAT[k * nsize + m] = B[i * intp1 + j];
for (j = 0, m = intp1; j < intp1; j++, m++)
MAT[k * nsize + m] = E[i * intp1 + j];
for (j = 0, m = 2 * intp1; j < intm1; j++, m++)
MAT[k * nsize + m] = F[j * intp1 + i];
RHS[k] = G[i];
}
for (i = 0, k = 2 * intp1; i < intm1; i++, k++)
{
for (j = 0, m = 0; j < intp1; j++, m++)
MAT[k * nsize + m] = C[i * intp1 + j];
for (j = 0, m = intp1; j < intp1; j++, m++)
MAT[k * nsize + m] = F[i * intp1 + j];
for (j = 0, m = 2 * intp1; j < intm1; j++, m++)
MAT[k * nsize + m] = 0.0;
RHS[k] = 0.0;
}
decomp(nsize, MAT, ipiv, &dinfo, wrk);
dsolve(nsize, MAT, ipiv, RHS, SLN);
if (dinfo != 0)
{
*info = dinfo;
goto cleanup;
}
for (i = 0; i <= ints; i++)
{
z[i] = SLN[i];
zp[i] = SLN[i + ints + 1];
}
for (i = 0; i < ints; i++)
{
c[4 * i + 0] = z[i];
c[4 * i + 1] = zp[i];
c[4 * i + 2] = -3.0 / (h[i] * h[i]) * (z[i] - z[i + 1]) - 1.0 / h[i] * (2.0 * zp[i] + zp[i + 1]);
c[4 * i + 3] = 2.0 / (h[i] * h[i] * h[i]) * (z[i] - z[i + 1]) + 1.0 / (h[i] * h[i]) * (zp[i] + zp[i + 1]);
}
cleanup:
return;
}
} // namespace WDSP