text:
using Pkg
Pkg.activate("Project.toml")
using Revise # reduce recompilation time
# using Plots
# using Documenter
using LinearAlgebra
using SparseArrays
using BenchmarkTools
using UnPack
using StaticArrays
using DelimitedFiles
using Polyester
using MuladdMacro
using DataFrames
using JLD2
using FileIO

push!(LOAD_PATH, "./src")

using CommonUtils
using Basis2DTri
using UniformTriMesh
using NodesAndModes

using SetupDG

# push!(LOAD_PATH, "./examples/EntropyStableEuler.jl/src")
# include("../EntropyStableEuler.jl/src/logmean.jl")
# include("SBP_quad_data.jl")
# using EntropyStableEuler
# using EntropyStableEuler.Fluxes2D

@muladd begin

function init_reference_tri_sbp_GQ(N, qnode_choice)
    include("SBP_quad_data.jl")
    # initialize a new reference element data struct
    rd = RefElemData()

    fv = tri_face_vertices() # set faces for triangle
    Nfaces = length(fv)
    @pack! rd = fv, Nfaces

    # Construct matrices on reference elements
    r, s = nodes(Tri(),N)
    VDM = vandermonde(Tri(),N, r, s)
    Vr, Vs = grad_vandermonde(Tri(),N, r, s)
    Dr = Vr/VDM
    Ds = Vs/VDM
    @pack! rd = r,s,VDM,Dr,Ds

    # low order interpolation nodes
    r1,s1 = nodes(Tri(),1)
    V1 = vandermonde(Tri(),1,r,s)/vandermonde(Tri(),1,r1,s1)
    @pack! rd = V1

    #Nodes on faces, and face node coordinate
    if qnode_choice == "GQ"
        r1D, w1D = gauss_quad(0,0,N)
    elseif qnode_choice == "GL" || qnode_choice == "tri_diage"
        r1D, w1D = gauss_lobatto_quad(0,0,N+1)
    end
    Nfp = length(r1D) # number of points per face
    e = ones(Nfp) # vector of all ones
    z = zeros(Nfp) # vector of all zeros
    rf = [r1D; -r1D; -e];
    sf = [-e; r1D; -r1D];
    wf = vec(repeat(w1D,3,1));
    nrJ = [z; e; -e]
    nsJ = [-e; e; z]
    @pack! rd = rf,sf,wf,nrJ,nsJ

    if qnode_choice == "GQ"
        rq,sq,wq = GQ_SBP[N];
    elseif qnode_choice == "GL"
        rq,sq,wq = GL_SBP[N];
    elseif qnode_choice == "tri_diage"
        rq,sq,wq = Tri_diage[N];
    end
    # rq,sq,wq = GQ_SBP[N]
    # rq,sq,wq = GL_SBP[N]
    # rq,sq,wq = Tri_diage[N]
    Vq = vandermonde(Tri(),N,rq,sq)/VDM
    M = Vq'*diagm(wq)*Vq
    Pq = M\(Vq'*diagm(wq))
    @pack! rd = rq,sq,wq,Vq,M,Pq

    Vf = vandermonde(Tri(),N,rf,sf)/VDM # interpolates from nodes to face nodes
    LIFT = M\(Vf'*diagm(wf)) # lift matrix used in rhs evaluation
    @pack! rd = Vf,LIFT

    # plotting nodes
    rp, sp = equi_nodes(Tri(),10)
    Vp = vandermonde(Tri(),N,rp,sp)/VDM
    @pack! rd = rp,sp,Vp

    return rd
end

function build_meshfree_sbp(rq,sq,wq,rf,sf,wf,nrJ,nsJ,α)
    # [-1,1,0], [-1,-1,sqrt(4/3)]
    equilateral_map(r,s) = (@. .5*(2*r+1*s+1), @. sqrt(3)*(1+s)/2 - 1/sqrt(3) )
    req,seq = equilateral_map(rq,sq)
    ref,sef = equilateral_map(rf,sf)
    barycentric_coords(r,s) = ((@. (1+r)/2), (@. (1+s)/2), (@. -(r+s)/2))

    Br = diagm(nrJ.*wf)
    Bs = diagm(nsJ.*wf)

    # build extrapolation matrix
    E = zeros(length(rf),length(rq))
    for i = 1:length(rf)
        # d = @. (λ1 - λ1f[i])^2 + (λ2 - λ2f[i])^2 + (λ3 - λ3f[i])^2
        d2 = @. (req-ref[i])^2 + (seq-sef[i])^2
        p = sortperm(d2)
        h2 = (wf[i]/sum(wf))*2/pi # set so that h = radius of circle with area w_i = face weight
        nnbrs = min(4,max(3,count(d2[p] .< h2))) # find 3 closest points
        p = p[1:nnbrs]
        Ei = vandermonde_2D(1,[rf[i]],[sf[i]])/vandermonde_2D(1,rq[p],sq[p])
        E[i,p] = Ei
    end
    E = Matrix(droptol!(sparse(E),1e-13))

    # build stencil
    A = spzeros(length(req),length(req))
    for i = 1:length(req)
        d2 = @. (req-req[i])^2 + (seq-seq[i])^2
        p = sortperm(d2)

        # h^2 = wq[i]/pi = radius of circle with area wq[i]
        # h2 =     (sqrt(3)/sum(wq))*wq[i]/pi
        h2 = α^2*(sqrt(3)/sum(wq))*wq[i]/pi

        nnbrs = count(d2[p] .< h2)
        nbrs = p[1:nnbrs]
        A[i,nbrs] .= one(eltype(A))
    end
    A = (A+A')
    A.nzval .= one(eltype(A)) # bool-ish

    # build graph Laplacian
    L1 = (A-diagm(diag(A))) # ignore
    L1 -= diagm(vec(sum(L1,dims=2)))

    b1r = -sum(.5*E'*Br*E,dims=2)
    b1s = -sum(.5*E'*Bs*E,dims=2)
    ψ1r = pinv(L1)*b1r
    ψ1s = pinv(L1)*b1s

    function fillQ(adj,ψ)
        Np = length(ψ)
        S = zeros(Np,Np)
        for i = 1:Np
            for j = 1:Np
                if adj[i,j] != 0
                        S[i,j] += (ψ[j]-ψ[i])
                end
            end
        end
        return S
    end

    S1r,S1s = fillQ.((A,A),(ψ1r,ψ1s))
    Qr = Matrix(droptol!(sparse(S1r + .5*E'*Br*E),1e-14))
    Qs = Matrix(droptol!(sparse(S1s + .5*E'*Bs*E),1e-14))

    return Qr,Qs,E,Br,Bs,A
end

const LIMITOPT = 2 # 1 if elementwise limiting lij, 2 if elementwise limiting li
const POSDETECT = 0 # 1 if turn on detection, 0 otherwise
const LBOUNDTYPE = 0.1 # 0 if use POSTOL as lower bound, if > 0, use LBOUNDTYPE*loworder
const TOL = 5e-16
const POSTOL = 1e-13
const WALLPT = 1.0/6.0
const Nc = 4 # number of components
"Approximation parameters"
const N = parse(Int,ARGS[1])     # N = 2,3,4
const K1D = parse(Int,ARGS[2])  # K = 2,4,8,16,32
const T = 2.0
const dt0 = 1e+2
const XLENGTH = 2.0
const CFL = 0.5
const NUM_THREADS = Threads.nthreads()
qnode_choice = "GQ" #"GQ" "GL" "tri_diage"

# Initial condition 2D shocktube
const γ = 1.4

# Mesh related variables
Kx = Int(round(XLENGTH*K1D))
Ky = K1D
VX, VY, EToV = uniform_tri_mesh(Kx,Ky)

# @. VX = 15*(1+VX)/2
# @. VY = 5*VY
@. VX = 10*(1+VX)/2*2
@. VY = 5*(1+VY)/2*2
FToF = CommonUtils.connect_mesh(EToV,tri_face_vertices())
Nfaces,K = size(FToF)

# Construct matrices on reference elements
rd = init_reference_tri_sbp_GQ(N, qnode_choice)
@unpack r,s,rf,sf,wf,rq,sq,wq,nrJ,nsJ = rd
@unpack VDM,V1,Vq,Vf,Dr,Ds,M,Pq,LIFT = rd

V = vandermonde_2D(N, r, s)
Vr, Vs = grad_vandermonde_2D(N, r, s)
M = inv(V*V')
Dr_ES = Vr/V; Ds = Vs/V;
M = Vq'*diagm(wq)*Vq

Qr_ES = M*Dr;
Qs_ES = M*Ds;
Pq = M\(Vq'*diagm(wq));

# diff. matrices redefined in terms of quadrature points
Qr_ES = Pq'*Qr_ES*Pq;
Qs_ES = Pq'*Qs_ES*Pq;
E_ES = Vf*Pq;

# Need to choose α so that Qr, Qs have zero row sums (and maybe a minimum number of neighbors)
# α = 4 # for N=1
# α = 2.5 #for N=2
α = 3.5 # for N=3,4
if N == 1
    α = 4
elseif N == 2
    α = 2.5
elseif N == 3 || N == 4
    α = 3.5
end
Qr_ID,Qs_ID,E,Br,Bs,A = build_meshfree_sbp(rq,sq,wq,rf,sf,wf,nrJ,nsJ,α)
if (norm(sum(Qr_ID,dims=2)) > 1e-10) | (norm(sum(Qs_ID,dims=2)) > 1e-10)
    error("Qr_ID or Qs_ID doesn't sum to zero for α = $α")
end
Qr_ID = Matrix(droptol!(sparse(Qr_ID),1e-15))
Qs_ID = Matrix(droptol!(sparse(Qs_ID),1e-15))
Qrskew_ID = .5*(Qr_ID-transpose(Qr_ID))
Qsskew_ID = .5*(Qs_ID-transpose(Qs_ID))

# "Construct global coordinates"
x = V1*VX[transpose(EToV)]
y = V1*VY[transpose(EToV)]

# "Connectivity maps"
xf,yf = (x->Vf*x).((x,y))
mapM,mapP,mapB = build_node_maps((xf,yf),FToF)
mapM = reshape(mapM,length(wf),K)
global mapP = reshape(mapP,length(wf),K)

mapM = reshape(collect(1:K*length(wf)),length(wf),K)
mapP = reshape(collect(1:K*length(wf)),length(wf),K)

for k = 1:K
    Npf = div(length(wf),Nfaces)
    k1 = Int(k+2*(mod(k,2)-.5))
    # k2 = Int(mod1(k+2*3*(mod(k-1,2)-.5),2*Kx)+2*Kx*div(k-1,2*Kx))
    k2 = Int(mod1(k+2*3*(mod(k,2)-.5),2*Kx)+2*Kx*div(k-1,2*Kx))
    k3 = Int(mod1(k+2*(2*Kx-1)*(mod(k-1,2)-.5),K))
    for i = 1:Npf
        mapP[i,k]       = mapM[Npf-i+1,k1]
        mapP[i+Npf,k]   = mapM[2*Npf-i+1,k2]
        mapP[i+2*Npf,k] = mapM[3*Npf-i+1,k3]
    end
end

# "Geometric factors and surface normals"
rxJ, sxJ, ryJ, syJ, J = CommonUtils.geometric_factors(x, y, Dr, Ds)
rxJ = Matrix(droptol!(sparse(rxJ),1e-14)); sxJ = Matrix(droptol!(sparse(sxJ),1e-14))
ryJ = Matrix(droptol!(sparse(ryJ),1e-14)); syJ = Matrix(droptol!(sparse(syJ),1e-14))
nxJ = (Vf*rxJ).*nrJ + (Vf*sxJ).*nsJ;
nyJ = (Vf*ryJ).*nrJ + (Vf*syJ).*nsJ;
sJ = @. sqrt(nxJ^2 + nyJ^2)
nx = nxJ./sJ; ny = nyJ./sJ;
rxJ,sxJ,ryJ,syJ,J = (x->Vq*x).((rxJ,sxJ,ryJ,syJ,J))

# TODO:
J = -J
rxJ = -rxJ
ryJ = -ryJ
sxJ = -sxJ
syJ = -syJ

QNr = [Qr_ES - .5*E_ES'*Br*E_ES .5*E_ES'*Br;
    -.5*Br*E_ES .5*Br];
QNs = [Qs_ES - .5*E_ES'*Bs*E_ES .5*E_ES'*Bs;
    -.5*Bs*E_ES .5*Bs];

VN_sbp = [eye(length(wq));E];
QNr_sbp = VN_sbp'*QNr*VN_sbp;
@show norm(QNr_sbp+QNr_sbp' - E'*diagm(wf.*nrJ)*E)
QNs_sbp = VN_sbp'*QNs*VN_sbp;
@show norm(QNs_sbp+QNs_sbp' - E'*diagm(wf.*nsJ)*E)

Qrskew_ES = .5*(QNr_sbp-QNr_sbp');
Qsskew_ES = .5*(QNs_sbp-QNs_sbp');

Sr = Qrskew_ES
Ss = Qsskew_ES
S0r = Qrskew_ID
S0s = Qsskew_ID
M   = diagm(wq)
Minv   = diagm(1 ./ wq)

Fmask = zeros(Int64,size(Vf,1))
for i = 1:size(Vf,1)
    tmparr = findall(abs.(E[i,:] .- 1) .< 1e-12)
    Fmask[i] = tmparr[1]
end

xq = Vq*x
yq = Vq*y

# gr(aspect_ratio=:equal,legend=false,
#    markerstrokewidth=0,markersize=2)
# VDM = vandermonde(Tri(),N, rq, sq)
# rp,sp = equi_nodes_2D(10)
# Vp = vandermonde_2D(N,rp,sp)/VDM

@inline function pfun(rho,rhou,E)
    return (γ-1)*(E-.5*rhou^2/rho)
end

@inline function pfun(rho,rhou,rhov,E)
    return (γ-1)*(E-.5*(rhou^2+rhov^2)/rho)
end

@inline function Efun(rho,u,v,p)
    return p/(γ-1) + .5*rho*(u^2+v^2)
end

@inline function wavespeed_1D(rho,rhou,E)
    p = pfun(rho,rhou,E)
    return abs(rhou/rho) + sqrt(γ*p/rho)
end

@inline function logmean(aL,aR,logL,logR)

    # "from: Entropy stable num. approx. for the isothermal and polytropic Euler"

    da = aR-aL;
    aavg = .5*(aR+aL);
    f = da/aavg;
    v = f^2;
    if abs(f)<1e-4
        # numbers assume the specific value γ = 1.4
        return aavg*(1 + v*(-.2-v*(.0512 - v*0.026038857142857)))
    else
        return -da/(logL-logR)
    end
end

@inline function euler_fluxes_2D(rhoL,uL,vL,betaL,rhologL,betalogL,
                                 rhoR,uR,vR,betaR,rhologR,betalogR)

    rholog  = logmean(rhoL,rhoR,rhologL,rhologR)
    betalog = logmean(betaL,betaR,betalogL,betalogR)

    # arithmetic avgs
    rhoavg = .5*(rhoL+rhoR)
    uavg   = .5*(uL+uR)
    vavg   = .5*(vL+vR)

    unorm = uL*uR + vL*vR
    pa    = rhoavg/(betaL+betaR)
    f4aux = rholog/(2*(γ-1)*betalog) + pa + .5*rholog*unorm

    FxS1 = rholog*uavg
    FxS2 = FxS1*uavg + pa
    FxS3 = FxS1*vavg
    FxS4 = f4aux*uavg

    FyS1 = rholog*vavg
    FyS2 = FxS3
    FyS3 = FyS1*vavg + pa
    FyS4 = f4aux*vavg

    return FxS1,FxS2,FxS3,FxS4,FyS1,FyS2,FyS3,FyS4
end

@inline function inviscid_flux_prim(rho,u,v,p)
    E = Efun(rho,u,v,p)

    rhou  = rho*u
    rhov  = rho*v
    rhouv = rho*u*v
    Ep    = E+p

    fx1 = rhou
    fx2 = rhou*u+p
    fx3 = rhouv
    fx4 = u*Ep

    fy1 = rhov
    fy2 = rhouv
    fy3 = rhov*v+p
    fy4 = v*Ep

    return fx1,fx2,fx3,fx4,fy1,fy2,fy3,fy4
end

@inline function limiting_param(rhoL,rhouL,rhovL,EL,rhoP,rhouP,rhovP,EP,Lrho,Lrhoe)
    # L - low order, P - P_ij
    l = 1.0
    # Limit density
    if rhoL + rhoP < Lrho
        l = max((Lrho-rhoL)/rhoP, 0.0)
    end

    p = pfun(rhoL+l*rhoP,rhouL+l*rhouP,rhovL+l*rhovP,EL+l*EP)
    if p/(γ-1) > Lrhoe
        return l
    end

    # limiting internal energy (via quadratic function)
    a = rhoP*EP-(rhouP^2+rhovP^2)/2.0
    b = rhoP*EL+rhoL*EP-rhouL*rhouP-rhovL*rhovP-rhoP*Lrhoe
    c = rhoL*EL-(rhouL^2+rhovL^2)/2.0-rhoL*Lrhoe

    d = 1.0/(2.0*a)
    e = b^2-4.0*a*c
    g = -b*d

    l_eps_ij = 1.0
    if e >= 0
        f = sqrt(e)
        h = f*d
        r1 = g+h
        r2 = g-h
        if r1 > TOL && r2 > TOL
            l_eps_ij = min(r1,r2)
        elseif r1 > TOL && r2 < -TOL
            l_eps_ij = r1
        elseif r2 > TOL && r1 < -TOL
            l_eps_ij = r2
        end
    end

    l = min(l,l_eps_ij)
    return l
end

@inline function get_infoP(mapP,Fmask,i,k)
    gP = mapP[i,k]           # exterior global face node number
    kP = fld1(gP,Nfp)        # exterior element number
    iP = Fmask[mod1(gP,Nfp)] # exterior node number
    return iP,kP
end

@inline function get_consP(U,f_x,f_y,t,mapP,Fmask,i,iM,xM,yM,uM,vM,k)
    iP,kP = get_infoP(mapP,Fmask,i,k)

    rhoP  = U[1,iP,kP]
    rhouP = U[2,iP,kP]
    rhovP = U[3,iP,kP]
    EP    = U[4,iP,kP]

    return rhoP,rhouP,rhovP,EP
end

@inline function get_fluxP(U,f_x,f_y,t,mapP,Fmask,i,iM,xM,yM,uM,vM,k,rhoP,rhouP,rhovP,EP)
    iP,kP = get_infoP(mapP,Fmask,i,k)

    fx_1_P = f_x[1,iP,kP]
    fx_2_P = f_x[2,iP,kP]
    fx_3_P = f_x[3,iP,kP]
    fx_4_P = f_x[4,iP,kP]
    fy_1_P = f_y[1,iP,kP]
    fy_2_P = f_y[2,iP,kP]
    fy_3_P = f_y[3,iP,kP]
    fy_4_P = f_y[4,iP,kP]

    return fx_1_P,fx_2_P,fx_3_P,fx_4_P,fy_1_P,fy_2_P,fy_3_P,fy_4_P
end

@inline function get_valP(U,f_x,f_y,t,mapP,Fmask,i,iM,xM,yM,uM,vM,k)
    rhoP,rhouP,rhovP,EP = get_consP(U,f_x,f_y,t,mapP,Fmask,i,iM,xM,yM,uM,vM,k)
    fx_1_P,fx_2_P,fx_3_P,fx_4_P,fy_1_P,fy_2_P,fy_3_P,fy_4_P = get_fluxP(U,f_x,f_y,t,mapP,Fmask,i,iM,xM,yM,uM,vM,k,rhoP,rhouP,rhovP,EP)
    return rhoP,rhouP,rhovP,EP,fx_1_P,fx_2_P,fx_3_P,fx_4_P,fy_1_P,fy_2_P,fy_3_P,fy_4_P
end

@inline function update_F_low!(F_low,k,tid,i,j,λ,S0xJ_ij,S0yJ_ij,U,fx,fy)
    # Tensor product elements: f is f_x or f_y
    rho_j  = U[1,j,k]
    rhou_j = U[2,j,k]
    rhov_j = U[3,j,k]
    E_j    = U[4,j,k]
    fx_1_j  = fx[1,j,k]
    fx_2_j  = fx[2,j,k]
    fx_3_j  = fx[3,j,k]
    fx_4_j  = fx[4,j,k]
    fy_1_j  = fy[1,j,k]
    fy_2_j  = fy[2,j,k]
    fy_3_j  = fy[3,j,k]
    fy_4_j  = fy[4,j,k]
    rho_i  = U[1,i,k]
    rhou_i = U[2,i,k]
    rhov_i = U[3,i,k]
    E_i    = U[4,i,k]
    fx_1_i  = fx[1,i,k]
    fx_2_i  = fx[2,i,k]
    fx_3_i  = fx[3,i,k]
    fx_4_i  = fx[4,i,k]
    fy_1_i  = fy[1,i,k]
    fy_2_i  = fy[2,i,k]
    fy_3_i  = fy[3,i,k]
    fy_4_i  = fy[4,i,k]

    FL1 = (S0xJ_ij*(fx_1_i+fx_1_j) + S0yJ_ij*(fy_1_i+fy_1_j) - λ*(rho_j-rho_i))
    FL2 = (S0xJ_ij*(fx_2_i+fx_2_j) + S0yJ_ij*(fy_2_i+fy_2_j) - λ*(rhou_j-rhou_i))
    FL3 = (S0xJ_ij*(fx_3_i+fx_3_j) + S0yJ_ij*(fy_3_i+fy_3_j) - λ*(rhov_j-rhov_i))
    FL4 = (S0xJ_ij*(fx_4_i+fx_4_j) + S0yJ_ij*(fy_4_i+fy_4_j) - λ*(E_j-E_i))

    F_low[1,i,j,tid] = FL1
    F_low[2,i,j,tid] = FL2
    F_low[3,i,j,tid] = FL3
    F_low[4,i,j,tid] = FL4

    F_low[1,j,i,tid] = -FL1
    F_low[2,j,i,tid] = -FL2
    F_low[3,j,i,tid] = -FL3
    F_low[4,j,i,tid] = -FL4
end

@inline function update_F_high!(F_high,k,tid,i,j,SxJ_ij_db,SyJ_ij_db,U,rholog,betalog)
    rho_j     = U[1,j,k]
    rhou_j    = U[2,j,k]
    rhov_j    = U[3,j,k]
    E_j       = U[4,j,k]
    p_j       = pfun(rho_j,rhou_j,rhov_j,E_j)
    u_j       = rhou_j/rho_j
    v_j       = rhov_j/rho_j
    beta_j    = rho_j/(2*p_j)
    rholog_j  = rholog[j,k]
    betalog_j = betalog[j,k]
    rho_i     = U[1,i,k]
    rhou_i    = U[2,i,k]
    rhov_i    = U[3,i,k]
    E_i       = U[4,i,k]
    p_i       = pfun(rho_i,rhou_i,rhov_i,E_i)
    u_i       = rhou_i/rho_i
    v_i       = rhov_i/rho_i
    beta_i    = rho_i/(2*p_i)
    rholog_i  = rholog[i,k]
    betalog_i = betalog[i,k]

    Fx1,Fx2,Fx3,Fx4,Fy1,Fy2,Fy3,Fy4 = euler_fluxes_2D(rho_i,u_i,v_i,beta_i,rholog_i,betalog_i,
                                                      rho_j,u_j,v_j,beta_j,rholog_j,betalog_j)

    FH1 = SxJ_ij_db*Fx1 + SyJ_ij_db*Fy1
    FH2 = SxJ_ij_db*Fx2 + SyJ_ij_db*Fy2
    FH3 = SxJ_ij_db*Fx3 + SyJ_ij_db*Fy3
    FH4 = SxJ_ij_db*Fx4 + SyJ_ij_db*Fy4

    F_high[1,i,j,tid] = FH1
    F_high[2,i,j,tid] = FH2
    F_high[3,i,j,tid] = FH3
    F_high[4,i,j,tid] = FH4

    F_high[1,j,i,tid] = -FH1
    F_high[2,j,i,tid] = -FH2
    F_high[3,j,i,tid] = -FH3
    F_high[4,j,i,tid] = -FH4
end

function rhs_Limited!(U,rhsU,T,dt,prealloc,ops,geom,in_s1)
    f_x,f_y,rholog,betalog,U_low,U_high,F_low,F_high,F_P,L,λ_arr,λf_arr,dii_arr,L_plot,U_hat,indicator = prealloc
    Sr,Ss,S0r,S0s,Br,Bs,Minv,Extrap,Pq,Vq,wq,Vq_hat,M = ops
    mapP,Fmask,rxJ,ryJ,sxJ,syJ,J = geom

    fill!(rhsU,0.0)
    @batch for k = 1:K
        for i = 1:Np
            rho  = U[1,i,k]
            rhou = U[2,i,k]
            rhov = U[3,i,k]
            E    = U[4,i,k]
            p          = pfun(rho,rhou,rhov,E)
            if p < 0
                @show "====================="
                @show "====================="
                @show "====================="
                @show "====================="
                @show "====================="
                @show i,k,p,rho
                @show "====================="
                @show "====================="
                @show "====================="
                @show "====================="
                @show "====================="
            end
            f_x[1,i,k] = rhou
            f_x[2,i,k] = rhou^2/rho+p
            f_x[3,i,k] = rhou*rhov/rho
            f_x[4,i,k] = E*rhou/rho+p*rhou/rho
            f_y[1,i,k] = rhov
            f_y[2,i,k] = rhou*rhov/rho
            f_y[3,i,k] = rhov^2/rho+p
            f_y[4,i,k] = E*rhov/rho+p*rhov/rho
            rholog[i,k]  = log(rho)
            betalog[i,k] = log(rho/(2*p))
        end
    end

    # Precompute wavespeeds
    @batch for k = 1:K
        rxJ_k = rxJ[1,k]
        ryJ_k = ryJ[1,k]
        sxJ_k = sxJ[1,k]
        syJ_k = syJ[1,k]

        for j = 2:Np
            rho_j  = U[1,j,k]
            rhou_j = U[2,j,k]
            rhov_j = U[3,j,k]
            E_j    = U[4,j,k]
            for i = 1:j-1
                nij_x = rxJ_k*S0r[i,j] + sxJ_k*S0s[i,j]
                nij_y = ryJ_k*S0r[i,j] + syJ_k*S0s[i,j]
                nij_norm = sqrt(nij_x^2+nij_y^2)
                rho_i  = U[1,i,k]
                rhou_i = U[2,i,k]
                rhov_i = U[3,i,k]
                E_i    = U[4,i,k]
                if nij_norm != 0
                    λ = nij_norm*max(wavespeed_1D(rho_i,nij_x/nij_norm*rhou_i+nij_y/nij_norm*rhov_i,E_i),
                                     wavespeed_1D(rho_j,nij_x/nij_norm*rhou_j+nij_y/nij_norm*rhov_j,E_j))
                    λ_arr[i,j,k] = λ
                    λ_arr[j,i,k] = λ
                    if in_s1
                        dii_arr[i,k] = dii_arr[i,k] + λ
                        dii_arr[j,k] = dii_arr[j,k] + λ
                    end
                end
            end
        end
    end

    # Interface dissipation coeff
    @batch for k = 1:K
        rxJ_k = rxJ[1,k]
        ryJ_k = ryJ[1,k]
        sxJ_k = sxJ[1,k]
        syJ_k = syJ[1,k]

        for i = 1:Nfp
            iM = Fmask[i]
            nij_x = abs(rxJ_k*Br[i,i]+sxJ_k*Bs[i,i])*.5
            nij_y = abs(ryJ_k*Br[i,i]+syJ_k*Bs[i,i])*.5
            nij_norm = sqrt(nij_x^2+nij_y^2)

            iP,kP = get_infoP(mapP,Fmask,i,k)

            rhoM  = U[1,iM,k]
            rhouM = U[2,iM,k]
            rhovM = U[3,iM,k]
            EM    = U[4,iM,k]
            rhoP  = U[1,iP,kP]
            rhouP = U[2,iP,kP]
            rhovP = U[3,iP,kP]
            EP    = U[4,iP,kP]

            λ = nij_norm*max(wavespeed_1D(rhoM,nij_x/nij_norm*rhouM+nij_y/nij_norm*rhovM,EM),
                             wavespeed_1D(rhoP,nij_x/nij_norm*rhouP+nij_y/nij_norm*rhovP,EP))
            λf_arr[i,k] = λ
            if in_s1
                dii_arr[iM,k] = dii_arr[iM,k] + λ
            end
        end
    end

    # If at the first stage, calculate the time step
    if in_s1
        for k = 1:K
            J_k = J[1,k]
            for i = 1:Np
                dt = min(dt,1.0/(Minv[i,i]/J_k)/2.0/dii_arr[i,k])
            end
        end
        dt = min(CFL*dt,T-t)
    end

    #=
    # # Troubled cell indicator
    for c = 1:Nc
        U_hat[c,:,:] = Vq_hat*U[c,:,:]
    end

    @batch for k = 1:K
        J_k = J[1,k]
        indicator[k] = sum(J_k*(M*abs.(U_hat[1,:,k]-U[1,:,k]).^2))/sum(J_k*(M*abs.(U[1,:,k]).^2)) +
                       #sum(J_k*(M*abs.(U_hat[2,:,k]-U[2,:,k]).^2))/sum(J_k*(M*abs.(U[2,:,k]).^2)) +
                       # TODO: ignore third zero component
                       #sum(J_k*(M*abs.(U_hat[3,:,k]-U[3,:,k]).^2))/sum(J_k*(M*abs.(U[3,:,k]).^2)) +
                       sum(J_k*(M*abs.(U_hat[4,:,k]-U[4,:,k]).^2))/sum(J_k*(M*abs.(U[4,:,k]).^2))

        if sum(J_k*(M*abs.(U[3,:,k]).^2)) > TOL
            indicator[k] = indicator[k] + sum(J_k*(M*abs.(U_hat[3,:,k]-U[3,:,k]).^2))/sum(J_k*(M*abs.(U[3,:,k]).^2))
        end
        if sum(J_k*(M*abs.(U[2,:,k]).^2)) > TOL
            indicator[k] = indicator[k] + sum(J_k*(M*abs.(U_hat[2,:,k]-U[2,:,k]).^2))/sum(J_k*(M*abs.(U[2,:,k]).^2))
        end
    end
    # @show indicator[1]
    # @show indicator[541]
    indicator = log.(10,indicator)
    # @show indicator[1]
    # @show indicator[541]
    s0 = log(10,1/N^4)
    @show s0
    @show maximum(indicator)
    @show minimum(indicator)
    # indicator = indicator .> s0 + .5 # N = 3
    # indicator = indicator .> s0 - .5# N = 4
    indicator = indicator .> s0 + 1
    @show sum(indicator)
    # @show indicator[1]
    =#

    @batch for k = 1:K
        tid = Threads.threadid()
        rxJ_k = rxJ[1,k]
        ryJ_k = ryJ[1,k]
        sxJ_k = sxJ[1,k]
        syJ_k = syJ[1,k]
        J_k   = J[1,k]

        for i = 1:Np
            for c = 1:Nc
                U_low[c,i,tid] = 0.0
                U_high[c,i,tid] = 0.0
            end
        end

        for j = 2:Np
            for i = 1:j-1
                SxJ_ij_db = 2*(rxJ_k*Sr[i,j] + sxJ_k*Ss[i,j])
                SyJ_ij_db = 2*(ryJ_k*Sr[i,j] + syJ_k*Ss[i,j])
                update_F_high!(F_high,k,tid,i,j,SxJ_ij_db,SyJ_ij_db,U,rholog,betalog)
            end
        end

        for j = 2:Np
            for i = 1:j-1
                λ = λ_arr[i,j,k]
                S0xJ_ij = rxJ_k*S0r[i,j] + sxJ_k*S0s[i,j]
                S0yJ_ij = ryJ_k*S0r[i,j] + syJ_k*S0s[i,j]
                update_F_low!(F_low,k,tid,i,j,λ,S0xJ_ij,S0yJ_ij,U,f_x,f_y)
            end
        end

        for i = 1:Nfp
            iM    = Fmask[i]
            BxJ_ii_halved = (rxJ_k*Br[i,i]+sxJ_k*Bs[i,i])*.5
            ByJ_ii_halved = (ryJ_k*Br[i,i]+syJ_k*Bs[i,i])*.5
            xM    = xq[iM,k]
            yM    = yq[iM,k]
            rhoM  = U[1,iM,k]
            rhouM = U[2,iM,k]
            rhovM = U[3,iM,k]
            EM    = U[4,iM,k]
            uM    = rhouM/rhoM
            vM    = rhovM/rhoM
            fx_1_M = f_x[1,iM,k]
            fx_2_M = f_x[2,iM,k]
            fx_3_M = f_x[3,iM,k]
            fx_4_M = f_x[4,iM,k]
            fy_1_M = f_y[1,iM,k]
            fy_2_M = f_y[2,iM,k]
            fy_3_M = f_y[3,iM,k]
            fy_4_M = f_y[4,iM,k]

            rhoP,rhouP,rhovP,EP,fx_1_P,fx_2_P,fx_3_P,fx_4_P,fy_1_P,fy_2_P,fy_3_P,fy_4_P = get_valP(U,f_x,f_y,t,mapP,Fmask,i,iM,xM,yM,uM,vM,k)
            λ = λf_arr[i,k]

            F_P[1,i,tid] = (BxJ_ii_halved*(fx_1_M+fx_1_P) + ByJ_ii_halved*(fy_1_M+fy_1_P)
                            -λ*(rhoP-rhoM) )
            F_P[2,i,tid] = (BxJ_ii_halved*(fx_2_M+fx_2_P) + ByJ_ii_halved*(fy_2_M+fy_2_P)
                            -λ*(rhouP-rhouM) )
            F_P[3,i,tid] = (BxJ_ii_halved*(fx_3_M+fx_3_P) + ByJ_ii_halved*(fy_3_M+fy_3_P)
                            -λ*(rhovP-rhovM) )
            F_P[4,i,tid] = (BxJ_ii_halved*(fx_4_M+fx_4_P) + ByJ_ii_halved*(fy_4_M+fy_4_P)
                            -λ*(EP-EM) )
        end

        # Calculate low order solution
        for j = 1:Np
            for i = 1:Np
                for c = 1:Nc
                    U_low[c,i,tid]  = U_low[c,i,tid]  + F_low[c,i,j,tid]
                    U_high[c,i,tid] = U_high[c,i,tid] + F_high[c,i,j,tid]
                end
            end
        end

        for i = 1:Nfp
            iM = Fmask[i]
            for c = 1:Nc
                U_low[c,iM,tid]  = U_low[c,iM,tid]  + F_P[c,i,tid]
                U_high[c,iM,tid] = U_high[c,iM,tid] + F_P[c,i,tid]
            end
        end

        for i = 1:Np
            for c = 1:Nc
                U_low[c,i,tid]  = U[c,i,k] - dt*Minv[i,i]/J_k*U_low[c,i,tid]
                U_high[c,i,tid] = U[c,i,k] - dt*Minv[i,i]/J_k*U_high[c,i,tid]
            end
        end

        is_H_positive = true
        for i = 1:Np
            rhoH_i  = U_high[1,i,tid]
            rhouH_i = U_high[2,i,tid]
            rhovH_i = U_high[3,i,tid]
            EH_i    = U_high[4,i,tid]
            pH_i    = pfun(rhoH_i,rhouH_i,rhovH_i,EH_i)
            if pH_i < POSTOL || rhoH_i < POSTOL
                is_H_positive = false
            end
        end

        if POSDETECT == 0
            is_H_positive = false
        end

       if !is_H_positive
            if LIMITOPT == 1 #&& indicator[k]
                # Calculate limiting parameters
                for j = 2:Np
                    for i = 1:j-1
                        coeff_i = dt*(Np-1)*Minv[i,i]/J_k
                        coeff_j = dt*(Np-1)*Minv[j,j]/J_k
                        rhoi  = U_low[1,i,tid]
                        rhoui = U_low[2,i,tid]
                        rhovi = U_low[3,i,tid]
                        Ei    = U_low[4,i,tid]
                        rhoj  = U_low[1,j,tid]
                        rhouj = U_low[2,j,tid]
                        rhovj = U_low[3,j,tid]
                        Ej    = U_low[4,j,tid]
                        rhoP  = (F_low[1,i,j,tid]-F_high[1,i,j,tid])
                        rhouP = (F_low[2,i,j,tid]-F_high[2,i,j,tid])
                        rhovP = (F_low[3,i,j,tid]-F_high[3,i,j,tid])
                        EP    = (F_low[4,i,j,tid]-F_high[4,i,j,tid])
                        rhoP_i  = coeff_i*rhoP
                        rhouP_i = coeff_i*rhouP
                        rhovP_i = coeff_i*rhovP
                        EP_i    = coeff_i*EP
                        rhoP_j  = -coeff_j*rhoP
                        rhouP_j = -coeff_j*rhouP
                        rhovP_j = -coeff_j*rhovP
                        EP_j    = -coeff_j*EP
                        Lrho  = POSTOL
                        Lrhoe = POSTOL
                        l = min(limiting_param(rhoi,rhoui,rhovi,Ei,rhoP_i,rhouP_i,rhovP_i,EP_i,Lrho,Lrhoe),
                                limiting_param(rhoj,rhouj,rhovj,Ej,rhoP_j,rhouP_j,rhovP_j,EP_j,Lrho,Lrhoe))
                        L[i,j,tid] = l
                        L[j,i,tid] = l
                    end
                end
            elseif LIMITOPT == 2 #&& indicator[k]
                li_min = 1.0
                for i = 1:Np
                    rhoi  = U_low[1,i,tid]
                    rhoui = U_low[2,i,tid]
                    rhovi = U_low[3,i,tid]
                    Ei    = U_low[4,i,tid]
                    rhoP_i  = 0.0
                    rhouP_i = 0.0
                    rhovP_i = 0.0
                    EP_i    = 0.0
                    coeff   = dt*Minv[i,i]/J_k
                    for j = 1:Np
                        rhoP   = (F_low[1,i,j,tid]-F_high[1,i,j,tid])
                        rhouP  = (F_low[2,i,j,tid]-F_high[2,i,j,tid])
                        rhovP  = (F_low[3,i,j,tid]-F_high[3,i,j,tid])
                        EP     = (F_low[4,i,j,tid]-F_high[4,i,j,tid])
                        rhoP_i  = rhoP_i  + coeff*rhoP
                        rhouP_i = rhouP_i + coeff*rhouP
                        rhovP_i = rhovP_i + coeff*rhovP
                        EP_i    = EP_i    + coeff*EP
                    end
                    if (LBOUNDTYPE == 0)
                        Lrho  = POSTOL
                        Lrhoe = POSTOL
                    elseif (LBOUNDTYPE > 0.0)
                        Lrho  = LBOUNDTYPE*rhoi
                        Lrhoe = LBOUNDTYPE*pfun(rhoi,rhoui,rhovi,Ei)/(γ-1)
                    end
                    l = limiting_param(rhoi,rhoui,rhovi,Ei,rhoP_i,rhouP_i,rhovP_i,EP_i,Lrho,Lrhoe)
                    li_min = min(li_min,l)
                end
                for i = 1:Np
                    for j = 1:Np
                        L[i,j,tid] = li_min
                    end
                end
            end
       end

        # Elementwise limiting
        l_e = 1.0
        for j = 1:Np
            for i = 1:Np
                l_e = min(l_e,L[i,j,tid])
            end
        end
        l_em1 = l_e-1.0

        if is_H_positive
            l_e = 1.0
            l_em1 = 0.0
        end

        # if !indicator[k]
        #     l_e = 1.0
        #     l_em1 = 0.0
        # end

        if in_s1
            L_plot[k] = l_e
        end

        for j = 2:Np
            for i = 1:j-1
                for c = 1:Nc
                    FL_ij = F_low[c,i,j,tid]
                    FH_ij = F_high[c,i,j,tid]
                    rhsU[c,i,k] = rhsU[c,i,k] + l_em1*FL_ij - l_e*FH_ij
                    rhsU[c,j,k] = rhsU[c,j,k] - l_em1*FL_ij + l_e*FH_ij
                end
            end
        end

        for i = 1:Nfp
            iM = Fmask[i]
            for c = 1:Nc
                rhsU[c,iM,k] = rhsU[c,iM,k] - F_P[c,i,tid]
            end
        end
    end

    @batch for k = 1:K
        J_k = J[1,k]
        for i = 1:Np
            for c = 1:Nc
                rhsU[c,i,k] = Minv[i,i]/J_k*rhsU[c,i,k]
            end
        end
    end

    @show minimum(L_plot)
    @show sum(L_plot)/K

    return dt
end

function rhs_IDP!(U,rhsU,dt,prealloc,ops,geom,in_s1)
    f_x,f_y,rholog,betalog,U_low,F_low,F_high,F_P,L,λ_arr,λf_arr,dii_arr = prealloc
    Sr,Ss,S0r,S0s,Br,Bs,Minv,Extrap = ops
    mapP,Fmask,rxJ,ryJ,sxJ,syJ,J = geom

    fill!(rhsU,0.0)
    @batch for k = 1:K
        for i = 1:Np
            rho  = U[1,i,k]
            rhou = U[2,i,k]
            rhov = U[3,i,k]
            E    = U[4,i,k]
            p          = pfun(rho,rhou,rhov,E)
            f_x[1,i,k] = rhou
            f_x[2,i,k] = rhou^2/rho+p
            f_x[3,i,k] = rhou*rhov/rho
            f_x[4,i,k] = E*rhou/rho+p*rhou/rho
            f_y[1,i,k] = rhov
            f_y[2,i,k] = rhou*rhov/rho
            f_y[3,i,k] = rhov^2/rho+p
            f_y[4,i,k] = E*rhov/rho+p*rhov/rho
            rholog[i,k]  = log(rho)
            betalog[i,k] = log(rho/(2*p))
        end
    end

    # Precompute wavespeeds
    @batch for k = 1:K
        rxJ_k = rxJ[1,k]
        ryJ_k = ryJ[1,k]
        sxJ_k = sxJ[1,k]
        syJ_k = syJ[1,k]

        for j = 2:Np
            rho_j  = U[1,j,k]
            rhou_j = U[2,j,k]
            rhov_j = U[3,j,k]
            E_j    = U[4,j,k]
            for i = 1:j-1
                nij_x = rxJ_k*S0r[i,j] + sxJ_k*S0s[i,j]
                nij_y = ryJ_k*S0r[i,j] + syJ_k*S0s[i,j]
                nij_norm = sqrt(nij_x^2+nij_y^2)
                rho_i  = U[1,i,k]
                rhou_i = U[2,i,k]
                rhov_i = U[3,i,k]
                E_i    = U[4,i,k]
                if nij_norm != 0
                    λ = nij_norm*max(wavespeed_1D(rho_i,nij_x/nij_norm*rhou_i+nij_y/nij_norm*rhov_i,E_i),
                                     wavespeed_1D(rho_j,nij_x/nij_norm*rhou_j+nij_y/nij_norm*rhov_j,E_j))
                    λ_arr[i,j,k] = λ
                    λ_arr[j,i,k] = λ
                    if in_s1
                        dii_arr[i,k] = dii_arr[i,k] + λ
                        dii_arr[j,k] = dii_arr[j,k] + λ
                    end
                end
            end
        end
    end

    # Interface dissipation coeff
    @batch for k = 1:K
        rxJ_k = rxJ[1,k]
        ryJ_k = ryJ[1,k]
        sxJ_k = sxJ[1,k]
        syJ_k = syJ[1,k]

        for i = 1:Nfp
            iM = Fmask[i]
            nij_x = abs(rxJ_k*Br[i,i]+sxJ_k*Bs[i,i])*.5
            nij_y = abs(ryJ_k*Br[i,i]+syJ_k*Bs[i,i])*.5
            nij_norm = sqrt(nij_x^2+nij_y^2)

            iP,kP = get_infoP(mapP,Fmask,i,k)

            rhoM  = U[1,iM,k]
            rhouM = U[2,iM,k]
            rhovM = U[3,iM,k]
            EM    = U[4,iM,k]
            rhoP  = U[1,iP,kP]
            rhouP = U[2,iP,kP]
            rhovP = U[3,iP,kP]
            EP    = U[4,iP,kP]

            λ = nij_norm*max(wavespeed_1D(rhoM,nij_x/nij_norm*rhouM+nij_y/nij_norm*rhovM,EM),
                             wavespeed_1D(rhoP,nij_x/nij_norm*rhouP+nij_y/nij_norm*rhovP,EP))
            λf_arr[i,k] = λ
            if in_s1
                dii_arr[iM,k] = dii_arr[iM,k] + λ
            end
        end
    end

    # If at the first stage, calculate the time step
    if in_s1
        for k = 1:K
            J_k = J[1,k]
            for i = 1:Np
                dt = min(dt,1.0/(Minv[i,i]/J_k)/2.0/dii_arr[i,k])
            end
        end
    end

    @batch for k = 1:K
        tid = Threads.threadid()
        rxJ_k = rxJ[1,k]
        ryJ_k = ryJ[1,k]
        sxJ_k = sxJ[1,k]
        syJ_k = syJ[1,k]

        for i = 1:Np
            for c = 1:Nc
                U_low[c,i,tid] = 0.0
            end
        end

        for j = 2:Np
            for i = 1:j-1
                λ = λ_arr[i,j,k]
                S0xJ_ij = rxJ_k*S0r[i,j] + sxJ_k*S0s[i,j]
                S0yJ_ij = ryJ_k*S0r[i,j] + syJ_k*S0s[i,j]
                update_F_low!(F_low,k,tid,i,j,λ,S0xJ_ij,S0yJ_ij,U,f_x,f_y)
            end
        end

        for i = 1:Nfp
            iM    = Fmask[i]
            BxJ_ii_halved = (rxJ_k*Br[i,i]+sxJ_k*Bs[i,i])*.5
            ByJ_ii_halved = (ryJ_k*Br[i,i]+syJ_k*Bs[i,i])*.5
            xM    = xq[iM,k]
            yM    = yq[iM,k]
            rhoM  = U[1,iM,k]
            rhouM = U[2,iM,k]
            rhovM = U[3,iM,k]
            EM    = U[4,iM,k]
            uM    = rhouM/rhoM
            vM    = rhovM/rhoM
            fx_1_M = f_x[1,iM,k]
            fx_2_M = f_x[2,iM,k]
            fx_3_M = f_x[3,iM,k]
            fx_4_M = f_x[4,iM,k]
            fy_1_M = f_y[1,iM,k]
            fy_2_M = f_y[2,iM,k]
            fy_3_M = f_y[3,iM,k]
            fy_4_M = f_y[4,iM,k]

            rhoP,rhouP,rhovP,EP,fx_1_P,fx_2_P,fx_3_P,fx_4_P,fy_1_P,fy_2_P,fy_3_P,fy_4_P = get_valP(U,f_x,f_y,t,mapP,Fmask,i,iM,xM,yM,uM,vM,k)
            λ = λf_arr[i,k]

            F_P[1,i,tid] = (BxJ_ii_halved*(fx_1_M+fx_1_P) + ByJ_ii_halved*(fy_1_M+fy_1_P)
                            -λ*(rhoP-rhoM) )
            F_P[2,i,tid] = (BxJ_ii_halved*(fx_2_M+fx_2_P) + ByJ_ii_halved*(fy_2_M+fy_2_P)
                            -λ*(rhouP-rhouM) )
            F_P[3,i,tid] = (BxJ_ii_halved*(fx_3_M+fx_3_P) + ByJ_ii_halved*(fy_3_M+fy_3_P)
                            -λ*(rhovP-rhovM) )
            F_P[4,i,tid] = (BxJ_ii_halved*(fx_4_M+fx_4_P) + ByJ_ii_halved*(fy_4_M+fy_4_P)
                            -λ*(EP-EM) )
        end

        for j = 2:Np
            for i = 1:j-1
                for c = 1:Nc
                    FL_ij = F_low[c,i,j,tid]
                    rhsU[c,i,k] = rhsU[c,i,k] - FL_ij
                    rhsU[c,j,k] = rhsU[c,j,k] + FL_ij
                end
            end
        end

        for i = 1:Nfp
            iM = Fmask[i]
            for c = 1:Nc
                rhsU[c,iM,k] = rhsU[c,iM,k] - F_P[c,i,tid]
            end
        end
    end

    @batch for k = 1:K
        J_k = J[1,k]
        for i = 1:Np
            for c = 1:Nc
                rhsU[c,i,k] = Minv[i,i]/J_k*rhsU[c,i,k]
            end
        end
    end

    return dt
end

function rhs_ESDG!(U,rhsU,prealloc,ops,geom)

    f_x,f_y,rholog,betalog,U_low,F_low,F_high,F_P,L,λ_arr,λf_arr,dii_arr = prealloc
    Sr,Ss,S0r,S0s,Br,Bs,Minv,Extrap = ops
    mapP,Fmask,rxJ,ryJ,sxJ,syJ,J = geom

    fill!(rhsU,0.0)
    @batch for k = 1:K
        for i = 1:Np
            rho  = U[1,i,k]
            rhou = U[2,i,k]
            rhov = U[3,i,k]
            E    = U[4,i,k]
            p          = pfun(rho,rhou,rhov,E)
            f_x[1,i,k] = rhou
            f_x[2,i,k] = rhou^2/rho+p
            f_x[3,i,k] = rhou*rhov/rho
            f_x[4,i,k] = E*rhou/rho+p*rhou/rho
            f_y[1,i,k] = rhov
            f_y[2,i,k] = rhou*rhov/rho
            f_y[3,i,k] = rhov^2/rho+p
            f_y[4,i,k] = E*rhov/rho+p*rhov/rho
            rholog[i,k]  = log(rho)
            betalog[i,k] = log(rho/(2*p))
        end
    end

    @batch for k = 1:K
        tid = Threads.threadid()
        rxJ_k = rxJ[1,k]
        ryJ_k = ryJ[1,k]
        sxJ_k = sxJ[1,k]
        syJ_k = syJ[1,k]

        for j = 2:Np
            for i = 1:j-1
                SxJ_ij_db = 2*(rxJ_k*Sr[i,j] + sxJ_k*Ss[i,j])
                SyJ_ij_db = 2*(ryJ_k*Sr[i,j] + syJ_k*Ss[i,j])
                update_F_high!(F_high,k,tid,i,j,SxJ_ij_db,SyJ_ij_db,U,rholog,betalog)
            end
        end

        for i = 1:Nfp
            iM       = Fmask[i]
            rhoM     = U[1,iM,k]
            rhouM    = U[2,iM,k]
            rhovM    = U[3,iM,k]
            EM       = U[4,iM,k]
            uM       = rhouM/rhoM
            vM       = rhovM/rhoM
            pM       = pfun(rhoM,rhouM,rhovM,EM)
            betaM    = rhoM/(2*pM)
            rhologM  = log(rhoM)
            betalogM = log(betaM)

            iP,kP = get_infoP(mapP,Fmask,i,k)
            rhoP     = U[1,iP,kP]
            rhouP    = U[2,iP,kP]
            rhovP    = U[3,iP,kP]
            EP       = U[4,iP,kP]
            uP       = rhouP/rhoP
            vP       = rhovP/rhoP
            pP       = pfun(rhoP,rhouP,rhovP,EP)
            betaP    = rhoP/(2*pP)
            rhologP  = log(rhoP)
            betalogP = log(betaP)

            Fx1,Fx2,Fx3,Fx4,Fy1,Fy2,Fy3,Fy4 = euler_fluxes_2D(rhoM,uM,vM,betaM,rhologM,betalogM,
                                                              rhoP,uP,vP,betaP,rhologP,betalogP)

            F_P[1,i,tid] = (rxJ_k*Br[i,i]+sxJ_k*Bs[i,i])*Fx1 + (ryJ_k*Br[i,i]+syJ_k*Bs[i,i])*Fy1
            F_P[2,i,tid] = (rxJ_k*Br[i,i]+sxJ_k*Bs[i,i])*Fx2 + (ryJ_k*Br[i,i]+syJ_k*Bs[i,i])*Fy2
            F_P[3,i,tid] = (rxJ_k*Br[i,i]+sxJ_k*Bs[i,i])*Fx3 + (ryJ_k*Br[i,i]+syJ_k*Bs[i,i])*Fy3
            F_P[4,i,tid] = (rxJ_k*Br[i,i]+sxJ_k*Bs[i,i])*Fx4 + (ryJ_k*Br[i,i]+syJ_k*Bs[i,i])*Fy4
        end

        for j = 2:Np
            for i = 1:j-1
                for c = 1:Nc
                    FH_ij = F_high[c,i,j,tid]
                    rhsU[c,i,k] = rhsU[c,i,k] - FH_ij
                    rhsU[c,j,k] = rhsU[c,j,k] + FH_ij
                end
            end
        end

        for i = 1:Nfp
            iM = Fmask[i]
            for c = 1:Nc
                rhsU[c,iM,k] = rhsU[c,iM,k] - F_P[c,i,tid]
            end
        end
    end

    @batch for k = 1:K
        J_k = J[1,k]
        for i = 1:Np
            for c = 1:Nc
                rhsU[c,i,k] = Minv[i,i]/J_k*rhsU[c,i,k]
            end
        end
    end
end

function vortex_sol(x,y,t)
    # x0 = 5
    # y0 = 0
    x0 = 9.0
    y0 = 5.0
    beta = 8.5
    r2 = @. (x-x0-t)^2 + (y-y0)^2

    u = @. 1 - beta*exp(1-r2)*(y-y0)/(2*pi)
    v = @. beta*exp(1-r2)*(x-x0-t)/(2*pi)
    rho = @. 1 - (1/(8*γ*pi^2))*(γ-1)/2*(beta*exp(1-r2))^2
    rho = @. rho^(1/(γ-1))
    p = @. rho^γ

    return (rho, u, v, p)
end

# const Np  = size(x,1)
const Np  = size(xq,1)
const Nfp = size(Vf,1)
U = zeros(Nc,Np,K)
for k = 1:K
    for i = 1:Np
        # rho,u,v,p = vortex_sol(x[i,k],y[i,k],0.0)
        rho,u,v,p = vortex_sol(xq[i,k],yq[i,k],0.0)
        U[1,i,k] = rho
        U[2,i,k] = rho*u
        U[3,i,k] = rho*v
        U[4,i,k] = Efun(rho,u,v,p)
    end
end

# Preallocation
rhsU   = zeros(Float64,size(U))
f_x    = zeros(Float64,size(U))
f_y    = zeros(Float64,size(U))
rholog  = zeros(Float64,Np,K)
betalog = zeros(Float64,Np,K)
U_low   = zeros(Float64,Nc,Np,NUM_THREADS)
U_high  = zeros(Float64,Nc,Np,NUM_THREADS)
F_low   = zeros(Float64,Nc,Np,Np,NUM_THREADS)
F_high  = zeros(Float64,Nc,Np,Np,NUM_THREADS)
F_P     = zeros(Float64,Nc,Nfp,NUM_THREADS)
L       =  ones(Float64,Np,Np,NUM_THREADS)
λ_arr    = zeros(Float64,Np,Np,K)
λf_arr   = zeros(Float64,Nfp,K)
dii_arr  = zeros(Float64,Np,K)
L_plot   = zeros(Float64,K)
U_hat    = zeros(Float64,size(U))
indicator = zeros(Float64,K)

VDM_hat = copy(VDM)
cnt = 0
for i = 0:N
    for j = 0:N-i
        global cnt = cnt+1
        if (j == N-i)
            VDM_hat[:,cnt] .= 0.0
        end
    end
end
Vq_hat = VDM_hat/VDM
prealloc = (f_x,f_y,rholog,betalog,U_low,U_high,F_low,F_high,F_P,L,λ_arr,λf_arr,dii_arr,L_plot,U_hat,indicator)
ops      = (Sr,Ss,S0r,S0s,Br,Bs,Minv,E,Pq,Vq,wq,Vq_hat,M)
geom     = (mapP,Fmask,rxJ,ryJ,sxJ,syJ,J)

# TODO: previous Time stepping
"Time integration"
t = 0.0
U = collect(U)
resW = zeros(size(U))

dt_hist = []
i = 1

# open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,x,convtri.txt","w") do io
#     writedlm(io,xq)
# end
# open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,y,convtri.txt","w") do io
#     writedlm(io,yq)
# end

@time while t < T
    # SSPRK(3,3)
    fill!(dii_arr,0.0)
    # dt = min(dt0,T-t)
    dt = dt0
    dt = rhs_Limited!(U,rhsU,T,dt,prealloc,ops,geom,true)
    # dt = rhs_IDP!(U,rhsU,dt,prealloc,ops,geom,true)
    # rhs_ESDG!(U,rhsU,prealloc,ops,geom)
    # dt = min(CFL*dt,T-t)
    @. resW = U + dt*rhsU
    rhs_Limited!(resW,rhsU,T,dt,prealloc,ops,geom,false)
    # rhs_IDP!(resW,rhsU,dt,prealloc,ops,geom,false)
    # rhs_ESDG!(resW,rhsU,prealloc,ops,geom)
    @. resW = resW+dt*rhsU
    @. resW = 3/4*U+1/4*resW
    rhs_Limited!(resW,rhsU,T,dt,prealloc,ops,geom,false)
    # rhs_IDP!(resW,rhsU,dt,prealloc,ops,geom,false)
    # rhs_ESDG!(resW,rhsU,prealloc,ops,geom)
    @. resW = resW+dt*rhsU
    @. U = 1/3*U+2/3*resW

    push!(dt_hist,dt)
    global t = t + dt
    println("Current time $t with time step size $dt, and final time $T, at step $i")
    flush(stdout)
    global i = i + 1
    if (mod(i,100) == 1)
        # open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,rho,convtri.txt","w") do io
        #     writedlm(io,U[1,:,:])
        # end
        # open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,rhou,convtri.txt","w") do io
        #     writedlm(io,U[2,:,:])
        # end
        # open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,rhov,convtri.txt","w") do io
        #     writedlm(io,U[3,:,:])
        # end
        # open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,E,convtri.txt","w") do io
        #     writedlm(io,U[4,:,:])
        # end
        # open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,L_plot,convtri.txt","w") do io
        #     writedlm(io,L_plot)
        # end
    end
end

# open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,rho,convtri.txt","w") do io
#     writedlm(io,U[1,:,:])
# end
# open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,rhou,convtri.txt","w") do io
#     writedlm(io,U[2,:,:])
# end
# open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,rhov,convtri.txt","w") do io
#     writedlm(io,U[3,:,:])
# end
# open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,E,convtri.txt","w") do io
#     writedlm(io,U[4,:,:])
# end
# open("/data/yl184/N=$N,K1D=$K1D,t=$t,CFL=$CFL,L_plot,convtri.txt","w") do io
#     writedlm(io,L_plot)
# end

exact_U = @. vortex_sol.(xq,yq,T)
exact_rho = [x[1] for x in exact_U]
exact_u = [x[2] for x in exact_U]
exact_v = [x[3] for x in exact_U]
exact_p = [x[4] for x in exact_U]
exact_rhou = exact_rho .* exact_u
exact_rhov = exact_rho .* exact_v
exact_E = Efun.(exact_rho,exact_u,exact_v,exact_p)

rho = U[1,:,:]
rhou = U[2,:,:]
rhov = U[3,:,:]
u = U[2,:,:]./U[1,:,:]
v = U[3,:,:]./U[1,:,:]
E = U[4,:,:]

M = diag(M)
J = J[1,1]

Linferr = maximum(abs.(exact_rho-rho))/maximum(abs.(exact_rho)) +
          maximum(abs.(exact_rhou-rhou))/maximum(abs.(exact_rhou)) +
          maximum(abs.(exact_rhov-rhov))/maximum(abs.(exact_rhov)) +
          maximum(abs.(exact_E-E))/maximum(abs.(exact_E))

L1err = sum(J*M.*abs.(exact_rho-rho))/sum(J*M.*abs.(exact_rho)) +
        sum(J*M.*abs.(exact_rhou-rhou))/sum(J*M.*abs.(exact_rhou)) +
        sum(J*M.*abs.(exact_rhov-rhov))/sum(J*M.*abs.(exact_rhov)) +
        sum(J*M.*abs.(exact_E-E))/sum(J*M.*abs.(exact_E))

L2err = sqrt(sum(J*M.*abs.(exact_rho-rho).^2))/sqrt(sum(J*M.*abs.(exact_rho).^2)) +
        sqrt(sum(J*M.*abs.(exact_rhou-rhou).^2))/sqrt(sum(J*M.*abs.(exact_rhou).^2)) +
        sqrt(sum(J*M.*abs.(exact_rhov-rhov).^2))/sqrt(sum(J*M.*abs.(exact_rhov).^2)) +
        sqrt(sum(J*M.*abs.(exact_E-E).^2))/sqrt(sum(J*M.*abs.(exact_E).^2))

println("convtri, N = $N, K = $K")
println("L1 error is $L1err")
println("L2 error is $L2err")
println("Linf error is $Linferr")

# df = DataFrame(N = Int64[], K = Int64[], T = Float64[], CFL = Float64[], LIMITOPT = Int64[], POSDETECT = Int64[], LBOUNDTYPE = Float64[], L1err = Float64[], L2err = Float64[], Linferr = Float64[])
df = load("dg2D_euler_tri_convergence.jld2","convergence_data")
push!(df,(N,K,T,CFL,LIMITOPT,POSDETECT,LBOUNDTYPE,L1err,L2err,Linferr))
save("dg2D_euler_tri_convergence.jld2","convergence_data",df)

end

max_stars_repo_name: yiminllin/ESDG-PosLimit

max_issues_repo_name: yiminllin/ESDG-PosLimit

max_forks_repo_name: yiminllin/ESDG-PosLimit


text:
"""
    MixedModelPermutation{T<:AbstractFloat}

Object returned by the various permutation methods with fields
- `fits`: the parameter estimates from the permutation replicates as a vector of named tuples.
- `λ`: `Vector{LowerTriangular{T,Matrix{T}}}` containing copies of the λ field from `ReMat` model terms
- `inds`: `Vector{Vector{Int}}` containing copies of the `inds` field from `ReMat` model terms
- `lowerbd`: `Vector{T}` containing the vector of lower bounds (corresponds to the identically named field of [`OptSummary`](@ref))
- `fcnames`: NamedTuple whose keys are the grouping factor names and whose values are the column names

The schema of `fits` is, by default,
```
Tables.Schema:
 :objective  T
 :σ          T
 :β          NamedTuple{β_names}{NTuple{p,T}}
 :se         StaticArrays.SArray{Tuple{p},T,1,p}
 :θ          StaticArrays.SArray{Tuple{k},T,1,k}
```
where the sizes, `p` and `k`, of the `β` and `θ` elements are determined by the model.

Characteristics of the permutation replicates can be extracted as properties.  The `σs` and
`σρs` properties unravel the `σ` and `θ` estimates into estimates of the standard deviations
and correlations of the random-effects terms.
"""
struct MixedModelPermutation{T<:AbstractFloat}
    fits::Vector
    λ::Vector{LowerTriangular{T,Matrix{T}}}
    inds::Vector{Vector{Int}}
    lowerbd::Vector{T}
    fcnames::NamedTuple
end

#####
##### Convert and Delegate Methods to MixedModelBootstrap
#####

# XXX This can be removed when MixedModels.jl introduces the abstract fit
#     collection type and broadens their method signatures

# non copying converstion to a bootstrap object.
_perm2boot(p::MixedModelPermutation) = MixedModelBootstrap(p.fits, p.λ, p.inds, p.lowerbd, p.fcnames)

# delegate methods
for f in (:allpars, :coefpvalues, :issingular, :setθ!, :tidyβ, :tidyσs)

    @eval begin
        MixedModels.$f(p::MixedModelPermutation) = MixedModels.$f(_perm2boot(p))
    end
end

Base.propertynames(p::MixedModelPermutation) = propertynames(_perm2boot(p))

function Base.getproperty(p::MixedModelPermutation, s::Symbol)
    if s ∈ [:objective, :σ, :θ, :se]
        getproperty.(getfield(p, :fits), s)
    elseif s == :β
        tidyβ(p)
    elseif s == :coefpvalues
        coefpvalues(p)
    elseif s == :σs
        tidyσs(p)
    elseif s == :allpars
        allpars(p)
    else
        getfield(p, s)
    end
end

ext: jl


text:
## A SortedDict is a wrapper around balancedTree with
## methods similiar to those of Julia container Dict.

mutable struct SortedDict{K, D, Ord <: Ordering} <: AbstractDict{K,D}
    bt::BalancedTree23{K,D,Ord}

    ## Base constructors
    """
        SortedDict{K,V}(o=Forward)

    Construct an empty `SortedDict` with key type `K` and value type
    `V` with `o` ordering (default to forward ordering).
    """
    SortedDict{K,D,Ord}(o::Ord) where {K, D, Ord <: Ordering} =
        new{K,D,Ord}(BalancedTree23{K,D,Ord}(o))

    function SortedDict{K,D,Ord}(o::Ord, kv) where {K, D, Ord <: Ordering}
        s = new{K,D,Ord}(BalancedTree23{K,D,Ord}(o))

        if eltype(kv) <: Pair
            # It's (possibly?) more efficient to access the first and second
            # elements of Pairs directly, rather than destructure
            for p in kv
                s[p.first] = p.second
            end
        else
            for (k,v) in kv
                s[k] = v
            end
        end
        return s
    end

end

# Any-Any constructors
"""
    SortedDict()

Construct an empty `SortedDict` with key type `Any` and value type
`Any`. Ordering defaults to `Forward` ordering.

**Note that a key type of `Any` or any other abstract type will lead
to slow performance, as the values are stored boxed (i.e., as
pointers), and insertion will require a run-time lookup of the
appropriate comparison function. It is recommended to always specify
a concrete key type, or to use one of the constructors below in
which the key type is inferred.**
"""
SortedDict() = SortedDict{Any,Any,ForwardOrdering}(Forward)

"""
    SortedDict(o=Forward)

Construct an empty `SortedDict` with key type `K` and value type
`V`. If `K` and `V` are not specified, the dictionary defaults to a
`SortedDict{Any,Any}`. Keys and values are converted to the given
type upon insertion. Ordering `o` defaults to `Forward` ordering.

**Note that a key type of `Any` or any other abstract type will lead
to slow performance, as the values are stored boxed (i.e., as
pointers), and insertion will require a run-time lookup of the
appropriate comparison function. It is recommended to always specify
a concrete key type, or to use one of the constructors below in
which the key type is inferred.**
"""
SortedDict(o::Ord) where {Ord <: Ordering} = SortedDict{Any,Any,Ord}(o)

# Construction from Pairs
# TODO: fix SortedDict(1=>1, 2=>2.0)
"""
    SortedDict(k1=>v1, k2=>v2, ...)
and `SortedDict{K,V}(k1=>v1, k2=>v2, ...)`

Construct a `SortedDict` from the given key-value pairs. If `K` and
`V` are not specified, key type and value type are inferred from the
given key-value pairs, and ordering is assumed to be `Forward`
ordering.
"""
SortedDict(ps::Pair...) = SortedDict(Forward, ps)
SortedDict{K,D}(ps::Pair...) where {K,D} = SortedDict{K,D,ForwardOrdering}(Forward, ps)

"""
    SortedDict(o, k1=>v1, k2=>v2, ...)

Construct a `SortedDict` from the given pairs with the specified
ordering `o`. The key type and value type are inferred from the
given pairs.
"""
SortedDict(o::Ordering, ps::Pair...) = SortedDict(o, ps)

"""
    SortedDict{K,V}(o, k1=>v1, k2=>v2, ...)

Construct a `SortedDict` from the given pairs with the specified
ordering `o`. If `K` and `V` are not specified, the key type and
value type are inferred from the given pairs. See below for more
information about ordering.
"""
SortedDict{K,D}(o::Ord, ps::Pair...) where {K,D,Ord<:Ordering} = SortedDict{K,D,Ord}(o, ps)

# Construction from AbstractDicts
SortedDict(o::Ord, d::AbstractDict{K,D}) where {K,D,Ord<:Ordering} = SortedDict{K,D,Ord}(o, d)

## Construction from iteratables of Pairs/Tuples

# Construction specifying Key/Value types
# e.g., SortedDict{Int,Float64}([1=>1, 2=>2.0])
"""
    SortedDict(iter, o=Forward)
and `SortedDict{K,V}(iter, o=Forward)`

Construct a `SortedDict` from an arbitrary iterable object of
`key=>value` pairs. If `K` and `V` are not specified, the key type
and value type are inferred from the given iterable. The ordering
object `o` defaults to `Forward`.
"""
SortedDict{K,D}(kv) where {K,D} = SortedDict{K,D}(Forward, kv)
function SortedDict{K,D}(o::Ord, kv) where {K,D,Ord<:Ordering}
    try
        SortedDict{K,D,Ord}(o, kv)
    catch e
        if not_iterator_of_pairs(kv)
            throw(ArgumentError("SortedDict(kv): kv needs to be an iterator of tuples or pairs"))
        else
            rethrow(e)
        end
    end
end

# Construction inferring Key/Value types from input
# e.g. SortedDict{}

SortedDict(o1::Ordering, o2::Ordering) = throw(ArgumentError("SortedDict with two parameters must be called with an Ordering and an interable of pairs"))

"""
    SortedDict(d, o=Forward)
and `SortedDict{K,V}(d, o=Forward)`

Construct a `SortedDict` from an ordinary Julia dict `d` (or any
associative type), e.g.:

```julia
d = Dict("New York" => 1788, "Illinois" => 1818)
c = SortedDict(d)
```

In this example the key-type is deduced to be `String`, while the
value-type is `Int`.

If `K` and `V` are not specified, the key type and value type are
inferred from the given dictionary. The ordering object `o` defaults
to `Forward`.
"""
SortedDict(kv, o::Ordering=Forward) = SortedDict(o, kv)
function SortedDict(o::Ordering, kv)
    try
        _sorted_dict_with_eltype(o, kv, eltype(kv))
    catch e
        if not_iterator_of_pairs(kv)
            throw(ArgumentError("SortedDict(kv): kv needs to be an iterator of tuples or pairs"))
        else
            rethrow(e)
        end
    end
end

_sorted_dict_with_eltype(o::Ord, ps, ::Type{Pair{K,D}}) where {K,D,Ord} = SortedDict{  K,  D,Ord}(o, ps)
_sorted_dict_with_eltype(o::Ord, kv, ::Type{Tuple{K,D}}) where {K,D,Ord} = SortedDict{  K,  D,Ord}(o, kv)
_sorted_dict_with_eltype(o::Ord, ps, ::Type{Pair{K}}  ) where {K,  Ord} = SortedDict{  K,Any,Ord}(o, ps)
_sorted_dict_with_eltype(o::Ord, kv, ::Type            ) where {    Ord} = SortedDict{Any,Any,Ord}(o, kv)

## TODO: It seems impossible (or at least very challenging) to create the eltype below.
##       If deemed possible, please create a test and uncomment this definition.
# _sorted_dict_with_eltype{  D,Ord}(o::Ord, ps, ::Type{Pair{K,D} where K}) = SortedDict{Any,  D,Ord}(o, ps)

const SDSemiToken = IntSemiToken

const SDToken = Tuple{SortedDict,IntSemiToken}

## This function implements m[k]; it returns the
## data item associated with key k.
"""
    v = sd[k]

Argument `sd` is a SortedDict and `k` is a key. In an expression,
this retrieves the value (`v`) associated with the key (or `KeyError` if
none). On the left-hand side of an assignment, this assigns or
reassigns the value associated with the key. (For assigning and
reassigning, see also `insert!` below.) Time: O(*c* log *n*)
"""
@inline function getindex(m::SortedDict, k_)
    i, exactfound = findkey(m.bt, convert(keytype(m),k_))
    !exactfound && throw(KeyError(k_))
    return m.bt.data[i].d
end

## This function implements m[k]=d; it sets the
## data item associated with key k equal to d.
"""
    sc[st] = v

If `st` is a semitoken and `sc` is a SortedDict or SortedMultiDict,
then `sc[st]` refers to the value field of the (key,value) pair that
the full token `(sc,st)` refers to. This expression may occur on
either side of an assignment statement. Time: O(1)
"""
@inline function setindex!(m::SortedDict{K,D,Ord}, d_, k_) where {K, D, Ord <: Ordering}
    insert!(m.bt, convert(K,k_), convert(D,d_), false)
    m
end

## push! is an alternative to insert!; it returns the container.
"""
    push!(sc, k=>v)

Argument `sc` is a SortedDict or SortedMultiDict and `k=>v` is a
key-value pair. This inserts the key-value pair into the container.
If the key is already present, this overwrites the old value. The
return value is `sc`. Time: O(*c* log *n*)
"""
@inline function push!(m::SortedDict{K,D}, pr::Pair) where {K,D}
    insert!(m.bt, convert(K, pr[1]), convert(D, pr[2]), false)
    m
end

## This function looks up a key in the tree;
## if not found, then it returns a marker for the
## end of the tree.

"""
    find(sd, k)

Argument `sd` is a SortedDict and argument `k` is a key. This
function returns the semitoken that refers to the item whose key is
`k`, or past-end semitoken if `k` is absent. Time: O(*c* log *n*)
"""
@inline function find(m::SortedDict, k_)
    ll, exactfound = findkey(m.bt, convert(keytype(m),k_))
    IntSemiToken(exactfound ? ll : 2)
end

## This function inserts an item into the tree.
## Unlike m[k]=d, it also returns a bool and a token.
## The bool is true if the inserted item is new.
## It is false if there was already an item
## with that key.
## The token points to the newly inserted item.

"""
    insert!(sc, k)

Argument `sc` is a SortedDict or SortedMultiDict, `k` is a key and
`v` is the corresponding value. This inserts the `(k,v)` pair into
the container. If the key is already present in a SortedDict, this
overwrites the old value. In the case of SortedMultiDict, no
overwriting takes place (since SortedMultiDict allows the same key
to associate with multiple values). In the case of SortedDict, the
return value is a pair whose first entry is boolean and indicates
whether the insertion was new (i.e., the key was not previously
present) and the second entry is the semitoken of the new entry. In
the case of SortedMultiDict, a semitoken is returned (but no
boolean). Time: O(*c* log *n*)
"""
@inline function insert!(m::SortedDict{K,D,Ord}, k_, d_) where {K,D, Ord <: Ordering}
    b, i = insert!(m.bt, convert(K,k_), convert(D,d_), false)
    b, IntSemiToken(i)
end

"""
    eltype(sc)

Returns the (key,value) type (a 2-entry pair, i.e., `Pair{K,V}`) for
SortedDict and SortedMultiDict. Returns the key type for SortedSet.
This function may also be applied to the type itself. Time: O(1)
"""
@inline eltype(m::SortedDict{K,D,Ord}) where {K,D,Ord <: Ordering} =  Pair{K,D}
@inline eltype(::Type{SortedDict{K,D,Ord}}) where {K,D,Ord <: Ordering} =  Pair{K,D}

"""
    in(p, sc)

Returns true if `p` is in `sc`. In the case that `sc` is a
SortedDict or SortedMultiDict, `p` is a key=>value pair. In the
case that `sc` is a SortedSet, `p` should be a key. Time: O(*c* log
*n* + *d*) for SortedDict and SortedSet, where *d* stands for the
time to compare two values. In the case of SortedMultiDict, the time
is O(*c* log *n* + *dl*), and *l* stands for the number of entries
that have the key of the given pair. (So therefore this call is
inefficient if the same key addresses a large number of values, and
an alternative should be considered.)
"""
@inline function in(pr::Pair, m::SortedDict{K,D,Ord}) where {K,D,Ord <: Ordering}
    i, exactfound = findkey(m.bt,convert(K,pr[1]))
    return exactfound && isequal(m.bt.data[i].d,convert(D,pr[2]))
end

@inline in(::Tuple{Any,Any}, ::SortedDict) =
    throw(ArgumentError("'(k,v) in sorteddict' not supported in Julia 0.4 or 0.5.  See documentation"))

"""
    keytype(sc)

Returns the key type for SortedDict, SortedMultiDict and SortedSet.
This function may also be applied to the type itself. Time: O(1)
"""
@inline keytype(m::SortedDict{K,D,Ord}) where {K,D,Ord <: Ordering} = K
@inline keytype(::Type{SortedDict{K,D,Ord}}) where {K,D,Ord <: Ordering} = K

"""
    valtype(sc)

Returns the value type for SortedDict and SortedMultiDict. This
function may also be applied to the type itself. Time: O(1)
"""
@inline valtype(m::SortedDict{K,D,Ord}) where {K,D,Ord <: Ordering} = D
@inline valtype(::Type{SortedDict{K,D,Ord}}) where {K,D,Ord <: Ordering} = D

"""
    ordtype(sc)

Returns the order type for SortedDict, SortedMultiDict and
SortedSet. This function may also be applied to the type itself.
Time: O(1)
"""
@inline ordtype(m::SortedDict{K,D,Ord}) where {K,D,Ord <: Ordering} = Ord
@inline ordtype(::Type{SortedDict{K,D,Ord}}) where {K,D,Ord <: Ordering} = Ord

"""
    orderobject(sc)

Returns the order object used to construct the container. Time: O(1)
"""
@inline orderobject(m::SortedDict) = m.bt.ord

## First and last return the first and last (key,data) pairs
## in the SortedDict.  It is an error to invoke them on an
## empty SortedDict.

"""
    first(sc)

Argument `sc` is a SortedDict, SortedMultiDict or SortedSet. This
function returns the first item (a `k=>v` pair for SortedDict and
SortedMultiDict or a key for SortedSet) according to the sorted
order in the container. Thus, `first(sc)` is equivalent to
`deref((sc,startof(sc)))`. It is an error to call this function on
an empty container. Time: O(log *n*)
"""
@inline function first(m::SortedDict)
    i = beginloc(m.bt)
    i == 2 && throw(BoundsError())
    return Pair(m.bt.data[i].k, m.bt.data[i].d)
end

"""
    last(sc)

Argument `sc` is a SortedDict, SortedMultiDict or SortedSet. This
function returns the last item (a `k=>v` pair for SortedDict and
SortedMultiDict or a key for SortedSet) according to the sorted
order in the container. Thus, `last(sc)` is equivalent to
`deref((sc,endof(sc)))`. It is an error to call this function on an
empty container. Time: O(log *n*)
"""
@inline function last(m::SortedDict)
    i = endloc(m.bt)
    i == 1 && throw(BoundsError())
    return Pair(m.bt.data[i].k, m.bt.data[i].d)
end

"""
    haskey(sc,k)

Returns true if key `k` is present for SortedDict, SortedMultiDict
or SortedSet `sc`. For SortedSet, `haskey(sc,k)` is a synonym for
`in(k,sc)`. For SortedDict and SortedMultiDict, `haskey(sc,k)` is
equivalent to `in(k,keys(sc))`. Time: O(*c* log *n*)
"""
@inline function haskey(m::SortedDict, k_)
    i, exactfound = findkey(m.bt, convert(keytype(m), k_))
    exactfound
end

"""
    get(sd,k,v)

Returns the value associated with key `k` where `sd` is a
SortedDict, or else returns `v` if `k` is not in `sd`. Time: O(*c*
log *n*)
"""
function get(default_::Union{Function,Type}, m::SortedDict{K,D}, k_) where {K,D}
    i, exactfound = findkey(m.bt, convert(K, k_))
    return exactfound ? m.bt.data[i].d : convert(D, default_())
end

get(m::SortedDict, k_, default_) = get(()->default_, m, k_)

"""
    get!(sd,k,v)
Returns the value associated with key `k` where `sd` is a
SortedDict, or else returns `v` if `k` is not in `sd`, and in the
latter case, inserts `(k,v)` into `sd`. Time: O(*c* log *n*)
"""
function get!(default_::Union{Function,Type}, m::SortedDict{K,D}, k_) where {K,D}
    k = convert(K,k_)
    i, exactfound = findkey(m.bt, k)
    if exactfound
        return m.bt.data[i].d
    else
        default = convert(D, default_())
        insert!(m.bt,k, default, false)
        return default
    end
end

get!(m::SortedDict, k_, default_) = get!(()->default_, m, k_)

"""
    getkey(sd,k,defaultk)

Returns key `k` where `sd` is a SortedDict, if `k` is in `sd` else
it returns `defaultk`. If the container uses in its ordering an `eq`
method different from isequal (e.g., case-insensitive ASCII strings
illustrated below), then the return value is the actual key stored
in the SortedDict that is equivalent to `k` according to the `eq`
method, which might not be equal to `k`. Similarly, if the user
performs an implicit conversion as part of the call (e.g., the
container has keys that are floats, but the `k` argument to `getkey`
is an Int), then the returned key is the actual stored key rather
than `k`. Time: O(*c* log *n*)
"""
function getkey(m::SortedDict{K,D,Ord}, k_, default_) where {K,D,Ord <: Ordering}
    i, exactfound = findkey(m.bt, convert(K, k_))
    exactfound ? m.bt.data[i].k : convert(K, default_)
end

## Function delete! deletes an item at a given
## key
"""
    delete!(sc, k)

Argument `sc` is a SortedDict or SortedSet and `k` is a key. This
operation deletes the item whose key is `k`. It is a `KeyError` if
`k` is not a key of an item in the container. After this operation
is complete, any token addressing the deleted item is invalid.
Returns `sc`. Time: O(*c* log *n*)
"""
@inline function delete!(m::SortedDict, k_)
    i, exactfound = findkey(m.bt, convert(keytype(m), k_))
    !exactfound && throw(KeyError(k_))
    delete!(m.bt, i)
    m
end

"""
    pop!(sc, k)

Deletes the item with key `k` in SortedDict or SortedSet `sc` and
returns the value that was associated with `k` in the case of
SortedDict or `k` itself in the case of SortedSet. A `KeyError`
results if `k` is not in `sc`. Time: O(*c* log *n*)
"""
@inline function pop!(m::SortedDict, k_)
    i, exactfound = findkey(m.bt, convert(keytype(m), k_))
    !exactfound && throw(KeyError(k_))
    d = m.bt.data[i].d
    delete!(m.bt, i)
    d
end

## Check if two SortedDicts are equal in the sense of containing
## the same (K,D) pairs.  This sense of equality does not mean
## that semitokens valid for one are also valid for the other.
"""
    isequal(sc1,sc2)

Checks if two containers are equal in the sense that they contain
the same items; the keys are compared using the `eq` method, while
the values are compared with the `isequal` function. In the case of
SortedMultiDict, equality requires that the values associated with a
particular key have same order (that is, the same insertion order).
Note that `isequal` in this sense does not imply any correspondence
between semitokens for items in `sc1` with those for `sc2`. If the
equality-testing method associated with the keys and values implies
hash-equivalence in the case of SortedDict, then `isequal` of the
entire containers implies hash-equivalence of the containers. Time:
O(*cn* + *n* log *n*)
"""
function isequal(m1::SortedDict, m2::SortedDict)
    ord = orderobject(m1)
    if !isequal(ord, orderobject(m2)) || !isequal(eltype(m1), eltype(m2))
        throw(ArgumentError("Cannot use isequal for two SortedDicts unless their element types and ordering objects are equal"))
    end
    p1 = startof(m1)
    p2 = startof(m2)
    while true
        if p1 == pastendsemitoken(m1)
            return p2 == pastendsemitoken(m2)
        end
        if p2 == pastendsemitoken(m2)
            return false
        end
        k1,d1 = deref((m1,p1))
        k2,d2 = deref((m2,p2))
        if !eq(ord,k1,k2) || !isequal(d1,d2)
            return false
        end
        p1 = advance((m1,p1))
        p2 = advance((m2,p2))
    end
end

function mergetwo!(m::SortedDict{K,D,Ord},
                   m2::AbstractDict{K,D}) where {K,D,Ord <: Ordering}
    for (k,v) in m2
        m[convert(K,k)] = convert(D,v)
    end
end

"""
    packcopy(sc)

This returns a copy of `sc` in which the data is packed. When
deletions take place, the previously allocated memory is not
returned. This function can be used to reclaim memory after many
deletions. Time: O(*cn* log *n*)
"""
function packcopy(m::SortedDict{K,D,Ord}) where {K,D,Ord <: Ordering}
    w = SortedDict(Dict{K,D}(), orderobject(m))
    mergetwo!(w,m)
    w
end

"""
    packdeepcopy(sc)

This returns a packed copy of `sc` in which the keys and values are
deep-copied. This function can be used to reclaim memory after many
deletions. Time: O(*cn* log *n*)
"""
function packdeepcopy(m::SortedDict{K,D,Ord}) where {K,D,Ord <: Ordering}
    w = SortedDict(Dict{K,D}(),orderobject(m))
    for (k,v) in m
        newk = deepcopy(k)
        newv = deepcopy(v)
        w[newk] = newv
    end
    w
end

"""
    merge!(sc, sc1...)

This updates `sc` by merging SortedDicts or SortedMultiDicts `sc1`,
etc. into `sc`. These must all must have the same key-value types.
In the case of keys duplicated among the arguments, the rightmost
argument that owns the key gets its value stored for SortedDict. In
the case of SortedMultiDict all the key-value pairs are stored, and
for overlapping keys the ordering is left-to-right. This function is
not available for SortedSet, but the `union!` function (see below)
provides equivalent functionality. Time: O(*cN* log *N*), where *N*
is the total size of all the arguments.
"""
function merge!(m::SortedDict{K,D,Ord},
                others::AbstractDict{K,D}...) where {K,D,Ord <: Ordering}
    for o in others
        mergetwo!(m,o)
    end
end

"""
    merge(sc1, sc2...)

This returns a SortedDict or SortedMultiDict that results from
merging SortedDicts or SortedMultiDicts `sc1`, `sc2`, etc., which
all must have the same key-value-ordering types. In the case of keys
duplicated among the arguments, the rightmost argument that owns the
key gets its value stored for SortedDict. In the case of
SortedMultiDict all the key-value pairs are stored, and for keys
shared between `sc1` and `sc2` the ordering is left-to-right. This
function is not available for SortedSet, but the `union` function
(see below) provides equivalent functionality. Time: O(*cN* log
*N*), where *N* is the total size of all the arguments.
"""
function merge(m::SortedDict{K,D,Ord},
               others::AbstractDict{K,D}...) where {K,D,Ord <: Ordering}
    result = packcopy(m)
    merge!(result, others...)
    result
end

"""
    similar(sc)

Returns a new SortedDict, SortedMultiDict, or SortedSet of the same
type and with the same ordering as `sc` but with no entries (i.e.,
empty). Time: O(1)
"""
similar(m::SortedDict{K,D,Ord}) where {K,D,Ord<:Ordering} =
    SortedDict{K,D,Ord}(orderobject(m))

isordered(::Type{T}) where {T<:SortedDict} = true


text:
using Test
using GeoParams

@testset "DiffusionCreepLaws" begin

# This tests the MaterialParameters structure
CharUnits_GEO   =   GEO_units(viscosity=1Pa*s, length=1m);

# Define a linear viscous creep law ---------------------------------
x1      =   DiffusionCreep()
@test Value(x1.n) == 1.0
@test Value(x1.p) == -3.0
@test Value(x1.A) == 1.5MPa^-1.0*s^-1*m^(3.0)

x2      =   DiffusionCreep(n=3, p=-3.0)
@test Value(x2.A) == 1.5MPa^-3.0*s^-1*m^(3.0)

# perform a computation with the dislocation creep laws
    # Calculate EpsII, using a set of pre-defined values
CharDim = GEO_units(length=1000km, viscosity=1e19Pa*s, stress=100MPa, temperature=1000C)
EpsII   = GeoUnit(1.0s^-1.0)
EpsII_nd= nondimensionalize(EpsII,CharDim)
TauII   = GeoUnit(0.3MPa)
TauII_nd= nondimensionalize(TauII,CharDim)
P       = GeoUnit(1.0e9Pa)
P_nd    = nondimensionalize(P,CharDim)
T       = GeoUnit(1400C)
T_nd    = nondimensionalize(T,CharDim)
f       = GeoUnit(1000NoUnits)
f_nd    = nondimensionalize(f,CharDim)
d       = GeoUnit(10mm)
d_nd    = nondimensionalize(d,CharDim)
c       = CreepLawVariables(P=P,T=T,f=f,d=d)
#Phase   = SetMaterialParams(Name="Viscous Matrix", Phase=1,
#                                     Density   = ConstantDensity(),
#                                     CreepLaws = DiffusionCreep(n=1.0NoUnits, r=1.0NoUnits, p=-3.0NoUnits, A=1.0e6MPa^(-1.0)*s^(-1.0)*µm^(3.0), E=335000.0J/mol, V=4.0e-6m^(3.0)/mol, Apparatus="Invariant"), CharDim = CharDim)
#EpsII.val = ComputeCreepLaw_EpsII(TauII,Phase.CreepLaws[1],c)
# Tested value is Dimensional so need to non-dimensionalize it back to compare
#@test EpsII.val  ≈ nondimensionalize(7.823165072337786e-15s^(-1.0), CharDim) rtol = 1e-12
    # Check that once inverted, we get back the TauII that we used to calculate EpsII
#NewTau = ComputeCreepLaw_TauII(EpsII,Phase.CreepLaws[1],c)
#@show NewTau
#@test NewTau ≈ TauII.val

# Given stress
#@test ComputeCreepLaw_EpsII(1e6Pa, x1, CreepLawParams())==5e-13/s                # dimensional input

# Given strainrate
#@test ComputeCreepLaw_TauII(1e-13/s, x1, CreepLawParams())==1e18*2*1e-13Pa       # dimensional input
# -------------------------------------------------------------------

end


text:
module Move_Pendulum

import Modia3D

# Properties
vmat1 = Modia3D.VisualMaterial(color="LightBlue", transparency=0.5)
vmat2 = Modia3D.VisualMaterial(color="Red")
Lx = 1.6
Ly = 0.2*Lx
Lz = 0.2*Lx
m  = 1.0

# Pendulum
world = Modia3D.Object3D(feature = Modia3D.Visual(shape =
    Modia3D.CoordinateSystem(length=0.5*Lx)))
body = Modia3D.Object3D(feature = Modia3D.Solid(
    shape = Modia3D.Beam(axis=1, length=Lx, width=Ly, thickness=Lz), massProperties=Modia3D.MassProperties(mass=m), visualMaterial=vmat1))
frame1 = Modia3D.Object3D(parent=body, translation=[-Lx/2, 0.0, 0.0])
cyl = Modia3D.Object3D(parent=frame1, feature = Modia3D.Visual(shape =
    Modia3D.Cylinder(axis=3, diameter=Ly/2, length=1.2*Ly), visualMaterial=vmat2))
rev = Modia3D.Revolute(obj1=world, obj2=frame1)

# Visualize pendulum
# Modia3D.visualizeWorld!(world, sceneOptions=Modia3D.SceneOptions(visualizeFrames=true, defaultFrameLength=0.3*Lx))

scene = Modia3D.Scene(Modia3D.SceneOptions(visualizeFrames=true, defaultFrameLength=0.3*Lx))
Modia3D.initAnalysis!(world, scene)

tStart = 0.0
tEnd   = 3.0

for time = range(tStart, stop=tEnd, length=101)
    delta_phi = Modia3D.linearMovement(2*pi, tStart, tEnd, time)
    Modia3D.setAngle!(rev, delta_phi)
    Modia3D.updatePosition!(world)
    Modia3D.visualize!(scene, time)
end
Modia3D.closeAnalysis!(scene)

println("... test/old/Move_Pendulum.jl completed.")

end


text:
# API definition file for data layer abstraction.
# The idea is for a one line call to change from internal Graphs.jl to CloudGraphs.jl
# switch for as data layer.

type DataLayerAPI
  addvertex!::Function
  getvertex::Function
  # setupvertgraph!::Function
  # setupfncvertgraph!::Function
  makeaddedge!::Function
  getedge::Function
  outneighbors::Function
  updatevertex!::Function
  updateedge!::Function
  deletevertex!::Function
  deleteedge!::Function
  cgEnabled::Bool
end

# global dlapi

dlapi = DataLayerAPI(addGraphsVert!,          # addvertex
                     getVertNode,             # getvertex
                     makeAddEdge!,            # makeaddedge
                     graphsGetEdge,           # getedge
                     graphsOutNeighbors,      # outneighbors
                     updateFullVertData!,     # updatevertex!
                     +,                       # updateedge!
                     graphsDeleteVertex!,     # deletevertex!
                     +,                       # deleteedge!
                     false )

# Remember 3rd party users interact with
# addNode!
# addFactor!
# prepBatchTree!
# inferOverTree!
#
## Visualization functions -- Visualization may well be separated or abstracted out
# writeGraphPdf
# drawHorBeliefsList


text:
#-------------------------------------------------------------------
#* EMSO Model Library (EML) Copyright (C) 2004 - 2007 ALSOC.
#*
#* This LIBRARY is free software; you can distribute it and/or modify
#* it under the therms of the ALSOC FREE LICENSE as available at
#* http://www.enq.ufrgs.br/alsoc.
#*
#* EMSO Copyright (C) 2004 - 2007 ALSOC, original code
#* from http://www.rps.eng.br Copyright (C) 2002-2004.
#* All rights reserved.
#*
#* EMSO is distributed under the therms of the ALSOC LICENSE as
#* available at http://www.enq.ufrgs.br/alsoc.
#*
#*-------------------------------------------------------------------
#* Model of costs for a reboiler
#*--------------------------------------------------------------------
#*
#*	Streams:
#*		* a liquid inlet stream
#*		* a liquid outlet stream
#*		* a vapour outlet stream
#*		* a feed stream
#*
#*	Assumptions:
#*		* perfect mixing of both phases
#*		* thermodynamics equilibrium
#*		* no liquid entrainment in the vapour stream
#*
#*	Specify:
#*		* the Feed stream
#*		* the Liquid inlet stream
#*		* the outlet flows: OutletV.F and OutletL.F
#*
#*	Initial:
#*		* the reboiler temperature (OutletL.T)
#*		* the reboiler liquid level (Ll)
#*		* (NoComps - 1) OutletL (OR OutletV) compositions
#*
#*	- This Model is valid if 14m^2 < Across < 1100 m^2
#*
#*----------------------------------------------------------------------
#* Author: N�bia do Carmo Ferreira
#* $Id: reboiler.mso 210 2007-03-15 12:52:28Z arge $
#*--------------------------------------------------------------------
type reboiler_cost
	reboiler_cost()=begin
		new(
			reboiler(),
			DanaSwitcher(Dict{Symbol,Any}(
				:Valid=>["Stainless steel 316", "Stainless steel 304", "Stainless steel 347", "Nickel 200", "Monel 400", "Inconel 600", "Incoloy 825", "Titanium", "Hastelloy"],
				:Default=>"Stainless steel 316"
			)),
			fill(DanaReal()),
			currency(Dict{Symbol,Any}(
				:Brief=>"Capital Cost"
			)),
			currency(Dict{Symbol,Any}(
				:Brief=>"Basic Cost"
			)),
			positive(Dict{Symbol,Any}(
				:Brief=>"Cost Factor based on the type of the heat exchanger"
			)),
			positive(Dict{Symbol,Any}(
				:Brief=>"Cost Factor based on the project pressure"
			)),
			positive(Dict{Symbol,Any}(
				:Brief=>"Cost Factor based on the construction material"
			)),
			pressure(Dict{Symbol,Any}(
				:Brief=>"Average pressure"
			)),
			[
				:(P = 0.5*(_base_1.InletLiquid.P + _base_1.OutletLiquid.P)),
				:(Ce = Cb*Fd*Fp*Fm),
				:(Cb = "US\$"*exp(Cost(1,1) + Cost(1,2)*ln(Across/"m^2") + Cost(1,3)*(ln(Across/"m^2"))^2)),
				:(Fd = exp(Cost(3,1) + Cost(3,2)*ln(Across/"m^2") + Cost(3,3)*ln(Across/"m^2"))),
				:(Fp = Cost(5,1) + Cost(5,2)*ln(Across/"m^2")),
				:(Fp = Cost(5,1) + Cost(5,2)*ln(Across/"m^2")),
				:(Fp = Cost(6,1) + Cost(6,2)*ln(Across/"m^2")),
				:(Fp = Cost(7,1) + Cost(7,2)*ln(Across/"m^2")),
				:(Fp = Cost(7,1) + Cost(7,2)*ln(Across/"m^2")),
				:(Fm = Cost(8,1) + Cost(8,2)*ln(Across/"m^2")),
			],
			[
				"Average pressure","Capital Cost","Basic Cost","Cost Factor based on the type of the heat exchanger","Cost Factor based on the project pressure","","","","","Cost Factor based on the construction material",
			],
			[:Material,:Cost,],
			[:Ce,:Cb,:Fd,:Fp,:Fm,:P,]
		)
	end
	_base_1::reboiler
	Material::DanaSwitcher
	Cost::Array{DanaReal}
	Ce::currency
	Cb::currency
	Fd::positive
	Fp::positive
	Fm::positive
	P::pressure
	equations::Array{Expr,1}
	equationNames::Array{String,1}
	parameters::Array{Symbol,1}
	variables::Array{Symbol,1}
	attributes::Dict{Symbol,Any}
end
export reboiler_cost
function setEquationFlow(in::reboiler_cost)
	addEquation(1)
	addEquation(2)
	addEquation(3)
	addEquation(4)
	if P <= 700 * "kPa"
		# verificar
		addEquation(5)
	else
		if 700 * "kPa" < P && P < 2100 * "kPa"
			addEquation(6)
		else
			if 2100 * "kPa" < P && P < 4200 * "kPa"
				addEquation(7)
			else
				if 4200 * "kPa" < P && P < 6200 * "kPa"
					addEquation(8)
				else
					addEquation(9)
					# verificar

				end
			end
		end
	end
	addEquation(10)
end
function atributes(in::reboiler_cost,_::Dict{Symbol,Any})
	fields::Dict{Symbol,Any}=Dict{Symbol,Any}()
	fields[:Pallete]=true
	fields[:Icon]="icon/Reboiler"
	drive!(fields,_)
	return fields
end
reboiler_cost(_::Dict{Symbol,Any})=begin
	newModel=reboiler_cost()
	newModel.attributes=atributes(newModel,_)
	newModel
end


text:
function sign_conditions_satisfied(funcs, xc, sign_conditions)
    return all(i -> funcs[i](xc) * sign_conditions[i] >= 0.0, 1:length(funcs))
end

function one_dimensional_quadrature(
    funcs,
    sign_conditions,
    lo,
    hi,
    quad1d::ReferenceQuadratureRule{T},
) where {T}

    nfuncs = length(funcs)
    nconds = length(sign_conditions)
    @assert nfuncs == nconds

    quad = TemporaryQuadrature(T, 1)

    roots = roots_and_ends(funcs, lo, hi)
    for j = 1:length(roots)-1
        xc = 0.5 * (roots[j] + roots[j+1])
        if sign_conditions_satisfied(funcs, xc, sign_conditions)
            p, w = transform(quad1d, roots[j], roots[j+1])
            update!(quad, p, w)
        end
    end
    return quad

end

function one_dimensional_quadrature(funcs, sign_conditions, interval, quad1d)

    return one_dimensional_quadrature(
        funcs,
        sign_conditions,
        interval.lo,
        interval.hi,
        quad1d,
    )
end

max_stars_repo_name: ArjunNarayanan/ImplicitDomainQuadrature

max_issues_repo_name: ArjunNarayanan/ImplicitDomainQuadrature

max_forks_repo_name: ArjunNarayanan/ImplicitDomainQuadrature


text:
# SOCP Constraints for angle constraints

#=

These are constraints of the form
||[x1; x2]|| ≤ α x3
Since this is a trajectory, we also likely want to use this constraint
multiple times (e.g. at each step in the discretization)

This file handles these constraints such that it can be integrated into
the "constraintManager" framework.

Mathematics:
For the rocket soft-landing problem, we want to limit the angle that
engine can swing. We make the following assumptions:
1. The angle is usually to a small angle < 90°.
This holds because we don't want the engine gimbal to move too far from its
nominal axial position (i.e. engine exhause aligned with the rocket's body).
We also don't want the rocket control to go unstable due to a large torque
from a large angle.
2. The thrust vector is assumed to be _exactly_ in line with the engine
pointing vector.
This is a good approximation for a good rocket. There may be small variations,
but the engine is designed to direct the exhaust.
3. The rocket is oriented with the ground. (i.e. the long axial direction of
the rocket is perpendicular to the ground)
This is a good approximation near landing since the rocket landing gear must
downward facing to interface with the ground.

Then, we calculate the radial distance as ||ux|| in 2D and ||[ux; uy]|| in 3D.
The angle against the z component is
θ = arctan(||uxy|| / -uz)
The "-uz" is to have "-uz > 0" since the engine points downward. We want
θ < θ_max
Equivalently,
arctan(||uxy|| / -uz) < θ_max
||uxy|| < - tan(θ_max) uz

Call tan(θ_max) = α. Thus, we recover
||ux|| < - α uy in 2D
||[ux; uy]|| < - α uz in 3D

as desired.

=#

using SparseArrays, LinearAlgebra

include("projections.jl")

# -----------------------
# Simple Second-Order Cone Constraint for angle type constraints
# -----------------------
@doc raw"""
    AL_simpleAngleCone(alpha::Float64,
                       sIndicate::Diagonal{Int64,Array{Int64,1}},
                       tIndicate::Array{Int64,1})

Creates a simple Second Order Cone Constraint (e.g. for an SOCP) of the form
```math
||sIndicate * x|| ≤ alpha * tIndicate * x
```

So, in more standard, this is becomes:
```math
||sIndicate * x|| - alpha * tIndicate * x ≤ 0
```

For compactness, we will write this as
```math
||s|| - alpha t ≤ 0
```

To check constraint satisfaction, use:
[`satisified`](@ref) and [`whichSatisfied`](@ref)

To evaluate the constraint, use:
- [`getRaw`](@ref) to evaluate the standard form.
- [`getNormToProjVals`](@ref) to evaluate the cone using projections.

"""
struct AL_simpleAngleCone <: constraint
    alpha::Float64
    sIndicate::Diagonal{Int64,Array{Int64,1}}
    tIndicate::Array{Int64,1}
end

get_s(r::AL_simpleAngleCone, x) = r.sIndicate * x
get_t(r::AL_simpleAngleCone, x) = r.alpha * r.tIndicate' * x

"""
    getRaw(r::AL_simpleAngleCone, x)

Evaluate the constraint without projections.
"""
getRaw(r::AL_simpleAngleCone, x) = norm(get_s(r, x)) - get_t(r, x)

"""
    satisfied(r::AL_simpleAngleCone, x)

Check constraint satisfaction. See also [`whichSatisfied`](@ref)
"""
satisfied(r::AL_simpleAngleCone, x) = (getRaw(r, x) ≤ 0)

"""
    whichSatisfied(r::AL_simpleAngleCone, xArr)

Check constraint satisfaction for each element in `xArr`.
"""
whichSatisfied(r::AL_simpleAngleCone, xArr) = [satisfied(r, x) for x in xArr]

"""
    getProjVecs(r::AL_simpleAngleCone, x, filled = true)

Gets the projection vectors for `x` onto the the second order cone defined
by `||sIndicate * x|| - alpha * tIndicate * x`.

See also [`AL_simpleAngleCone`](@ref)
"""
function getProjVecs(r::AL_simpleAngleCone, x, filled = true)
    return projSecondOrderCone(get_s(r, x), get_t(r, x), filled)
end

"""
    getNormToProjVals(r::AL_simpleAngleCone, x, λ = 0)

Get the constraint violation for the second order cone constraints.

The parameter `λ` describes the dual of the constraint and is used to determine
if the constraint is active. See [`coneActive`](@ref) for more information.
"""
function getNormToProjVals(r::AL_simpleAngleCone, x, λ = 0)
    s = get_s(r, x)
    t = get_t(r, x)
    active, filled = coneActive(s, t, λ)
    pv = getProjVecs(r, x, filled)

    return [norm([s; t] - pv)]
end

"""
    getGradC(r::AL_simpleAngleCone, t)

Calculates the gradient of a constraint.

For `r::AL_simpleAngleCone`, this is the `∇c(x) = [s / ||s||; - alpha]`

There is a singularity at the tip.
"""
function getGradC(r::AL_simpleAngleCone, x)

    if satisfied(r, x)
        return zeros(size(x, 1))
    end

    s = get_s(r, x)

    # ds will have 0 in the disregarded spot for dt
    # Ex: s1 = Diagonal([1; 0; 1]) * [4; 5; 6] -> [4; 0; 6]
    #     ds1 = [0.5547001962252291;  0.0;  0.8320502943378437]
    if norm(s) == 0.0
        ds = 0.0 * s
    else
        ds = s / norm(s)
    end

    # dt will have 0 in the disregarded spots for ds
    # Ex: t1 = [0; 1; 0]' * [4; 5; 6]
    #     dt1 = [0.0;  -0.2;  -0.0]
    dt = - r.alpha * r.tIndicate

    # Adding it together gets the actual grad.
    return ds + dt
end

"""
    getHessC(r::AL_simpleAngleCone, t)

Calculate the hessian of a constraint.

For `r::AL_simpleAngleCone`, `H(c(x)) = 0`
"""
function getHessC(r::AL_simpleAngleCone, x)
    return spzeros(size(x, 1), size(x, 1))
end

"""
    getHessC_ALTerm(r::AL_simpleAngleCone, t, rho = 1)

This is the hessian of the *constraint term* as it appears in the
augmented lagrangian.

For `r::AL_simpleAngleCone`,
`H(ρ c(x)'c(x) + λ c(x)) = ρ [s * s' / norm(s)^2, -alpha s/norm(s);
-alpha s/norm(s), alpha^2`
"""
function getHessC_ALTerm(r::AL_simpleAngleCone, x, rho = 1)
    #=
    The augmented lagrangian constraint term is of the form:
    ρ c(x)'c(x) + λ c(x)

    where c(x) = x / ||x||.

    The Hessian matrix is then
    Htot = ρ (c.H + J.J + H.c) + λ H

    But H = 0 for Affine equalities, so when the constraint is not satisfied,
    Htot = ρ (J.J) =
    ρ * [s * s' / norm(s)^2     -alpha s/norm(s)]
        [-alpha s'/norm(s)       alpha^2]
    When the constraint is satisfied, it is just zero.

    However, we want this to work which ever order we have "s" and "t". And,
    more precisely, we have "s" as the same length as "x", with zeros in the
    gaps for t. Hence, we use four indicators:

    s * s' / norm(s)^2 -> sI2 * sI2'
    -alpha s/norm(s)   -> sI2 * tI'
    -alpha s'/norm(s)  -> (sI2 * tI')'
    alpha^2            -> tI * tI'

    where tI is r.tIndicate and sI2 is diag(sI)
    =#

    if satisfied(r, x)
        # Returns a sparse zero matrix of the right dimensions
        return getHessC(r, x)
    end

    # Get s. t is not used
    s = get_s(r, x)
    ns = norm(s)

    # Get the indicators
    sI2 = diag(r.sIndicate)
    tI = r.tIndicate

    a = (s * s' / (ns^2)) .* (sI2 * sI2')
    b = (- r.alpha * s / ns) .* (sI2 * tI')
    bT = (- r.alpha * s' / ns) .* (sI2 * tI')'
    c = (r.alpha^2) * (tI * tI')

    return rho * Symmetric(a + b + bT + c)
end

# -----------------------
# Simple Second-Order Cone Constraints for Multiple angle type constraints
# -----------------------
@doc raw"""
    AL_Multiple_AngleCone(
                        sc::AL_simpleAngleCone,
                        nDim::Int64,
                        indicatorList::Tuple{Array{Int64,1},Array{Int64,1}})

Handles Multiple Second Order Cone Constraint (SOCP) of the form
```math
||s_k|| ≤ alpha t_k \quad \text{for} \ k ∈ N
```
where T is a list of primal decision vectors.

So, in more standard form, this is becomes:
```math
||s_k|| - alpha t_k ≤ 0 \quad \text{for} \ k ∈ N
```

To check constraint satisfaction, use:
[`satisified`](@ref) and [`whichSatisfied`](@ref)

To evaluate the constraint, use:
- [`getRaw`](@ref) to evaluate each cone without projections.
- [`getNormToProjVals`](@ref) to evaluate each cone using projections.

"""
struct AL_Multiple_AngleCone <: constraint
    sc::AL_simpleAngleCone
    nDim::Int64
    #Type of findnz(sparseArray)[1] or findall(!iszero, sparseArray)
    indicatorList::Array{Int64,1}
end

function makeAL_Multiple_AngleCone(alpha::Float64,
                sIndicate::Diagonal{Int64,Array{Int64,1}},
                tIndicate::Array{Int64,1}, nDim::Int64,
                indicatorList::Array{Int64,1})
    return AL_Multiple_AngleCone(
                    AL_simpleAngleCone(alpha, sIndicate, tIndicate),
                    nDim, indicatorList)
end

"""
    parseRelevantSteps(r::AL_Multiple_AngleCone, x)

Run through the indicator list and use the dimensions to extract the
relevant vectors from x
"""
function parseRelevantSteps(r::AL_Multiple_AngleCone, x)
    # This array will store the relevant parts of the x vector.
    steps = []

    for ind in r.indicatorList
        # Find the relevant parts and push them as a group
        push!(steps, x[ind:(ind + r.nDim - 1)])
    end

    return steps
end

"""
    getRaw(r::AL_Multiple_AngleCone, x)

Evaluate each of the simple cone constraints without projection.
"""
function getRaw(r::AL_Multiple_AngleCone, x)
    # Use the parsed steps and calculate the raw value for each set.
    steps = parseRelevantSteps(r, x)
    return [getRaw(r.sc, s) for s in steps]
end

"""
    getProjVecs(r::AL_Multiple_AngleCone, steps, filled)

Get the projection defined by [`AL_simpleAngleCone`](@ref) for each of the
constraints.

See also [`AL_Multiple_AngleCone`](@ref)
"""
function getProjVecs(r::AL_Multiple_AngleCone, steps, filled)
    # Then loop through the parsed steps to get the projection.
    pv = []
    for (i, s) in enumerate(steps)
        # Filled is needed to determine which projection and may be different
        # for each of the different SOCP constraints in question.
        push!(pv, getProjVecs(r.sc, s, filled[i]))
    end

    return pv
end

"""
    getNormToProjVals(r::AL_Multiple_AngleCone, x, λ)

Get the constraint violation for multiple second order cone constraints. The
length of the vector `λ` must match the number of steps.

See also [`AL_Multiple_AngleCone`](@ref) and [`coneActive`](@ref) for more
information.
"""
function getNormToProjVals(r::AL_Multiple_AngleCone, x, λ)
    # Use the parsed steps to separate our each constraint
    steps = parseRelevantSteps(r, x)

    # Determine which of the constraints (if any) require an unfilled cone/
    fillVec = Bool[]
    for (ind, st) in enumerate(steps)
        s = get_s(r.sc, st)
        t = get_t(r.sc, st)
        push!(fillVec, coneActive(s, t, λ[ind])[2])
    end

    # Using the filled information and the steps in question, get the
    # projection vectors
    pvs = getProjVecs(r, steps, fillVec)

    # Use the projection vectors to calculate the constraints.
    constVio = []
    for (st, p) in zip(steps, pvs)
        s = get_s(r.sc, st)
        t = get_t(r.sc, st)
        push!(constVio, norm([s; t] - p))
    end

    # If there are NaN values (this happens if you take the norm of a zero
    # vector that is large), use the snippet of code below.
    return replace(constVio, NaN=>0.0)
end

function getNormToProjVals(r::AL_Multiple_AngleCone, x)
    lambdaVec = spzeros(size(r.indicatorList, 1))
    return getNormToProjVals(r, x, lambdaVec)
end

"""
    getGradC(r::AL_Multiple_AngleCone, x)

Parses the relevant `x` vector for each cone constraint. It calculates the
gradient of each and then stiches it into a jacobian of the same dimension
as `x` and number of constraints with the zeros in relevant indicies.

(Result is a sparse array)

See also [`AL_Multiple_AngleCone`](@ref) and [`AL_simpleCone`](@ref)
"""
function getGradC(r::AL_Multiple_AngleCone, x)
    # Use the parsed steps to separate our each constraint
    steps = parseRelevantSteps(r, x)

    # Use the AL_simpleCone to get the gradient for each constraint
    gradList = [getGradC(r.sc, st) for st in steps]

    # Stitch it back together.
    gradStitched = spzeros(size(r.indicatorList, 1), size(x, 1))

    for (ind, pos) in enumerate(r.indicatorList)
        posEnd = pos + r.nDim - 1
        gradStitched[ind, pos:posEnd] = gradList[ind]
    end

    return gradStitched
end

"""
    getHessC(r::AL_Multiple_AngleCone, x)

Calculate the hessian of a constraint.

For `r::AL_Multiple_AngleCone`, `H(c(x)) = 0`

See also [`AL_Multiple_AngleCone`](@ref) and [`AL_simpleAngleCone`](@ref)
"""
function getHessC(r::AL_Multiple_AngleCone, x)
    return spzeros(size(x, 1), size(x, 1))
end

"""
    getHessC_ALTerm(r::AL_Multiple_AngleCone, x, rho = 1)

This is the hessian of the *constraint term* as it appears in the
augmented lagrangian.

For `r::AL_Multiple_AngleCone`,
`H(ρ c(x)'c(x) + λ c(x)) = ρ s * s' / norm(s)^2` for each constraint.

Parses the relevant `x` vector for each cone constraint. It calculates the
hessian of each and then stiches it into a vector of the same dimension
as `x`×`x` with the zeros in relevant indicies.

(Result is a sparse array)

See also [`AL_Multiple_AngleCone`](@ref) and [`AL_simpleAngleCone`](@ref)
"""
function getHessC_ALTerm(r::AL_Multiple_AngleCone, x, rho = 1)
    #=
    The augmented lagrangian constraint term is of the form:
    ρ c(x)'c(x) + λ c(x)

    where c(x) = x / ||x||.

    The Hessian matrix is then
    Htot = ρ (c.H + J.J + H.c) + λ H

    But H = 0 for Affine equalities, so
    Htot = ρ (J.J) = ρ s * s' / norm(s)^2

    For each constraint!
    =#

    # Use the parsed steps to separate our each constraint
    steps = parseRelevantSteps(r, x)

    # Use the AL_simpleCone to get the total hessian for each constraint
    hessList = [Symmetric(getHessC_ALTerm(r.sc, st)) for st in steps]

    # Stitch it back together.
    hessStitched = spzeros(size(x, 1), size(x, 1))

    for (ind, pos) in enumerate(r.indicatorList)
        posEnd = pos + r.nDim - 1
        hessStitched[pos:posEnd, pos:posEnd] = hessList[ind]
    end

    return hessStitched

end

max_stars_repo_name: danineamati/TrajOptSOCPs


text:
using OrdinaryDiffEq, StaticArrays, Test

## Mass Matrix

function rober_oop(u,p,t)
  y₁,y₂,y₃ = u
  k₁,k₂,k₃ = p
  du1 = -k₁*y₁ + k₃*y₂*y₃
  du2 =  k₁*y₁ - k₃*y₂*y₃ - k₂*y₂^2
  du3 =  y₁ + y₂ + y₃ - 1
  [du1,du2,du3]
end
M = [1. 0  0
     0  1. 0
     0  0  0]
f_oop = ODEFunction(rober_oop,mass_matrix=M)
prob_mm = ODEProblem(f_oop,[1.0,0.0,0.0],(0.0,1e5),(0.04,3e7,1e4))
sol = solve(prob_mm,Rosenbrock23(autodiff=false),reltol=1e-8,abstol=1e-8)
@test sol[1] == [1.0,0.0,0.0] # Ensure initialization is unchanged if it works at the start!
sol = solve(prob_mm,Rosenbrock23(),reltol=1e-8,abstol=1e-8,initializealg=ShampineCollocationInit())
@test sol[1] == [1.0,0.0,0.0] # Ensure initialization is unchanged if it works at the start!

prob_mm = ODEProblem(f_oop,[1.0,0.0,0.2],(0.0,1e5),(0.04,3e7,1e4))
sol = solve(prob_mm,Rosenbrock23(),reltol=1e-8,abstol=1e-8)
@test sum(sol[1]) ≈ 1
@test sol[1] ≈ [1.0,0.0,0.0]
for alg in [Rosenbrock23(autodiff=false), Trapezoid()]
  local sol
  sol = solve(prob_mm,alg,reltol=1e-8,abstol=1e-8,initializealg=ShampineCollocationInit())
  @test sum(sol[1]) ≈ 1
end

function rober(du,u,p,t)
  y₁,y₂,y₃ = u
  k₁,k₂,k₃ = p
  du[1] = -k₁*y₁ + k₃*y₂*y₃
  du[2] =  k₁*y₁ - k₃*y₂*y₃ - k₂*y₂^2
  du[3] =  y₁ + y₂ + y₃ - 1
  nothing
end
M = [1. 0  0
     0  1. 0
     0  0  0]
f = ODEFunction(rober,mass_matrix=M)
prob_mm = ODEProblem(f,[1.0,0.0,0.0],(0.0,1e5),(0.04,3e7,1e4))
sol = solve(prob_mm,Rodas5(autodiff=false),reltol=1e-8,abstol=1e-8)
@test sol[1] == [1.0,0.0,0.0] # Ensure initialization is unchanged if it works at the start!
sol = solve(prob_mm,Rodas5(),reltol=1e-8,abstol=1e-8,initializealg=ShampineCollocationInit())
@test sol[1] == [1.0,0.0,0.0] # Ensure initialization is unchanged if it works at the start!

prob_mm = ODEProblem(f,[1.0,0.0,1.0],(0.0,1e5),(0.04,3e7,1e4))
sol = solve(prob_mm,Rodas5(),reltol=1e-8,abstol=1e-8)
@test sum(sol[1]) ≈ 1
@test sol[1] ≈ [1.0,0.0,0.0]

for alg in [Rodas5(autodiff=false), Trapezoid()]
  local sol
  sol = solve(prob_mm,alg,reltol=1e-8,abstol=1e-8,initializealg=ShampineCollocationInit())
  @test sum(sol[1]) ≈ 1
end

## DAEProblem

f = function (du,u,p,t)
  out1 = - 0.04u[1]              + 1e4*u[2]*u[3] - du[1]
  out2 = + 0.04u[1] - 3e7*u[2]^2 - 1e4*u[2]*u[3] - du[2]
  out3 = u[1] + u[2] + u[3] - 1.0
  [out1,out2,out3]
end

u₀ = [1.0, 0, 0]
du₀ = [0.0, 0.0, 0.0]
tspan = (0.0,100000.0)
differential_vars = [true,true,false]
prob = DAEProblem(f,du₀,u₀,tspan,differential_vars=differential_vars)
integrator = init(prob, DABDF2())

@test integrator.du[1] ≈ -0.04 atol=1e-9
@test integrator.du[2] ≈  0.04 atol=1e-9
@test integrator.u ≈ u₀ atol=1e-9

integrator = init(prob, DImplicitEuler())

@test integrator.du[1] ≈ -0.04 atol=1e-9
@test integrator.du[2] ≈  0.04 atol=1e-9
@test integrator.u ≈ u₀ atol=1e-9

integrator = init(prob, DFBDF())

@test integrator.du[1] ≈ -0.04 atol=1e-9
@test integrator.du[2] ≈  0.04 atol=1e-9
@test integrator.u ≈ u₀ atol=1e-9

u₀ = [1.0, 0, 0.2]
prob = DAEProblem(f,du₀,u₀,tspan,differential_vars=differential_vars)
integrator = init(prob, DABDF2())
@test integrator.u ≈ [1.0, 0, 0.0] atol=1e-9
integrator = init(prob, DABDF2(), initializealg = ShampineCollocationInit())
@test !(integrator.u ≈ [1.0, 0, 0.0])

u₀ = [1.0, 0, 0.2]
prob = DAEProblem(f,du₀,u₀,tspan)
integrator = init(prob, DABDF2())
@test !(integrator.u ≈ [1.0, 0, 0.0])

f = function (out,du,u,p,t)
  out[1] = - 0.04u[1]              + 1e4*u[2]*u[3] - du[1]
  out[2] = + 0.04u[1] - 3e7*u[2]^2 - 1e4*u[2]*u[3] - du[2]
  out[3] = u[1] + u[2] + u[3] - 1.0
end

u₀ = [1.0, 0, 0]
du₀ = [0.0, 0.0, 0.0]
tspan = (0.0,100000.0)
differential_vars = [true,true,false]
prob = DAEProblem(f,du₀,u₀,tspan,differential_vars=differential_vars)
integrator = init(prob, DABDF2())

@test integrator.du[1] ≈ -0.04 atol=1e-9
@test integrator.du[2] ≈  0.04 atol=1e-9
@test integrator.u ≈ u₀ atol=1e-9

integrator = init(prob, DImplicitEuler())

@test integrator.du[1] ≈ -0.04 atol=1e-9
@test integrator.du[2] ≈  0.04 atol=1e-9
@test integrator.u ≈ u₀ atol=1e-9

integrator = init(prob, DFBDF())

@test integrator.du[1] ≈ -0.04 atol=1e-9
@test integrator.du[2] ≈  0.04 atol=1e-9
@test integrator.u ≈ u₀ atol=1e-9

u₀ = [1.0, 0, 0.2]
prob = DAEProblem(f,du₀,u₀,tspan,differential_vars=differential_vars)
integrator = init(prob, DABDF2())
@test integrator.u ≈ [1.0, 0, 0.0] atol=1e-9
integrator = init(prob, DABDF2(), initializealg = ShampineCollocationInit())
@test !(integrator.u ≈ [1.0, 0, 0.0])

u₀ = [1.0, 0, 0.2]
prob = DAEProblem(f,du₀,u₀,tspan)
integrator = init(prob, DABDF2())
@test !(integrator.u ≈ [1.0, 0, 0.0])

# Need to be able to find the consistent solution of this problem, broken right now
# analytical solution:
# 	u[1](t) ->  cos(t)
#	u[2](t) -> -sin(t)
#	u[3](t) -> 2cos(t)
f = function (out,du,u,p,t)
	out[1] = du[1] - u[2]
	out[2] = du[2] + u[3] - cos(t)
	out[3] = u[1] - cos(t)
end

u₀ = [1.0, 0.0, 0.0]
du₀ = [0.0, 0.0, 0.0]
tspan = (0.0,1.0)
differential_vars = [true, true, false]
prob = DAEProblem(f,du₀,u₀,tspan,differential_vars=differential_vars)
integrator = init(prob, DABDF2())

@test integrator.du[1] ≈ 0.0 atol=1e-9
@test_broken integrator.du[2] ≈ -1.0 atol=1e-9
@test_broken integrator.u[3] ≈ 2.0 atol=1e-9

f = function (du,u,p,t)
	du - u
end

u₀ = SVector(1.0)
du₀ = SVector(0.0)
tspan = (0.0, 1.0)
differential_vars = SVector(true)
prob = DAEProblem(f,du₀,u₀,tspan,differential_vars=differential_vars)
integrator = init(prob, DABDF2())

@test integrator.du ≈ [1.0] atol=1e-9

f = function (du,u,p,t)
	du .- u
end

u₀ = SA[1.0, 1.0]
du₀ = SA[0.0, 0.0]
tspan = (0.0,1.0)
differential_vars = [true, true]
prob = DAEProblem(f,du₀,u₀,tspan,differential_vars=differential_vars)
integrator = init(prob, DABDF2())

@test integrator.du[1] ≈ 1.0 atol=1e-9
@test integrator.du[2] ≈ 1.0 atol=1e-9


text:
test = """
    35
    20
    15
    25
    47
    40
    62
    55
    65
    95
    102
    117
    150
    182
    127
    219
    299
    277
    309
    576
    """

function parse_input(input::AbstractString)::Vector{Int}
    return parse.(Int, split(input, r"\s+"; keepempty = false))
end

function q1(numbers::AbstractVector{<: Integer}; cycle::Integer)
    i = cycle + findfirst(cycle + 1:length(numbers)) do i
        seeds = i - cycle:i - 1
        return !any(seeds) do j
            return numbers[i] - numbers[j] ∈ numbers[setdiff(seeds, j)]
        end
    end
    return numbers[i]
end

function q2(numbers::AbstractVector{<: Integer}, target_sum::Integer)::Int
    (a, b) = (1, 1)
    rolling_sum = numbers[1]
    while rolling_sum != target_sum
        if rolling_sum < target_sum
            b += 1
            rolling_sum += numbers[b]
        else
            rolling_sum -= numbers[a]
            a += 1
        end
    end
    return numbers[a:b] |> extrema |> sum
end

let numbers = parse_input(test),
    broken = q1(numbers; cycle = 5)
    @assert broken  == 127
    @assert q2(numbers, broken) == 62
end

numbers = parse_input(read("day09.in", String))
broken = q1(numbers; cycle = 25)
println("Q1: ", broken)
println("Q2: ", q2(numbers, broken))

max_stars_repo_name: dhanak/competitive-coding

max_issues_repo_name: dhanak/competitive-coding

max_forks_repo_name: dhanak/competitive-coding