"""
import numpy as np
import Steppers.scovel as scv
import Plotting.TwoDimensions as plt

# inital values and preallocted arrays
Dt = np.float64(0.1)
T = np.float64(100.0)
N = np.int(T/Dt)
p = np.zeros((N, 3), dtype = np.float64)
q = np.zeros((N, 3), dtype = np.float64)
q[0, :] = np.ones(3)
b = np.zeros(3)
b[2] = 1.0
V = lambda x, y, z: -1/np.sqrt(x**2 + y**2 + z**2)
dVdq = lambda q: q/(q[0]**2 + q[1]**2 + q[2]**2)**3/2
M = np.float64(1.0)

# integrate Hamiltonian
stepper = scv._ScovelsMethod(dVdq, M, b, Dt)
for nn in range(N - 1):
    # 2nd Order Scovel method calculations
    q[nn + 1, :], p[nn + 1, :] = stepper.m_integrate(q[nn, :], p[nn, :])
    
#%% Plot
# plot in 2D
plt.plotTrajectory(1, q, [-2.0, 2.0, -2.0, 2.0])#[0.98, 0.995, -0.01, 0.01])
plt.plotPhaseSpace(2, q, p)#, 
                   #[-1.5, 1.5, -1.5, 1.5], [-1.5, 1.5, -1.5, 1.5])
plt.plot(3, q, p)#, 
                   #[-1.5, 1.5, -1.5, 1.5], [-1.5, 1.5, -1.5, 1.5])
def V(x, y, z):
    return -(x**2 + y**2)/2 - mu1/r1(x, y) - mu2/r2(x, y)
def dVdq(q):
    return - q - \
           mu1/r1(q[0], q[1])**3*np.array([mu2 - q[0], 
                                           -q[1],
                                           0.0]) + \
           mu2/r2(q[0], q[1])**3*np.array([mu1 + q[0], 
                                           q[1],
                                           0.0])

# integrate Hamiltonian
stepper = {}
for Dt in Dts:
    key = 'Scovel, Dt = {}'.format(Dt)
    stepper[key] = stp._ScovelsMethod(dVdq, 1.0, b, Dt)
    key = '4th order, s = 5, Dt = {}'.format(Dt)
    w = np.array([0.28, 0.62546642846767004501])
    stepper[key] = stp.Composition(dVdq, 1.0, b, w, Dt)
    key = '6th order, s = 7, Dt = {}'.format(Dt)
    w = np.array([0.78451361047755726382, 0.23557321335935813368, \
                  -1.17767998417887100695])
    stepper[key] = stp.Composition(dVdq, 1.0, b, w, Dt)
    key = '6th order, s = 9, Dt = {}'.format(Dt)
    w = np.array([0.39216144400731413928, 0.33259913678935943860, \
                  -0.70624617255763935981, 0.08221359629355080023])
    stepper[key] = stp.Composition(dVdq, 1.0, b, w, Dt)
    key = '8th order, s = 15, Dt = {}'.format(Dt)
    w = np.array([0.74167036435061295345, -0.40910082580003159400,
                  0.19075471029623837995, -0.57386247111608226666,
                  0.29906418130365592384, 0.33462491824529818378,