#!/usr/bin/python

# tzspline.py
#       --copyright--                   Copyright 2007 (C) Tranzoa, Co. All rights reserved.    Warranty: You're free and on your own here. This code is not necessarily up-to-date or of public quality.
#       --url--                         http://www.tranzoa.net/tzpython/
#       --email--                       pycode is the name to send to. tranzoa.com is the place to send to.
#       --bodstamps--
#       May 5, 2007             bar
#       October 25, 2007        bar     get a_spliner code from the web that's working (i believe)
#       October 26, 2007        bar     add the tree printout code
#       November 1, 2007        bar     a_smoother
#       November 18, 2007       bar     turn on doxygen
#       November 27, 2007       bar     insert boilerplate copyright
#       December 22, 2007       bar     handle vertical movement
#       May 17, 2008            bar     email adr
#       September 14, 2009      bar     comment
#       July 6, 2011            bar     comment
#       November 29, 2011       bar     pyflake cleanup
#       May 27, 2012            bar     doxygen namespace
#       August 19, 2015         bar     pyflakes
#       --eodstamps--
##      \file
#       \namespace              tzpython.tzspline
#
#
#
#       Spline stuff
#
#
#
#       a_smoother may have tiny anomalies at the points - hikes.py puts out glitches - see what happens when the big points get shoulders to protect 'em.
#
#       !!!! The old code doesn't work - the slope is wrong when crossing a point because the phoney points are extrapolated dumbly and pick up the wrong slope
#
#           !!!! The proper control points must be generated from the points given
#
#           Curious about another way:
#
#               Minimize the acceleration:
#
#                   Specifically, create a mid-point between each pair of control points and adjust the mid-points up and down.
#
#                   Assume that 3 control points are named: A C E and that the mid point between A and C is b, and 'tween C and E is d.
#                   So: AbCdE are the points.
#                   Initialize b and d (and the other mid-points) using linear interpolation. e.g.  b = (A + C) / 2
#
#                   If the maximum acceleration (e.g. ((d - C) - (C - b)) ) of all control possible points is at C, then do one of these:
#
#                       d = ( E + C + C + C - b) / 3
#                     or:
#                       b = (-d + C + C + C + A) / 3
#
#                   Until nothing better can be done with any control point.
#                   Then double 'em up again - until there are enough points on the smoothed line.
#
#               Or is the idea to minimize the total acceleration? (nope - don't want any huge acceleration, anywhere)
#               Or the square of the acceleration?
#
#
#       Javascript spline code in this page:
#           http://www.akiti.ca/CubicSpline.html
#
#       Nice small interpolation code (linear, cosine, cubic, hermite):
#           http://paulbourke.net/miscellaneous/interpolation/
#
#


import  math

from    types                   import ListType, TupleType



if  False :

    def _cccc_calc(a, i, f) :

        v0  = a[i]
        v3  = a[i + 1]
        v1  = v0  + ((v0 - a[i - 1]) / 3.0)         # extrapolate forward a 1/3 of the way from the point before this pair
        v2  = v3  + ((v3 - a[i + 2]) / 3.0)         # extrapolate back    a 1/3 of the way from the next point after this pair

        A   = v3 - (3.0 * v2) + (3.0 * v1) -        v0
        B   =      (3.0 * v2) - (6.0 * v1) + (3.0 * v0)
        C   =                   (3.0 * v1) - (3.0 * v0)
        D   =                                       v0

        ff  = f * f
        fff = f * ff

        return(A * fff + B * ff + C * f + D)



    #
    #   from util\stuff\splice.bas
    #
    def _calc(a, i, f) :

        v0  = a[i]
        v3  = a[i + 1]
        v1  = v0  + ((v0 - a[i - 1]) / 3.0)         # extrapolate forward a 1/3 of the way from the point before this pair
        v2  = v3  + ((v3 - a[i + 2]) / 3.0)         # extrapolate back    a 1/3 of the way from the next point after this pair

        if  False :
            v0  = a[i - 1]
            v1  = a[i]
            v2  = a[i + 1]
            v3  = a[i + 2]

        ff  = f * f

        f1      = 1.0 - f
        ff1     = f1 * f1

        A   =     f1 * ff1
        B   = 3 * f  * ff1
        C   = 3 *      ff * f1
        D   =     f  * ff

        return(A * v0 + B * v1 + C * v2 + D * v3)



    def spline_point(ax, ay, fx) :
        """
            Given an array of ax and ay values,
            and given a float 1.0 <= fx <= len(ay) - 2.0,
            return the y value at fx.
        """

        ln  = len(ay)

        if  not ax :
            ax  = range(ln)

        if  (not isinstance(ax, ListType)) and (not isinstance(ax, TupleType)) :
            ax  = [ x + ax for x in xrange(ln) ]

        if len(ax) != ln :
            raise ValueError, "Different length of ax and ay array"

        ifx = int(fx)

        if  abs(fx - float(ifx)) < 0.001 :
            fx  = float(ifx)

        if  not (1.0 <= fx <= ln - 2.0 + 0.001) :
            s   = "fx is out of range - not (1.0 <= %f <= %f)" % ( fx, float(ln) - 2.0 )
            raise IndexError, s

        # if  abs(float(ifx) - fx) < 0.001 :
        #    return( ( ax[ifx], ay[ifx] ) )

        fx -= float(ifx)

        x   = _calc(ax, ifx, fx)
        y   = _calc(ay, ifx, fx)

        return( ( x, y ) )


    pass



#
#
#   Modified version of code snagged at http://cars9.uchicago.edu/software/python/
#                                       http://cars.uchicago.edu/software/pub/python_epics.tar
#
#
class a_spliner :
    """
        Given two parallel arrays, one with X coordinates, the other with Y,
        create an object that gives "spline" Y values for arbitrary X values.
    """

    def __init__(me, x_array, y_array, begin_slope = None, end_slope = None) :
        if  len(x_array) != len(y_array) :
            raise "a_spliner: Arrays not same length: x[%u] y[%u]!" % ( len(x_array), len(y_array) )

        me.x_vals       = [ float(v) for v in x_array ]
        me.y_vals       = [ float(v) for v in y_array ]

        me.begin_slope  = begin_slope
        me.end_slope    = end_slope

        me.calc_ypp()



    def calc_ypp(me):
        x_vals          = me.x_vals
        y_vals          = me.y_vals
        n               = len(x_vals)

        y2_vals         = [ 0.0 ] * n
        u               = [ 0.0 ] * (n - 1)

        if me.begin_slope != None :
            u[0]        = (3.0 / (x_vals[1] - x_vals[0])) * ((y_vals[1] - y_vals[0]) / (x_vals[1] - x_vals[0]) - me.begin_slope)
            y2_vals[0]  = -0.5
        else:
            u[0]        = 0.0
            y2_vals[0]  = 0.0       # natural spline

        x               = x_vals[0]
        y               = y_vals[0]
        nx              = x_vals[1]
        ny              = y_vals[1]
        for i in xrange(1, n - 1) :
            nnx         = x_vals[i + 1]
            nny         = y_vals[i + 1]

            sig         = (nx - x) / (nnx - x)
            p           = (sig * y2_vals[i-1]) + 2.0
            y2_vals[i]  = (sig - 1.0) / p
            u[i]        = ((nny - ny) / (nnx - nx)) - ((ny - y) / (nx - x))
            u[i]        = (6.0 * u[i] / (nnx -  x)  - (sig * u[i-1])) / p

            x           = nx
            y           = ny
            nx          = nnx
            ny          = nny


        if me.end_slope != None :
            qn          = 0.5
            un          = (3.0 / (x_vals[n-1] - x_vals[n-2])) * (me.end_slope - (y_vals[n-1] - y_vals[n-2]) / (x_vals[n-1] - x_vals[n-2]))
        else:
            qn          = 0.0
            un          = 0.0               # natural spline

        y2_vals[n-1]    = (un - (qn * u[n-2])) / ((qn * y2_vals[n-1]) + 1.0)

        rng             = range(n - 1)
        rng.reverse()
        for k in rng:               # backsubstitution step
            y2_vals[k]  = (y2_vals[k] * y2_vals[k+1]) + u[k]
        me.y2_vals      =  y2_vals



    # compute approximation
    def __call__(me, arg):
        if  isinstance(arg, ListType) or isinstance(arg, TupleType) :
            return(map(me.call, arg))
        else:
            return(me.call(arg))
        pass



    def call(me, x):
        "Evaluate the spline, assuming x is a scalar."

        # if out of range, return endpoint
        if  x <=   me.x_vals[0] :
            return(me.y_vals[0])
        if  x >=   me.x_vals[-1] :
            return(me.y_vals[-1])

        #
        #   binary search to find the x index above the arg
        #
        pos     = 0
        lo      = 0
        hi      = len(me.x_vals)
        while lo < hi :
            pos = (lo + hi) / 2

            if  x      >= me.x_vals[pos] :
                pos    += 1
                lo      = pos
            else :
                hi      = pos
            pass


        h       = me.x_vals[pos] - me.x_vals[pos - 1]
        if h   <= 0.0 :
            es  = "a_spliner: Bad X positions : %f %f!" % ( me.x_vals[pos], me.x_vals[pos - 1] )
            raise es

        a       = (    me.x_vals[pos    ] - x) / h
        b       = (x - me.x_vals[pos - 1]    ) / h
        return(  (a * me.y_vals[pos - 1])
               + (b * me.y_vals[pos    ])
               + (  ((a * a * a - a) * me.y2_vals[pos - 1])
                  + ((b * b * b - b) * me.y2_vals[pos    ])
                 ) * h * h / 6.0
              )



    pass            # a_spliner



#
#
#       http://aspn.activestate.com/ASPN/Cookbook/Python/Recipe/457658
#
#

class Interpolator:
    def __init__(self, name, func, points, deriv=None):
        self.name = name  # used for naming the C function
        self.intervals = intervals = [ ]
        # Generate a cubic spline for each interpolation interval.
        for u, v in map(None, points[:-1], points[1:]):
            FU, FV = func(u), func(v)
            # adjust h as needed, or pass in a derivative function
            if deriv == None:
                h = 0.01
                DU = (func(u + h) - FU) / h
                DV = (func(v + h) - FV) / h
            else:
                DU = deriv(u)
                DV = deriv(v)
            denom = (u - v)**3
            A = ((-DV - DU) * v + (DV + DU) * u +
                 2 * FV - 2 * FU) / denom
            B = -((-DV - 2 * DU) * v**2  +
                  u * ((DU - DV) * v + 3 * FV - 3 * FU) +
                  3 * FV * v - 3 * FU * v +
                  (2 * DV + DU) * u**2) / denom
            C = (- DU * v**3  +
                 u * ((- 2 * DV - DU) * v**2  + 6 * FV * v
                                    - 6 * FU * v) +
                 (DV + 2 * DU) * u**2 * v + DV * u**3) / denom
            D = -(u *(-DU * v**3  - 3 * FU * v**2) +
                  FU * v**3 + u**2 * ((DU - DV) * v**2 + 3 * FV * v) +
                  u**3 * (DV * v - FV)) / denom
            intervals.append((u, A, B, C, D))

    def __call__(self, x):
        def getInterval(x, intervalList):
            # run-time proportional to the log of the length
            # of the interval list
            n = len(intervalList)
            if n < 2:
                return intervalList[0]
            n2 = n / 2
            if x < intervalList[n2][0]:
                return getInterval(x, intervalList[:n2])
            else:
                return getInterval(x, intervalList[n2:])
        # Tree-search the intervals to get coefficients.
        u, A, B, C, D = getInterval(x, self.intervals)
        # Plug coefficients into polynomial.
        return ((A * x + B) * x + C) * x + D

    def c_code(self):
        """Generate C code to efficiently implement this
        interpolator. Run the C code through 'indent' if you
        need it to be legible."""
        def codeChoice(intervalList):
            n = len(intervalList)
            if n < 2:
                return ("A=%.10e;B=%.10e;C=%.10e;D=%.10e;"
                        % intervalList[0][1:])
            n2 = n / 2
            return ("if (x < %.10e) {%s} else {%s}"
                    % (intervalList[n2][0],
                       codeChoice(intervalList[:n2]),
                       codeChoice(intervalList[n2:])))
        return ("double interpolator_%s(double x) {" % self.name +
                "double A,B,C,D;%s" % codeChoice(self.intervals) +
                "return ((A * x + B) * x + C) * x + D;}")



#
#
#       Print out a decision tree to find the maximum and minimum values that interpolated values may take
#       if interpolation is done to make sure that there are not gross swings away from the points
#       and to make sure that monotonically increasing or decreasing Y values stay that way in the interpolations.
#
#

def play_with_something() :

    import  re

    class   a_choice :

        def __init__(me, order, h, l) :
            me.order    = order
            me.h        = h
            me.l        = l

        def __str__(me) :
            return("%s %s %s" % ( me.order, me.h, me.l ) )

        pass        # a_choice




    def _unique(ca) :
        cnt = {}
        for c in ca :
            cnt[c.h + c.l]    = True

        return(len(cnt))


    def _without(regxs, regx) :
        return( [ r for r in regxs if regx.pattern != r.pattern ] )




    def _split(indent, regxs, ca, tf = "\\-") :

        if  ca :

            ind     = indent[0:-1]

            print "%s|" % ( ind )

            if  _unique(ca) == 1 :
                print "%s%s %s" % ( ind, tf, str([ str(c) for c in ca ]) )
                return

            best    = len(ca)

            for regx in regxs :
                fnd = []
                nfn = []

                for c in ca :
                    if  regx.search(c.order) :
                        fnd.append(c)
                    else :
                        nfn.append(c)
                    pass

                if  (len(fnd) and len(nfn)) or (len(ca) == 1) :
                    v   = abs(_unique(fnd) - _unique(nfn))
                    if  best    > v :
                        best    = v
                        brx     = regx
                        bfnd    = fnd
                        bnfn    = nfn
                    pass
                pass

            ind     = indent[0:-1]
            print "%s%s %s-\\"   % ( ind, tf, brx.pattern)

            _split(indent + "        |", _without(regxs, brx), bfnd, "+-T")
            _split(indent + "         ", _without(regxs, brx), bnfn, "\-F")
        pass



    def find_cmps(ca) :

        lts = {}

        for c in ca :
            for i in list(c.order) :
                lts[i] = True
            pass

        regxs   = []
        for c1 in lts.keys() :
            for c2 in lts.keys() :
                if  c1 != c2 :
                    regxs.append(re.compile(c1 + ".*" + c2))
                pass
            pass


        _split("", regxs, ca)


    #   Numbers are 1 2 3 4, left to right
    #   2   =   left  middle number is limit
    #   3   =   right middle number is limit
    #   w   =   limit is 2 + 2 - 1
    #   e   =   limit is 3 + 3 - 4
    #   M   =   max(w, e)
    #   m   =   min(w, e)

    choices = [                #  max   min
                a_choice('1234', 'h2', 'l3'),
                a_choice('1243', 'h2', 'lm'),
                a_choice('1324', 'he', 'lw'),
                a_choice('1342', 'he', 'lw'),
                a_choice('1423', 'h2', 'lm'),
                a_choice('1432', 'h3', 'lw'),
                a_choice('2134', 'hM', 'l3'),
                a_choice('2143', 'hw', 'le'),
                a_choice('2314', 'hM', 'l3'),
                a_choice('2341', 'hM', 'l3'),
                a_choice('2413', 'hw', 'le'),
                a_choice('2431', 'hw', 'le'),
                a_choice('3124', 'he', 'lw'),
                a_choice('3142', 'he', 'lw'),
                a_choice('3214', 'hM', 'l2'),
                a_choice('3241', 'hM', 'l2'),
                a_choice('3412', 'he', 'lw'),
                a_choice('3421', 'hM', 'l2'),
                a_choice('4123', 'h2', 'le'),
                a_choice('4132', 'h3', 'lm'),
                a_choice('4213', 'hw', 'le'),
                a_choice('4231', 'hw', 'le'),
                a_choice('4312', 'h3', 'lm'),
                a_choice('4321', 'h3', 'l2'),
              ]

    find_cmps(choices)

    pass



def smooth_max_min_of_4_y_values(y1, y2, y3, y4) :
    """
        Note: this works find except that it can create butterflies, 'cause it doesn't know that the angle coming in to y2 is not from y1, but rather has overshot y2 and is coming back.
        Oh well.

        Given 4 Y values,
        return the maximum and minimum Y values
        that a smooth curve going through the points
        should have in Y between the middle (y2 and y3) points.

        These max/min values can be used to minimize gross swings beyond the bounds of reality.
    """

    return( ( max(y2, y3), min(y2, y3) ) )

    if  y1 >= y4 :
        if  y1 >= y2 :
            if  y3 >= y2 :
                if  y3 >= y4 :
                    return( (                   y3 + y3 - y4,           y2 + y2 - y1                ) )
                return(     (                   y3,                     y2 + y2 - y1                ) )
            elif y3 >= y4 :
                return(     (     y2,                                                 y3            ) )
            return(         (     y2,                               min(y2 + y2 - y1, y3 + y3 - y4) ) )
        elif y1 >- y3 :
            if  y3 >= y4 :
                return(     ( max(y2 + y2 - y1, y3 + y3 - y4),                        y3            ) )
            return(         (     y2 + y2 - y1,                                       y3 + y3 - y4  ) )
        elif y3 >= y2 :
            return(         ( max(y2 + y2 - y1, y3 + y3 - y4),          y2                          ) )
        return(             ( max(y2 + y2 - y1, y3 + y3 - y4),                        y3            ) )
    elif y1 >= y2 :
        if  y1 >= y3 :
            if  y3 >= y2 :
                return(     (                   y3,                 min(y2 + y2 - y1, y3 + y3 - y4) ) )
            return(         (     y2,                                                 y3 + y3 - y4  ) )
        elif y3 >= y4 :
            return(         (                   y3 + y3 - y4,           y2 + y2 - y1                ) )
        return(             (                   y3,                 min(y2 + y2 - y1, y3 + y3 - y4) ) )
    elif y3 >= y2 :
        if  y3 >= y4 :
            return(         ( max(y2 + y2 - y1, y3 + y3 - y4),          y2                          ) )
        return(             (                   y3,                     y2                          ) )
    elif y3 >= y4 :
        return(             ( max(y2 + y2 - y1, y3 + y3 - y4),                        y3            ) )
    return(                 (     y2 + y2 - y1,                                       y3 + y3 - y4  ) )





class a_smoother :
    """
        Given two parallel arrays, one with X coordinates, the other with Y,
        create an object that gives "smooth" Y values for arbitrary X values.
    """

    def __init__(me, x_array, y_array) :
        if  len(x_array) != len(y_array) :
            raise "a_smoother: Arrays not same length: x[%u] y[%u]!" % ( len(x_array), len(y_array) )

        me.x_vals   = []
        me.y_vals   = []
        if  x_array :
            px          = x_array[0]
            ty          = y_array[0]
            tc          = 1.0
            for i in xrange(len(x_array)) :
                x       = x_array[i]
                y       = y_array[i]
                if  x  == px :
                    ty += y                             # average the Y values for multiple-same X values
                    tc += 1.0
                else :
                    me.x_vals.append(px)
                    me.y_vals.append(ty / tc)
                    px  = x
                    ty  = y
                    tc  = 1.0
                i  += 1
            me.x_vals.append(px)
            me.y_vals.append(ty / tc)

        me.calc_mm()

        me.sig_mult     = (1.0 + math.exp((1.0 - 0.5) * -8.0))



    def calc_mm(me):
        x_vals          = me.x_vals
        y_vals          = me.y_vals

        me.west_yd      = [ 0.0 for x in x_vals ]
        me.east_yd      = [ 0.0 for x in x_vals ]

        if  len(x_vals) :

            if  len(x_vals) < 2 :
                me.west_yd[0]   = me.east_yd[0]     = y_vals[0]
            else :
                x_vals.insert(0, x_vals[1] + x_vals[1] - x_vals[0])
                y_vals.insert(0, y_vals[1] + y_vals[1] - y_vals[0])

                x_vals.append(x_vals[-1] + x_vals[-1] - x_vals[-2])
                y_vals.append(y_vals[-1] + y_vals[-1] - y_vals[-2])

                y2              =   y_vals[0]
                y3              =   y_vals[1]
                y4              =   y_vals[2]
                for i in xrange(1, len(y_vals) - 2) :
                    y1          = y2
                    y2          = y3
                    y3          = y4
                    y4          = y_vals[i + 2]

                    ( mx, mn )  = smooth_max_min_of_4_y_values(y1, y2, y3, y4)

                    # print i - 1, mn, mx, y1, y2, y3, y4

                    d           = (x_vals[i + 1] - x_vals[i])

                    y                   = y_vals[i    ] + (d * ((y_vals[i    ] - y_vals[i - 1]) / (x_vals[i    ] - x_vals[i - 1])))
                    me.east_yd[i - 1]   = max(mn, min(mx, y)) -  y_vals[i    ]

                    y                   = y_vals[i + 1] - (d * ((y_vals[i + 2] - y_vals[i + 1]) / (x_vals[i + 2] - x_vals[i + 1])))
                    me.west_yd[i - 1]   = max(mn, min(mx, y)) -  y_vals[i + 1]

                me.x_vals       = x_vals[1:-1]
                me.y_vals       = y_vals[1:-1]
            pass

        # print me.west_yd
        # print me.east_yd
        pass
    pass


    # compute approximation
    def __call__(me, arg):
        if  isinstance(arg, ListType) or isinstance(arg, TupleType) :
            return(map(me.call, arg))
        else:
            return(me.call(arg))
        pass

    def call(me, x):
        "Evaluate the smoother, assuming x is a scalar."

        # if out of range, return endpoint
        if  x <=   me.x_vals[0] :
            return(me.y_vals[0])
        if  x >=   me.x_vals[-1] :
            return(me.y_vals[-1])

        #
        #   binary search to find the x index above the arg
        #
        pos     = 0
        lo      = 0
        hi      = len(me.x_vals)
        while lo < hi :
            pos = (lo + hi) / 2

            if  x      >= me.x_vals[pos] :
                pos    += 1
                lo      = pos
            else :
                hi      = pos
            pass


        h       = me.x_vals[pos] - me.x_vals[pos - 1]
        if h   <= 0.0 :
            es  = "a_smoother: Bad X positions : %f %f!" % ( me.x_vals[pos], me.x_vals[pos - 1] )
            raise es

        pos    -= 1

        b       = (x - me.x_vals[pos]) / h

        #
        #   Note: rather than linearly morphing from fy to by, it might be interesting to make the right hand multipliers in these statements follow a sigmoid function.
        #         blah
        #
        # b     = me.sig_mult / (1.0 + math.exp((b - 0.5) * -8.0))

        fy      = (me.y_vals[pos    ] + (me.east_yd[pos] *  b     )) * (1 - b)
        by      = (me.y_vals[pos + 1] + (me.west_yd[pos] * (1 - b))) *      b

        return(fy + by)


    pass            # a_smoother



#
#
#   Test code.
#
#
if __name__ == '__main__' :
    import  sys

    import  TZCommandLineAtFile

    del(sys.argv[0])

    TZCommandLineAtFile.expand_at_sign_command_line_files(sys.argv)

    # play_with_something()


    ys  = [ 0, 2, 5, 2, 10, 5 ]
    xs  = [ 0, 1, 2, 7,  8, 9 ]

    # xs  = [ 32,             33,             40,             46,             47,             54,             61,             62,             69,             75 ]
    # xs  = [ 32.0,           33.0,           34.0,           35.0,           36.0,           37.0,           38.0,           39.0,           40.0,           41.0 ]
    # ys  = [ 122.3,          122.4,          122.5,          122.6,          122.601,        122.602,        122.7,          122.8,          122.85,         122.9 ]

    # for x in xrange(1, len(ys) - 2) :
    #     print x, smooth_max_min_of_4_y_values(ys[x - 1], ys[x], ys[x + 1], ys[x + 2])


    sp  = a_smoother(xs, ys)

    i   = xs[0] - 1.0
    pv  = 0.0
    while i < xs[-1] + 1.0 :
        v   = sp(i)
        print i, v,
        # if  v < pv :
        #     print "ouch",
        pv  = v
        print
        i  += 0.25


    x   = [          ii * 0.3  for ii in xrange(10) ]
    y   = [ math.cos(ii * 0.3) for ii in xrange(10) ]

    sp  = a_smoother(x, y) # , True, True)

    v   = [ 0.5 ]

    if  sys.argv :
        v   = [ float(a) for a in sys.argv ]
    print sp(v), [ math.cos(c) for c in v ]


#
#
#
# eof
