def test_gauss_quadrature_dynamic(verbose = False):
    n = 5

    A = mp.randmatrix(2 * n, 1)

    def F(x):
        r = 0
        for i in xrange(len(A) - 1, -1, -1):
            r = r * x + A[i]
        return r

    def run(qtype, FW, R, alpha = 0, beta = 0):
        X, W = mp.gauss_quadrature(n, qtype, alpha = alpha, beta = beta)

        a = 0
        for i in xrange(len(X)):
            a += W[i] * F(X[i])

        b = mp.quad(lambda x: FW(x) * F(x), R)

        c = mp.fabs(a - b)

        if verbose:
            print(qtype, c, a, b)

        assert c < 1e-5

    run("legendre", lambda x: 1, [-1, 1])
    run("legendre01", lambda x: 1, [0, 1])
    run("hermite", lambda x: mp.exp(-x*x), [-mp.inf, mp.inf])
    run("laguerre", lambda x: mp.exp(-x), [0, mp.inf])
    run("glaguerre", lambda x: mp.sqrt(x)*mp.exp(-x), [0, mp.inf], alpha = 1 / mp.mpf(2))
    run("chebyshev1", lambda x: 1/mp.sqrt(1-x*x), [-1, 1])
    run("chebyshev2", lambda x: mp.sqrt(1-x*x), [-1, 1])
    run("jacobi", lambda x: (1-x)**(1/mp.mpf(3)) * (1+x)**(1/mp.mpf(5)), [-1, 1], alpha = 1 / mp.mpf(3), beta = 1 / mp.mpf(5) )

def two_largest(num_list):
    largest_num = mp.mpf(-1e100)
    second_largest_num = mp.mpf(-1e99)
    for num in num_list:
        if num > largest_num:
            second_largest_num = largest_num
            largest_num = num
        if largest_num < num < second_largest_num:
            second_largest_num = num
    return (largest_num, second_largest_num)

    def __init__(self, npts):
        # Legendre poly
        lp = lambda x: mp.legendre(npts - 1, x)

        # Coefficients of lp
        cf = mp.taylor(lp, 0, npts - 1)

        # Coefficients of dlp/dx
        dcf = [i*c for i, c in enumerate(cf[1:], start=1)]

        self.points = [mp.mpf(-1)] + mp.polyroots(dcf[::-1]) + [mp.mpf(1)]
        self.weights = [2/(npts*(npts - 1)*lp(p)**2) for p in self.points]

def _Interpolate1DNoVelocityLimit(x0, x1, v0, v1, am):
    # Check types
    if type(x0) is not mp.mpf:
        x0 = mp.mpf("{:.15e}".format(x0))
    if type(x1) is not mp.mpf:
        x1 = mp.mpf("{:.15e}".format(x1))
    if type(v0) is not mp.mpf:
        v0 = mp.mpf("{:.15e}".format(v0))
    if type(v1) is not mp.mpf:
        v1 = mp.mpf("{:.15e}".format(v1))
    if type(am) is not mp.mpf:
        am = mp.mpf("{:.15e}".format(am))

    # Check inputs
    assert(am > zero)

    # Check for an appropriate acceleration direction of the first ramp
    d = Sub(x1, x0)
    dv = Sub(v1, v0)
    difVSqr = Sub(v1**2, v0**2)
    
    if Abs(dv) < epsilon:
        if Abs(d) < epsilon:
            # Stationary ramp
            ramp0 = Ramp(zero, zero, zero, x0)
            return ParabolicCurve([ramp0])

        else:
            dStraight = zero
    else:    
        dStraight = mp.fdiv(difVSqr, Prod([2, mp.sign(dv), am]))
    
    if IsEqual(d, dStraight):
        # With the given distance, v0 and v1 can be directly connected using max/min
        # acceleration. Here the resulting profile has only one ramp.
        a0 = mp.sign(dv) * am
        ramp0 = Ramp(v0, a0, mp.fdiv(dv, a0), x0)
        return ParabolicCurve([ramp0])

    sumVSqr = Add(v0**2, v1**2)
    sigma = mp.sign(Sub(d, dStraight))
    a0 = sigma * am # acceleration of the first ramp
    vp = sigma * mp.sqrt(Add(Mul(pointfive, sumVSqr), Mul(a0, d)))
    t0 = mp.fdiv(Sub(vp, v0), a0)
    t1 = mp.fdiv(Sub(vp, v1), a0)
    ramp0 = Ramp(v0, a0, t0, x0)    
    assert(IsEqual(ramp0.v1, vp)) # check soundness
    ramp1 = Ramp(vp, Neg(a0), t1)

    curve = ParabolicCurve([ramp0, ramp1])
    assert(IsEqual(curve.d, d)) # check soundness
    return curve

def smallest_largest_elements(_matrix):
    l = len(_matrix) - 1
    smallest_element = mp.mpf(1e100)  # initialize to very large number
    largest_element = mp.mpf(0)
    for i in range(l):
        for j in range(l):
            num = _matrix[i, j]
            if num != 0:
                abs_num = fabs(num)
                if abs_num < smallest_element:
                    smallest_element = abs_num
                if abs_num > largest_element:
                    largest_element = abs_num
    return largest_element, smallest_element

 def EvalVel(self, t):
     if type(t) is not mp.mpf:
         t = mp.mpf("{:.15e}".format(t))
     assert(t >= -epsilon)
     assert(t <= self.duration + epsilon)
     
     return Add(self.v0, Mul(self.a, t))

    def EvalAcc(self, t):
        if type(t) is not mp.mpf:
            t = mp.mpf("{:.15e}".format(t))
        assert(t >= -epsilon)
        assert(t <= self.duration + epsilon)

        return self.a

    def jac_ortho_basis_at_mp(self, p, q, r):
        a = 2 * p / (1 - r) if r != 1 else 0
        b = 2 * q / (1 - r) if r != 1 else 0
        c = r

        sk = [mp.mpf(2) ** (-k - 0.25) * mp.sqrt(k + 0.5) for k in range(self.order)]
        fc = [s * jp for s, jp in zip(sk, jacobi(self.order - 1, 0, 0, a))]
        gc = [s * jp for s, jp in zip(sk, jacobi(self.order - 1, 0, 0, b))]

        dfc = [s * jp for s, jp in zip(sk, jacobi_diff(self.order - 1, 0, 0, a))]
        dgc = [s * jp for s, jp in zip(sk, jacobi_diff(self.order - 1, 0, 0, b))]

        ob = []
        for i, (fi, dfi) in enumerate(zip(fc, dfc)):
            for j, (gj, dgj) in enumerate(zip(gc, dgc)):
                h = jacobi(self.order - max(i, j) - 1, 2 * (i + j + 1), 0, c)
                dh = jacobi_diff(self.order - max(i, j) - 1, 2 * (i + j + 1), 0, c)

                for k, (hk, dhk) in enumerate(zip(h, dh)):
                    ck = mp.sqrt(2 * (k + j + i) + 3)

                    tmp = (1 - c) ** (i + j - 1) if i + j > 0 else 1

                    pijk = 2 * tmp * dfi * gj * hk
                    qijk = 2 * tmp * fi * dgj * hk
                    rijk = (
                        tmp * (a * dfi * gj + b * fi * dgj - (i + j) * fi * gj) * hk
                        + (1 - c) ** (i + j) * fi * gj * dhk
                    )

                    ob.append([ck * pijk, ck * qijk, ck * rijk])

        return ob

    def __init__(self, rule):
        pts = []
        wts = []

        rule = re.sub(r'(?<=\))\s*,?\s*(?!$)', r'\n', rule)
        rule = re.sub(r'\(|\)|,', '', rule).strip()
        rule = rule[1:-1] if rule.startswith('[') else rule

        for l in rule.splitlines():
            if not l:
                continue

            # Parse the line
            args = [mp.mpf(f) for f in l.split()]

            if len(args) == self.ndim:
                pts.append(args)
            elif len(args) == self.ndim + 1:
                pts.append(args[:-1])
                wts.append(args[-1])
            else:
                raise ValueError('Invalid points in quadrature rule')

        if len(wts) and len(wts) != len(pts):
            raise ValueError('Invalid number of weights')

        # Flatten 1D rules
        if self.ndim == 1:
            pts = [p[0] for p in pts]

        # Cast
        self.pts = np.array(pts, dtype=np.float)
        self.wts = np.array(wts, dtype=np.float)

    def Trim(self, deltaT):
        """
        Trim trims the curve such that it has the duration of self.duration - deltaT.

        Trim also takes care of where to trim out the deltaT. However, normally we should not have any problem
        since deltaT is expected to be very small. This function is aimed to be used when combining Curves to
        get ParabolicCurvesND.

        Return True if the operation is successful, False otherwise.
        """
        if type(deltaT) is not mp.mpf:
            dt = mp.mpf("{:.15e}".format(deltaT))
        else:
            dt = deltaT
        if dt > self.duration:
            return False # cannot trim
        if Abs(dt) < epsilon:
            return True # no trimming needed
        
        if dt < self.ramps[-1].duration:
            # trim the last ramp
            newDur = Sub(self.ramps[-1].duration, dt)
            self.ramps[-1].UpdateDuration(newDur)
            self.v1 = self.ramps[-1].v1
            return True
        else:
            # have not decided what to do here yet. This is not likely to happen, though, since
            # deltaT is expected to be very small.
            return False

def DynamicPathStringToParabolicCurvesND(dynamicpathstring):
    dynamicpathstring = dynamicpathstring.strip()
    data = dynamicpathstring.split("\n")
    ndof = int(data[0])
    nlines = ndof + 2 # the number of lines containing the data for 1 ParabolicRampND

    curves = [ParabolicCurve() for _ in xrange(ndof)]
    nParabolicRampND = len(data)/(nlines)

    for iramp in xrange(nParabolicRampND):
        curoffset = iramp*nlines
        for idof in xrange(ndof):
            ramp1ddata = data[curoffset + 2 + idof]
            x0, v0, x1, v1, a1, v, a2, tswitch1, tswitch2, ttotal = [mp.mpf(x) for x in ramp1ddata.split(" ")]
            ramps = []
            ramp0 = Ramp(v0, a1, tswitch1, x0)
            if ramp0.duration > epsilon:
                ramps.append(ramp0)
            ramp1 = Ramp(v, 0, tswitch2 - tswitch1, ramp0.x1)
            if ramp1.duration > epsilon:
                ramps.append(ramp1)
            ramp2 = Ramp(v, a2, ttotal - tswitch2, ramp1.x1)
            if ramp2.duration > epsilon:
                ramps.append(ramp2)
            assert(len(ramps) > 0)
            curve = ParabolicCurve(ramps)
            
            curves[idof].Append(curve)

    return ParabolicCurvesND(curves)

    def ortho_basis_at_mp(self, p, q, r):
        r = r if r != 1 else r + mp.eps

        a = 2*p/(1 - r)
        b = 2*q/(1 - r)
        c = r

        sk = [mp.mpf(2)**(-k - 0.25)*mp.sqrt(k + 0.5)
              for k in xrange(self.order)]
        pa = [s*jp for s, jp in zip(sk, jacobi(self.order - 1, 0, 0, a))]
        pb = [s*jp for s, jp in zip(sk, jacobi(self.order - 1, 0, 0, b))]

        ob = []
        for i, pi in enumerate(pa):
            for j, pj in enumerate(pb):
                cij = (1 - c)**(i + j)
                pij = pi*pj

                pc = jacobi(self.order - max(i, j) - 1, 2*(i + j + 1), 0, c)
                for k, pk in enumerate(pc):
                    ck = mp.sqrt(2*(k + j + i) + 3)

                    ob.append(cij*ck*pij*pk)

        return ob

    def UpdateDuration(self, newDur):
        if type(newDur) is not mp.mpf:
            newDur = mp.mpf("{:.15e}".format(newDur))
        assert(newDur >= -epsilon)

        self.duration = newDur
        self.v1 = Add(self.v0, Mul(self.a, self.duration))
        self.d = Prod([pointfive, Add(self.v0, self.v1), self.duration])

 def SetInitialValue(self, x0):
     if type(x0) is not mp.mpf:
         x0 = mp.mpf("{:.15e}".format(x0))
     self.x0 = x0
     newx0 = x0
     for ramp in self.ramps:
         ramp.x0 = newx0
         newx0 = Add(newx0, ramp.d)

    def EvalPos(self, t):
        if type(t) is not mp.mpf:
            t = mp.mpf("{:.15e}".format(t))
        assert(t >= -epsilon)
        assert(t <= self.duration + epsilon)        

        d_incr = Mul(t, Add(self.v0, Prod([pointfive, t, self.a])))
        return Add(self.x0, d_incr)

def geometric_mean(prices):
    for price in prices:
        if not isinstance(price, (int, long, float)):
            raise TypeError('Prices must be a valid number.')

    # We're using the mp module here to avoid the python error
    # "OverflowError: long int too large to convert to float"
    # See http://stackoverflow.com/a/29866503
    return reduce(lambda x, y: x * y, prices) ** mp.mpf(1.0 / len(prices))

def error(coefs, progress=True):
    (a, b) = coefs
    xs = (x * mp.pi / mp.mpf(4096) for x in range(-4096, 4097))
    err = max(fabs(sin(x) - f(x, a, b)) for x in xs)
    if progress:
        print('(a, b, c): ({}, {}, {})'.format(a, b, c(a, b)))
        print('evaluated error: ', err)
        print()
    return float(err)

def InterpolateZeroVelND(x0Vect, x1Vect, vmVect, amVect, delta=zero):
    """Interpolate a trajectory connecting two waypoints, x0Vect and x1Vect. Velocities at both
    waypoints are zeros.

    """
    ndof = len(x0Vect)
    assert(ndof == len(x1Vect))
    assert(ndof == len(vmVect))
    assert(ndof == len(amVect))

    # Convert all vector elements into mp.mpf (if necessary)
    x0Vect_ = ConvertFloatArrayToMPF(x0Vect)
    x1Vect_ = ConvertFloatArrayToMPF(x1Vect)
    vmVect_ = ConvertFloatArrayToMPF(vmVect)
    amVect_ = ConvertFloatArrayToMPF(amVect)
    
    dVect = x1Vect - x0Vect

    if type(delta) is not mp.mpf:
        delta = mp.mpf("{:.15e}".format(delta))

    vMin = inf # the tightest velocity bound
    aMin = inf # the tightest acceleration bound
    for i in range(ndof):
        if not IsEqual(x1Vect[i], x0Vect[i]):
            vMin = min(vMin, vmVect[i]/Abs(dVect[i]))
            aMin = min(aMin, amVect[i]/Abs(dVect[i]))

    if (not (vMin < inf and aMin < inf)):
        # dVect is zero.
        curvesnd = ParabolicCurvesND()
        curvesnd.SetConstant(x0Vect_, 0)
        return curvesnd

    if delta == zero:
        sdProfile = Interpolate1D(zero, one, zero, zero, vMin, aMin) # parabolic ramp (velocity profile sd(t))
    else:
        # Not handle interpolation with switch-time constraints yet
        raise NotImplementedError

    # Scale each DOF according to the obtained sd-profile
    curves = [ParabolicCurve() for _ in range(ndof)] # a list of (empty) parabolic curves
    for sdRamp in sdProfile:
        aVect = sdRamp.a * dVect
        v0Vect = sdRamp.v0 * dVect
        dur = sdRamp.duration

        for j in range(ndof):
            ramp = Ramp(v0Vect[j], aVect[j], dur, x0Vect[j])
            curve = ParabolicCurve([ramp])
            curves[j].Append(curve)

    for (i, curve) in enumerate(curves):
        curve.SetInitialValue(x0Vect[i])        
    curvesnd = ParabolicCurvesND(curves)

    return curvesnd

def _ImposeVelocityLimit(curve, vm):
    """_ImposeVelocityLimit imposes the given velocity limit to the ParabolicCurve. In case the velocity
    limit cannot be satisfied, this function will return an empty ParabolicCurve.

    """
    # Check types
    if type(vm) is not mp.mpf:
        vm = mp.mpf("{:.15e}".format(vm))

    # Check inputs
    assert(vm > zero)
    assert(len(curve) == 2)
    assert(Add(curve[0].a, curve[1].a) == zero)

    if Sub(Abs(curve[0].v0), vm) > epsilon:
        # Initial velocity violates the constraint
        return ParabolicCurve()

    if Sub(Abs(curve[1].v1), vm) > epsilon:
        # Final velocity violates the constraint
        return ParabolicCurve()
    
    vp = curve[1].v0
    if Abs(vp) <= vm:
        # Velocity limit is not violated
        return curve

    # h = Sub(Abs(vp), vm)
    # t = mp.fdiv(h, Abs(curve[0].a))
    
    ramp0, ramp1 = curve
    h = Sub(Abs(vp), vm)
    t = mp.fdiv(h, Abs(ramp0.a))

    # import IPython; IPython.embed()

    ramps = []
    if IsEqual(Abs(ramp0.v0), vm) and (mp.sign(ramp0.v0) == mp.sign(vp)):
        assert(IsEqual(ramp0.duration, t)) # check soundness
    else:
        newRamp0 = Ramp(ramp0.v0, ramp0.a, Sub(ramp0.duration, t), ramp0.x0)
        ramps.append(newRamp0)
        
    nom = h**2
    denom = Mul(Abs(curve[0].a), vm)
    newRamp1 = Ramp(Mul(mp.sign(vp), vm), zero, Sum([t, t, mp.fdiv(nom, denom)]), curve.x0)
    ramps.append(newRamp1)

    if IsEqual(Abs(ramp1.v1), vm) and (mp.sign(ramp1.v1) == mp.sign(vp)):
        assert(IsEqual(ramp1.duration, t)) # check soundness
    else:
        newRamp2 = Ramp(Mul(mp.sign(vp), vm), ramp1.a, Sub(ramp1.duration, t))
        ramps.append(newRamp2)

    return ParabolicCurve(ramps)

    def __init__(self, v0, a, dur, x0=zero):
        if type(dur) is not mp.mpf:
            dur = mp.mpf("{:.15e}".format(dur))
        assert(dur >= -epsilon)

        # Check types
        if type(x0) is not mp.mpf:
            x0 = mp.mpf("{:.15e}".format(x0))
        if type(v0) is not mp.mpf:
            v0 = mp.mpf("{:.15e}".format(v0))
        if type(a) is not mp.mpf:
            a = mp.mpf("{:.15e}".format(a))
        
        self.x0 = x0
        self.v0 = v0
        self.a = a
        self.duration = dur
        
        self.v1 = Add(self.v0, Mul(self.a, self.duration))
        self.d = Prod([pointfive, Add(self.v0, self.v1), self.duration])