function segway_rk4(xt::Vector{Float64}, ut::AbstractVector, dt::Number, params::Vector)
    xt1 = copy(xt)
    segway_rk4!(xt1, ut, dt, params)
    return xt1
end

function segway_rk4!(xt::AbstractVector, ut::AbstractVector, dt::Number, params::Vector)
    temp = Array{Float64}(undef, 7)

    #get partial results dy1-dy4
    dy1 = dxdt_segway(xt, ut, params);
    temp .= dy1.*(dt/2.0) .+ xt;

    dy2 = dxdt_segway(temp, ut, params);
    temp .= dy2.*(dt/2.0) .+ xt;

    dy3 = dxdt_segway(temp, ut, params);
    temp .= dy3.*dt .+ xt;

    dy4 = dxdt_segway(temp, ut, params);

    # update xt
    @. temp = dt*(dy1+2.0*(dy2+dy3)+dy4)/6.0

    #Check if segway has fallen #TODO limits anpassen
    if -pi/2>=xt[4]+temp[4]
        xt[4] = -pi/2
		xt[5:7] = 0.0
    elseif pi/2<=xt[4]+temp[4]
        xt[4] = pi/2
		xt[5:7] = 0.0
    else
        # Limit angle to lie between 0 and 2*pi
        @. xt += temp
        #xt[3] = mod(xt[3], 2*pi)
    end
    return nothing
end

"""
Segway dynamics, taken from Pathak2005.
State vector is [x0, y0, phi, alpha, dalpha, v, dphi]
"""
function dxdt_segway(x::Vector{Float64}, u::AbstractVector, params::Vector)
    g = 9.81 #N/kg

    # Rename variables
    Mb = params[1]
    Mw = params[2]
    R = params[3]
    cz = params[4]
    b = params[5]
    Ixx = params[6]
    Iyy = params[7]
    Izz = params[8]
    Iwa = params[9]
    Iwd = params[10]

    phi = x[3]
    alpha = x[4]
    dalpha = x[5]
    v = x[6]
    dphi = x[7]

    # Rigid Body Dynamics
    Dalpha = Mb^2*cos(alpha)^2*cz^2*R^2+((-Mb^2-2*Mw*Mb)*cz^2-2*Iyy*Mw-Iyy*Mb)*R^2-2*Mb*cz^2*Iwa-2*Iyy*Iwa
    Galpha = (-Mb*cz^2+Izz-Ixx)*R^2*cos(alpha)^2 + (Mb*cz^2+Ixx+2*Iwd+2*b^2*Mw)*R^2+2*b^2*Iwa
    H = 1/2*Mb*R^2*Izz+Iwa*Izz-Mw*R^2*Ixx-Iwa*Ixx-Mb*cz^2*Mw*R^2 - Mb*cz^2*Iwa -1/2*Mb*R^2*Ixx+Mw*R^2*Izz
    Kalpha = (-4*Iyy*Mb*R^2*cz-3*R^2*Mb^2*cz^3+Mb*R^2*cz*(Ixx-Izz))*sin(alpha) + (Mb*R^2*cz*(Ixx-Izz)+R^2*Mb^2*cz^3)*sin(3*alpha)
    f21 = sin(2*alpha)*dphi^2*H/Dalpha + Mb^2*cz^2*R^2*sin(2*alpha)*dalpha^2/(2*Dalpha) + (-2*Mb^2*R^2*cz-4*Iwa*Mb*cz-4*Mw*R^2*Mb*cz)*g*sin(alpha)/(2*Dalpha)
    f22 = Kalpha*dphi^2+(Mb^2*cz^2*R^2*g*sin(2*alpha))/(2*Dalpha)+(-4*Iyy*Mb*R^2*cz-4*R^2*Mb^2*cz^3)*sin(alpha)*dalpha^2/(4*Dalpha)
    f23 = (-(Ixx-Izz)*R^2-Mb*cz^2*R^2)*sin(2*alpha)*dalpha*dphi/Galpha-sin(alpha)*R^2*Mb*cz*v*dphi/Galpha
    g21 = (u[1]+u[2])*(Mb*R^2+2*Mw*R^2+2*Iwa+Mb*cos(alpha)*cz*R)/Dalpha
    g22 = -(u[1]+u[2])*R*(Mb*cos(alpha)*cz*R+Iyy+Mb*cz^2)/Dalpha
    g23 = (u[1]-u[2])*R*b/Galpha

    xdot = Array{Float64}(undef, 7)
    xdot[1] = cos(phi)*v
    xdot[2] = sin(phi)*v
    xdot[3] = x[7]
    xdot[4] = x[5]
    xdot[5] = f21 + g21
    xdot[6] = f22 + g22
    xdot[7] = f23 + g23

    return xdot
end


using PQEcolaPoint
using Dates

# setup DTE -
T = 36.0
DTE = range(T, stop=14.0,step=-1.0) |> collect

# setup underlying range -
Sₒ = 79.74
IV = 0.4456
σ = Sₒ*(IV)*(√(T/365))
L = Sₒ - 2.96*σ
U = Sₒ + 2.96*σ
S = range(L,stop=U, length=10) |> collect

# setup IV array -
IV_array = range(0.1, stop = 1.2, step = 0.1) |> collect

# setup strike coordinates -
K = range(50.0, stop = 125.0, step = 5.0) |> collect

# what is the contract?
short_put_contract = PutContractModel()
short_put_contract.ticker = "MU"
short_put_contract.expiration_date = Date(2022, 05, 06)
short_put_contract.strike_price = 75.0
short_put_contract.premium = 0.0
short_put_contract.number_of_contracts = 1
short_put_contract.direction = 1

# compute the grid -
lattice_model = projection(short_put_contract, S, K, IV_array, DTE; number_of_levels = 80)


function WSM(A,b,C,d,XLB,XUB,VariableDef,Lambda;MyModel = [],TimeLim = 999999)
    if MyModel == []
        MyModel = WSM(A,b,C,d,XLB,XUB,VariableDef)
    end
    X = getindex(MyModel,:X);
    ObjNum = size(C)[1];
    m,n = size(A);
    @objective(MyModel, Min, sum(Lambda[i]*(sum(C[i,j]*X[j] for j = 1:n )+d[i])  for i = 1:ObjNum))

    setsolver(MyModel, CplexSolver(CPXPARAM_Threads = 1,CPXPARAM_ScreenOutput = 0,CPXPARAM_TimeLimit = TimeLim ))
    status = solve(MyModel)
    if status != :Optimal
        return [], [], [];
    end

    Var = getvalue(X)
    Y = zeros(ObjNum)
    for i =1:ObjNum
        Y[i] = sum(C[i,:].*Var)+d[i]
    end
    return Y, Var,MyModel;
end

function WSM(A,b,C,d,XLB,XUB,VariableDef)
    m,n = size(A);
    MyModel = Model();
    @variable(MyModel,XLB[i] <= X[i=1:n] <= XUB[i],category = VariableType(VariableDef,i))#
    for i = 1:m
        @constraint(MyModel,sum(A[i,j]*X[j] for j = 1:n) >= b[i] )
    end

    return MyModel
end


using ReachabilityBenchmarks, MathematicalSystems, Reachability, Plots

include(@relpath "beam_model.jl")
include(@relpath "beam_specifications.jl")

S = beam_model()
X0, options = beam_specification()
options = Dict(options)

# initial value problem
problem = InitialValueProblem(S, X0)

# partition with 1D blocks
partition = [i:i for i in 1:348]

# reachability algorithms
algorithm_reach = BFFPSV18(:δ => 1e-3, :vars => [89], :partition => partition)
algorithm_check = BFFPSV18(:δ => 5e-5, :vars => [89], :partition => partition)

# check property
options[:mode] = "check"
solution = solve(problem, options; op=algorithm_check)
@assert solution.satisfied

# compute flowpipe
options[:mode] = "reach"
solution = solve(problem, options; op=algorithm_reach)

# project flowpipe to time and x₈₉
solution.options[:plot_vars] = [0, 89]
solution_proj = project(solution)

# plot projection
plot(solution_proj)
savefig("beam")


"""
    LD(A::Diagonal)
    LD(A::HBlikDiag)
    LD(A::DenseMatrix)

Return `log(det(tril(A)))` evaluated in place.
"""
LD(d::Diagonal{T}) where {T<:Number} = sum(log, d.diag)

function LD(d::UniformBlockDiagonal{T}) where {T}
    dat = d.data
    return sum(log, dat[j, j, k] for j in axes(dat, 2), k in axes(dat, 3))
end

LD(d::DenseMatrix{T}) where {T} = sum(log, d[k] for k in diagind(d))

"""
    logdet(m::LinearMixedModel)

Return the value of `log(det(Λ'Z'ZΛ + I)) + m.optsum.REML * log(det(LX*LX'))`
evaluated in place.

Here LX is the diagonal term corresponding to the fixed-effects in the blocked
lower Cholesky factor.
"""
function LinearAlgebra.logdet(m::LinearMixedModel{T}) where {T}
    L = m.L
    nre = m.dims.nretrms
    s = LD(first(L))
    for j in 2:nre
        s += LD(L[kp1choose2(j)])
    end
    if m.optsum.REML
        lastL = last(L)
        s += LD(lastL)  # this adds the log of sqrtpwrss
        s -= log(last(lastL))
    end
    return s + s
end


function HermiteReduce(f::AbstractAlgebra.ResFieldElem{P}, DE::AlgebraicExtensionDerivation) where
    {T<:FieldElement, P<:PolyElem{T}}
    # "Lazy" Hermite reduction, see Section 2.1 of:
    # Manuel Bronstein. Symbolic integration tutorial. ISSAC’98, 1998.
    # http://www-sop.inria.fr/cafe/Manuel.Bronstein/publications/issac98.pdf
    iscompatible(f, DE) || error("rational function f must be in the domain of derivation D")

    E = parent(f)
    F = base_ring(base_ring(E))
    y = E(gen(base_ring(E)))
    p = modulus(E)
    n = degree(p)
    M_n_n = MatrixSpace(F, n, n)
    M_n = MatrixSpace(F, n, 1)
    g = zero(E)

    if all([iszero(coeff(p, j)) for j=1:n-1]) # simple radical extension
        # Simpler method for simple radical extensions, see Section 2.2. of Bronstein's tutorial.
        @assert isone(leading_coefficient(p))
        a = constant_coefficient(p)
        A = numerator(a)
        D = denominator(a)
        As = Squarefree(A*D^(n-1))
        k = length(As)
        qrs = [divrem(i, n) for i=1:k]
        qs = [q for (q, r) in qrs]
        rs = [r for (q, r) in qrs]
        F = prod([As[i]^qs[i] for i=1:k])
        H = prod([As[i]^rs[i] for i=1:k])
        z = y*D//F
        Hs = Squarefree(H)
        m = length(Hs)
        ws = [z^(i-1)*1//prod([Hs[j]^div((i-1)*j, n) for j=1:m]) for i=1:n]
        Ds = [prod([Hs[j]^div((i-1)*j, n) for j=1:m]) for i=1:n] # note: index shift in i
        dDs_over_Ds = [derivative(D)//D for D in Ds]
        dH_over_H = derivative(H)//H
        Winv = inv(M_n_n([coeff(data(w), j) for j=0:n-1, w in ws]))
        while true
            fs = Winv*M_n([coeff(data(f), i) for i=0:n-1])
            D = lcm(denominator.(fs)[:,1])
            As = [numerator(f*D) for f in fs]
            if degree(gcd(D, derivative(D)))<= 0 #  D squarefree?
                return g, f, ws, As, D
            end

            Ds1 = Squarefree(D) # "Ds1" because "Ds" already in use and needed below
            m = length(Ds1)
            V = Ds1[m]
            U = divexact(D, V^m)
            dV = derivative(V)

            fs = [As[i]//(U*(V*((i-1)//n*dH_over_H - dDs_over_Ds[i]) - (m-1)*dV)) for i=1:n]
            Q = lcm(denominator.(fs)[:,1])
            Ts = [numerator(f*Q) for f in fs]
            _, _, R = gcdx(V, Q)
            B = sum([rem(R*Ts[i], V)*ws[i] for i=1:n])*1//V^(m - 1)
            g += B
            f -= DE(B)
        end
    end

    ws = [y^j for j=0:n-1]
    Winv = M_n_n(1)

    while true
        fs = Winv*M_n([coeff(data(f), i) for i=0:n-1])
        D = lcm(denominator.(fs)[:,1])
        As = [numerator(f*D) for f in fs]
        if degree(gcd(D, derivative(D)))<= 0 #  D squarefree?
            return g, f, ws, As, D
        end

        Ds = Squarefree(D)
        m = length(Ds)
        V = Ds[m]
        U = divexact(D, V^m)

        Ss = [U*V^m*DE(w*1//V^(m-1)) for w in ws]

        SM = M_n_n([coeff(data(S), j) for j=0:n-1, S in Ss])
        d, N = nullspace(SM)
        if d>0 # Theorem 1
            _, N = nullspace(SM)
            w0 = U//V*sum(N[i,1]*ws[i] for i=1:n)
        else
            fs = solve(SM, M_n(As))
            Q = lcm(denominator.(fs)[:,1])
            Ts = [numerator(f*Q) for f in fs]
            w0 = zero(parent(f))
            gcdVQ = gcd(V, Q)
            if degree(gcdVQ)>=1 # Theorem 2
                w0 = U*divexact(V, gcdVQ)//gcdVQ*sum([Ts[i]*ws[i] for i=1:n])
            else
                j = 1
                while j<=n
                    dw = DE(ws[j])
                    dws = Winv*M_n([coeff(data(dw), i) for i=0:n-1])
                    F = lcm(denominator.(dws)[:,1])
                    if degree(gcd(F, derivative(F)))>=1 # denominator F of ws[j]' not squarefree, apply Theorem 3
                        w0 = prod(Squarefree(F))*dw
                        break
                    end
                    j += 1
                end
                if j>n # all denominators of the ws[j]' squarefree, do the reduction
                    _, _, R = gcdx(V, Q)
                    B = sum([rem(R*Ts[i], V)*ws[i] for i=1:n])*1//V^(m - 1)
                    g += B
                    f -= DE(B)
                    ws = [y^j for j=0:n-1]
                    Winv = M_n_n(1)
                    continue
                end
            end
        end

        C0s = solve(M_n_n([coeff(data(w), j) for j=0:n-1, w in ws]), M_n([coeff(data(w0), i) for i=0:n-1]))
        F = lcm(denominator.(C0s)[:,1])
        Cs = [numerator(C0*F) for C0 in C0s]

        C = zero_matrix(parent(F), n+1, n)
        for j=1:n
            C[j,j] = F
        end
        for j=1:n
            C[n+1,j] = Cs[j]
        end
        C = hnf(C)

        ws = [sum([C[i,j]*ws[j] for j=1:n])*1//F for i=1:n]
        Winv = inv(M_n_n([coeff(data(w), j) for j=0:n-1, w in ws]))
    end
end

function IntegralBasis(E::AbstractAlgebra.ResField{P}) where {T<:FieldElement, P<:PolyElem{T}}
    # Trager's algorithm, see Chapter 2 of
    # B.M. Trager. On the integration of algebraic functions. PhD thesis, MIT, Computer Science, 1984.
    # https://dspace.mit.edu/bitstream/handle/1721.1/15391/12487590-MIT.pdf
    Ky = base_ring(E)
    K = base_ring(Ky)
    y = E(gen(Ky))
    f = modulus(E)
    n = degree(f)
    M_n_n = MatrixSpace(K, n, n)
    MP_n_n = MatrixSpace(base_ring(K), n, n)
    D = resultant(f, derivative(f))
    @assert isone(denominator(D))
    D = numerator(D)
    D = 1//leading_coefficient(D) * D
    k = D
    bs = [y^j for j=0:n-1]
    B = M_n_n([coeff(data(bs[j]), i) for i=0:n-1, j=1:n])
    while true
        Ds = SI.Squarefree(D)
        if length(Ds)<2
            return bs
        end
        Q = gcd(prod(SI.Squarefree(D)[2:end]), k)
        if degree(Q)<=0
            return bs
        end

        # Compute J, the Q-trace radical of V
        ZE = zero(E)
        S = [i<=j ? bs[i]*bs[j] : ZE  for i=1:n, j=1:n] # only upper triangle needed
        ZK = zero(K)
        # TODO: optimize computation of TM, which is one of the bottlenecks.
        # Compute only upper triangle of TM, lower triangle by symmetry:
        TM = [i<=j ? numerator(sum([coeff(data(S[i, j]*y^l), l) for l=0:n-1])) : ZK for i=1:n, j=1:n]
        for i=1:n
            for j=1:i-1
                TM[i, j] = TM[j, i]
            end
        end
        H = hnf(vcat(MP_n_n(TM), MP_n_n(Q)))[1:n,:] # Hermite normal form
        J = inv(map(x->x//1, H)) # map(x->x/1, H) is H as element in FractionField

        # Compute the idealizer of J
        JB = J*B
        ms = [E(Ky(reshape(Matrix(JB[:,k]), n))) for k=1:n]
        Minv = inv(M_n_n([coeff(data(m), i) for i=0:n-1, m in ms]))
        MM = 0
        for k=1:n
            mbs = [data(ms[k]*b) for b in bs]
            Mk = Minv*M_n_n([coeff(mbs[j], i) for i=0:n-1, j=1:n])
            MM = k==1 ? Mk : vcat(MM, Mk)
        end
        Mhat = hnf(map(x->numerator(x), MM))[1:n,:] # Hermite normal form
        Mhatinv = inv(map(x->x//1, Mhat))
        k = det(Mhat)

        if degree(k)<=0 # is k a unit ?
            return bs
        end

        #Update bs by applying the change of basis
        B = transpose(Mhatinv)*B
        bs = [E(Ky(reshape(Matrix(B[:,k]), n))) for k=1:n]
        D = divexact(D, k^2)
    end
end


function arithmetic (a = parse(Int, readline()), b = parse(Int, readline()))
  for op in  [+,-,*,div,rem]
    println("a $op b = $(op(a,b))")
  end
end


export intersect, renderscene!

function intersect(ray::Ray, sphere::Sphere)
    oc = (ray.origin - sphere.center)

    a = ray.dir ⋅ ray.dir
    bhalf = oc ⋅ ray.dir
    c = oc ⋅ oc - sphere.radius^2
    Δ = bhalf^2 - a * c

    if Δ > 0
        sqrtΔ = sqrt(Δ)

        t1 = (-bhalf - sqrtΔ) / a
        if t1 > 0
            return t1
        end

        t2 = (-bhalf + sqrtΔ) / a
        if t2 > 0
            return t2
        end
    end

    return zero(Δ)
end

function renderscene!(camera, shapes, image; background=RGB{Float64}(0, 0, 0))
    height, width = size(image)
    for y in 1:height
        for x in 1:width
            ray = cast_ray(camera, x, y, width, height)

            closest_dist = zero(Float64)
            closest_shape = 0
            closest_loc_ray = ray
            firsthit = true
            for i in eachindex(shapes)
                loc_ray = transform(shapes[i]) * ray
                cur_dist = intersect(loc_ray, shapes[i])
                if cur_dist > 0 && (firsthit || cur_dist < closest_dist)
                    closest_loc_ray = loc_ray
                    closest_dist = cur_dist
                    closest_shape = i
                    firsthit = false
                end
            end

            image[height - y + 1, x] = if closest_shape == 0
                background
            else
                getcolor(shapes[closest_shape], ray_at_dist(closest_loc_ray, closest_dist))
            end
        end
    end
end


include("holtslag_psim.jl")
include("holtslag_psis.jl")
include("constants.jl")

function VelocityScale(z,h,L,ustar,θv=nothing,wθv=nothing)
  xi=z/L
  psis=holtslag_psis(xi)
  psim=holtslag_psim(xi)
  if xi>0
      wt=ustar/psis
      wm=wt
  else
      if z/h<0.1
          wt=ustar/psis
          wm=ustar/psim
      else
          c1=0.6
          a=7.2
          wθv<0 ? println("wθv",wθv) : nothing
          #println("θv",θv)
          wstar=((gravity_mks/θv)*wθv*h)^(1/3)
          wm=(ustar^3+c1*wstar^3)^(1/3)
          xi1=0.1*z/L
          psis1=holtslag_psis(xi1)
          psim1=holtslag_psim(xi1)
          Pr=psis1/psim1*abs(xi1)+a*karman*(0.1*(wstar/wm))
          wt=wm/Pr
      end
  end
  return wt,wm
end


#
# PBE correlation (without LDA part) - spin-polarized
# iflag = 1: J.P.Perdew, K.Burke, M.Ernzerhof, PRL 77, 3865 (1996).
#
function XC_c_pbe_spin( rho, zeta, grho )

    ga = 0.031091
    be = 0.06672455060314922
    third = 1.0/3.0
    pi34 = 0.6203504908994

    xkf = 1.919158292677513
    xks = 1.128379167095513

    rs = pi34/rho^third

    ec, vcup, vcdw = XC_c_pw_spin( rho, zeta )

    kf = xkf / rs
    ks = xks * sqrt(kf)
    fz = 0.5 * ( (1.0 + zeta)^(2.0 / 3.0) + (1.0 - zeta)^(2.0 / 3.0) )

    fz2 = fz * fz
    fz3 = fz2 * fz
    fz4 = fz3 * fz
    dfz = ( (1.0 + zeta)^(-1.0/3.0) - (1.0 - zeta)^( -1.0 / 3.0) ) / 3.0

    t = sqrt(grho) / (2.0 * fz * ks * rho)
    expe = exp( - ec / (fz3 * ga) )
    af = be / ga * (1.0 / (expe-1.0) )

    bfup = expe * (vcup - ec) / fz3
    bfdw = expe * (vcdw - ec) / fz3

    y = af * t * t

    xy = (1.0 + y) / (1.0 + y + y * y)
    qy = y * y * (2.0 + y) / (1.0 + y + y * y)^2

    s1 = 1.0 + be / ga * t * t * xy
    h0 = fz3 * ga * log(s1)

    dh0up = be * t * t * fz3 / s1 * ( -7.0 / 3.0 * xy - qy * (af * bfup / be - 7.0 / 3.0) )

    dh0dw = be * t * t * fz3 / s1 * ( -7.0 / 3.0 * xy - qy * (af * bfdw / be - 7.0 / 3.0) )

    dh0zup = (3.0 * h0 / fz - be * t * t * fz2 / s1 * (2.0 * xy -
             qy * (3.0 * af * expe * ec / fz3 / be + 2.0) ) ) * dfz * (1.0 - zeta)

    dh0zdw = -(3.0 * h0 / fz - be * t * t * fz2 / s1 * (2.0 * xy -
             qy * (3.0 * af * expe * ec / fz3 / be + 2.0) ) ) * dfz * (1.0 + zeta)

    ddh0 = be * fz / (2.0 * ks * ks * rho) * (xy - qy) / s1

    sc = rho * h0
    v1cup = h0 + dh0up + dh0zup
    v1cdw = h0 + dh0dw + dh0zdw
    v2c = ddh0

    return sc, v1cup, v1cdw, v2c

end


function mandelbrot(x, y)
    z = c = x + y*im
    for i in 1:100.0; abs(z) > 2 && return i; z = z^2 + c; end; 0
end

x = Node(range(-2, 1, length = 400))
y = Node(range(-1, 1, length = 300))

fig, ax, img = heatmap(x, y, mandelbrot, colormap = Reverse(:deep),
    figure = (resolution = (400, 300),))
hidedecorations!(ax)

record(fig, "mandelbrot.mp4", 1:200) do frame
    x.val = x[] .+ ((-0.562 .- x[]) .* 0.05)
    y[] = y[] .+ ((0.645 .- y[]) .* 0.05)
    autolimits!(ax)
end


using Oceananigans
using Oceananigans.Units
using Statistics
using Printf
using Oceananigans.MultiRegion
using LinearAlgebra, SparseArrays
using Oceananigans.Solvers: constructors, unpack_constructors
using Oceananigans.Grids: architecture
using Oceananigans.Utils

function geostrophic_adjustment_test(free_surface, grid; regions = 1)

    if architecture(grid) isa GPU
        devices = (0, 0)
    else
        devices = nothing
    end
    mrg = MultiRegionGrid(grid, partition = XPartition(regions), devices = devices)

    coriolis = FPlane(f = 1e-4)

    model = HydrostaticFreeSurfaceModel(grid = mrg,
                                        coriolis = coriolis,
                                        free_surface = free_surface)

    gaussian(x, L) = exp(-x^2 / 2L^2)

    U = 0.1 # geostrophic velocity
    L  = grid.Lx / 40 # gaussian width
    x₀ = grid.Lx / 4 # gaussian center

    vᵍ(x, y, z) = -U * (x - x₀) / L * gaussian(x - x₀, L)

    g = model.free_surface.gravitational_acceleration
    η = model.free_surface.η

    η₀ = coriolis.f * U * L / g # geostrohpic free surface amplitude

    ηᵍ(x) = η₀ * gaussian(x - x₀, L)

    ηⁱ(x, y) = 2 * ηᵍ(x)

    set!(model, v = vᵍ)
    @apply_regionally set!(η, ηⁱ)

    gravity_wave_speed = sqrt(g * grid.Lz) # hydrostatic (shallow water) gravity wave speed
    Δt = 2 * model.grid.Δxᶜᵃᵃ / gravity_wave_speed

    for step in 1:10
        time_step!(model, Δt)
    end

    return η
end

Lh = 100kilometers
Lz = 400meters

for arch in archs

    free_surface   = ImplicitFreeSurface(solver_method = :HeptadiagonalIterativeSolver, maximum_iterations = 64 * 3)
    topology_types = (Bounded, Periodic, Bounded), (Periodic, Periodic, Bounded)

    @testset "Testing multi region implicit free surface" begin
        for topology_type in topology_types
            grid = RectilinearGrid(arch,
                        size = (64, 3, 1),
                        x = (0, Lh), y = (0, Lh), z = (-Lz, 0),
                        topology = topology_type)

            ηs = geostrophic_adjustment_test(free_surface, grid);
            ηs = Array(interior(ηs));

            for regions in [2, 4]
                @info "  Testing $regions partitions on $(topology_type) on the $arch"
                η = geostrophic_adjustment_test(free_surface, grid, regions = regions)
                η = Array(interior(reconstruct_global_field(η)))

                @test all(η .≈ ηs)
            end
        end
    end
end


using FixedPointAcceleration
function funcfunc(x::Array{Float64,1})
    # The first coordinate convergences to 4.0 by 1 unit per iterate.
    output = Array{Float64,1}(x)
    if abs(x[1] - 4.0) <= 1.0
        output[1] = 4.0
    elseif x[1] > 4.0
        output[1] = x[1] - 1.0
    else
        output[1] = x[1] + 1.0
    end
    # The second does aitken convergence to 2.3
    output[2] = x[2] + (2.3-x[2])/2.0
    return output
end
Inputs = [19.0,10.0]
fp_anderson = fixed_point(funcfunc, Inputs; Algorithm = :Anderson, PrintReports = false, ReplaceInvalids = :ReplaceElements)
fp_anderson.TerminationCondition_ == :ReachedConvergenceThreshold
fp_aitken = fixed_point(funcfunc, Inputs; Algorithm = :Aitken, PrintReports = false, ReplaceInvalids = :ReplaceElements)
fp_aitken.TerminationCondition_ == :ReachedConvergenceThreshold
fp_newton = fixed_point(funcfunc, Inputs; Algorithm = :Newton, PrintReports = false, ReplaceInvalids = :ReplaceElements)
fp_newton.TerminationCondition_ == :ReachedConvergenceThreshold
fp_simple = fixed_point(funcfunc, Inputs; Algorithm = :Simple, PrintReports = false, ReplaceInvalids = :ReplaceElements, ReportingSigFig =  Integer(10))
fp_simple.TerminationCondition_ == :ReachedConvergenceThreshold
fp_SEA = fixed_point(funcfunc, Inputs; Algorithm = :SEA, PrintReports = false, ReplaceInvalids = :ReplaceElements)
fp_SEA.TerminationCondition_ == :ReachedConvergenceThreshold
fp_VEA = fixed_point(funcfunc, Inputs; Algorithm = :VEA, PrintReports = false, ReplaceInvalids = :ReplaceElements)
fp_VEA.TerminationCondition_ == :ReachedConvergenceThreshold
fp_RRE = fixed_point(funcfunc, Inputs; Algorithm = :RRE, PrintReports = false, ReplaceInvalids = :ReplaceElements)
fp_RRE.TerminationCondition_ == :ReachedMaxIter # This one fails because it keeps proposing bad ideas.
fp_MPE = fixed_point(funcfunc, Inputs; Algorithm = :MPE, PrintReports = false, ReplaceInvalids = :ReplaceElements)
fp_MPE.TerminationCondition_ == :ReachedConvergenceThreshold


# export
#     ParamsEP,
#     epNP,
#     epdNP,
#     epMP,
#     epSNEP

immutable ParamsEP
    prior::EFamily
    logfactors::Vector{Function} # NOTE these functions need to be able to handle matrices of size pxN
    nEP::Int
    nIS::Int    # NOTE would have to generalise this --> could be SAMPLER obj
    damp::Float # NOTE would have to generalise this (stepping)
    minvar::Float
    # alpha::Float
    # ------------
    dim::Int
    nfactors::Int
    function ParamsEP(prior,factors;
                        nEP=100,nIS=100,damp=0.1,minvar=1e-5)
        @assert nIS>1 "Need at least two samples per estimator"
        #@assert 0.0<damp<=1.0 "Damping parameter should be in (0,1]"
        new(prior, factors, nEP, nIS, damp, minvar,
            length(mean(prior)), length(factors))
    end
end

"""
    epNP

Serial EP updates with damping in the Natural Parameter space.
This is the "SMS" style update.
"""
function epNP(p::ParamsEP)
    globapprox_mem = []

    # For each factor, we start with a very flat spherical gaussian
    locapprox  = [ones(GaussianNatParam, p.dim)/100 for i in 1:p.nfactors]
    globapprox = p.prior + sum(locapprox)

    push!(globapprox_mem, globapprox)

    for iter in 1:p.nEP
        for i in randperm(p.nfactors)
            cavity      = globapprox - locapprox[i]
            lltilted(x) = p.logfactors[i](x) + uloglik(cavity, x)
            (iss,w)     = impsampling(lltilted, globapprox, p.nIS)
            muhat       = sum( w[j]*suffstats(GaussianNatParam, iss[:,j])
                                for j in 1:p.nIS )
            # Cast it as mean parameter
            muhat = GaussianMeanParam(muhat[1], muhat[2])
            # Transform to natural parameter, project in case out of NP space
            np_muhat     = natparam(project(muhat,minvar=p.minvar))
            globapprox   = (1.0-p.damp)*globapprox + p.damp*np_muhat
            locapprox[i] = globapprox-cavity
        end
        push!(globapprox_mem, globapprox)
    end
    (globapprox, globapprox_mem)
end

"""
    epNP

Serial EP updates with damping in the Natural Parameter space.
"""
function epdNP(p::ParamsEP)
    locapprox  = [ones(DiagGaussianNatParam, p.dim)/100 for i in 1:p.nfactors]
    globapprox = p.prior + sum(locapprox)
    updatecounter = 0
    for iter in 1:p.nEP
        for i in randperm(p.nfactors)
            cavity      = globapprox - locapprox[i]
            lltilted(x) = p.logfactors[i](x) + uloglik(cavity, x)
            (iss,w)     = impsampling(lltilted, globapprox, p.nIS)
            muhat       = sum( w[j]*suffstats(DiagGaussianNatParam, iss[:,j])
                                for j in 1:p.nIS )
            # Cast it as mean parameter
            muhat = DiagGaussianMeanParam(muhat[1], muhat[2])
            # Transform to natural parameter, project in case out of NP space
            np_muhat = natparam(muhat)
            # Perform the tentative damping step
            t_globapprox = (1.0-p.damp)*globapprox+p.damp*np_muhat
            if maximum(-t_globapprox.theta2) < 1.0/p.minvar
                globapprox   = t_globapprox
                locapprox[i] = globapprox-cavity
                updatecounter += 1
            end
        end
    end
    println("Ratio of accepted updates: $updatecounter/$(p.nEP*p.nfactors)")
    globapprox
end

"""
    epMP

Serial EP updates with damping in the Mean Parameter space.
"""
function epMP(p::ParamsEP)
    globapprox_mem = []

    locapprox  = [ones(GaussianNatParam, p.dim)/100 for i in 1:p.nfactors]
    globapprox = p.prior + sum(locapprox)

    push!(globapprox_mem, globapprox)

    for iter in 1:p.nEP
        for i in randperm(p.nfactors)
            cavity      = globapprox - locapprox[i]
            lltilted(x) = p.logfactors[i](x) + uloglik(cavity, x)
            (iss,w)     = impsampling(lltilted, globapprox, p.nIS)
            muhat       = sum( w[j]*suffstats(GaussianNatParam, iss[:,j])
                                for j in 1: p.nIS )
            globapprox = natparam( (1.0-p.damp)*meanparam(globapprox)+
                                    p.damp*GaussianMeanParam(muhat[1], muhat[2])
                                  )
            locapprox[i] = globapprox-cavity
        end
        push!(globapprox_mem, globapprox)
    end
    (globapprox, globapprox_mem)
end

"""
    epSNEP

Serial SNEP updates attempt
"""
function epSNEP(p::ParamsEP)
    globapprox_mem = []

    locapprox  = [ones(GaussianNatParam, p.dim)/100 for i in 1:p.nfactors]
    globapprox = p.prior + sum(locapprox)

    push!(globapprox_mem, globapprox)

    for iter in 1:p.nEP
        for i in randperm(p.nfactors)
            cavity      = globapprox - locapprox[i]
            lltilted(x) = p.logfactors[i](x) + uloglik(cavity, x)
            (iss,w)     = impsampling(lltilted, globapprox, p.nIS)
            muhat       = sum( w[j]*suffstats(GaussianNatParam, iss[:,j])
                                for j in 1: p.nIS )
            locapprox[i] = natparam( meanparam(locapprox[i]) + p.damp*(
                                GaussianMeanParam(muhat[1], muhat[2]) -
                                meanparam(globapprox)
                                )
                              )
            globapprox = cavity + locapprox[i]
        end
        push!(globapprox_mem, globapprox)
    end
    (globapprox, globapprox_mem)
end


#Horizontal line
function get_line(x0,x1 , E)

    x =  collect(x0:0.1:x1)
    y = similar(x)
    y .= E

    return x,y
end

#Exponential decay
function get_line(x0 , x1 , a , E  )

    x = collect(x0:0.1:x1)
    y = E.*exp.(-a.*(x.-x0))

    return x,y

end

function get_simple_theoretical_model_data(p::Param , tmax = p.tmax)

    #Storage Vectors
    t_vec = Float64[]
    y_vec = Float64[]

    #If no dissipation
    if(!p.dissipation)

        #Initial configurations
        E = 1.0
        t = 0

        t_vec , y_vec = get_line(t , tmax  , E)

    elseif(p.R_orbit == 0.0)

        #Initial configurations
        E = 1.0
        t = 0
        absorb = false

        #Initial pulse configuration
        Ro = 0.9*p.xmax
        Ri = p.R1
        Rorbit = p.R_orbit

        #First iteration
        x , y = get_line(t , t + (p.r0-Ri)  , E)

        append!(t_vec, x)
        append!(y_vec, y)

        #push!(t_vec , x)
        #push!(y_vec , y)

        absorb = true
        t+= (p.r0-Ri)

        while(t < tmax)

            #absorption
            if(absorb)
                x , y = get_line(t , t+2*Ri , p.alpha1 ,E)

                append!(t_vec, x)
                append!(y_vec, y)

                                #push!(t_vec , x)
                #push!(y_vec , y)

                E = E*exp(- p.alpha1*(2*Ri))
                absorb = false
                t+= 2*Ri
            else
                x , y = get_line(t , t+ 2*(Ro-Ri) , E)

                append!(t_vec, x)
                append!(y_vec, y)

                absorb = true
                t+= 2*(Ro-Ri)
            end

        end

    else

        #Initial configuration
        E = 1.0
        t = 0

        Ro = 0.9*p.xmax
        Ri = p.R1
        Rorbit = p.R_orbit

        #Geometrical factor and phase of interaction
        phase = 1
        gfactor =  (2/pi)*(Ri/(Rorbit+Ri))

        alpha = gfactor*p.alpha1

        #First iteration
        x , y = get_line(t , t + (p.r0-Rorbit)  , E)

        push!(t_vec , x)
        push!(y_vec , y)

        phase = 2
        t+= (p.r0-Rorbit)

        while(t < tmax)

            #absorption
            if(phase == 1)

                x , y = get_line(t , t+(Ro-Rorbit-Ri) , E)

                push!(t_vec , x)
                push!(y_vec , y)

                phase = 2
                t+= (Ro-Rorbit-Ri)

            elseif(phase == 2)

                x , y = get_line(t , t+ 2*Ri , alpha , E)

                push!(t_vec , x)
                push!(y_vec , y)

                E = E*exp(- alpha*(2*Ri))
                phase = 3
                t+= 2*Ri

            elseif(phase == 3)

                x , y = get_line(t , t+2*(Rorbit-Ri) , E)

                push!(t_vec , x)
                push!(y_vec , y)

                phase = 4
                t+= 2*(Rorbit-Ri)

            elseif(phase == 4)

                x , y = get_line(t , t+ 2*Ri , alpha , E)

                push!(t_vec , x)
                push!(y_vec , y)

                E = E*exp(- alpha*(2*Ri))
                phase = 5
                t+= 2*Ri

            elseif(phase == 5)

                x , y = get_line(t , t + (Ro-Rorbit+ Ri) ,  E)

                push!(t_vec , x)
                push!(y_vec , y)

                phase = 1
                t+= (Ro-Rorbit)

            end
        end
    end

    return t_vec , y_vec

end


using .LazySets: LazySet, support_vector

function ConvexBodyProximityQueries.support(obj::LazySet{N}, dir::SVector{D, N}) where {D, N}
    return support_vector(dir, obj) |> SVector{D, N}
end


#-----------------------------------------------------------------#
# This routine advances the solution in time using Midpoint RK2
# Written by F.X. Giraldo / P.R. Mugg on 9/19/2019
#           Department of Applied Mathematics
#           Naval Postgraduate School
#           Monterey, CA 93943-5216
#------------------------------------------------------------------#

function rk2(q0,c,Δt,time)

    qi = q0

    #Evaluate at Beginning of Interval
    (rhs,f,g) = rhs_function(q0,c,time)

    #update to Midpoint of Interval
    qi = q0 + (Δt/2)*rhs

    #update to End of Interval
    (rhs,f,g) = rhs_function(qi,c,time+(Δt/2))
    qi = q0 + Δt*rhs

    #update
    return qp = qi
end


include("adjoint_lsssolve.jl")
"""
Implements the NILSAS algorithm due to Ni and Talnikar 2019.
"Adjoint sensitivity analysis on chaotic dynamical
systems by Non-Intrusive Least Squares Adjoint
Shadowing (NILSAS)"

Below, u -> F(u) is the time-1 primal map and u_trj refers
to a primal trajectory of length n.
Inputs:
    du_trj: dxdxn n-length timeseries of Jacobian matrices
    		along u_trj
    X: dxn n-length timeseries of perturbation vectors.
    		X[:,i] = dF/ds(u_trj[i,:]) which is in the
    		tangent space of u_trj[i+1,:].
    f: dxn n-length timeseries of the neutral CLV. By
    		assumption, the only neutral CLV is the RHS of
    		the primal ode: du/dt = f(u,s).
    		In case of maps, u_n = F(u_{n+1}), f is zeros.
    J:mxn n-length timeseries of m scalar functions J(u)
    		evaluated along u_trj.
    dJ: mxdxn n-length timeseries of DJ(u) evaluated along
    		u_trj
    s: array of parameters
    d_u: integer dimension of the unstable subspace. You can 		set a guess value to obtain correct sensitivities
    	as long as the guess value is greater than or equal
    	to the true d_u.
Outputs:
    vsh: dxn n-length timeseries of shadowing tangent
    	vectors that are bounded solutions of the
    	conventional tangent equation:
    		v[:,i+1] = du_trj[:,i]*v[:,i] + X[:,i]

    dJds: scalar-valued sensitivity of interest:
    	d/ds sum(J) ≈ d/ds <J, mu>, where mu is
    	the SRB measure.
"""
function lss(du_trj, X, f, s, d_u, g)
    d, n = size(X)
    lyap_exps = zeros(d_u)
    R = zeros(d_u,d_u,n)
    Q = zeros(d,d_u,n)
    v = zeros(d,1,n) #assume one parameter
    v[:,1,1] = zeros(d)
    A = qr!(rand(d,d_u))
    Q[:,:,1] = Array(A.Q)
    R[:,:,1] = A.R

    b = zeros(d_u,1,n) #1 is the # parameters
    pf_Q = zeros(d_u,1,n) #1 is the # parameters
    pf_v = zeros(n)

    ff = zeros(n)
    if all(isapprox.(f,0.))
    	ff = ones(n)
    else
    	[ff[i] = sum(x -> x^2, f[:,i]) for i = 1:n]
    end
    pf_Q[:,1,1] = Q[:,:,1]'*g[:,1]
    [Q[:,j,1] = Q[:,j,1] - (pf_Q[j,1,1]*
     		f[:,1]/ff[1]) for j=1:d_u]

    for i=2:n
        v[:,:,i] = du_trj[:,:,i-1]*v[:,:,i-1] +
    				X[:,i-1]
    	Q[:,:,i] = du_trj[:,:,i-1]*Q[:,:,i-1]
    	pf_Q[:,:,i] = Q[:,:,i]'*g[:,i]
    	#[Q[:,j,i] = Q[:,j,i] - pf_Q[j,1,i]*f[:,i]/
    	#				ff[i] for j=1:d_u]
    	A = qr!(Q[:,:,i])
        Q[:,:,i] = Array(A.Q)
        R[:,:,i] = A.R
    	pf_v[i] = dot(v[:,1,i],g[:,i])
    	v[:,:,i] = v[:,:,i] - (pf_v[i]/ff[i]*f[:,i])
        b[:,:,i] = (Q[:,:,i]')*v[:,:,i]
    	v[:,:,i] = v[:,:,i] - Q[:,:,i]*b[:,:,i]
	lyap_exps .+= log.(abs.(diag(R[:,:,i])))./n
    end

    b = reshape(collect(b),d_u, n)
    a = lsssolve(R,b,sum(pf_v),pf_Q[:,1,:])
    v = reshape(collect(v),d,n)
    vsh = zeros(d, n)
    xi = zeros(n)
    vsh[:,1] = Q[:,:,1]*reshape(a[:,1], d_u, 1) +
    			  v[:,1]
    for i = 2:n
    	vsh[:,i] = v[:,i] + Q[:,:,i]*reshape(a[:,i],d_u,1)
    end
    return vsh, xi
end
function compute_sens(vsh, xi, dJ, f)
    m, d, n = size(dJ)
    dJds = zeros(m)
    for i = 1:n
    	dJds .= dJds .+ dJ[:,:,i]*vsh[:,i]
    end
    return dJds
end


"""
Struct of mesh information

"""
struct Mesh{
    A,
    B<:AbstractMatrix{<:AbstractFloat},
    C<:AbstractMatrix{<:Integer},
    D<:AbstractVector{<:Integer},
    E<:AbstractVector{<:AbstractFloat},
    F<:AbstractArray{<:AbstractFloat,3},
}
    cells::A # all information: cell, line, vertex
    points::B # locations of vertices

    cellid::C # node indices of elements
    cellType::D # inner/boundary cell
    cellNeighbors::C # neighboring cells id
    cellFaces::C # cell edges id
    cellCenter::B # cell center location
    cellArea::E # cell size
    cellNormals::F # unit normal vectors of cell

    facePoints::C # point ids affiliated to face
    faceCells::C # ids of neighbor cells
    faceCenter::B # center location of face
    faceType::D
    faceArea::E # face area
end

function Mesh(file::T) where T<:AbstractString
    cells, points = read_mesh(file)
    p = mesh_connectivity_2D(cells, points)

    return Mesh((cells, points)..., p...)
end

"""
    struct Cells{T1,T2}
        type::T1
        index::T2
    end

Cell connectivity information
"""
struct Cells{T1,T2}
    type::T1
    index::T2
end

"""
    extract_cell(cells::Cells)

Extract cell id list from struct
"""
function extract_cell(cells::Cells)
    for i in eachindex(cells.type)
        if (cells.type[i] in ["triangle", "quad"])
            return cells.index[i]
        end
    end
end


include("opfdata.jl")
using JuMP
using Ipopt

function acopf_solve(opfmodel, opf_data)

  #
  # Initial point - needed especially for pegase cases
  #
  Pg0,Qg0,Vm0,Va0 = acopf_initialPt_IPOPT(opf_data)
  setvalue(getvariable(opfmodel, :Pg), Pg0)
  setvalue(getvariable(opfmodel, :Qg), Qg0)
  setvalue(getvariable(opfmodel, :Vm), Vm0)
  setvalue(getvariable(opfmodel, :Va), Va0)

  status = solve(opfmodel)

  if status != :Optimal
    println("Could not solve the model to optimality.")
  end
  return opfmodel,status
end

function acopf_model(opf_data)
  #shortcuts for compactness
  lines = opf_data.lines; buses = opf_data.buses; generators = opf_data.generators; baseMVA = opf_data.baseMVA
  busIdx = opf_data.BusIdx; FromLines = opf_data.FromLines; ToLines = opf_data.ToLines; BusGeners = opf_data.BusGenerators;

  nbus  = length(buses); nline = length(lines); ngen  = length(generators)

  #branch admitances
  YffR,YffI,YttR,YttI,YftR,YftI,YtfR,YtfI,YshR,YshI = computeAdmitances(lines, buses, baseMVA)

  #
  # JuMP model now
  #
  opfmodel = Model(solver=IpoptSolver(print_level=0))

  @variable(opfmodel, generators[i].Pmin <= Pg[i=1:ngen] <= generators[i].Pmax)
  @variable(opfmodel, generators[i].Qmin <= Qg[i=1:ngen] <= generators[i].Qmax)

  @variable(opfmodel, buses[i].Vmin <= Vm[i=1:nbus] <= buses[i].Vmax)
  @variable(opfmodel, Va[1:nbus])
  #fix the voltage angle at the reference bus
  setlowerbound(Va[opf_data.bus_ref], buses[opf_data.bus_ref].Va)
  setupperbound(Va[opf_data.bus_ref], buses[opf_data.bus_ref].Va)

  # minimize active power
#  @NLobjective(opfmodel,
#		  Min,
#		  sum{ generators[i].coeff[generators[i].n] +
#		       sum{generators[i].coeff[generators[i].n-k]*(baseMVA*Pg[i])^k, k=1:generators[i].n-1},
#		       i=1:ngen}
#		 )

  @NLobjective(opfmodel, Min, sum{ generators[i].coeff[generators[i].n-2]*(baseMVA*Pg[i])^2
			             +generators[i].coeff[generators[i].n-1]*(baseMVA*Pg[i])
				     +generators[i].coeff[generators[i].n  ], i=1:ngen})

  #
  # power flow balance
  #
  for b in 1:nbus
    #real part
    @NLconstraint(
      opfmodel,
      ( sum{ YffR[l], l in FromLines[b]} + sum{ YttR[l], l in ToLines[b]} + YshR[b] ) * Vm[b]^2
      + sum{ Vm[b]*Vm[busIdx[lines[l].to]]  *( YftR[l]*cos(Va[b]-Va[busIdx[lines[l].to]]  ) + YftI[l]*sin(Va[b]-Va[busIdx[lines[l].to]]  )), l in FromLines[b] }
      + sum{ Vm[b]*Vm[busIdx[lines[l].from]]*( YtfR[l]*cos(Va[b]-Va[busIdx[lines[l].from]]) + YtfI[l]*sin(Va[b]-Va[busIdx[lines[l].from]])), l in ToLines[b]   }
      - ( sum{baseMVA*Pg[g], g in BusGeners[b]} - buses[b].Pd ) / baseMVA      # Sbus part
      ==0)
#  end
#  for b in 1:nbus
    #imaginary part
    @NLconstraint(
      opfmodel,
      ( sum{-YffI[l], l in FromLines[b]} + sum{-YttI[l], l in ToLines[b]} - YshI[b] ) * Vm[b]^2
      + sum{ Vm[b]*Vm[busIdx[lines[l].to]]  *(-YftI[l]*cos(Va[b]-Va[busIdx[lines[l].to]]  ) + YftR[l]*sin(Va[b]-Va[busIdx[lines[l].to]]  )), l in FromLines[b] }
      + sum{ Vm[b]*Vm[busIdx[lines[l].from]]*(-YtfI[l]*cos(Va[b]-Va[busIdx[lines[l].from]]) + YtfR[l]*sin(Va[b]-Va[busIdx[lines[l].from]])), l in ToLines[b]   }
      - ( sum{baseMVA*Qg[g], g in BusGeners[b]} - buses[b].Qd ) / baseMVA      #Sbus part
      ==0)
  end
  #
  # branch/lines flow limits
  #
  nlinelim=0
  for l in 1:nline
    if lines[l].rateA!=0 && lines[l].rateA<1.0e10
      nlinelim += 1
      flowmax=(lines[l].rateA/baseMVA)^2

      #branch apparent power limits (from bus)
      Yff_abs2=YffR[l]^2+YffI[l]^2; Yft_abs2=YftR[l]^2+YftI[l]^2
      Yre=YffR[l]*YftR[l]+YffI[l]*YftI[l]; Yim=-YffR[l]*YftI[l]+YffI[l]*YftR[l]
      @NLconstraint(
        opfmodel,
	Vm[busIdx[lines[l].from]]^2 *
	( Yff_abs2*Vm[busIdx[lines[l].from]]^2 + Yft_abs2*Vm[busIdx[lines[l].to]]^2
	  + 2*Vm[busIdx[lines[l].from]]*Vm[busIdx[lines[l].to]]*(Yre*cos(Va[busIdx[lines[l].from]]-Va[busIdx[lines[l].to]])-Yim*sin(Va[busIdx[lines[l].from]]-Va[busIdx[lines[l].to]]))
	)
        - flowmax <=0)
#    end
#  end
#  for l in 1:nline
#    if lines[l].rateA!=0 && lines[l].rateA<1.0e10
#
#      flowmax=(lines[l].rateA/baseMVA)^2

      #branch apparent power limits (to bus)
      Ytf_abs2=YtfR[l]^2+YtfI[l]^2; Ytt_abs2=YttR[l]^2+YttI[l]^2
      Yre=YtfR[l]*YttR[l]+YtfI[l]*YttI[l]; Yim=-YtfR[l]*YttI[l]+YtfI[l]*YttR[l]
      @NLconstraint(
        opfmodel,
	Vm[busIdx[lines[l].to]]^2 *
        ( Ytf_abs2*Vm[busIdx[lines[l].from]]^2 + Ytt_abs2*Vm[busIdx[lines[l].to]]^2
          + 2*Vm[busIdx[lines[l].from]]*Vm[busIdx[lines[l].to]]*(Yre*cos(Va[busIdx[lines[l].from]]-Va[busIdx[lines[l].to]])-Yim*sin(Va[busIdx[lines[l].from]]-Va[busIdx[lines[l].to]]))
        )
        - flowmax <=0)
    end
  end

  @printf("Buses: %d  Lines: %d  Generators: %d\n", nbus, nline, ngen)
  println("Lines with limits  ", nlinelim)

  return opfmodel
end

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

  #values = zeros(2*nbus+2*ngen)
  ## values[1:2*nbus+2*ngen] = readdlm("/sandbox/petra/work/installs/matpower5.1/vars2.txt")
  #values[1:2*nbus+2*ngen] = readdlm("/sandbox/petra/work/installs/matpower5.1/vars3_9241.txt")
  #d = JuMP.NLPEvaluator(opfmodel)
  #MathProgBase.initialize(d, [:Jac])

  #g = zeros(2*nbus+2*nlinelim)
  #MathProgBase.eval_g(d, g, values)
  #println("f=", MathProgBase.eval_f(d,values))

  #gmat=zeros(2*nbus+2*nlinelim)
  #gmat[1:end] = readdlm("/sandbox/petra/work/installs/matpower5.1/cons3_9241.txt")
  #println("diff: ", norm(gmat-g))

  #println(opfmodel)

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

function acopf_outputAll(opfmodel, opf_data)
  #shortcuts for compactness
  lines = opf_data.lines; buses = opf_data.buses; generators = opf_data.generators; baseMVA = opf_data.baseMVA
  busIdx = opf_data.BusIdx; FromLines = opf_data.FromLines; ToLines = opf_data.ToLines; BusGeners = opf_data.BusGenerators;

  nbus  = length(buses); nline = length(lines); ngen  = length(generators)

  # OUTPUTING
  println("Objective value: ", getobjectivevalue(opfmodel), "USD/hr")
  VM=getvalue(getvariable(opfmodel,:Vm)); VA=getvalue(getvariable(opfmodel,:Va))
  PG=getvalue(getvariable(opfmodel,:Pg)); QG=getvalue(getvariable(opfmodel,:Qg))

  println("============================= BUSES ==================================")
  println("  BUS    Vm     Va   |   Pg (MW)    Qg(MVAr) ")   # |    P (MW)     Q (MVAr)")  #|         (load)   ")

  println("                     |     (generation)      ")
  println("----------------------------------------------------------------------")
  for i in 1:nbus
    @printf("%4d | %6.2f  %6.2f | %s  | \n",
	    buses[i].bus_i, VM[i], VA[i]*180/pi,
	    length(BusGeners[i])==0?"   --          --  ":@sprintf("%7.2f     %7.2f", baseMVA*PG[BusGeners[i][1]], baseMVA*QG[BusGeners[i][1]]))
  end
  println("\n")

  within=20 # percentage close to the limits

  nflowlim=0
  for l in 1:nline
    if lines[l].rateA!=0 && lines[l].rateA<1.0e10
      nflowlim += 1
    end
  end

  if nflowlim>0
    println("Number of lines with flow limits: ", nflowlim)

    optvec=zeros(2*nbus+2*ngen)
    optvec[1:ngen]=PG
    optvec[ngen+1:2*ngen]=QG
    optvec[2*ngen+1:2*ngen+nbus]=VM
    optvec[2*ngen+nbus+1:2*ngen+2*nbus]=VA

    d = JuMP.NLPEvaluator(opfmodel)
    MathProgBase.initialize(d, [:Jac])

    consRhs = zeros(2*nbus+2*nflowlim)
    MathProgBase.eval_g(d, consRhs, optvec)

    #println(consRhs)

    @printf("================ Lines within %d %s of flow capacity ===================\n", within, "\%")
    println("Line   From Bus    To Bus    At capacity")

    nlim=1
    for l in 1:nline
      if lines[l].rateA!=0 && lines[l].rateA<1.0e10
        flowmax=(lines[l].rateA/baseMVA)^2
        idx = 2*nbus+nlim

        if( (consRhs[idx]+flowmax)  >= (1-within/100)^2*flowmax )
          @printf("%3d      %3d      %3d        %5.3f%s\n", l, lines[l].from, lines[l].to, 100*sqrt((consRhs[idx]+flowmax)/flowmax), "\%" )
          #@printf("%7.4f   %7.4f    %7.4f \n", consRhs[idx], consRhs[idx]+flowmax,  flowmax)
        end
        nlim += 1
      end
    end
  end

  #println(getvalue(Vm))
  #println(getvalue(Va)*180/pi)

  #println(getvalue(Pg))
  #println(getvalue(Qg))

  return
end

# Compute initial point for IPOPT based on the values provided in the case data
function acopf_initialPt_IPOPT(opfdata)
  Pg=zeros(length(opfdata.generators)); Qg=zeros(length(opfdata.generators)); i=1
  for g in opfdata.generators
    # set the power levels in in between the bounds as suggested by matpower
    # (case data also contains initial values in .Pg and .Qg - not used with IPOPT)
    Pg[i]=0.5*(g.Pmax+g.Pmin)
    Qg[i]=0.5*(g.Qmax+g.Qmin)
    i=i+1
  end
  @assert i-1==length(opfdata.generators)

  Vm=zeros(length(opfdata.buses)); i=1;
  for b in opfdata.buses
    # set the ini val for voltage magnitude in between the bounds
    # (case data contains initials values in Vm - not used with IPOPT)
    Vm[i]=0.5*(b.Vmax+b.Vmin);
    i=i+1
  end
  @assert i-1==length(opfdata.buses)

  # set all angles to the angle of the reference bus
  Va = opfdata.buses[opfdata.bus_ref].Va * ones(length(opfdata.buses))

  return Pg,Qg,Vm,Va
end


# Tensor product polynomials, arbitrary domain

function chebyshev_weights(f::AbstractArray{T,1},nodes_1::Array{T,1},order::Array{S,1},domain=[ones(1);-ones(1)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain)

  polynomial_1 = chebyshev_polynomial(order[1],x1)

  weights = Array{T}(order[1]+1)

  for i = 1:order[1]+1

    numerator   = zero(T)
    denominator = zero(T)

    for s1 = 1:length(nodes_1)

      numerator   += f[s1]*polynomial_1[s1,i]
      denominator += (polynomial_1[s1,i])^2

    end

    weights[i] = numerator/denominator

  end

  return weights

end

function chebyshev_weights(f::AbstractArray{T,2},nodes_1::Array{T,1},nodes_2::Array{T,1},order::Array{S,1},domain=[ones(1,2);-ones(1,2)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain[:,1])
  x2 = normalize_node(nodes_2,domain[:,2])

  polynomial_1 = chebyshev_polynomial(order[1],x1)
  polynomial_2 = chebyshev_polynomial(order[2],x2)

  weights = Array{T}(order[1]+1,order[2]+1)

  for j = 1:order[2]+1
    for i = 1:order[1]+1

      numerator   = zero(T)
      denominator = zero(T)

      for s2 = 1:length(nodes_2)
        for s1 = 1:length(nodes_1)

          numerator   += f[s1,s2]*polynomial_1[s1,i]*polynomial_2[s2,j]
          denominator += (polynomial_1[s1,i]*polynomial_2[s2,j])^2

        end
      end

      weights[i,j] = numerator/denominator

    end
  end

  return weights

end

function chebyshev_weights(f::AbstractArray{T,3},nodes_1::Array{T,1},nodes_2::Array{T,1},nodes_3::Array{T,1},order::Array{S,1},domain=[ones(1,3);-ones(1,3)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain[:,1])
  x2 = normalize_node(nodes_2,domain[:,2])
  x3 = normalize_node(nodes_3,domain[:,3])

  polynomial_1 = chebyshev_polynomial(order[1],x1)
  polynomial_2 = chebyshev_polynomial(order[2],x2)
  polynomial_3 = chebyshev_polynomial(order[3],x3)

  weights = Array{T}(order[1]+1,order[2]+1,order[3]+1)

  for k = 1:order[3]+1
    for j = 1:order[2]+1
      for i = 1:order[1]+1

        numerator   = zero(T)
        denominator = zero(T)

        for s3 = 1:length(nodes_3)
          for s2 = 1:length(nodes_2)
            for s1 = 1:length(nodes_1)

              numerator   += f[s1,s2,s3]*polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]
              denominator += (polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k])^(2)

            end
          end
        end

        weights[i,j,k] = numerator/denominator

      end
    end
  end

  return weights

end

function chebyshev_weights(f::AbstractArray{T,4},nodes_1::Array{T,1},nodes_2::Array{T,1},nodes_3::Array{T,1},nodes_4::Array{T,1},order::Array{S,1},domain=[ones(1,4);-ones(1,4)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain[:,1])
  x2 = normalize_node(nodes_2,domain[:,2])
  x3 = normalize_node(nodes_3,domain[:,3])
  x4 = normalize_node(nodes_4,domain[:,4])

  polynomial_1 = chebyshev_polynomial(order[1],x1)
  polynomial_2 = chebyshev_polynomial(order[2],x2)
  polynomial_3 = chebyshev_polynomial(order[3],x3)
  polynomial_4 = chebyshev_polynomial(order[4],x4)

  weights = Array{T}(order[1]+1,order[2]+1,order[3]+1,order[4]+1)

  for l = 1:order[4]+1
    for k = 1:order[3]+1
      for j = 1:order[2]+1
        for i = 1:order[1]+1

          numerator   = zero(T)
          denominator = zero(T)

          for s4 = 1:length(nodes_4)
            for s3 = 1:length(nodes_3)
              for s2 = 1:length(nodes_2)
                for s1 = 1:length(nodes_1)

                  numerator   += f[s1,s2,s3,s4]*polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l]
                  denominator += (polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l])^(2.0)

                end
              end
            end
				  end

          weights[i,j,k,l] = numerator/denominator

        end
      end
    end
	 end

   return weights

end

function chebyshev_weights(f::AbstractArray{T,5},nodes_1::Array{T,1},nodes_2::Array{T,1},nodes_3::Array{T,1},nodes_4::Array{T,1},nodes_5::Array{T,1},order::Array{S,1},domain=[ones(1,5);-ones(1,5)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain[:,1])
  x2 = normalize_node(nodes_2,domain[:,2])
  x3 = normalize_node(nodes_3,domain[:,3])
  x4 = normalize_node(nodes_4,domain[:,4])
  x5 = normalize_node(nodes_5,domain[:,5])

  polynomial_1 = chebyshev_polynomial(order[1],x1)
  polynomial_2 = chebyshev_polynomial(order[2],x2)
  polynomial_3 = chebyshev_polynomial(order[3],x3)
  polynomial_4 = chebyshev_polynomial(order[4],x4)
  polynomial_5 = chebyshev_polynomial(order[5],x5)

  weights = Array{T}(order[1]+1,order[2]+1,order[3]+1,order[4]+1,order[5]+1)

	for m = 1:order[5]+1
	  for l = 1:order[4]+1
      for k = 1:order[3]+1
        for j = 1:order[2]+1
          for i = 1:order[1]+1

            numerator   = zero(T)
            denominator = zero(T)

            for s5 = 1:length(nodes_5)
              for s4 = 1:length(nodes_4)
                for s3 = 1:length(nodes_3)
                  for s2 = 1:length(nodes_2)
                    for s1 = 1:length(nodes_1)

                      numerator   += f[s1,s2,s3,s4,s5]*polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l]*polynomial_5[s5,m]
                      denominator += (polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l]*polynomial_5[s5,m])^(2.0)

                    end
                  end
                end
              end
						end

            weights[i,j,k,l,m] = numerator/denominator

          end
        end
      end
    end
	end

  return weights

end

function chebyshev_weights(f::AbstractArray{T,6},nodes_1::Array{T,1},nodes_2::Array{T,1},nodes_3::Array{T,1},nodes_4::Array{T,1},nodes_5::Array{T,1},nodes_6::Array{T,1},order::Array{S,1},domain=[ones(1,6);-ones(1,6)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain[:,1])
  x2 = normalize_node(nodes_2,domain[:,2])
  x3 = normalize_node(nodes_3,domain[:,3])
  x4 = normalize_node(nodes_4,domain[:,4])
  x5 = normalize_node(nodes_5,domain[:,5])
  x6 = normalize_node(nodes_6,domain[:,6])

  polynomial_1 = chebyshev_polynomial(order[1],x1)
  polynomial_2 = chebyshev_polynomial(order[2],x2)
  polynomial_3 = chebyshev_polynomial(order[3],x3)
  polynomial_4 = chebyshev_polynomial(order[4],x4)
  polynomial_5 = chebyshev_polynomial(order[5],x5)
  polynomial_6 = chebyshev_polynomial(order[6],x6)

  weights = Array{T}(order[1]+1,order[2]+1,order[3]+1,order[4]+1,order[5]+1,order[6]+1)

	for n = 1:order[6]+1
	  for m = 1:order[5]+1
	    for l = 1:order[4]+1
        for k = 1:order[3]+1
          for j = 1:order[2]+1
            for i = 1:order[1]+1

              numerator   = zero(T)
              denominator = zero(T)

              for s6 = 1:length(nodes_6)
                for s5 = 1:length(nodes_5)
                  for s4 = 1:length(nodes_4)
                    for s3 = 1:length(nodes_3)
                      for s2 = 1:length(nodes_2)
                        for s1 = 1:length(nodes_1)

                          numerator   += f[s1,s2,s3,s4,s5,s6]*polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l]*polynomial_5[s5,m]*polynomial_6[s6,n]
                          denominator += (polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l]*polynomial_5[s5,m]*polynomial_6[s6,n])^(2.0)

                        end
                      end
                    end
                  end
                end
						  end

              weights[i,j,k,l,m,n] = numerator/denominator

            end
          end
        end
      end
    end
	end

  return weights

end

# Complete polynomials, arbitrary domain

function chebyshev_weights(f::AbstractArray{T,1},nodes_1::Array{T,1},order::S,domain=[ones(1);-ones(1)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain)

  polynomial_1 = chebyshev_polynomial(order,x1)

  weights = Array{T}(order+1)

  for i = 1:order+1

    numerator   = zero(T)
    denominator = zero(T)

    for s1 = 1:length(nodes_1)

      numerator   += f[s1]*polynomial_1[s1,i]
      denominator += (polynomial_1[s1,i])^2

    end

    weights[i] = numerator/denominator

  end

  return weights

end

function chebyshev_weights(f::AbstractArray{T,2},nodes_1::Array{T,1},nodes_2::Array{T,1},order::S,domain=[ones(1,2);-ones(1,2)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain[:,1])
  x2 = normalize_node(nodes_2,domain[:,2])

  polynomial_1 = chebyshev_polynomial(order,x1)
  polynomial_2 = chebyshev_polynomial(order,x2)

  weights = zeros(order+1,order+1)

  for j = 1:order+1
    for i = 1:order+1

      numerator   = zero(T)
      denominator = zero(T)

      if (i+j <= order+2)

        for s2 = 1:length(nodes_2)
          for s1 = 1:length(nodes_1)

            numerator   += f[s1,s2]*polynomial_1[s1,i]*polynomial_2[s2,j]
            denominator += (polynomial_1[s1,i]*polynomial_2[s2,j])^(2)

          end
        end

        weights[i,j] = numerator/denominator

      end

    end
  end

  return weights

end

function chebyshev_weights(f::AbstractArray{T,3},nodes_1::Array{T,1},nodes_2::Array{T,1},nodes_3::Array{T,1},order::S,domain=[ones(1,3);-ones(1,3)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain[:,1])
  x2 = normalize_node(nodes_2,domain[:,2])
  x3 = normalize_node(nodes_3,domain[:,3])

  polynomial_1 = chebyshev_polynomial(order,x1)
  polynomial_2 = chebyshev_polynomial(order,x2)
  polynomial_3 = chebyshev_polynomial(order,x3)

  weights = zeros(order+1,order+1,order+1)

  for k = 1:order+1
    for j = 1:order+1
      for i = 1:order+1

        numerator   = zero(T)
        denominator = zero(T)

        if (i+j+k <= order+3)

          for s3 = 1:length(nodes_3)
            for s2 = 1:length(nodes_2)
              for s1 = 1:length(nodes_1)

                numerator   += f[s1,s2,s3]*polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]
                denominator += (polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k])^2

              end
            end
          end

          weights[i,j,k] = numerator/denominator

        end

      end
    end
  end

  return weights

end

function chebyshev_weights(f::AbstractArray{T,4},nodes_1::Array{T,1},nodes_2::Array{T,1},nodes_3::Array{T,1},nodes_4::Array{T,1},order::S,domain=[ones(1,4);-ones(1,4)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain[:,1])
  x2 = normalize_node(nodes_2,domain[:,2])
  x3 = normalize_node(nodes_3,domain[:,3])
  x4 = normalize_node(nodes_4,domain[:,4])

  polynomial_1 = chebyshev_polynomial(order,x1)
  polynomial_2 = chebyshev_polynomial(order,x2)
  polynomial_3 = chebyshev_polynomial(order,x3)
  polynomial_4 = chebyshev_polynomial(order,x4)

  weights = zeros(order+1,order+1,order+1,order+1)

  for l = 1:order+1
    for k = 1:order+1
      for j = 1:order+1
        for i = 1:order+1

          numerator   = zero(T)
          denominator = zero(T)

          if (i+j+k+l <= order+4)

            for s4 = 1:length(nodes_4)
              for s3 = 1:length(nodes_3)
                for s2 = 1:length(nodes_2)
                  for s1 = 1:length(nodes_1)

                    numerator   += f[s1,s2,s3,s4]*polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l]
                    denominator += (polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l])^2

                  end
                end
              end
            end

            weights[i,j,k,l] = numerator/denominator

          end

        end
      end
    end
  end

  return weights

end

function chebyshev_weights(f::AbstractArray{T,5},nodes_1::Array{T,1},nodes_2::Array{T,1},nodes_3::Array{T,1},nodes_4::Array{T,1},nodes_5::Array{T,1},order::S,domain=[ones(1,5);-ones(1,5)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain[:,1])
  x2 = normalize_node(nodes_2,domain[:,2])
  x3 = normalize_node(nodes_3,domain[:,3])
  x4 = normalize_node(nodes_4,domain[:,4])
  x5 = normalize_node(nodes_5,domain[:,5])

  polynomial_1 = chebyshev_polynomial(order,x1)
  polynomial_2 = chebyshev_polynomial(order,x2)
  polynomial_3 = chebyshev_polynomial(order,x3)
  polynomial_4 = chebyshev_polynomial(order,x4)
  polynomial_5 = chebyshev_polynomial(order,x5)

  weights = zeros(order+1,order+1,order+1,order+1,order+1)

  for m = 1:order+1
    for l = 1:order+1
      for k = 1:order+1
        for j = 1:order+1
          for i = 1:order+1

            numerator   = zero(T)
            denominator = zero(T)

            if (i+j+k+l+m <= order+5)

              for s5 = 1:length(nodes_5)
                for s4 = 1:length(nodes_4)
                  for s3 = 1:length(nodes_3)
                    for s2 = 1:length(nodes_2)
                      for s1 = 1:length(nodes_1)

                        numerator   += f[s1,s2,s3,s4,s5]*polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l]*polynomial_5[s5,m]
                        denominator += (polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l]*polynomial_5[s5,m])^2

                      end
                    end
                  end
                end
              end

              weights[i,j,k,l,m] = numerator/denominator

            end

          end
        end
      end
    end
  end

  return weights

end

function chebyshev_weights(f::AbstractArray{T,6},nodes_1::Array{T,1},nodes_2::Array{T,1},nodes_3::Array{T,1},nodes_4::Array{T,1},nodes_5::Array{T,1},nodes_6::Array{T,1},order::S,domain=[ones(1,6);-ones(1,6)]) where {T<:AbstractFloat,S<:Integer}

  x1 = normalize_node(nodes_1,domain[:,1])
  x2 = normalize_node(nodes_2,domain[:,2])
  x3 = normalize_node(nodes_3,domain[:,3])
  x4 = normalize_node(nodes_4,domain[:,4])
  x5 = normalize_node(nodes_5,domain[:,5])
  x6 = normalize_node(nodes_6,domain[:,6])

  polynomial_1 = chebyshev_polynomial(order,x1)
  polynomial_2 = chebyshev_polynomial(order,x2)
  polynomial_3 = chebyshev_polynomial(order,x3)
  polynomial_4 = chebyshev_polynomial(order,x4)
  polynomial_5 = chebyshev_polynomial(order,x5)
  polynomial_6 = chebyshev_polynomial(order,x6)

  weights = zeros(order+1,order+1,order+1,order+1,order+1,order+1)

  for n = 1:order+1
    for m = 1:order+1
      for l = 1:order+1
        for k = 1:order+1
          for j = 1:order+1
            for i = 1:order+1

              numerator   = zero(T)
              denominator = zero(T)

              if (i+j+k+l+m+n <= order+6)

                for s6 = 1:length(nodes_6)
                  for s5 = 1:length(nodes_5)
                    for s4 = 1:length(nodes_4)
                      for s3 = 1:length(nodes_3)
                        for s2 = 1:length(nodes_2)
                          for s1 = 1:length(nodes_1)

                            numerator   += f[s1,s2,s3,s4,s5,s6]*polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l]*polynomial_5[s5,m]*polynomial_6[s6,n]
                            denominator += (polynomial_1[s1,i]*polynomial_2[s2,j]*polynomial_3[s3,k]*polynomial_4[s4,l]*polynomial_5[s5,m]*polynomial_6[s6,n])^2.0

                          end
                        end
                      end
                    end
                  end
                end

                weights[i,j,k,l,m,n] = numerator/denominator

              end

            end
          end
        end
      end
    end
  end

  return weights

end


#=
Copyright 2017 INSIGNEO Institute for in silico Medicine

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

   http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=#

#

#

# In a conjunction the left hand side vessel is called *parent* vessel and
# the right hand side vessel is called *daughter* vessel.
#
# The conjunction is solved by imposing the conservation of mass and
# __*total*__
# pressure at the interface node $j$. Two additional relations are obtained
# by extrapolating the outgoing characteristics from the two vessels. From
# parent vessel outlet we have
# $$
#   W_1 = u_1 + 4c_1,
# $$
# and for the daughter vessel inlet we have
# $$
#   W_2 = u_2 - 4c_2.
# $$
# The mass conservation reads $A_1u_1 - A_2u_2 = 0$, and the total pressure
# conservation requires $P_{t1} = P_{t2}$. By defining the unknown vector $U$
# as
# $$
#   U = \{U_i\} = \left\{ \begin{array}{c}
#                           u_1 \\
#                           u_2 \\
#                           A_1^{1/4} \\
#                           A_2^{1/4}
#                        \end{array} \right\} , \quad i =1, ..., 4,
# $$
# the four relations read
# $$
#   F = \left\{f_i \right\} =
#   \begin{cases}
#     U_1 + 4k_1U_3 - W_1^* = 0, \\
#     U_2 - 4k_2U_4 - W_2^* = 0, \\
#     U_1U_3^4 - U_2U_4^4 = 0, \\
#     \beta_1 \left(\tfrac{U_3^2}{A_{01}^{1/2}} -1 \right) +
#     \frac{1}{2}\rho U_1^2 -
#     \beta_2 \left(\tfrac{U_4^2}{A_{02}^{1/2}} -1 \right) -
#     \frac{1}{2}\rho U_2^2 = 0,
#   \end{cases}
# $$
# where $c_i = k_i A_i^{1/4}$, $k_i = \sqrt{3/2 \gamma_i}$. $F(U)=0$ is solved
# iteratively with Newton's method [^1]
# $$
#   \begin{cases}
#     J \cdot \delta U = - F(U), \\
#     U^{new} = U + \delta U,
#   \end{cases}
# $$
# where $J$ is the Jacobian
# $$
#   J = \left[ \begin{array}{cccc}
#         1 & 0 & 4k_1 & 0 \\
#         0 & 1 & 0 & -4k_2 \\
#         U_3^4 & -U_4^4 & 4U_1 U_3^3 & -4 U_2 U_4^3 \\
#         \rho U_1 & -\rho U_2 & 2 \beta_1 U_3/A_{01}^{1/2} &
#           -2\beta_2 U_4/A_{02}^{1/2}
#       \end{array} \right].
# $$
# The entire process is handled by
# [`solveConjunction`](conjunctions.html#solveConjunction) function.

# *function* __`solveConjunction`__
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------------- --------------------------------------------------------
# `b`                 `::Blood` model matrix row.
#
# `v1`                `::Vessel` data structure for the parent vessel.
#
# `v2`                `::Vessel` data structure for the daughter vessel.
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# Firstly, the unknown vector $U$ and parameters vector $k$ are initialised
# with data from parent and daughter vessels.
# ----------------------------------------------------------------------------
#
function solveConjunction(b :: Blood, v1 :: Vessel, v2 :: Vessel)

  U = [v1.u[end],
       v2.u[ 1 ],
       sqrt(sqrt(v1.A[end])),
       sqrt(sqrt(v2.A[ 1 ]))]

  k1 = sqrt(0.5*3*v1.gamma[end])
  k2 = sqrt(0.5*3*v2.gamma[ 1 ])
  k  = [k1, k2]
  # Riemann invariants are initialised by
  # [`calculateWstarConj`](conjunctions.html#calculateWstarConj), the
  # Jacobian is calculated by
  # [`calculateJacobianConj`](conjunctions.html#calculateJacobianConj), and
  # the $F$ vector is calculated by
  # [`calculateFofUconj`](conjunctions.html#calculateFofUconj), and
  W = calculateWstarConj(U, k)
  J = calculateJacobianConj(b, v1, v2, U, k)
  F = calculateFofUconj(b, v1, v2, U, k, W)
  # Newton-Raphson method iterates until a tolerance on the error is met. The
  # tolerance is defined on $U$ and $F$.
  nr_toll_U = 1.e-5
  nr_toll_F = 1.e-5
  # At the beginning of each iteration, $\delta U$ is calculated by means of
  # LU decomposition by julia's command `\`. $U$ is updated with an iterative
  # step of 0.01.
  while true
    dU = J\(-F)
    # U_new = U + 0.01*dU
    U_new = U + dU
    # In the case the solution diverges, $F$ will contain `nan` elements. In
    # this case `openBF` exits the simulation and returns an error.
    if any(isnan(dot(F,F)))
      println(F)
      break
    end
    # When all the elements in $\delta U$ and $F$ meet the set tolerances, the
    # solver stops the iterative loop.
    u_ok = 0
    f_ok = 0
    for i in 1:length(dU)
      if abs(dU[i]) <= nr_toll_U || abs(F[i]) <= nr_toll_F
        u_ok += 1
        f_ok += 1
      end
    end

    if u_ok == length(dU) || f_ok == length(dU)
      U = U_new
      break
    # If tolerances are not met, new $U$, $W$, and $F$ vectors are computed
    # and a new iteration starts.
    else
      U = U_new
      W = calculateWstarConj(U, k)
      F = calculateFofUconj(b, v1, v2, U, k, W)
    end
  end

  # Once the solution is found, quantities at both sides of the interface are
  # updated.
  v1.u[end] = U[1]
  v2.u[ 1 ] = U[2]

  v1.A[end] = U[3]*U[3]*U[3]*U[3]
  v1.Q[end] = v1.u[end]*v1.A[end]

  v2.A[ 1 ] = U[4]^4
  v2.Q[ 1 ] = v2.u[1]*v2.A[1]

  v1.P[end] = pressure(v1.A[end], v1.A0[end], v1.beta[end], v1.Pext)
  v2.P[ 1 ] = pressure(v2.A[ 1 ], v2.A0[ 1 ], v2.beta[ 1 ], v2.Pext)

  v1.c[end] = waveSpeed(v1.A[end], v1.gamma[end])
  v2.c[ 1 ] = waveSpeed(v2.A[ 1 ], v2.gamma[ 1 ])
end

# *function* __`calculateWstarConj`__ $\rightarrow$
# `W::Array{Float, 1}`
#
# ----------------------------------------------------------------------------
# Parameters:
# -------------- -------------------------------------------------------------
# `U`            `::Array` junction unknown vector.
#
# `k`            `::Array` junction parameters vector.
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# Riemann invariants are computed starting from unknown and parameters
# vectors as
# $$
#   W_1 = u_1 + 4c_1, \quad W_2 = u_2 - 4c_2.
# $$
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Returns:
# ----------- ----------------------------------------------------------------
# `W`         `::Array` containing outgoing characteristics at both sides
#             of the conjunction.
# ----------------------------------------------------------------------------
#
function calculateWstarConj(U :: Array, k :: Array)

  W1 = U[1] + 4*k[1] * U[3]
  W2 = U[2] - 4*k[2] * U[4]

  return [W1, W2]
end

# *function* __`calculateFofUconj`__ $\rightarrow$ `F::Array`
#
# ----------------------------------------------------------------------------
# Parameters:
# -------------- -------------------------------------------------------------
# `b`            `::Blood` data structure.
#
# `v1`           `::Vessel` parent vessel data structure.
#
# `v2`           `::Vessel` daughter vessel data structure.
#
# `U`            `::Array` junction unknown vector.
#
# `k`            `::Array` junction parameters vector.
#
# `W`            `::Array` outgoing characteristics array.
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# $F$ array is computed by imposing the conservation of mass and total
# pressure. $F$ reads
# $$
#   F = \left\{f_i \right\} =
#   \begin{cases}
#     U_1 + 4k_1U_3 - W_1^* = 0, \\
#     U_2 - 4k_2U_4 - W_2^* = 0, \\
#     U_1U_3^4 - U_2U_4^4 = 0, \\
#     \beta_1 \left(\tfrac{U_3^2}{A_{01}^{1/2}} -1 \right) +
#     \frac{1}{2}\rho U_1^2 -
#     \beta_2 \left(\tfrac{U_4^2}{A_{02}^{1/2}} -1 \right) -
#     \frac{1}{2}\rho U_2^2 = 0,
#   \end{cases}
# $$
# ----------------------------------------------------------------------------
# Static pressure conservation can be imposed by replacing `f4` with
#
#     f4 = v1.beta*( U[3] ^2 /sqrt(v1.A0) - 1) -
#        ( v2.beta*((U[4])^2 /sqrt(v2.A0) - 1) )
#
# ----------------------------------------------------------------------------
# Returns:
# ----------- ----------------------------------------------------------------
# `F`         `::Array` Newton's method relations.
# ----------------------------------------------------------------------------
#
function calculateFofUconj(b :: Blood, v1 :: Vessel, v2 :: Vessel,
                           U :: Array, k :: Array, W :: Array)

  f1 = U[1] + 4*k[1]*U[3] - W[1]

  f2 = U[2] - 4*k[2]*U[4] - W[2]

  f3 = U[1]*(U[3]*U[3]*U[3]*U[3]) - U[2]*(U[4]*U[4]*U[4]*U[4])

  f4 = 0.5*b.rho*U[1]*U[1] + v1.beta[end]*(U[3]*U[3]/sqrt(v1.A0[end]) - 1) -
     ( 0.5*b.rho*U[2]*U[2] + v2.beta[ 1 ]*(U[4]*U[4]/sqrt(v2.A0[ 1 ]) - 1) )

  return [f1, f2, f3, f4]
end

# *function* __`calculateJacobianConj`__ $\rightarrow$
# `W::Array{Float, 1}`
#
# ----------------------------------------------------------------------------
# Parameters:
# -------------- -------------------------------------------------------------
# `b`            `::Blood` data structure.
#
# `v1`           `::Vessel` parent vessel data structure.
#
# `v2`           `::Vessel` daughter vessel data structure.
#
# `U`            `::Array` junction unknown vector.
#
# `k`            `::Array` junction parameters vector.
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# The Jacobian is computed as
# $$
#   J = \left[ \begin{array}{cccc}
#         1 & 0 & 4k_1 & 0 \\
#         0 & 1 & 0 & -4k_2 \\
#         U_3^4 & -U_4^4 & 4U_1 U_3^4 & -4 U_2 U_4^4 \\
#         \rho U_1 & -\rho U_2 & 2 \beta_1 U_3/A_{01}^{1/2} &
#           -2\beta_2 U_4/A_{02}^{1/2}
#       \end{array} \right].
# $$
# ----------------------------------------------------------------------------
#
# The conservation of static pressure is imposed by setting to zero elements
# $J_{4,1}$ and $J_{4,2}$.
#
# ----------------------------------------------------------------------------
# Returns:
# ----------- ----------------------------------------------------------------
# `J`         `::Array{Float, 2}` Jacobian matrix.
# ----------------------------------------------------------------------------
#
function calculateJacobianConj(b :: Blood, v1 :: Vessel, v2 :: Vessel,
                               U :: Array, k :: Array)

  J = eye(4)

  J[1,3] =  4*k[1]
  J[2,4] = -4*k[2]

  J[3,1] =  U[3]*U[3]*U[3]*U[3]
  J[3,2] = -U[4]*U[4]*U[4]*U[4]
  J[3,3] =  4*U[1]*(U[3]*U[3]*U[3]^3)
  J[3,4] = -4*U[2]*(U[4]*U[4]*U[4]^3)

  J[4,3] =  2*v1.beta[end]*U[3]/sqrt(v1.A0[end])
  J[4,4] = -2*v2.beta[ 1 ]*U[4]/sqrt(v2.A0[ 1 ])

  J[4,1] =  b.rho*U[1]
  J[4,2] = -b.rho*U[2]

  return J
end

# ### References
#
# [^1]: Press, William H. Numerical recipes 3rd edition:
# The art of scientific computing. Cambridge university press, 2007.


abstract GradientBoosting

type GradientBoostingClassifier <: GradientBoosting
    n_clf::Int64
    learning_rate::Float64
    max_depth::Int64
    min_sample_split::Int64
    min_gain::Float64
    init_estimate::Union{String, Real, Vector}
    trees::Array{ClassificationTree}
end

type GradientBoostingRegressor <: GradientBoosting
    n_clf::Int64
    learning_rate::Float64
    max_depth::Int64
    min_sample_split::Int64
    min_gain::Float64
    init_estimate::Union{String, Real, Vector}
    trees::Array{RegressionTree}
end

function GradientBoostingRegressor(;
                      n_clf::Int64 = 10,
                      learning_rate::Float64 = 0.5,
                      min_sample_split::Int64 = 20,
                      min_gain::Float64 = 1e-4,
                      max_depth = 4,
                      init_estimate::Union{String, Real, Vector} = "nothing"
                      )
    trees = []
    for i = 1:n_clf
        push!(trees, RegressionTree(min_samples_split = min_sample_split,
            min_gain = min_gain, max_depth = max_depth))
    end
    return GradientBoostingRegressor(n_clf, learning_rate,
                        max_depth, min_sample_split,
                        min_gain, init_estimate, trees)
end

function GradientBoostingClassifier(;
                      n_clf::Int64 = 10,
                      learning_rate::Float64 = 0.5,
                      min_sample_split::Int64 = 20,
                      min_gain::Float64 = 1e-4,
                      max_depth = 4,
                      init_estimate::Union{String, Real, Vector} = "nothing"
                      )
    trees = []
    for i = 1:n_clf
        push!(trees, ClassificationTree(min_samples_split = min_sample_split,
            min_gain = min_gain, max_depth = max_depth))
    end
    return GradientBoostingClassifier(n_clf, learning_rate,
                        max_depth, min_sample_split,
                        min_gain, init_estimate, trees)
end

function train!(model::GradientBoosting, X::Matrix, y::Vector)
    if typeof(model) <: GradientBoostingClassifier
        y = one_hot(y)
    end
    n_sample, n_feature = size(X)
    y_pred = mean(y, 1)
    if typeof(model) <: GradientBoostingClassifier
        y_pred = repmat(y_pred,n_sample,1)
    else
        y_pred = y_pred * ones(n_sample)
    end
    count = 1
    for tree in model.trees
        residual = -(y - y_pred)
        train!(tree, X, residual)
        residual_pred = predict(tree,X)
        y_pred -= model.learning_rate * residual_pred
        #println("$(count)th tree trained!")
        count += 1
    end
end

function predict(model::GradientBoosting,
                 x::Matrix)
    count = 1
    y_pred = 0
    for tree in model.trees
        if count == 1
            y_pred = - model.learning_rate * predict(tree,x)
            count = count + 1
        else
            y_pred -= model.learning_rate * predict(tree, x)
        end
    end

    if typeof(model) <: GradientBoostingClassifier
        return sign(softmax(y_pred)-1.5)
    end
    return y_pred

end

function test_GradientBoostingRegressor()
    X_train, X_test, y_train, y_test = make_reg(n_features = 1)
    model = GradientBoostingRegressor()
    train!(model,X_train, y_train)
    predictions = predict(model,X_test)
    print("regression msea", mean_squared_error(y_test, predictions))
    PyPlot.scatter(X_test, y_test, color = "black")
    PyPlot.scatter(X_test, predictions, color = "green")
    legend(loc="upper right",fancybox="true")
end

function test_GradientBoostingClassifier()
    X_train, X_test, y_train, y_test = make_cla()
    model = GradientBoostingClassifier()
    train!(model,X_train, y_train)
    predictions = predict(model,X_test)
    println("classification accuracy: ", accuracy(y_test, predictions))
end


include("../base.jl")
import Random

f(x, y) = (y/5.5)+x^2

println("hola")
println("hola")
x = collect(-5:0.5:5)
y = collect(0:0.5:10)
nvalues = length(x)
objs = Array{Number}(undef, nvalues)
for i=1:nvalues
    objs[i] = f(x[i], y[i])
end

setIndividualType(GEPGenotype)
setRandomSeed(34)
setGPExperimentInfo()
setStopCondition(maxIterations=50)
setEvaluator([FitnessFunction(compareFunctions, objs, x, y, weight=-1)])
setGenerator(rampedHalfHalfGenerator, popSize = 50, generateOneByOne = false)
setSelector(tournamentSelector, 4)
setCrossoverOperator(kPointRecombinationCross, 2, probability=0.9)
setMutationOperator(geneMutation, probability=0.2)
setReplacementOperator(replaceAllPopulation, eliteSize=5)
setExperimentSummary(batchSize=5, printDuringExperiment=true)

@time runGenJ(verbose=false)


# ==============================================================================
# Max-Plus Algebra toolbox for Julia >= 1.0.3
# In the way of the ScicosLab max-plus toolbox.
# ==============================================================================

module MaxPlus

using
    LinearAlgebra, SparseArrays, Printf

# ==============================================================================
# Max-Plus core

export
    MP, SpaMP, SpvMP, ArrMP, VecMP,
    mpzero, mpone, mp0, mp1, mptop,
    mpI, mpeye, mpzeros, mpones,
    mpsparse, full, dense, array,
    plustimes, minplus,
    mptrace, mpnorm, mpstar,
    mp_change_display, LaTeX

# ==============================================================================
# Max-Plus Linear system

export
    MPSysLin,
    mpsyslin, mpsimul, mpexplicit

# ==============================================================================
# Max-Plus flowhop

export
    mpgraph,
    flowshop, LaTeXG, flowshop_graph, flowshop_simu

# ==============================================================================

"""
    MP{T}

Max-Plus immutable structure. Promote a number of type T (ideally a Float64) to
a max-plus number.

# Examples
```julia-repl
julia> a=MP(3.0); typeof(a)
MP{Float64}

julia> a=MP(3); typeof(a)
MP{Int64}
```
"""
struct MP{T <: Real} <: Real
    λ::T
end

# ==============================================================================
# Type alias but with shorter number of characters

const Sparse{T,U} = SparseMatrixCSC{T,U}
const SpaMP{T,U}  = SparseMatrixCSC{MP{T},U}
const SpvMP{T,U}  = SparseVector{MP{T},U}
const ArrMP{T,N}  = Array{MP{T},N}
const VecMP{T,N}  = Vector{MP{T}}

# ==============================================================================
# Copy constructor

MP(x::MP) = MP(x.λ)

# ==============================================================================

"""
    zero(::MP{T})

Create the max-plus constant zero equal to -Inf.
This value is the neutral for the ⨁ operator.
"""
Base.zero(::Type{MP{T}}) where T = MP(typemin(T))
Base.zero(x::MP{T})      where T = zero(typeof(x))

"""
    mpzero(::Type{T})

Create the max-plus constant zero equal to -Inf.
This value is the neutral for the ⨁ operator.
"""
mpzero(::Type{T})        where T = MP(typemin(T))

# ==============================================================================

"""
    one(::MP{T})

Create the max-plus constant one equal to 0.
This value is the neutral for operators ⨁ and ⨂.
"""
Base.one(::Type{MP{T}})  where T = MP(zero(T))
Base.one(x::MP{T})       where T = one(typeof(x))

"""
    one(::Type{T})

Create the max-plus constant one equal to 0.
This value is the neutral for operators ⨁ and ⨂.
"""
mpone(::Type{T})         where T = MP(zero(T))

# ==============================================================================

"""
    mp0

Max-plus constant zero equal to -Inf.
This value is the neutral for the ⨁ operator.

Equivalent to ScicosLab code: `%0 = #(-%inf) = .`

# Examples
```julia-repl
julia> mp0 * 5
-Inf

julia> mp0 + 5
5.0
```
"""
const global mp0 = mpzero(Float64)

# ==============================================================================

"""
    mp1

Max-plus constant one equal to 0.
This value is the neutral for operators ⨁ and ⨂.

Equivalent to ScicosLab code: `%1 = #(1) = 0`

# Examples
```julia-repl
julia> mp1 * 5
5.0

julia> mp1 + 5
5.0
```
"""
const global mp1 = mpone(Float64)

# ==============================================================================

"""
    mptop

Min-plus constant "top" equal to +Inf.

Equivalent to ScicosLab code: `%top = #(%inf)`
"""
const global mptop = MP{Float64}(Inf)

# ==============================================================================
# Conversions

Base.promote_rule(::Type{MP{T}}, ::Type{U}) where {T, U} = MP{T}
Base.convert(::MP{T}, x::Number)            where T = MP{T}(x)

# ==============================================================================

"""
    MP(x::UnitRange)

Create a max-plus dense vector.

# Examples
```julia-repl
julia> MP(1:3)
3-element Array{MP{Int64},1}:
 1
 2
 3
```
"""
MP(x::UnitRange{T}) where T = MP(Vector{T}(x))

# ==============================================================================

"""
    MP(A::Array)

Convert a dense array to a max-plus dense array.

# Examples
```julia-repl
julia> MP([1.0 -Inf; 0.0 4])
2×2 Array{MP{Float64},2}:
 1.0  -Inf
 0.0  4.0
```
"""
MP(A::Array) = map(MP, A)

# ==============================================================================

"""
    MP(A::SparseArray)

Convert a sparse array to a max-plus sparse array.
By default values are max-plus zeros values are preserved else if
parameter `preserve` is set then

# Examples
```julia-repl
julia> MP(sparse([1, 2, 3], [1, 2, 3], [-Inf, 2, 0]))
  [1, 1]  =  -Inf
  [2, 2]  =  2.0
  [3, 3]  =  0.0

julia> MP(sparse([1, 2, 3], [1, 2, 3], [-Inf, 2, 0]), preserve=false)
  [2, 2]  =  2.0
```
"""
function MP(S::Sparse{T,U}; preserve=true) where {T, U}
    if preserve
        convert(SpaMP{T,U}, S)
    else
        M = spzeros(MP{T}, size(S,1), size(S,2))
        for c = 1:size(S, 2), r = nzrange(S, c)
            if (S[r,c] != zero(T)) && (MP(S[r,c]) != mpzero(T))
                M[r,c] = convert(MP{T}, S[r,c])
            end
        end
        M
    end
end

# ==============================================================================

"""
    MP(A::SparseVector)

Convert a sparse vector to a max-plus sparse vector.
By default values are max-plus zeros values are preserved else if
parameter `preserve` is set then

# Examples
```julia-repl
julia> MP(sparse([1.0, 0.0, 1.0]))
3-element SparseVector{MP{Float64},Int64} with 2 stored entries:
  [1]  =  1.0
  [3]  =  1.0
```
"""
function MP(V::SparseVector{T,U}; preserve=true) where {T, U}
    if preserve
        convert(SparseVector{MP{T},U}, V)
    else
        S = spzeros(MP{T}, size(V,1), size(V,2))
        for c = V.n
            if (S[c] != zero(T)) && (MP(S[c]) != mpzero(T))
                M[c] = convert(MP{T}, S[c])
            end
        end
        S
    end
end

# ==============================================================================

global mpstyle = 1 # Style ScicosLab

"""
    mp_change_display(style::Int)

Change the style of display for the Base.show(io::IO, x::MP) function.
See help of show(io::IO,x::MP) for examples.
"""
mp_change_display(style::Int) = (global mpstyle = min(max(style, 0), 4))

"""
    show(io::IO,x::MP)

Display a max-plus number depending on the currently set style:
- -Inf are displayed either with 'ε' (style 2 or 3) or '.' symbols (style 1).
- 0 are displayed either with 'e' (style 3) or '0' symbols (style 1 or 2).
- else -Inf and 0 are displayed in Julia default sytle (style 0).
By default the ScicosLab style is used (style 1).

# Examples
```julia-repl
julia> mp_change_display(0); mpeye(Float64, 2,2)
2×2 Array{MP{Float64},2}:
  0.0  -Inf
 -Inf   0.0

julia> mp_change_display(0); mpeye(Int64, 2,2)
2×2 Array{MP{Int64},2}:
                    0  -9223372036854775808
 -9223372036854775808                     0

julia> mp_change_display(1); mpeye(Float64, 2,2) # or mpeye(Int64, 2,2)
2×2 Array{MP{Float64},2}:
 0  .
 .  0

julia> mp_change_display(2); mpeye(Float64, 2,2) # or mpeye(Int64, 2,2)
2×2 Array{MP{Float64},2}:
 e  .
 .  e

julia> mp_change_display(3); mpeye(Float64, 2,2) # or mpeye(Int64, 2,2)
2×2 Array{MP{Float64},2}:
 0  ε
 ε  0

julia> mp_change_display(4); mpeye(Float64, 2,2) # or mpeye(Int64, 2,2)
2×2 Array{MP{Float64},2}:
 e  ε
 ε  e
```
"""
function Base.show(io::IO, x::MP{T}) where T
    if mpstyle == 0
        show(io, x.λ)
    elseif x == mpzero(T)
        (mpstyle == 1 || mpstyle == 2) ? (@printf io ".") : (@printf io "ε")
    elseif x == mpone(T)
        (mpstyle == 1 || mpstyle == 3) ? (@printf io "0") : (@printf io "e")
    elseif x.λ == trunc(x.λ)
        (@printf io "%d" x.λ)
    else
        show(io, x.λ)
    end
end

# ==============================================================================
# FIXME: not a column-major traversal
"""
    LaTeX(io::IO, A::Array{MP})

Convert a max-plus dense matrix to a LaTeX formula. The display depends on
mp_change_display(style).

# Examples
```julia-repl
julia> LaTeX(stdout, MP([4 3; 7 -Inf]))
\\left[
\\begin{array}{*{20}c}
4 & 3 \\\\
7 & . \\\\
\\end{array}
\\right]
```
"""
function LaTeX(io::IO, A::ArrMP{T}) where T
    (@printf io "\\left[\n\\begin{array}{*{20}c}\n")
    for i in 1:size(A,1)
        for j in 1:size(A,2)
            if A[i,j] == mpzero(T)
                if mpstyle == 0
                    (@printf io "-\\infty")
                elseif mpstyle == 3 || mpstyle == 4
                    (@printf io "\\varepsilon")
                else
                    (@printf io ".")
                end
            elseif A[i,j] == mpone(T)
                if mpstyle == 2 || mpstyle == 4
                    (@printf io "e")
                else
                    (@printf io "%d" A[i,j].λ)
                end
            elseif A[i,j].λ == trunc(A[i,j].λ)
                (@printf io "%d" A[i,j].λ)
            else
                show(io, A[i,j].λ)
            end
            if j < size(A, 2)
                (@printf io " & ")
            end
        end
        (@printf io " \\\\\n")
    end
    (@printf io "\\end{array}\n\\right]\n")
end

LaTeX(io::IO, S::SpaMP{T}) where {T,U} = LaTeX(io, full(S))

# ==============================================================================

"""
    +(x::MP, y::MP)

Max operator ⨁. Return the maximum of `x` and `y`.

# Examples
```julia-repl
julia> MP(1.0) + MP(3.0)
MP{Float64}(3.0)
```
"""
Base.:(+)(x::MP,   y::MP)   = MP(max(x.λ, y.λ))
Base.:(+)(x::MP,   y::Real) = MP(max(x.λ, y))
Base.:(+)(x::Real, y::MP)   = MP(max(x,   y.λ))

# ==============================================================================

"""
    *(x::MP, y::MP)

Addition operator ⨂. Return the summation of `x` and `y`.

# Examples
```julia-repl
julia> MP(1.0) * MP(3.0)
MP{Float64}(4.0)
```
"""
Base.:(*)(x::MP,   y::MP)   = MP(x.λ + y.λ)
Base.:(*)(x::MP,   y::Real) = MP(x.λ + y)
Base.:(*)(x::Real, y::MP)   = MP(x   + y.λ)

# ==============================================================================
# These functions fix Julia bugs

@inline Base.literal_pow(::typeof(^), x::MP, ::Val{0}) = one(x)
@inline Base.literal_pow(::typeof(^), x::MP, ::Val{p}) where {p} = MP(x.λ * p)
@inline Base.:(^)(x::MP, y::Number) = MP(x.λ * y)
@inline Base.literal_pow(::typeof(^), A::ArrMP{T}, ::Val{p}) where {T, p} = A^p
@inline Base.abs2(x::MP) = x.λ + x.λ

# ==============================================================================

"""
    /(x::MP, y::MP)

Divisor operator. Return the difference between `x` and `y`.

# Examples
```julia-repl
julia> MP(1.0) / MP(2.0)
MP{Float64}(-1.0)
```
"""
Base.:(/)(x::MP,   y::MP)   = MP(x.λ - y.λ)
Base.:(/)(x::MP,   y::Real) = MP(x.λ - b)
Base.:(/)(x::Real, y::MP)   = MP(a   - y.λ)

# ==============================================================================

"""
    -(x::MP, y::MP)

Minus operator (not used for max-plus but for min-plus).
Return the difference between `x` and `y`.

# Examples
```julia-repl
julia> MP(1.0) - MP(2.0)
MP{Float64}(-1.0)
```
"""
Base.:(-)(x::MP,   y::MP)   = MP(x.λ - y.λ)
Base.:(-)(x::MP,   y::Real) = MP(x.λ - y)
Base.:(-)(x::Real, y::MP)   = MP(x   - y.λ)

# ==============================================================================

"""
    min(x::MP, y::MP)

Return the minimun of `x` and `y`.

# Examples
```julia-repl
julia> min(MP(1), -3)
MP{Int64}(-3)

julia> min(MP([10 1; 10 1]), MP([4 5; 6 5]))
2×2 Array{MP{Int64},2}:
 4  1
 6  1
```
"""
Base.:min(x::MP,   y::MP)   = MP(min(x.λ, y.λ))
Base.:min(x::MP,   y::Real) = MP(min(x.λ, y))
Base.:min(x::Real, y::MP)   = MP(min(x,   y.λ))
Base.:min(A::ArrMP, B::ArrMP) = map(Base.:min, A, B)
Base.:min(A::SpaMP, B::SpaMP) = map(Base.:min, A, B)

# ==============================================================================

Base.:(==)(x::MP,   y::MP)   = (x.λ == y.λ)
Base.:(==)(x::MP,   y::Real) = (x.λ == y)
Base.:(==)(x::Real, y::MP)   = (x   == y.λ)

Base.:(<=)(x::MP,   y::MP)   = (x.λ <= y.λ)
Base.:(<=)(x::MP,   y::Real) = (x.λ <= y)
Base.:(<=)(x::Real, y::MP)   = (x   <= y.λ)

Base.:(<)(x::MP,   y::MP)   = (x.λ < y.λ)
Base.:(<)(x::MP,   y::Real) = (x.λ < y)
Base.:(<)(x::Real, y::MP)   = (x   < y.λ)

Base.isless(x::MP,   y::MP)   = x.λ < y.λ
Base.isless(x::MP,   y::Real) = x.λ < y
Base.isless(x::Real, y::MP)   = x < y.λ

# ==============================================================================

"""
    mpeye(::Type{T}, n::Int64)

Construct a max-plus identity n-by-n matrix.

# Examples
```julia-repl
julia> mpeye(Float64, 2)
2×2 Array{MP{Float64},2}:
  0.0  -Inf
 -Inf   0.0
```
"""
mpeye(::Type{T}, n::Int64) where T = Matrix{MP{T}}(mpI, n, n)

# ==============================================================================

"""
    mpeye(::Type{T}, m::Int64, n::Int64)

Construct a max-plus identity m-by-n matrix.

# Examples
```julia-repl
julia> mpeye(Float64, 2, 2)
2×2 Array{MP{Float64},2}:
  0.0  -Inf
 -Inf   0.0
```
"""
mpeye(::Type{T}, m::Int64, n::Int64) where T = Matrix{MP{T}}(mpI, m, n)

# ==============================================================================

"""
    mpzeros(::Type{T}, n::Int64)

Construct a max-plus zero n-by-m sparse matrix.

# Examples
```julia-repl
julia> mpzeros(Float64, 2)
2-element SparseVector{MP{Float64},Int64} with 0 stored entries
```
"""
mpzeros(::Type{T}, n::Int64) where T = spzeros(MP{T}, n)

# ==============================================================================

"""
    mpzeros(::Type{T}, n::Int64)

Construct a sparse max-plus zero m-by-n matrix.

# Examples
```julia-repl
julia> mpzeros(Float64, 2,5)
2×5 SparseMatrixCSC{MP{Float64},Int64} with 0 stored entries
```
"""
mpzeros(::Type{T}, m::Int64, n::Int64) where T = spzeros(MP{T}, m, n)

"""
    mpfzeros(::Type{T}, n::Int64)

Construct a dense max-plus zero m-by-n matrix.

# Examples
```julia-repl
julia> mpfzeros(Float64, 2,2)
2×2 Array{MP{Float64},2}:
 -Inf  -Inf
 -Inf  -Inf
```
"""
mpfzeros(::Type{T}, m::Int64, n::Int64) where T = full(mpzeros(T, m, n))

# ==============================================================================

"""
    mpzeros(x::UnitRange{T})

Construct a max-plus zero vector.
# Examples
```julia-repl
julia> mpzeros(1:3)
3-element Array{MP{Int64},1}:
 -Inf
 -Inf
 -Inf
```
"""
mpzeros(x::UnitRange{T}) where {T} = fill!(MP(similar(x)), mpzero(T))

# ==============================================================================

"""
    mpones(::Type{T}, n::Int64)

Construct a max-plus one n-by-n matrix.

# Examples
```julia-repl
julia> mpones(Float64, 2)
2-element Array{MP{Float64},1}:
 0.0
 0.0
```
"""
mpones(::Type{T}, n::Int64) where T = ones(MP{T}, n)

# ==============================================================================

"""
    mpones(::Type{T}, m::Int64, n::Int64)

Construct a max-plus one m-by-n matrix.

# Examples
```julia-repl
julia> mpones(Float64, 2,2)
2×2 Array{MP{Float64},2}:
 0.0  0.0
 0.0  0.0
```
"""
mpones(::Type{T}, m::Int64, n::Int64) where T = ones(MP{T}, m, n)

# ==============================================================================

"""
    mpones(x::UnitRange{T})

Construct a max-plus one vector.
# Examples
```julia-repl
julia> mpones(1:3)
3-element Array{MP{Int64},1}:
 0
 0
 0
```
"""
mpones(x::UnitRange{T}) where {T} = fill!(MP(similar(x)), mpone(T))

# ==============================================================================

"""
    mpsparse(A::Array{T}; preserve=false)

Transform a dense matrix to a max-plus sparse matrix.

# Examples
```julia-repl
julia> mpsparse([4 0; 7 -Inf])
2×2 SparseMatrixCSC{MP{Float64},Int64} with 3 stored entries:
  [1, 1]  =  4.0
  [2, 1]  =  7.0
  [1, 2]  =  0.0
```
"""
mpsparse(A::Array{T}) where T = mpsparse(MP(A))
# ; preserve=false) where T = mpsparse(MP(A), preserve)

"""
    mpsparse(A::ArrMP{T}; preserve=false)

Transform a dense matrix to a max-plus sparse matrix. By default
Remove -Inf values. To keep them set param preserve to true.

# Examples
```julia-repl
julia> mpsparse([4 0; 7 -Inf])
2×2 SparseMatrixCSC{MP{Float64},Int64} with 3 stored entries:
  [1, 1]  =  4.0
  [2, 1]  =  7.0
  [1, 2]  =  0.0

mpsparse([4 0; 7 -Inf], preserve=true)
2×2 SparseMatrixCSC{MP{Float64},Int64} with 3 stored entries:
  [1, 1]  =  4.0
  [2, 1]  =  7.0
  [1, 2]  =  0.0
  [2, 2]  = -Inf
```
"""
function mpsparse(M::ArrMP{T}) where T #; preserve=false) where T
    #if preserve
    #    sparse(M)
    #else
        R = Array{Int64, 1}[];
        C = Array{Int64, 1}[];
        V = Array{MP{T}, 1}[];

        for j = 1:size(M, 2), i=1:size(M, 1)
            M[i,j] != zero(MP{T}) && (R = [R; i]; C = [C; j]; V = [V; M[i,j]])
        end

        sparse(R, C, V)
    #end
end

# ==============================================================================

"""
    array(::ArrMP{T})

Convert a max-plus array to a standard type array.

# Examples
```julia-repl
julia> array([MP(1.0) 2.0; 3.0 4.0])
2×2 Array{Float64,2}:
 1.0  2.0
 3.0  4.0
```
"""
array(A::ArrMP{T}) where T = map(x -> x.λ, A)

# ==============================================================================

"""
    array(::SpaMP{T}})

Convert a sparse max-plus array to a dense non max-plus array.

# Examples
```julia-repl
julia> array(mpzeros(Float64, 2,2))
2×2 SparseMatrixCSC{Float64,Int64} with 4 stored entries:
  [1, 1]  =  -Inf
  [2, 1]  =  -Inf
  [1, 2]  =  -Inf
  [2, 2]  =  -Inf
```
"""
array(S::SpaMP{T,U}) where {T, U} = map(x -> x.λ, S)

# ==============================================================================

SparseArrays.sparse(S::SpaMP{T,U}) where {T, U} = map(x -> x.λ, S)

# ==============================================================================

"""
    full(::SpaMP{T}})

Convert a sparse max-plus array to a dense max-plus array.
Alternative function name: dense.

# Examples
```julia-repl
julia> full(mpzeros(Float64, 2,5))
2×5 Array{MP{Float64},2}:
 -Inf  -Inf  -Inf  -Inf  -Inf
 -Inf  -Inf  -Inf  -Inf  -Inf
```
"""
full(S::SpaMP{T,U}) where {T, U} = Matrix(S)

# ==============================================================================

"""
    dense(::SpaMP{T}})

Convert a sparse max-plus array to a dense max-plus array.
Alternative function name: full.

# Examples
```julia-repl
julia> dense(mpzeros(Float64, 2,5))
2×5 Array{MP{Float64},2}:
 -Inf  -Inf  -Inf  -Inf  -Inf
 -Inf  -Inf  -Inf  -Inf  -Inf
```
"""
dense(S::SpaMP{T,U}) where {T, U} = Matrix(S)

# ==============================================================================

"""
    plustimes(x::MP{T})

Convert max-plus number to the standard type.
Note: the function name comes from ScicosLab.

# Examples
```julia-repl
julia> typeof(plustimes(MP(1)))
Int64

julia> plustimes([MP(1.0) 2.0; 3.0 4.0])
2×2 Array{Float64,2}:
 1.0  2.0
 3.0  4.0
```
"""
plustimes(n::MP{T})      where T      = n.λ
plustimes(A::ArrMP{T})   where T      = array(A)
plustimes(S::SpaMP{T,U}) where {T, U} = array(S)

# ==============================================================================
# Conversion to min-plus algebra. Convert +Inf and -Inf

"""
    minplus(A::Array)

Conversion to min-plus algebra. Convert +Inf and -Inf

# Examples
```julia-repl
julia> A = MP([0 3 Inf 1; 1 2 2 -Inf; -Inf Inf 1 0])
minplus(A);
```
"""
minplus(n::MP{T})      where T      = map(x -> (x.λ == mp0) ? mptop : ((x.λ == mptop) ? mp0 : x), n)
minplus(A::ArrMP{T})   where T      = map(x -> minplus(x), A)
minplus(S::SpaMP{T,U}) where {T, U} = map(x -> minplus(x), S)

# ==============================================================================

"""
    mptrace(A::Array)

Compute the trace of the matrix (summation of diagonal elements).

# Examples
```julia-repl
julia> mptrace([MP(1) 2; 3 4])
4
```
"""
mptrace(A::ArrMP{T})    where T = isempty(A) ? mp0 : sum(diag(A))
mptrace(A::Array{T})    where T = isempty(A) ? mp0 : sum(MP(diag(A)))
mptrace(S::SpaMP{T,U})  where {T, U} = isempty(S) ? mp0 : sum(diag(S))
mptrace(S::Sparse{T,U}) where {T, U} = sum(MP(diag(S)))

# ==============================================================================

"""
    mpnorm(A)

Compute the norm of the full or sparce matrix A.
Return the largest entry minus smallest entry of A.

# Examples
```
julia-repl
julia> A = MP([1 20 2;30 400 4;4 50 10]);
S = mpsparse(A);
[mpnorm(A) mpnorm(S)]
1×2 Array{MP{Int64},2}:
 399  399
```
"""
mpnorm(A::Array{T}) where T = A[argmax(A)].λ - A[argmin(A)].λ
mpnorm(S::SpaMP{T}) where T = S[argmax(S)].λ - S[argmin(S)].λ

# ==============================================================================
function hstar(B::Array{T}) where T
    n = size(B, 1)
    if n != size(B, 2) error("Matrix shall be squared") end
    C = B
    for k in 1:n
        t = (C[k,k] <= zero(T)) ? zero(T) : typemax(T);
        for j in 1:n, i in 1:n
            C[i,j] = max(C[i,j], C[i,k] + C[k,j] + t)
        end
    end

    for k in 1:n
        C[k,k] = max(C[k,k], zero(T));
    end
    C
end

"""
    mpstar(A::Array{T})

TODO
"""
mpstar(x::MP{T})    where T = mpstar(plustimes(x))
mpstar(x::T)        where T = MP(hstar([x]))[1,1]
mpstar(A::Array{T}) where T = MP(hstar(A))
mpstar(A::ArrMP{T}) where T = MP(hstar(plustimes(A)))
#mpstar(S::Sparse{T,U}) where {T, U} = MP(hstar(S))
#mpstar(S::SpaMP{T,U}) where {T, U} = MP(hstar(plustimes(S)))

# ==============================================================================
# TODO insertion out of bounds => Sparse:  d=MP([1.0 2; 3 4]); d[5,5] = MP(6.0)

# ==============================================================================
include("juliabugs.jl")
include("howard.jl")
include("syslin.jl")
include("flowshop.jl")

end # MaxPlus module