from math import *
import cornu

def mod_2pi(th):
    u = th / (2 * pi)
    return 2 * pi * (u - floor(u + 0.5))

# Given clothoid k(s) = k0 + k1 s, compute th1 - th0 of chord from s = -.5
# to .5.
def compute_dth(k0, k1):
    if k1 < 0:
        return -compute_dth(k0, -k1)
    elif k1 == 0:
        return 0
    sqrk1 = sqrt(2 * k1)
    t0 = (k0 - .5 * k1) / sqrk1
    t1 = (k0 + .5 * k1) / sqrk1
    (y0, x0) = cornu.eval_cornu(t0)
    (y1, x1) = cornu.eval_cornu(t1)
    chord_th = atan2(y1 - y0, x1 - x0)
    return mod_2pi(t1 * t1 - chord_th) - mod_2pi(chord_th - t0 * t0)

def compute_chord(k0, k1):
    if k1 == 0:
        if k0 == 0:
            return 1
        else:
            return sin(k0 * .5) / (k0 * .5)
    sqrk1 = sqrt(2 * abs(k1))
    t0 = (k0 - .5 * k1) / sqrk1
    t1 = (k0 + .5 * k1) / sqrk1
    (y0, x0) = cornu.eval_cornu(t0)
    (y1, x1) = cornu.eval_cornu(t1)
    return hypot(y1 - y0, x1 - x0) / abs(t1 - t0)

# Given th0 and th1 at endpoints (measured from chord), return k0
# and k1 such that the clothoid k(s) = k0 + k1 s, evaluated from
# s = -.5 to .5, has the tangents given
def solve_clothoid(th0, th1, verbose = False):
    k0 = th0 + th1

    # initial guess
    k1 = 6 * (th1 - th0)
    error = (th1 - th0) - compute_dth(k0, k1)
    if verbose:
        print k0, k1, error

    k1_old, error_old = k1, error
    # second guess based on d(dth)/dk1 ~ 1/6
    k1 += 6 * error
    error = (th1 - th0) - compute_dth(k0, k1)
    if verbose:
        print k0, k1, error

    # secant method
    for i in range(10):
        if abs(error) < 1e-9: break
        k1_old, error_old, k1 = k1, error, k1 + (k1_old - k1) * error / (error - error_old)
        error = (th1 - th0) - compute_dth(k0, k1)
        if verbose:
            print k0, k1, error

    return k0, k1

if __name__ == '__main__':
    print solve_clothoid(.06, .05, True)