using TaylorSeries

# Parámetros para el integrador de Taylor
const ordenTaylor = 28
const epsAbs = 1.0e-20

println(" Taylor order = $ordenTaylor\n Eps = $epsAbs\n")

function taylorStepper{T<:Real}( jetEqs::Function, vec0::Array{T,1}, order::Int64, epsilon::T)
    n = length( vec0 )
    vec0T = [ Taylor([vec0[i]], order) for i=1:n ]
    vec1T = jetEqs( vec0T )
    
    # Step-size
    hh = Inf
    for i=1:n
        h1 = stepsize( vec1T[i], epsilon )
        hh = min( hh, h1 )
    end
    #hh = hh*0.125
    
    # Values at t0+h
    for i=1:n
        vec0[i] = evalTaylor( vec1T[i], hh )
    end
    
    return hh, vec0
end

function stepsize{T<:Real}(x::Taylor{T}, epsilon::Float64)
    ord = x.order
    h = Inf
    for k in [ord-1, ord]
        kinv = 1.0/k
        aux = abs( x.coeffs[k+1] )
        h = min(h, (epsilon/aux)^kinv)
    end
    return h
end

energy{T<:Real}( x::T, vx::T ) = 0.5*vx^2 - cos(x)

function jetPendulum{T<:Real}( vec::Array{Taylor{T},1} )
    xT = vec[1]
    vxT = vec[2]
    ord = xT.order
    # Now the implementation
    for k = 0:ord-1
        knext = k+1
        # Taylor expansions up to order k <--- This part makes it somewhat slower
        xTt = Taylor( xT.coeffs[1:k+1], k)
        vxTt = Taylor( vxT.coeffs[1:k+1], k)
        # Eqs of motion <--- This is quite straight :-)
        xDot = vxTt
        vxDot = -sin(xTt)
        # The equations of motion define the recurrencies
        xT.coeffs[knext+1]  = xDot.coeffs[knext] / knext
        vxT.coeffs[knext+1] = vxDot.coeffs[knext] / knext
    end    
    return [ Taylor(xT, ord), Taylor(vxT, ord) ]
end

x0 = pi-0.001
vx0 = 0.0

energy(x0,vx0)

taylorStepper( jetPendulum, [x0, vx0], ordenTaylor, epsAbs )

function pendulumIntegration( x0::Float64, vx0::Float64, time_max::Float64, jetEqs::Function, 
                            orden::Int64=ordenTaylor, epsilon::Float64=epsAbs )
    # Initial conditions and energy
    t0 = 0.0
    ene0 = energy(x0, vx0)
    
    # Change, measured in the local `eps` of the change of energy
    eps_ene = eps(ene0); dEne = zero(Int64)
    
    # Vectors to plot the orbit with PyPlot
    tV, xV, vxV = Float64[], Float64[], Float64[]
    DeneV = Int64[]
    push!(tV, t0)
    push!(xV, x0)
    push!(vxV, vx0)
    push!(DeneV, zero(Int64))
    
    # This is the main loop; we include a minimum step size for security
    dt = 1.0
    while t0 < time_max && dt>1.0e-8
        # Here we integrate
        dt, (x1, vx1) = taylorStepper( jetEqs, [x0, vx0], orden, epsilon );
        t0 += dt
        push!(tV,t0)
        push!(xV,x1)
        push!(vxV, vx1)
        eneEnd = energy(x1, vx1)
        dEne = itrunc( (eneEnd-ene0)/eps_ene )
        push!(DeneV, dEne)
        x0, vx0 = x1, vx1
    end

    return tV, xV, vxV, DeneV
end

tV, xV, vxV, DeneV = pendulumIntegration( x0, vx0, 2pi, jetPendulum);
@time tV, xV, vxV, DeneV = pendulumIntegration( x0, vx0, 73.0, jetPendulum);

minimum([tV[i+1]-tV[i] for i=1:length(tV)-1])

using PyPlot

plot(tV, xV, "-", tV, vxV, "-")

plot(tV, DeneV, "-")