#!/usr/bin/env python

# Generate a coefficient table for a ringfilter. This solves an
# overconstrained linear system using a pseudoinverse based on SVD.  This
# represents a least-squares parametric fit to uniformly sampled data.

# E.g. a quadratic:
#
#    y = a0 + a1*t + a2*t*t
#
# can be sampled in time to be represented as a matrix F multiplied by
# the quadratic coefficients:
#
#    y = F x
#
# where
#    y = []               a Nx1 matrix vector of measurements over time
#    F = [1 t t**2]       a Nx3 matrix of polynomial terms for a set of time values
#    x = [a0 a1 a2]       a 3x1 matrix of the unknown coefficients
#
# The solution for x uses the pseudoinverse F+:
#
#   x = F+ y, where F+ is the peudoinverse

import sys, argparse
import numpy as np
import scipy.linalg

################################################################
# Generate a fitting matrix which can compute the weight terms (a0, a1, a2) for
# a quadratic polynomial by left-multiplying with a vector of uniformly sampled
# y values.  In the ring buffer, the values are ordered from oldest to newest,
# with the newest by default considered to be at t=0.0.  The polynomial basis
# terms are evaluated over the negative time range corresponding to the ring
# buffer data, so the default is a causal filter estimating the parameters for
# the quadratic with t=0 at the present moment.  The optional 'endt' parameter
# can change the assumed endpoint to t=endt, e.g. to make an acausal filter with
# the quadratic t=0 centered in the buffer data.

# The quadratic function has the following linear form:
#  y(t) = [1 t t**2] * [a0 a1 a2]'

def quadratic_fit_matrix(dt = 0.005, N = 50, endt = 0.0):
    t0 = np.ones(N)
    t1 = np.linspace(endt - (N-1)*dt, endt, N, endpoint=True)
    t2 = t1*t1
    F = np.vstack((t0,t1,t2)).transpose()
    Finv = scipy.linalg.pinv2(F)
    return Finv

################################################################
# Write out a C declaration for a fitting matrix in a
# form convenient for use with the ringfilter

def write_fitting_matrix_in_C(stream, F, name="coeff_e", precision='float'):
    order = F.shape[0]
    N     = F.shape[1]
    stream.write("static const int %s_order = %d;\nstatic const int %s_length = %d;\n" % (name, order, name, N))
    stream.write("static %s %s[%d][%d] = {\n" % (precision, name, order, N))    
    for i in range(order):
        stream.write("  {")
        for j,v in enumerate(F[i][:]):
            stream.write("%10.5f" % v)
            if j < N-1:
                stream.write(", ")
                if j%8 == 7: stream.write("\n   ");
        if i < order-1: stream.write(" },\n")
        else: stream.write(" }\n")
    stream.write("};\n")
        
################################################################
if __name__ == "__main__":
    parser = argparse.ArgumentParser(description = """Generate fitting matrices for use with ringfilter.""")
    parser.add_argument('--dt', default=0.01, type=float, help = 'Sampling interval in seconds.')
    parser.add_argument('-N', default=20, type=int, help = 'Number of samples.')
    parser.add_argument('--end', default=0.0, type=float, help = 'Time reference for end sample.')
    parser.add_argument('--name', default='coeff', type=str, help = 'C name of fitting matrix.')
    parser.add_argument( '--out', type=argparse.FileType('w'), default=None, help='Path of C output file.')
    args = parser.parse_args()

    F = quadratic_fit_matrix(dt = args.dt, N = args.N, endt = args.end)
    stream = sys.stdout if args.out is None else args.out
    stream.write("// Ring filter coefficients for quadratic.\n")
    stream.write("static const float %s_dt  = %f; // sampling interval\n" % (args.name, args.dt))
    stream.write("static const float %s_end = %f; // sample end time\n" % (args.name, args.end))
    write_fitting_matrix_in_C(stream, F, args.name)