% Least-squares spline modeling
% Copyright (C) Markus Nentwig 2014
% Distributed under the MIT licence.
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the Software is
% furnished to do so, subject to the following conditions:
% 
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
% 
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
% THE SOFTWARE.

close all;
clear all;

% *****************************************************
% basis polynomials for 3rd order spline
% *****************************************************
p1 = [1/4,0,-3/4,1/2]; 	% [p(-1), p(1), dp/dx(-1), dp/dx(1)] = [1 0 0 0]
p2=[-1/4,0,3/4,1/2]; 	% [p(-1), p(1), dp/dx(-1), dp/dx(1)] = [0 1 0 0]
p3=[1/4,-1/4,-1/4,1/4]; % [p(-1), p(1), dp/dx(-1), dp/dx(1)] = [0 0 1 0]
p4=[1/4,1/4,-1/4,-1/4]; % [p(-1), p(1), dp/dx(-1), dp/dx(1)] = [0 0 0 1]

if 1
    % *****************************************************
    % plot basis polynomials
    % *****************************************************
    figure(1); hold on;
    xt = linspace(-1, 1, 1000);
    h = plot(xt, polyval(p1, xt), 'k'); set(h, 'lineWidth', 3);
    h = plot(xt, polyval(p2, xt), 'b'); set(h, 'lineWidth', 3);
    h = plot(xt, polyval(p3, xt), 'g'); set(h, 'lineWidth', 3);
    h = plot(xt, polyval(p4, xt), 'r'); set(h, 'lineWidth', 3);
    legend('p1', 'p2', 'p3', 'p4');
    xlabel('x in segment');
    ylabel('p(x)');
    title('basis functions');
end

% *****************************************************
% example function
% *****************************************************
if 1
    % *****************************************************
    % "volume control" example: 40 dB range, 0=0 (-inf dB)
    % *****************************************************
    nSup = 5;
    nPts = 1000*nSup;
    x = linspace(0, 1, nPts+1); x = x(1:end-1);
    y = 10 .^ (2*x);
    y = y - y(1);
    y = y / y(end);
elseif 0
    % *****************************************************
    % Chirp example
    % *****************************************************
    nSup = 17;
    nPts = 1000*nSup;
    x = linspace(0, 1, nPts+1); x = x(1:end-1);
    y = cos(2*pi*x.^2*5);
elseif 1
    % *****************************************************
    % Hammond drawbar example
    % *****************************************************
    nSup = 17;
    y = load('drawbar.dat');
    nPts = numel(y);
    x = linspace(0, 1, nPts+1); x = x(1:end-1);
    y = y / max(abs(y));
else
    % *****************************************************
    % linear function to debug RTL
    % *****************************************************
    nSup = 17;
    nPts = 1000*nSup;
    x = linspace(0, 1, nPts+1); x = x(1:end-1);
    y = x;
end

% *****************************************************
% leave a bit of headroom for the digital implementation:
% approximation may overshoot a little (also, "+1" does not
% exist in fixed point)
% *****************************************************
y = y * 0.99;

if 0
    % *****************************************************
    % demonstrate non-uniform grid:
    % Add noise to sampling instants
    % *****************************************************
    x = x + randn(size(x))*0.01;
end

if 0
    % *****************************************************
    % demonstrate noisy data:
    % Add noise to input values
    % *****************************************************
    y = y + randn(size(y))*0.03*max(abs(y));
end

%figure(); h = plot(x, y, 'k+'); set(h, 'lineWidth', 3);

% scale so that segments span (0..1), (1..2), (2..3), ...(nSup-2..nSup)
x = x * (nSup-1); 

% Number of coefficients:
% Every support point allows variable level and slope
nCoeff = 2*nSup;
M = [];

% The least-squares fit operates on basis polynomials. Matrix MM traces back "how much c0, c1, c2, c3" 
% is contributed to the result for a unity entry in c. This is done individually per segment.
MM = zeros(nCoeff, 4*nSup-4);

% Indices in MM for a support point adding a base polynomial in positive direction
cR_ahead=1:4;

% Indices in MM for a support point adding a base polynomial in negative direction
cR_back=cR_ahead-4;

% Iterate all support points
% collect contributed basis polynomials
% build backtracing matrix MM
for ixSeg = 1:nSup
    
    % location of current segment's value / slope coefficient in c vector
    ixVal = ixSeg*2-1;
    ixSlope = ixVal+1;
    
    vVal = zeros(nPts, 1); 	% scaled by first coefficient
    vSlope = zeros(nPts, 1); 	% scaled by second coefficient
    v = zeros(nPts, 1);
    
    % domain interval [-0.5, 0.5[ looking back 
    if (ixSeg > 1)
        xEv = interp1([ixSeg-2:ixSeg-1], [-1, 1], x+1e-15, 'linear', 'extrap');
        mask = abs(xEv) > 1;
        yEv = polyval(p2, xEv);
        yEv(mask) = 0;
        vVal = vVal + yEv(:);
        MM(ixVal, cR_back) = MM(ixVal, cR_back) + p2;
        
        yEv = polyval(p4, xEv);
        yEv(abs(xEv) > 1) = 0;
        vSlope = vSlope + yEv(:);
        MM(ixSlope, cR_back) = MM(ixSlope, cR_back) + p4;
    end
    
    % domain interval [-0.5, 0.5[ looking forwards
    if (ixSeg < nSup)
        xEv = interp1([ixSeg-1:ixSeg], [-1, 1], x+1e-15, 'linear', 'extrap');
        mask = abs(xEv) > 1;
        yEv = polyval(p1, xEv);
        yEv(mask) = 0;
        vVal = vVal + yEv(:);
        MM(ixVal, cR_ahead) = MM(ixVal, cR_ahead) + p1;
 
        yEv = polyval(p3, xEv);
        yEv(abs(xEv) > 1) = 0;
        vSlope = vSlope + yEv(:);
        MM(ixSlope, cR_ahead) = MM(ixSlope, cR_ahead) + p3;
    end
    
    M = [M, vVal(:), vSlope(:)];
    cR_back = cR_back + 4;
    cR_ahead = cR_ahead + 4;
end

% *****************************************************
% Least-squares solution: Reconstruct y as sum of basis
% polynomials in shared coefficients (enforcing continuity
% in level and 1st derivative)
% *****************************************************
c = pinv(M) * y(:);
figure(); hold on;

% *****************************************************
% Plot least-squares fit
% should be identical to final result
% *****************************************************
h = plot(x, y, 'k');  set(h, 'lineWidth', 3);
h = plot(x, M*c, 'b');  set(h, 'lineWidth', 3);
legend('original', 'least-squares fit');
title('evaluation via basis functions');

% *****************************************************
% Backtrace to polynomial coefficients
% *****************************************************
d = MM.' * c;

% *****************************************************
% Set up an example with explicit bank indexing (intPart)
% and independent variable (fracPart)
% *****************************************************
% enforce x-limits for below code (out-of-range wraps)
x(x < 0) = 0;
x(x >= nSup-1) = nSup-1-1e-15;

intPart = floor(x); intPart = intPart(:);
fracPart = (mod(x, 1) - 0.5)*2; fracPart = fracPart(:);

if 1
    figure(); hold on;
    h = plot(intPart, 'k'); set(h, 'lineWidth', 3);
    h = plot(fracPart-3, 'b'); set(h, 'lineWidth', 3)
    legend('integer part (bank index', 'fractional part (indep. variable)');;    
end

ixc3 = 4*intPart+1;
ixc2 = 4*intPart+2;
ixc1 = 4*intPart+3;
ixc0 = 4*intPart+4;
% evaluate conventionally
yy = d(ixc0) + d(ixc1) .* fracPart + d(ixc2) .* fracPart.^2 + d(ixc3) .* fracPart.^3;

% evaluate via Horner scheme (same as above)
z = d(ixc3);
z = z .* fracPart;
z = z + d(ixc2);
z = z .* fracPart;
z = z + d(ixc1);
z = z .* fracPart;
z = z + d(ixc0);

figure(33); hold on;
h = plot(x, y, 'k'); set(h, 'lineWidth', 3);
%h = plot(x, yy, 'b'); set(h, 'lineWidth', 3);
h = plot(x, z, 'b'); set(h, 'lineWidth', 3);
title('final evaluation via back-traced segment polynomials');
legend('original', 'model');

figure(301); hold on;
h = plot(x, 20*log10(y(:)-z(:)), 'r-'); set(h, 'lineWidth', 3);
xlabel('input range (linear)');
ylabel('gain error / dB');
title('Logarithmic volume function: accuracy');


dy = d(ixc1) + 2*d(ixc2) .* fracPart.^1 + 3*d(ixc3) .* fracPart.^2;
figure(34); hold on;
h = plot(x, dy, 'k'); set(h, 'lineWidth', 3);
title('derivative');

% *****************************************************
% export fixed-point data
% *****************************************************
f = fopen('fixedPoint.txt', 'w'); assert(f);
SIGNEDPREC=18;
INTFRACBITS=17; % SIGNEDPREC-1 or lower: need sign bit
for ix = 1 : numel(d)    
    fpVal = floor(d(ix)*2^INTFRACBITS+0.5);
    assert(fpVal >= -2^(SIGNEDPREC-1)); assert(fpVal < 2^(SIGNEDPREC-1));
    fprintf(f, 'mem[%4i]=%6i;\n', ix-1, fpVal);
end
fclose(f);
