def solve_mec(constraint_fnl):
    delta = 1e-3
    params = [pi, 0, 0]
    for i in range(20):
        k, lam1, lam2 = params
        xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
        #print i * .05, 'setgray'
        #plot(xys)
        c1c, c2c, costc = constraint_fnl(cost, x, y, th)
        print '% constraint_fnl =', c1c, c2c, 'cost =', costc

        dc1s = []
        dc2s = []
        for j in range(len(params)):
            params1 = N.array(params)
            params1[j] += delta
            k, lam1, lam2 = params1
            xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
            c1p, c2p, costp = constraint_fnl(cost, x, y, th)
            params1 = N.array(params)
            params1[j] -= delta
            k, lam1, lam2 = params1
            xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
            c1m, c2m, costm = constraint_fnl(cost, x, y, th)
            dc1s.append((c1p - c1m) / (2 * delta))
            dc2s.append((c2p - c2m) / (2 * delta))
        xp = cross_prod(dc1s, dc2s)
        xp = N.divide(xp, sqrt(N.dot(xp, xp))) # Normalize to unit length

        print '% dc1s =', dc1s
        print '% dc2s =', dc2s
        print '% xp =', xp
        
        # Compute second derivative wrt orthogonal vec
        params1 = N.array(params)
        for j in range(len(params)):
            params1[j] += delta * xp[j]
        k, lam1, lam2 = params1
        xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
        c1p, c2p, costp = constraint_fnl(cost, x, y, th)
        print '% constraint_fnl+ =', c1p, c2p, 'cost =', costp
        params1 = N.array(params)
        for j in range(len(params)):
            params1[j] -= delta * xp[j]
        k, lam1, lam2 = params1
        xys, cost, x, y, th = run_elastica(-.5, .5, k, lam1, lam2)
        c1m, c2m, costm = constraint_fnl(cost, x, y, th)
        print '% constraint_fnl- =', c1m, c2m, 'cost =', costm
        d2cost = (delta * delta) / (costp + costm - 2 * costc)
        dcost = (costp - costm) / (2 * delta)

        print '% dcost =', dcost, 'd2cost =', d2cost
        if d2cost < 0: d2cost = .1
        # Make Jacobian matrix to invert
        jm = N.array([dc1s, dc2s, [x * d2cost for x in xp]])
        #print jm
        ji = la.inverse(jm)
        #print ji

        dp = N.dot(ji, [c1c, c2c, dcost])
        print '% dp =', dp
        print '% [right]=', [c1c, c2c, dcost]
        params -= dp * .1
        print '%', params
        sys.stdout.flush()
    return params
dataset/ETHPy150Open google/roboto/third_party/spiro/curves/mecsolve.py/solve_mec/swapped operands of `/`