import math
from pprint import pprint

import matplotlib.pyplot as plt
from scipy.optimize import minimize

from frispy import Disc
from frispy import Discs
from frispy import Model

model = Discs.roc
mph_to_mps = 0.44704
v = 56 * mph_to_mps
rot = -v / model.diameter

ceiling = 4 # 4 meter ceiling
tunnel_width = 4 # 4 meter wide tunnel

def distance(x):
    a, nose_up, hyzer = x
    d = Disc(model, {"vx": math.cos(a * math.pi / 180) * v, "dgamma": rot, "vz": math.sin(a * math.pi / 180) * v,
                     "nose_up": nose_up, "hyzer": hyzer})
    r = d.compute_trajectory(15.0, **{"max_step": .2})
    rx = r.x[-1]
    ry = abs(r.y[-1])

    overCelingIndex = next(filter(lambda i: r.z[i] > ceiling, range(len(r.z))), None)
    if overCelingIndex is not None:
        return -r.x[overCelingIndex]

    outsideTunnelIndex = next(filter(lambda i: math.fabs(r.y[i]) > tunnel_width / 2, range(len(r.z))), None)
    if outsideTunnelIndex is not None:
        return -r.x[outsideTunnelIndex]

    return -rx + ry / (rx + ry)

bnds = [(-90, 90)] * 3
x0 = [6, -3, 10]
res = minimize(distance, x0, method='powell', bounds=bnds, options={'xtol': 1e-8, 'disp': True})
pprint(res)
a, nose_up, hyzer = res.x
disc = Disc(model, {"vx": math.cos(a * math.pi / 180) * v, "dgamma": rot, "vz": math.sin(a * math.pi / 180) * v,
                 "nose_up": nose_up, "hyzer": hyzer})

result = disc.compute_trajectory(15.0, **{"max_step": .2})
times = result.times
t, x, y, z = result.times, result.x, result.y, result.z

#plt.plot(x, y)
#plt.plot(x, z)

#plt.plot(t, x)
plt.plot(t, y)
plt.plot(t, z)

pprint(x[-1] * 3.28084) # feet

plt.show()

[((39, 6, 39, 96), 'scipy.optimize.minimize', 'minimize', (), '', False, 'from scipy.optimize import minimize\n'), ((40, 0, 40, 11), 'pprint.pprint', 'pprint', ({(40, 7, 40, 10): 'res'}, {}), '(res)', False, 'from pprint import pprint\n'), ((53, 0, 53, 14), 'matplotlib.pyplot.plot', 'plt.plot', ({(53, 9, 53, 10): 't', (53, 12, 53, 13): 'y'}, {}), '(t, y)', True, 'import matplotlib.pyplot as plt\n'), ((54, 0, 54, 14), 'matplotlib.pyplot.plot', 'plt.plot', ({(54, 9, 54, 10): 't', (54, 12, 54, 13): 'z'}, {}), '(t, z)', True, 'import matplotlib.pyplot as plt\n'), ((56, 0, 56, 23), 'pprint.pprint', 'pprint', ({(56, 7, 56, 22): '(x[-1] * 3.28084)'}, {}), '(x[-1] * 3.28084)', False, 'from pprint import pprint\n'), ((58, 0, 58, 10), 'matplotlib.pyplot.show', 'plt.show', ({}, {}), '()', True, 'import matplotlib.pyplot as plt\n'), ((42, 26, 42, 53), 'math.cos', 'math.cos', ({(42, 35, 42, 52): 'a * math.pi / 180'}, {}), '(a * math.pi / 180)', False, 'import math\n'), ((42, 80, 42, 107), 'math.sin', 'math.sin', ({(42, 89, 42, 106): 'a * math.pi / 180'}, {}), '(a * math.pi / 180)', False, 'import math\n'), ((21, 27, 21, 54), 'math.cos', 'math.cos', ({(21, 36, 21, 53): 'a * math.pi / 180'}, {}), '(a * math.pi / 180)', False, 'import math\n'), ((21, 81, 21, 108), 'math.sin', 'math.sin', ({(21, 90, 21, 107): 'a * math.pi / 180'}, {}), '(a * math.pi / 180)', False, 'import math\n'), ((31, 47, 31, 64), 'math.fabs', 'math.fabs', ({(31, 57, 31, 63): 'r.y[i]'}, {}), '(r.y[i])', False, 'import math\n')]