'''
Luke Marshall
Abir Quabili
University of Delaware
MEEG211 - Dynamics
Project 1
Problem 2 - Rectangle
'''

import math
import matplotlib.pyplot as plt

# FUNCTIONS ##################################################################

# def trajCalc(vox, vy, g, t, xa, h):

#     vy = vy - g*t

#     xa = xa + vox*t

#     h = h + vy*t - (1/2)*(g)*(t**2)

#     t = t + 0.1

#     # print(vy, h, xa);

#     return [xa, h]

# def alphaCalc(vo, g, xa, ht):

#     a = -(g*(xa**2))/(2*(vo**2))
#     b = (xa)
#     c = -(g*(xa**2))/(2*(vo**2)) - ht

#     tanAlpha = (-b - (b**2 - 4*a*c)**(1/2))/(2*a)
#     alpha = (-math.atan(tanAlpha))

#     vox = vo*(math.cos(alpha));
#     vy = vo*(math.sin(alpha));

#     return [alpha, vox, vy]

##############################################################################

g = 9.8
h = 0.0
xa = -50.0
t = 0
d = -10

xao = xa

n1 = 0

x = []
y = []
xvoList = []

nx = len(x) - 1;
ny = len(y) - 1;

vo = 32
ht = 33

z = 0

j = 1

a = -(g*(xa**2))/(2*(vo**2))
b = (xa)
c = -(g*(xa**2))/(2*(vo**2)) - ht

tanAlpha = (-b - (b**2 - 4*a*c)**(1/2))/(2*a)
alpha = (-math.atan(tanAlpha))

vox = vo*(math.cos(alpha));
vy = vo*(math.sin(alpha));

while (z != 1):

    print('loop: ' + str(j))

    j = j + 1

    while ((xao <= 0) and (h != ht)):

        #print(vy, h, xa);

        vy = vy - g*t

        xao = xao + vox*t

        h = h + vy*t - (1/2)*(g)*(t**2)

        t = t + 0.01

        x.append(xa)
        y.append(h)

    xa = xa - 0.1

    n = len(x) - 1

    m = 0

    while m != n:

        if x[m] < -10:

            m = m + 1

        elif y[m] > 33:

            print('Yay')

            break

        elif y[m] <= 33:

            print('Hit')

            vo = vo - 0.1

            break

        else:

            print('No')

            break

    for i in y:

        if i > 35:

            vo = vo - 0.1

            break

    # while ((x[m] >= -10) and (y[m] >= 33)):

    # print(x)

    # print(y)

    # while m != n:

    #     if (x[m] < -10):

    #         print('If')

    #         m = m + 1

    #     elif (y[m] <= 33):

    #         print('Elif 1')

    #         # vo = vo - 0.1

    #         xa = xa + 0.1

    #         break

    #     elif (y[m] > 35):

    #         print('Elif 2')

    #         # xa = xa + 0.1

    #         vo = vo + 0.1

    #         break

    #     else:

    #         print('Else')

    #         z = 1

    #         break

    xvoList.append([vo, xa])

    a = -(g*(xa**2))/(2*(vo**2))
    b = (xa)
    c = -(g*(xa**2))/(2*(vo**2)) - ht

    # while ((b**2) >= (4*a*c)):

    #     vo = vo + 0.1

    #     a = -(g*(xa**2))/(2*(vo**2))

    #     b = (xa)

    #     c = -(g*(xa**2))/(2*(vo**2)) - ht

    tanAlpha = (-b - (b**2 - 4*a*c)**(1/2))/(2*a)
    alpha = (-math.atan(tanAlpha))

    vox = vo*(math.cos(alpha));
    vy = vo*(math.sin(alpha));

    # alphaList = alphaCalc(vo, g, xa, ht)

    # alpha = alphaList[0]
    # vox = alphaList[1]
    # vy = alphaList[2]

print(xvoList)

xvoListSuccessN = len(x) - 2;        # [vo and xa]

xvoListSuccess = xvoList[xvoListSuccessN]

voxOptimal = xvoListSuccess[0]

xaOptimal = xvoListSuccess[1]

print('Optimal Initial Velocity: ' + str(voxOptimal))
print('Optimal X Distance: ' + str(xaOptimal))

plt.plot(x,y)
plt.xlabel('X')
plt.ylabel('h')