from scipy.integrate import trapz

def osc(q, t, w, coefs):
    fu = np.poly1d(coefs)
    x, y = q
    dqdt = [y, fu(x) - w*x]
    return dqdt

f = open('homework3_dataFinal.txt', 'r')
t = f.readline().rstrip().split()
t = list(map(float,t))
real = f.readline().rstrip().split()
real = list(map(float,real))
real = np.array(real)
q0 = [2,1]
coefs = [-0.01001609,-0.0101361,-0.01107698, -0.01775297, -0.02, -0.02]
w = 0

def bound(q, tt, w, coefs):
    x, y = q
    fu = np.poly1d(coefs)
    fp = np.polyder(fu)
    dqdt = [y, -w * x + x * fp(x)]
    return dqdt

lq0 = [0, -2*(real[49] - odeint(osc, q0, t, args=(w, coefs), full_output = 0)[49,0])]

lambd = odeint(bound, lq0, list(reversed(t)), args=(w, coefs), full_output = 0)[:,0]

product = [np.flipud(lambd) * (odeint(osc,q0,t, args=(w, coefs))[:,0] ** i) for i in range(len(coefs))]

res = trapz(product, t)



Cal
def gradi(coefs):
    lq0 = [0, -2*(real[49] - odeint(osc, q0, t, args=(w, coefs), full_output = 0)[49,0])]

    lambd = odeint(bound, lq0, list(reversed(t)), args=(w, coefs), full_output = 0)[:,0]

    product = [np.flipud(lambd) * (odeint(osc,q0,t, args=(w, coefs))[:,0] ** i) for i in range(len(coefs))]

    res = trapz(product, t)
    return np.flipud((-1 * res))
def mine(coefs):
    return (np.sum(np.abs(real - odeint(osc, q0, t, args=(w, coefs), full_output = 0)[:,0])))

def dx(f, x):
    return abs(0-f(x))

def newtons_method(f, df, x0):
    a = np.ones(len(x0))
    delta = dx(f, x0)
    oldf = delta
    i = 0
    oldx = 0
    while delta > 40:
        oldx = x0
        x0 = x0 - ((f(x0))/(a * df(x0)))
        if (f(x0) > oldf):
            x0 = oldx
            a[i % len(x0)] *= 100
            i+=1
            if i % 100 == 0:
                print ("hi",f(x0))
                print ("hey",f(oldx))
        else:
            print ("here")
            print (f(x0))
            print (x0)
            print (gradi(x0))
            oldf = f(x0)
            a = np.ones(len(x0))
            i = 0
            delta = dx(f, x0)
    print ('Root is at: ', x0)
    print ('f(x) at root is: ', f(x0))

newtons_method(mine,gradi,[0,  -66.90116106,  -393.85982963, -1183.98710777,   223.52126842])