import sympy as sp
from sympy.interactive import printing
printing.init_printing(use_latex=True)

P, C, t0, t1, t2 = sp.symbols('P C t0 t1 t2')
X = sp.Matrix([[C, 0], [C, P]])
n = X.inv() * sp.Matrix([[t1-t0], [t2-t0]])
roots = sp.solve( n.norm()-1, t0 )
roots[1]

C = 1.0
P = 2.0

def calc_t0(t1, t2):
    D = 1.0 - ((t1-t2) / P)**2
    if D < 0.0 or t1 < t2:
        return min(t1 + C, t2 + sqrt(C*C + P*P))
    return t1 + C*sqrt(D)
   

h, w = 50, 100
a = np.zeros((h, w), np.float64)
a[:] = 1000
a[25,0] = 0

for j in xrange(1,w):
    for i in xrange(1,h-1):
        a[i, j] = min(calc_t0(a[i, j-1], a[i-1, j-1]), a[i, j])
        a[i, j] = min(calc_t0(a[i, j-1], a[i+1, j-1]), a[i, j])

        
imshow(a, vmax=80)


v0, v1, v2 = sp.symbols('v0, v1, v2')