from bisect import *
from pylab import *

set_printoptions(3)

def f_pendulum(t, u):
    # Defining the right 
    # hand side of the ODE describing
    # the pendelum 
    E = 10.0
    D = 4.0
    L = 1.0
    m = 1.0

    x = u[0:2]
    v = u[2:4]
    
    x0 = array([0.0, 0.0])
    v0 = array([0.0, 0.0])

    i = 0

    F01 = 0.0
    r01 = norm(x - x0)
    e01 = (x - x0) / r01
    F01 = -E*(r01 - L)*e01 - D*dot(v - v0, e01)*e01

    g = array([0.0, -1.0*0.1])

    val = zeros(u.size)
    val[0:2] = v
    val[2:4] = F01 / m + g

    return val

def piecewise_linear_adapter(tarray, uarray):
    def u_adapted(t):
        # Find the endpoints of the containing interval
        i1 = bisect_right(tarray, t)
        if(i1 == size(tarray)):
            i1 -= 1
        i0 = i1 - 1

        t0 = tarray[i0]
        t1 = tarray[i1]

        u0 = uarray[:, i0]
        u1 = uarray[:, i1]

        # Lagrange polynomial weights
        w0 = (t1 - t) / (t1 - t0)
        w1 = (t - t0) / (t1 - t0)

        ut = w0*u0 + w1*u1
        return ut

    return u_adapted

def derivative_adapter(tarray, uarray):
    def u_adapted(t):
        # Find the endpoints of the containing interval
        i1 = bisect_right(tarray, t)
        if(i1 == size(tarray)):
            i1 -= 1
        i0 = i1 - 1

        t0 = tarray[i0]
        t1 = tarray[i1]

        u0 = uarray[:, i0]
        u1 = uarray[:, i1]

        k = t1 - t0
        dudt = (u1 - u0) / (t1 - t0)

        ut = dudt
        return ut

    return u_adapted

def jacobian(f, t, u):
    # Computes the Jacobian df/du given a function f(t,u)

    y = f(t, u)
    if(size(u) > 1):
        J = zeros([size(y), size(u)])
    else:
        J = 0.0

    h = 1.0e-7
    for i in range(0, size(u)):
        hv = zeros([size(u)])
        hv[i] = h
        Jcol = (f(t, u + hv) - f(t, u - hv)) / (2*h)

        if(size(u) > 1):
            J[:, i] = Jcol
        else:
            J = Jcol
        hv[i] = 0.0

    return J


    return flin

def linearization(f, uarray,tarray):
    # Returns the right hand side, A(t)*fi, of the 
    # linearized equation where A(t) is the Jacobian
    # of f(t,u)
    # INPUT:
    # uarray: containing the numerically computed solution 
    #         for all components in the system
    # tarray: containing corresponding time values
    # f:      is a function defining the rhs of du/dt=f(t,u)
    # OUTPUT:
    # flin:   array (same size as f) flin=A(t)*fi
    
    ulin=piecewise_linear_adapter(tarray, uarray)
    def flin(t, u):
        J = jacobian(f, t, ulin(t))
        return dot(J, u)

    return flin

def f3body(t, u):
    # Fill in yourself (planetsystem)
    f=0
    return f


def f_chemical(t, u):
    # Fill in yourself the right hand 
    # side of the chemical system
    f=0
    return f
 
def Sd_estimate(tarray,uarray,f,I):
    # INPUT: 
    # uarray: containing the numerically computed solution 
    #         for all components in the system
    # tarray: containing corresponding time values
    # f:      is a function defining the rhs of du/dt=f(t,u)
    # I:      time interval 
    
    # OUTPUT:
    # Sd:     Scalar value of stability factor Sd
    
    # 1) Solve numerically the linearized problem dfi/dt=A(t)*fi
    #    using intialdata,  fi0,  corresponding to the 
    #    perturbations. A(t)*fi can be computed by  using the function
    #    linearization. 

    # 2) Use the numerical solution of the linearized problem, 
    #    fi, to compute the stability factor as the maximum value
    #    over the whole time interval and all the components in the 
    #    system. 
    
    Sd=0
    return Sd

def Sc_estimate(tarray,uarray,phiarray,f, k):
    # INPUT: 
    # uarray:   containing the numerically computed solution 
    #           for all components in the system
    # tarray:   containing corresponding time values
    # phiarray: solution to the dual problem 
    # f:        is a function defining the rhs of du/dt=f(t,u)
    # k:        timestep
    

    # Output:
    # Sc:       Scalar value of stability factor Sc 
    Sc=0
    return Sc