function makeH_Ising(Nx::Int, Ny::Int, J::Real, hx, hz; pinning::Bool=false)
    s = Index(2, "Site,SpinInd")
    Z = ITensor(s, s')
    Z[s(1), s'(1)] = 0.5
    Z[s(2), s'(2)] = -0.5
    X = ITensor(s, s')
    X[s(1), s'(2)] = 0.5
    X[s(2), s'(1)] = 0.5

    hxs = hx isa Matrix ? hx : fill(hx, Ny, Nx)
    hzs = hz isa Matrix ? hz : fill(hz, Ny, Nx)

    H = Matrix{Vector{Operator}}(undef, Ny, Nx)
    @inbounds for col in 1:Nx, row in 1:Ny
        H[row, col] = Vector{Operator}()
        if row < Ny
            if J != 0.0
                op_a  = J * copy(Z)
                op_b  = copy(Z)
                sites = [row=>col, row+1=>col]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Vertical))
            end
        end
        if col < Nx
            if J != 0.0
                op_a  = J * Z
                op_b  = copy(Z)
                sites = [row=>col, row=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Horizontal))
            end
        end
        if hzs[ row, col ] != 0.0
            push!(H[row, col], Operator([row=>row], [hzs[row,col]*copy(Z)], s, Field))
        end
        if hxs[ row, col ] != 0.0
            push!(H[row, col], Operator([row=>row], [hxs[row,col]*copy(X)], s, Field))
        end
    end
    return H
end

function makeCuH_Ising(Nx::Int, Ny::Int, J::Real, hx, hz; pinning::Bool=false)
    s = Index(2, "Site,SpinInd")
    Z = ITensor(s, s')
    Z[s(1), s'(1)] = 0.5
    Z[s(2), s'(2)] = -0.5
    X = ITensor(s, s')
    X[s(1), s'(2)] = 0.5
    X[s(2), s'(1)] = 0.5

    hxs = hx isa Matrix ? hx : fill(hx, Ny, Nx)
    hzs = hz isa Matrix ? hz : fill(hz, Ny, Nx)

    H = Matrix{Vector{Operator}}(undef, Ny, Nx)
    @inbounds for col in 1:Nx, row in 1:Ny
        H[row, col] = Vector{Operator}()
        if row < Ny
            if J != 0.0
                op_a  = -J * Z
                op_b  = copy(Z)
                sites = [row=>col, row+1=>col]
                push!(H[row, col], Operator(sites, [cuITensor(op_a); cuITensor(op_b)], s, Vertical))
            end
        end
        if col < Nx
            if J != 0.0
                op_a  = -J * Z
                op_b  = copy(Z)
                sites = [row=>col, row=>col+1]
                push!(H[row, col], Operator(sites, [cuITensor(op_a); cuITensor(op_b)], s, Horizontal))
            end
        end
        if hzs[ row, col ] != 0.0
            push!(H[row, col], Operator([row=>col], [cuITensor(hzs[row,col]*copy(Z))], s, Field))
        end
        if hxs[ row, col ] != 0.0
            push!(H[row, col], Operator([row=>col], [cuITensor(hxs[row,col]*copy(X))], s, Field))
        end
    end
    return H
end

function makeH_XXZ(Nx::Int, Ny::Int, J::Real; pinning::Bool=false)
    s = Index(2, "Site,SpinInd")
    Z = ITensor(s, s')
    Z[s(1), s'(1)] = 0.5
    Z[s(2), s'(2)] = -0.5
    P = ITensor(s, s')
    M = ITensor(s, s')
    P[s(1), s'(2)] = 1.0
    M[s(2), s'(1)] = 1.0
    H = Matrix{Vector{Operator}}(undef, Ny, Nx)
    @inbounds for col in 1:Nx, row in 1:Ny
        H[row, col] = Vector{Operator}()
        if row < Ny
            op_a  = 0.5 * P
            op_b  = copy(M)
            sites = [row=>col, row+1=>col]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Vertical))

            op_a  = 0.5 * M
            op_b  = copy(P)
            sites = [row=>col, row+1=>col]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Vertical))

            if J != 0.0
                op_a  = J * Z
                op_b  = copy(Z)
                sites = [row=>col, row+1=>col]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Vertical))
            end
        end
        if col < Nx
            op_a  = 0.5 * P
            op_b  = copy(M)
            sites = [row=>col, row=>col+1]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Horizontal))

            op_a  = 0.5 * M
            op_b  = copy(P)
            sites = [row=>col, row=>col+1]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Horizontal))

            if J != 0.0
                op_a  = J * Z
                op_b  = copy(Z)
                sites = [row=>col, row=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Horizontal))
            end
        end
    end
    # pinning fields
    J_ = 1.;
    @inbounds for row in 1:Ny
        op = isodd(row-1) ? (J_/2.0) * Z : (-J_/2.0) * Z
        push!(H[row, 1], Operator([row=>1], [op], s, Field))
        op = isodd(row-1) ? (-J_/2.0) * Z : (J_/2.0) * Z
        push!(H[row, Nx], Operator([row=>Nx], [op], s, Field))
    end
    @inbounds for col in 2:Nx-1
        op = isodd(col-1) ? (-J_/2.0) * Z : (J_/2.0) * Z
        push!(H[Ny, col], Operator([Ny=>col], [op], s, Field))
        op = isodd(col-1) ? (J_/2.0) * Z : (-J_/2.0) * Z
        push!(H[1, col], Operator([1=>col], [op], s, Field))
    end
    return H
end

function makeH_J1J2(Nx::Int, Ny::Int, J1::Real, J2::Real; pinning::Bool=false)
    s = Index(2, "Site,SpinInd")
    Z = ITensor(s, s')
    Z[s(1), s'(1)] = 0.5
    Z[s(2), s'(2)] = -0.5
    P = ITensor(s, s')
    M = ITensor(s, s')
    P[s(1), s'(2)] = 1.0
    M[s(2), s'(1)] = 1.0
    H = Matrix{Vector{Operator}}(undef, Ny, Nx)
    @inbounds for col in 1:Nx, row in 1:Ny
        H[row, col] = Vector{Operator}()
        if row < Ny
            op_a  = 0.5 * J1 * P
            op_b  = copy(M)
            sites = [row=>col, row+1=>col]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Vertical))

            op_a  = 0.5 * J1 * M
            op_b  = copy(P)
            sites = [row=>col, row+1=>col]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Vertical))

            op_a  = J1 * Z
            op_b  = copy(Z)
            sites = [row=>col, row+1=>col]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Vertical))
        end
        if col < Nx
            op_a  = 0.5 * J1 * P
            op_b  = copy(M)
            sites = [row=>col, row=>col+1]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Horizontal))

            op_a  = 0.5 * J1 * M
            op_b  = copy(P)
            sites = [row=>col, row=>col+1]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Horizontal))

            op_a  = J1 * Z
            op_b  = copy(Z)
            sites = [row=>col, row=>col+1]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Horizontal))
        end
        # handle J2
        if col < Nx
            if row < Ny
                op_a  = 0.5 * J2 * P
                op_b  = copy(M)
                sites = [row=>col, row+1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))

                op_a  = 0.5 * J2 * M
                op_b  = copy(P)
                sites = [row=>col, row+1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))

                op_a  = J2 * Z
                op_b  = copy(Z)
                sites = [row=>col, row+1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))
            end
            if row > 1
                op_a  = 0.5 * J2 * P
                op_b  = copy(M)
                sites = [row=>col, row-1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))

                op_a  = 0.5 * J2 * M
                op_b  = copy(P)
                sites = [row=>col, row-1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))

                op_a  = J2 * Z
                op_b  = copy(Z)
                sites = [row=>col, row-1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))
            end
        end
    end
    return H
end

function makeCuH_J1J2(Nx::Int, Ny::Int, J1::Real, J2::Real; pinning::Bool=false)
    s = Index(2, "Site,SpinInd")
    Z = ITensor(s, s')
    Z[s(1), s'(1)] = 0.5
    Z[s(2), s'(2)] = -0.5
    P = ITensor(s, s')
    M = ITensor(s, s')
    P[s(1), s'(2)] = 1.0
    M[s(2), s'(1)] = 1.0
    H = Matrix{Vector{Operator}}(undef, Ny, Nx)
    @inbounds for col in 1:Nx, row in 1:Ny
        H[row, col] = Vector{Operator}()
        if row < Ny
            op_a  = cuITensor(0.5 * J1 * P)
            op_b  = cuITensor(copy(M))
            sites = [row=>col, row+1=>col]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Vertical))

            op_a  = cuITensor(0.5 * J1 * M)
            op_b  = cuITensor(copy(P))
            sites = [row=>col, row+1=>col]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Vertical))

            op_a  = cuITensor(J1 * Z)
            op_b  = cuITensor(copy(Z))
            sites = [row=>col, row+1=>col]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Vertical))
        end
        if col < Nx
            op_a  = cuITensor(0.5 * J1 * P)
            op_b  = cuITensor(copy(M))
            sites = [row=>col, row=>col+1]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Horizontal))

            op_a  = cuITensor(0.5 * J1 * M)
            op_b  = cuITensor(copy(P))
            sites = [row=>col, row=>col+1]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Horizontal))

            op_a  = cuITensor(J1 * Z)
            op_b  = cuITensor(copy(Z))
            sites = [row=>col, row=>col+1]
            push!(H[row, col], Operator(sites, [op_a; op_b], s, Horizontal))
        end
        # handle J2
        if col < Nx
            if row < Ny
                op_a  = cuITensor(0.5 * J2 * P)
                op_b  = cuITensor(copy(M))
                sites = [row=>col, row+1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))

                op_a  = cuITensor(0.5 * J2 * M)
                op_b  = cuITensor(copy(P))
                sites = [row=>col, row+1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))

                op_a  = cuITensor(J2 * Z)
                op_b  = cuITensor(copy(Z))
                sites = [row=>col, row+1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))
            end
            if row > 1
                op_a  = cuITensor(0.5 * J2 * P)
                op_b  = cuITensor(copy(M))
                sites = [row=>col, row-1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))

                op_a  = cuITensor(0.5 * J2 * M)
                op_b  = cuITensor(copy(P))
                sites = [row=>col, row-1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))

                op_a  = cuITensor(J2 * Z)
                op_b  = cuITensor(copy(Z))
                sites = [row=>col, row-1=>col+1]
                push!(H[row, col], Operator(sites, [op_a; op_b], s, Diag))
            end
        end
    end
    return H
end

function makeCuH_XXZ(Nx::Int, Ny::Int, J::Real; pinning::Bool=false)
    s = Index(2, "Site,SpinInd")
    Z = ITensor(s, s')
    Z[s(1), s'(1)] = 0.5
    Z[s(2), s'(2)] = -0.5
    #Ident = spinI(s)
    P = ITensor(s, s')
    M = ITensor(s, s')
    P[s(1), s'(2)] = 1.0
    M[s(2), s'(1)] = 1.0
    H = Matrix{Vector{Operator}}(undef, Ny, Nx)
    @inbounds for col in 1:Nx, row in 1:Ny
        H[row, col] = Vector{Operator}()
        if row < Ny
            op_a  = 0.5 * P
            op_b  = copy(M)
            sites = [row=>col, row+1=>col]
            push!(H[row, col], Operator(sites, [cuITensor(op_a); cuITensor(op_b)], s, Vertical))

            op_a  = 0.5 * M
            op_b  = copy(P)
            sites = [row=>col, row+1=>col]
            push!(H[row, col], Operator(sites, [cuITensor(op_a); cuITensor(op_b)], s, Vertical))

            if J != 0.0
                op_a  = J * Z
                op_b  = copy(Z)
                sites = [row=>col, row+1=>col]
                push!(H[row, col], Operator(sites, [cuITensor(op_a); cuITensor(op_b)], s, Vertical))
            end
        end
        if col < Nx
            op_a  = 0.5 * P
            op_b  = copy(M)
            sites = [row=>col, row=>col+1]
            push!(H[row, col], Operator(sites, [cuITensor(op_a); cuITensor(op_b)], s, Horizontal))

            op_a  = 0.5 * M
            op_b  = copy(P)
            sites = [row=>col, row=>col+1]
            push!(H[row, col], Operator(sites, [cuITensor(op_a); cuITensor(op_b)], s, Horizontal))

            if J != 0.0
                op_a  = J * Z
                op_b  = copy(Z)
                sites = [row=>col, row=>col+1]
                push!(H[row, col], Operator(sites, [cuITensor(op_a); cuITensor(op_b)], s, Horizontal))
            end
        end
    end
    # pinning fields
    J_ = 1.
    @inbounds for row in 1:Ny
        op = isodd(row-1) ? (J_/2.0) * Z : (-J_/2.0) * Z
        push!(H[row, 1], Operator([row=>1], [cuITensor(op)], s, Field))
        op = isodd(row-1) ? (-J_/2.0) * Z : (J_/2.0) * Z
        push!(H[row, Nx], Operator([row=>Nx], [cuITensor(op)], s, Field))
    end
    @inbounds for col in 2:Nx-1
        op = isodd(col-1) ? (-J_/2.0) * Z : (J_/2.0) * Z
        push!(H[Ny, col], Operator([Ny=>col], [cuITensor(op)], s, Field))
        op = isodd(col-1) ? (J_/2.0) * Z : (-J_/2.0) * Z
        push!(H[1, col], Operator([1=>col], [cuITensor(op)], s, Field))
    end
    return H
end


# convex relaxation (envelope) of cos function
function cv_cos(x::T,xL::T,xU::T) where {T<:AbstractFloat}
  r::T = zero(T)
  kL::Int64 = Base.ceil((-0.5-xL/(2.0*pi)))
  if (x<=(pi-2.0*pi*kL))
    xL1::T = convert(T,xL+2.0*pi*kL)
    if (xL1 >= pi/2.0)
      return cos(x),-sin(x)
    end
    xU1::T = convert(T,min(xU+2.0*pi*kL,pi))
    if ((xL1>=(-pi/2))&&(xU1<=(pi/2)))
      if (abs(xL-xU)<MC_param.env_tol)
        r = zero(T)
      else
        r = (cos(xU)-cos(xL))/(xU-xL)
      end
      return cos(xL)+r*(x-xL),r
    end
    return cv_cosin(convert(T,x+(2.0*pi)*kL),xL1,xU1)
  end
  kU::Int64 = Base.floor((0.5-xU/(2.0*pi)))
  if (x>=(-pi-2.0*pi*kU))
    xU2::T = convert(T,xU+2.0*pi*kU)
    if (xU2<=-pi/2.0)
      return cos(x),-sin(x)
    end
    return cv_cosin(convert(T,x+2.0*pi*kU),convert(T,max(xL+2.0*pi*kU,-pi)),xU2)
  end
  return -one(T),zero(T)
end
# function for computing convex relaxation over nonconvex and nonconcave regions
function cv_cosin(x::T,xL::T,xU::T) where {T<:AbstractFloat}
  xj::T = -Inf
  if (abs(xL)<=abs(xU))
    left::Bool = false
    x0::T = xU
    xm::T = xL
  else
    left = true
    x0 = xL
    xm = xU
  end
  try
    xj = newton(x0,xL,xU,cv_cosenv,dcv_cosenv,xm,zero(x0))
  catch e
    if isa(e, ErrorException)
      xj = golden_section(xL,xU,cv_cosenv,xm,zero(x0))
    end
  end
  if ((left && x<=xj)||((~left) && x>=xj))
    return cos(x),-sin(x)
  else
    if abs(xm-xj)<MC_param.env_tol
      r::T = zero(T)
    else
      r = (cos(xm)-cos(xj))/(xm-xj)
    end
    return cos(xm)+r*(x-xm),r
  end
end
# pivot point calculation function for convex relaxation of cosine
function cv_cosenv(x::T,y::T,z::T) where {T<:AbstractFloat}
  return (x-y)*sin(x)+cos(x)-cos(y)
end
function dcv_cosenv(x::T,y::T,z::T) where {T<:AbstractFloat}
  return (x-y)*cos(x)
end
# concave relaxation (envelope) of cos function
function cc_cos(x::T,xL::T,xU::T) where {T<:AbstractFloat}
  temp = cv_cos(convert(T,x-pi),convert(T,xL-pi),convert(T,xU-pi))
  return -temp[1],-temp[2]
end
function cos_arg(xL::T,xU::T) where {T<:AbstractFloat}
  kL::Int64 = Base.ceil(-0.5-xL/(2.0*pi))
  xL1::T = convert(T,xL+2.0*pi*kL)
  xU1::T = convert(T,xU+2.0*pi*kL)
  if ~((xL1>=-pi)&&(xL1<=pi))
    error("Cosine Argument Calculation: xL out of bounds.")
  end
  if (xL1<=zero(T))
    if (xU1<=zero(T))
      arg1::T = xL
      arg2::T = xU
    elseif (xU1>=pi)
      arg1 = convert(T,pi-2.0*pi*kL)
      arg2 = convert(T,-2.0*pi*kL)
    else
      arg1 = (cos(xL1)<=cos(xU1)) ? xL : xU
      arg2 = convert(T,-2.0*pi*kL)
    end
  end
  if (xU1<=pi)
    arg1 = xU
    arg2 = xL
  elseif (xU1>=(2.0*pi))
    arg1 = convert(T,pi-2.0*pi*kL)
    arg2 = convert(T,2.0*pi-2.0*pi*kL)
  else
    arg1 = convert(T,pi-2.0*pi*kL)
    arg2 = (cos(xL1)>=cos(xU1)) ? xL : xU
  end
  return [arg1,arg2]
end

function cos(x::SMCg{N,T}) where {N,T<:AbstractFloat}
  eps_max::T,eps_min::T = cos_arg(x.Intv.lo,x.Intv.hi)
  midcc::T,cc_id::Int64 = mid3(x.cc,x.cv,eps_max)
	midcv::T,cv_id::Int64 = mid3(x.cc,x.cv,eps_min)
  cc::T,dcc::T = cc_cos(midcc,x.Intv.lo,x.Intv.hi)
  cv::T,dcv::T = cv_cos(midcv,x.Intv.lo,x.Intv.hi)
  if (MC_param.mu>=1)
    gcc1::T,gdcc1::T = cc_cos(x.cv,x.Intv.lo,x.Intv.hi)
    gcv1::T,gdcv1::T = cv_cos(x.cv,x.Intv.lo,x.Intv.hi)
    gcc2::T,gdcc2::T = cc_cos(x.cc,x.Intv.lo,x.Intv.hi)
    gcv2::T,gdcv2::T = cv_cos(x.cc,x.Intv.lo,x.Intv.hi)
    cv_grad::SVector{N,T} = max(zero(T),gdcv1)*x.cv_grad + min(zero(T),gdcv2)*x.cc_grad
    cc_grad::SVector{N,T} = min(zero(T),gdcc1)*x.cv_grad + max(zero(T),gdcc2)*x.cc_grad
  else
    cc_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    cv_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*dcv
  end
  return SMCg{N,T}(cc, cv, cc_grad, cv_grad, cos(x.Intv),x.cnst, x.IntvBox, x.xref)
end

function sin(x::SMCg{N,T}) where {N,T<:AbstractFloat}
  cos(x-pi/2.0)
end

# pivot point calculation function for convex relaxation of complimentary error function
@inline function tan_env(x::T,y::T,z::T) where {T<:AbstractFloat}
  return (x-y)-(tan(x)-tan(y))/(one(T)+tan(x)^2)
end
# derivative of pivot point calculation function for convex relaxation of complimentary error function
@inline function tan_envd(x::T,y::T,z::T) where {T<:AbstractFloat}
  return 2*tan(x)/(one(T)+tan(x)^2)*(tan(x)-tan(y))
end
# convex relaxation (envelope) of tangent function
@inline function cv_tan(x::T,xL::T,xU::T) where {T<:AbstractFloat}
  p::T = zero(T)
  if (xL>=zero(T))
    return tan(x),sec(x)^2
  elseif (xU<=zero(T))
    return line_seg(x,xL,tan(xL),xU,tan(xU)),dline_seg(x,xL,tan(xL),xU,tan(xU),sec(x)^2)
  else
    try
      p = secant(zero(T),xU,zero(T),xU,tan_env,xL,zero(T))
    catch e
      if isa(e, ErrorException)
        p = golden_section(zero(T),xU,tan_env,xL,zero(T))
      end
    end
    if (x<=p)
      return line_seg(x,xL,tan(xL),p,tan(p)),dline_seg(x,xL,tan(xL),p,tan(p),sec(x)^2)
    else
      return tan(x),sec(x)^2
    end
  end
end
# concave relaxation (envelope) of tangent function
@inline function cc_tan(x::T,xL::T,xU::T) where {T<:AbstractFloat}
  p::T = zero(T)
  if (xL>=zero(T))
    return line_seg(x,xL,tan(xL),xU,tan(xU)),dline_seg(x,xL,tan(xL),xU,tan(xU),sec(x)^2)
  elseif (xU<=zero(T))
    return tan(x),sec(x)^2
  else
    try
      p = secant(zero(T),xL,xL,zero(T),tan_env,xU,zero(T))
    catch e
      if isa(e, ErrorException)
        p = golden_section(xL,zero(T),tan_env,xU,zero(T))
      end
    end
    if (x<=p)
       return tan(x),sec(x)^2
    else
       return line_seg(x,p,tan(p),xU,tan(xU)),dline_seg(x,p,tan(p),xU,tan(xU),sec(x)^2)
     end
  end
end
@inline function tan(x::SMCg{N,T}) where {N,T<:AbstractFloat}
  if ((x.Intv.lo==-Inf)||(x.Intv.hi==Inf))
    error("Function unbounded on domain")
  end
  eps_max::T = x.Intv.hi
  eps_min::T = x.Intv.lo
  midcc::T,cc_id::Int64 = mid3(x.cc,x.cv,eps_max)
  midcv::T,cv_id::Int64 = mid3(x.cc,x.cv,eps_min)
  cc::T,dcc::T = cc_tan(midcc,x.Intv.lo,x.Intv.hi)
  cv::T,dcv::T = cv_tan(midcv,x.Intv.lo,x.Intv.hi)
  if (MC_param.mu >= 1)
    gcc1::T,gdcc1::T = cc_tan(x.cv,x.Intv.lo,x.Intv.hi,c)
    gcv1::T,gdcv1::T = cv_tan(x.cv,x.Intv.lo,x.Intv.hi,c)
    gcc2::T,gdcc2::T = cc_tan(x.cc,x.Intv.lo,x.Intv.hi,c)
    gcv2::T,gdcv2::T = cv_tan(x.cc,x.Intv.lo,x.Intv.hi,c)
    cv_grad::SVector{N,T} = max(zero(T),gdcv1)*x.cv_grad + min(zero(T),gdcv2)*x.cc_grad
    cc_grad::SVector{N,T} = min(zero(T),gdcc1)*x.cv_grad + max(zero(T),gdcc2)*x.cc_grad
  else
    cc_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    cv_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*dcv
  end
  return SMCg{N,T}(cc, cv, cc_grad, cv_grad, tan(x.Intv),x.cnst, x.IntvBox, x.xref)
end

@inline function acos(x::SMCg{N,T}) where {N,T<:AbstractFloat}
  return asin(-x)+pi/2.0
end

# pivot point calculation function for convex relaxation of arcsine
@inline function asin_env(x::T,y::T,z::T) where {T<:AbstractFloat}
  return (asin(x)-asin(y))/(x-y)-one(T)/sqrt(one(T)-x^2)
end
# derivative of pivot point calculation function for convex relaxation of arcsine
@inline function asin_envd(x::T,y::T,z::T) where {T<:AbstractFloat}
  return one(T)/((x-y)*sqrt(one(T)-x^2))-x/(one(T)-x^2)^(3/2)-(asin(x)-asin(y))/(x-y)^2
end
# convex relaxation (envelope) of arcsine function
@inline function cv_asin(x::T,xL::T,xU::T) where {T<:AbstractFloat}
  p::T = zero(T)
  if (xL>=zero(T))
    return asin(x),one(T)/sqrt(one(T)-x^2)
  elseif (xU<=zero(T))
    return line_seg(x,xL,asin(xL),xU,asin(xU)),dline_seg(x,xL,asin(xL),xU,asin(xU),one(T)/sqrt(one(T)-x^2))
  else
    try
      p = newton(xU,xL,xU,asin_env,asin_envd,xL,xU)
    catch e
      if isa(e, ErrorException)
        p = golden_section(zero(x),xU,asin_env,xL,zero(T))
      end
    end
    if (x<=p)
      return line_seg(x,xL,asin(xL),p,asin(p)),dline_seg(x,xL,asin(xL),p,asin(p),one(T)/sqrt(one(T)-x^2))
    else
      return asin(x),one(T)/sqrt(one(T)-x^2)
    end
  end
end
# concave relaxation (envelope) of arcsine function
@inline function cc_asin(x::T,xL::T,xU::T) where {T<:AbstractFloat}
  p::T = zero(T)
  if (xL>=zero(T))
    return line_seg(x,xL,asin(xL),xU,asin(xU)),dline_seg(x,xL,asin(xL),xU,asin(xU),one(T)/sqrt(one(T)-x^2))
  elseif (xU<=zero(T))
    return asin(x),one(T)/sqrt(one(T)-x^2)
  else
    try
      p = secant(zero(T),xL,xL,zero(T),asin_env,xU,zero(T))
    catch e
      if isa(e, ErrorException)
        p = golden_section(xL,zero(T),asin_env,xU,zero(T))
      end
    end
    if (x<=p)
      return asin(x),one(T)/sqrt(one(T)-x^2)
    else
      return line_seg(x,p,asin(p),xU,asin(xU)),dline_seg(x,p,asin(p),xU,asin(xU),one(T)/sqrt(one(T)-x^2))
    end
  end
end
@inline function asin(x::SMCg{N,T}) where {N,T<:AbstractFloat}
  if ((x.Intv.lo==-Inf)||(x.Intv.hi==Inf))
    error("Function unbounded on domain")
  end
  eps_max::T = x.Intv.hi
  eps_min::T = x.Intv.lo
  midcc::T,cc_id::Int64 = mid3(x.cc,x.cv,eps_max)
  midcv::T,cv_id::Int64 = mid3(x.cc,x.cv,eps_min)
  cc::T,dcc::T = cc_asin(midcc,x.Intv.lo,x.Intv.hi)
  cv::T,dcv::T = cv_asin(midcv,x.Intv.lo,x.Intv.hi)
  if (MC_param.mu >= 1)
    gcc1::T,gdcc1::T = cc_asin(x.cv,x.Intv.lo,x.Intv.hi,c)
    gcv1::T,gdcv1::T = cv_asin(x.cv,x.Intv.lo,x.Intv.hi,c)
    gcc2::T,gdcc2::T = cc_asin(x.cc,x.Intv.lo,x.Intv.hi,c)
    gcv2::T,gdcv2::T = cv_asin(x.cc,x.Intv.lo,x.Intv.hi,c)
    cv_grad::SVector{N,T} = max(zero(T),gdcv1)*x.cv_grad + min(zero(T),gdcv2)*x.cc_grad
    cc_grad::SVector{N,T} = min(zero(T),gdcc1)*x.cv_grad + max(zero(T),gdcc2)*x.cc_grad
  else
    cc_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    cv_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*dcv
  end
  return SMCg{N,T}(cc, cv, cc_grad, cv_grad, asin(x.Intv),x.cnst, x.IntvBox, x.xref)
end

# pivot point calculation function for convex relaxation of arctangent
@inline function atan_env(x::T,y::T,z::T) where {T<:AbstractFloat}
  return (x-y)-(one(T)+x^2)*(atan(x)-atan(y))
end
# derivative of pivot point calculation function for convex relaxation of arctangent
@inline function atan_envd(x::T,y::T,z::T) where {T<:AbstractFloat}
  return -2*x*(atan(x)-atan(y))
end
# convex relaxation (envelope) of arctan function
@inline function cv_atan(x::T,xL::T,xU::T) where {T<:AbstractFloat}
  p::T = zero(T)
  if (xL>=zero(T))
    return line_seg(x,xL,atan(xL),xU,atan(xU)),dline_seg(x,xL,atan(xL),xU,atan(xU),one(T)/(x^2+one(T)))
  elseif (xU<=zero(T))
    return atan(x),one(T)/(x^2+one(T))
  else
    try
      p = newton(xL,xL,zero(T),atan_env,atan_envd,xU,zero(T))
    catch e
      if isa(e, ErrorException)
        p = golden_section(xL,zero(T),atan_env,xU,zero(T))
      end
    end
    if (x<=p)
      return atan(x),one(T)/(x^2+one(T))
    else
      return line_seg(x,p,atan(p),xU,atan(xU)),dline_seg(x,p,atan(p),xU,atan(xU),one(T)/(x^2+one(T)))
    end
  end
end
# concave relaxation (envelope) of arctan function
@inline function cc_atan(x::T,xL::T,xU::T) where {T<:AbstractFloat}
  p::T = zero(T)
  if (xL>=zero(T))
    return atan(x),one(T)/(x^2+one(T))
  elseif (xU<=zero(T))
    return line_seg(x,xL,atan(xL),xU,atan(xU)),dline_seg(x,xL,atan(xL),xU,atan(xU),one(T)/(x^2+one(T)))
  else
    try
      p = newton(xU,zero(T),xU,atan_env,atan_envd,xL,zero(T))
    catch e
      if isa(e, ErrorException)
        p = golden_section(zero(T),xU,atan_env,xL,zero(T))
      end
    end
    if (x<=p)
      return line_seg(x,xL,atan(xL),p,atan(p)),dline_seg(x,xL,atan(xL),p,atan(p),one(T)/(x^2+one(T)))
    else
      return atan(x),one(T)/(x^2+one(T))
    end
  end
end
@inline function atan(x::SMCg{N,T}) where {N,T<:AbstractFloat}
  eps_max::T = x.Intv.hi
  eps_min::T = x.Intv.lo
  midcc::T,cc_id::Int64 = mid3(x.cc,x.cv,eps_max)
  midcv::T,cv_id::Int64 = mid3(x.cc,x.cv,eps_min)
  cc::T,dcc::T = cc_atan(midcc,x.Intv.lo,x.Intv.hi)
  cv::T,dcv::T = cv_atan(midcv,x.Intv.lo,x.Intv.hi)
  if (MC_param.mu >= 1)
    gcc1::T,gdcc1::T = cc_atan(x.cv,x.Intv.lo,x.Intv.hi,c)
    gcv1::T,gdcv1::T = cv_atan(x.cv,x.Intv.lo,x.Intv.hi,c)
    gcc2::T,gdcc2::T = cc_atan(x.cc,x.Intv.lo,x.Intv.hi,c)
    gcv2::T,gdcv2::T = cv_atan(x.cc,x.Intv.lo,x.Intv.hi,c)
    cv_grad::SVector{N,T} = max(zero(T),gdcv1)*x.cv_grad + min(zero(T),gdcv2)*x.cc_grad
    cc_grad::SVector{N,T} = min(zero(T),gdcc1)*x.cv_grad + max(zero(T),gdcc2)*x.cc_grad
  else
    cc_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    cv_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*dcv
  end
  return SMCg{N,T}(cc, cv, cc_grad, cv_grad, atan(x.Intv),x.cnst, x.IntvBox, x.xref)
end


#------------------------------------------------------------------------------------------------------------------------
# Implements solution to the diffusion equation in an infinite medium as given in [1].
# Solutions are given in the steady-state, frequency, and time domains for an isotroptic source.
#
# [1] Patterson et. al., "Time resolved reflectance and transmittance for the noninvasive measurement of tissue optical properties,"
#     Appl. Opt. 28, 2331-2336 (1989)
#-------------------------------------------------------------------------------------------------------------------------
""" Structure containing inputs for simulating the fluence under the diffusion approximation in the infinite space."""
struct DAinf_params{T <: AbstractFloat} <: DiffusionParameters
    ρ::T                                 # distance away from isotropic point source (cm)
    μa::T                                # absorption coefficient (cm⁻¹)
    μsp::T                               # reduced scattering coefficient (cm⁻¹)
    n_med::T                             # medium's index of refraction
    ω::T                                 # modulation frequency (1/ns)
    t::Union{T, AbstractVector{T}}       # time vector (ns)

    # do not need to provide these arguments (calculated from previous inputs)
    D::T                                 # Diffusion coefficient
    ν::T                                 # Speed of light in medium (cm/ns)
    function DAinf_params{T}(ρ::T, μa::T, μsp::T, n_med::T, ω::T, t::Union{T, AbstractVector{T}}) where {T <: AbstractFloat}
        @assert ρ > zero(T) "ρ must be positive"
        @assert μa >= zero(T) "μa must greater than or equal to 0"
        @assert μsp > zero(T) "μsp must greater than 0"
        @assert n_med > zero(T) "n_med must be positive"
        @assert all(t .> zero(T)) "t must be positive"
        return new{T}(ρ, μa, μsp, n_med, ω, t, D_coeff(μsp), ν_coeff(n_med))
    end
end

"""
    Generator function for DAinf_params structure.

Provides parameters to use in the infinite space fluence calculation in either the CW, FD, or TD.
Arguments are given as keyword arguments (key = value)

# Keyword Arguments
- `ρ`: source-detector separation (cm⁻¹)
- `μa`: absorption coefficient (cm⁻¹)
- `μsp`: reduced scattering coefficient (cm⁻¹)
- `n_med`: medium's index of refraction
- `ω`: modulation frequency (1/ns)
- `t`: the time vector (ns).

# Examples
```
julia> data = DAinf_params() # return default parameters
julia> data = DAinf_params(ρ = 1.5) # return ρ = 1.5 with the rest of the parameters given by defaults
julia> fluence_DA_inf_CW(data) # can then be used with corresponding functions
```
"""
function DAinf_params(;
    ρ::T = 1.0,
    μa::T = 0.1,
    μsp::T = 10.0,
    n_med::T = 1.0,
    ω::T = 0.0,
    t::Union{T, AbstractVector{T}} = 1.0
) where {T<:AbstractFloat}
    return DAinf_params{T}(ρ, μa, μsp, n_med, ω, t)
end

#-------------------------------------
# Steady-State Fluence
#-------------------------------------
"""
    fluence_DA_inf_CW(ρ, μa, μsp)

Compute the steady-state fluence in an infinite medium.
This is an unsafe implementation and will not check parameters. ρ and μsp should be > 0.0 and μa >= 0.0.

# Arguments
- `ρ`: source-detector separation (cm⁻¹)
- `μa`: absorption coefficient (cm⁻¹)
- `μsp`: reduced scattering coefficient (cm⁻¹)

# Examples
```
julia> fluence_DA_inf_CW(1.0, 0.1, 10.0)
```
"""
function fluence_DA_inf_CW(ρ, μa, μsp)
    params = DiffusionKernelParams(μsp)
    return _kernel_fluence_DA_inf_CW(ρ, μa, μsp, params.D)
end
"""
    fluence_DA_inf_CW(data::DiffusionParameters)

Wrapper to `fluence_DA_inf_CW(ρ, μa, μsp)`

# Examples
```
julia> data = DAinf_params(ρ = 1.0) # use structure to generate inputs
julia> fluence_DA_inf_CW(data) # then call the function
```
"""
function fluence_DA_inf_CW(data::DiffusionParameters)
    return _kernel_fluence_DA_inf_CW(data.ρ, data.μa, data.μsp, data.D)
end

_kernel_fluence_DA_inf_CW(ρ, μa, μsp, D) = exp(-sqrt(3 * μsp * μa) * ρ) / (4 * π * ρ * D)

#-------------------------------------
# Frequency-Domain Fluence
#-------------------------------------
"""
    fluence_DA_inf_FD(ρ, μa, μsp; ω = 0.0, n_med = 1.0)

Compute the fluence for a frequency modulated source in an infinite medium.

# Arguments
- `ρ`: source-detector separation (cm⁻¹)
- `μa`: absorption coefficient (cm⁻¹)
- `μsp`: reduced scattering coefficient (cm⁻¹)

# Keyword Arguments
- `ω`: modulation frequency (1/ns)
- `n_med`: medium's index of refraction

# Examples
```
julia> fluence_DA_inf_FD(1.0, 0.1, 10.0, ω = 1.0, n_med = 1.4)
```
"""
function fluence_DA_inf_FD(ρ, μa, μsp; ω = 0.0, n_med = 1.0)
    params = DiffusionKernelParams(μsp, n_med)
    μa_complex = μa + ω * im / params.ν
    return _kernel_fluence_DA_inf_CW(ρ, μa_complex, μsp, params.D)
end
"""
    fluence_DA_inf_FD(data::DiffusionParameters)

Wrapper to `fluence_DA_inf_FD(ρ, μa, μsp; ω = 0.0, n_med = 1.0)`

# Examples
```
julia> data = DAinf_params(ω = 1.0) # use structure to generate inputs
julia> fluence_DA_inf_FD(data) # then call the function
```
"""
function fluence_DA_inf_FD(data::DiffusionParameters)
    μa_complex = data.μa + data.ω * im / data.ν
    return _kernel_fluence_DA_inf_CW(data.ρ, μa_complex, data.μsp, data.D)
end

#-------------------------------------
# Time-Domain Fluence
#-------------------------------------
"""
    fluence_DA_inf_TD(t, ρ, μa, μsp; n_med = 1.0)

Compute the time-domain fluence in an infinite medium with Eqn. 3 of Patterson. et al. 1989.
This is an unsafe implementation and will not check parameters. t, ρ and μsp should be > 0.0 and μa >= 0.0.

# Arguments
- `t`: the time vector (ns).
- `ρ`: source-detector separation (cm⁻¹)
- `μa`: absorption coefficient (cm⁻¹)
- `μsp`: reduced scattering coefficient (cm⁻¹)

# Keyword Arguments
- `n_med`= 1.0: medium's index of refraction

# Examples
```
julia> fluence_DA_inf_TD(0.1:0.5:5.0, 1.0, 0.1, 10.0, n_med = 1.4)
```
"""
function fluence_DA_inf_TD(t, ρ, μa, μsp; n_med = 1.0)
    params = DiffusionKernelParams(μsp, n_med)
    return map(t -> _kernel_fluence_DA_inf_TD(t, ρ, μa, params.ν, params.D), t)
end
"""
    fluence_DA_inf_TD(data::DiffusionParameters)

Wrapper to `fluence_DA_inf_TD(t, ρ, μa, μsp; n_med = 1.0)`.

# Examples
```
julia> data = DAinf_params(t = 0.1:0.05:2.0) # use structure to generate inputs
julia> fluence_DA_inf_TD(data) # then call the function
```
"""
function fluence_DA_inf_TD(data::DiffusionParameters)
    return map(t -> _kernel_fluence_DA_inf_TD(t, data.ρ, data.μa, data.ν, data.D), data.t)
end

@inline function _kernel_fluence_DA_inf_TD(t, ρ, μa, ν, D)
    tmp1 = 4 * D * ν * t
    ϕ = exp(-(ρ^2 / tmp1) - μa * ν * t)
    ϕ *= ν / ((tmp1 * π )^(3/2))
end


using CollectiveSpins

a = 0.5
b = 0.7
N = 500

@time CollectiveSpins.effective_interaction.cubiclattice_orthogonal(a, 4)
(om, ga) = @time CollectiveSpins.effective_interaction.cubiclattice_orthogonal(a, N)

@time CollectiveSpins.effective_interaction.hexagonallattice3d_orthogonal(a, b, 4)
(om, ga) = @time CollectiveSpins.effective_interaction.hexagonallattice3d_orthogonal(a, b, N)


module Bijective
import ..BijectiveHilbert: encode_hilbert_zero, decode_hilbert_zero!, HilbertAlgorithm, axis_type

struct Simple2D{T} <: HilbertAlgorithm{T}
end

Simple2D(::Type{T}) where {T} = Simple2D{T}()

axis_type(::Simple2D{T}) where {T} = Int

function encode_hilbert_zero(::Simple2D{T}, X::Vector{A})::T where {A, T}
    x = X[1]
    y = X[2]
    z = zero(T)
    if x == zero(A) && y == zero(A)
        return z
    end
    rmin = convert(Int, floor(log2(max(x, y))) + 1)
    w = one(A) << (rmin - 1)
    while rmin > 0
        z <<= 2
        if rmin&1 == 1  # odd
            if x < w
                if y >= w
                    # 1
                    x, y = (y - w, x)
                    z += one(T)
                end  # else x, y remain the same.
            else
                if y < w
                    x, y = ((w<<1) - x - one(w), w - y - one(w))
                    # 3
                    z += T(3)
                else
                    x, y = (y - w, x - w)
                    z += T(2)
                    # 2
                end
            end
        else  # even
            if x < w
                if y >= w
                    # Quadrant 3
                    x, y = (w - x - one(w), (w << 1) - y - one(w))
                    z += T(3)
                end  # else do nothing for quadrant 0.
            else
                if y < w
                    # 1
                    x, y = (y, x-w)
                    z += one(T)
                else
                    # 2
                    x, y = (y-w, x-w)
                    z += T(2)
                end
            end
        end
        rmin -= 1
        w >>= 1
    end
    z
end

function decode_hilbert_zero!(::Simple2D{T}, X::Vector{A}, z::T) where {A,T}
    r = z & T(3)
    x, y = [(zero(A), zero(A)), (zero(A), one(A)), (one(A), one(A)), (one(A), zero(A))][r + 1]
    z >>= 2
    rmin = 2
    w = one(A) << 1
    while z > zero(T)
        r = z & T(3)
        parity = 2 - rmin&1
        if rmin & 1 != 0
            # Nothing to do for quadrant 0.
            if r == 1
                x, y = (y, x + w)
            elseif r == 2
                x, y = (y + w, x + w)
            elseif r == 3
                x, y = ((w << 1) - x - one(A), w - y - one(A))
            end
        else
            if r == 1
                x, y = (y + w, x)
            elseif r == 2
                x, y = (y + w, x + w)
            elseif r == 3
                x, y = (w - x - one(A), (w << 1) - y - one(A))
            end
        end
        z >>= 2
        rmin += 1
        w <<= 1
    end
    X[1] = x
    X[2] = y
end

end


# Usage:
# julia --project=benchmark
# ```julia
# using PkgBenchmark, Convolution2D
# rst = PkgBenchmark.benchmarkpkg(Convolution2D)
# open("benchmark_report.md", "w") do io
#     PkgBenchmark.export_markdown(io, rst)
# end
# ```
using BenchmarkTools
using ImageFiltering
using ImageTransformations
using TestImages
using Convolution2D

const SUITE = BenchmarkGroup()

X = Float32.(imresize(testimage("cameraman"), (255, 255)))
kern = [0 1 0
    1 -4 1
    0 1 0]
SUITE["Convolution2D_255x255_gray"] = @benchmarkable conv2D($X, $kern)
SUITE["ImageFiltering_255x255_gray"] = @benchmarkable imfilter($X, $kern)


"""
  pow(x, y)

Exponentiation operator, returns `x` raised to the power `y`.
"""
function pow(x::T, y::T) where {T<:Union{Float32,Float64}}
    yi = unsafe_trunc(Int, y)
    yisint = yi == y
    yisodd = isodd(yi) && yisint

    result = expk(dmul(logk(abs(x)), y))

    result = isnan(result) ? T(Inf) : result
    result *= (x > 0 ? T(1.0) : (!yisint ? T(NaN) : (yisodd ? -T(1.0) : T(1.0))))

    efx = flipsign(abs(x) - 1, y)
    isinf(y) && (result = efx < 0 ? T(0.0) : (efx == 0 ? T(1.0) : T(Inf)))
    (isinf(x) || x == 0) && (result = (yisodd ? _sign(x) : T(1.0)) * ((x == 0 ? -y : y) < 0 ? T(0.0) : T(Inf)))
    (isnan(x) || isnan(y)) && (result = T(NaN))
    (y == 0 || x == 1) && (result = T(1.0))

    return result
end

let
global cbrt_fast
global cbrt

c6d = -0.640245898480692909870982
c5d = 2.96155103020039511818595
c4d = -5.73353060922947843636166
c3d = 6.03990368989458747961407
c2d = -3.85841935510444988821632
c1d = 2.2307275302496609725722

c6f = -0.601564466953277587890625f0
c5f =  2.8208892345428466796875f0
c4f = -5.532182216644287109375f0
c3f =  5.898262500762939453125f0
c2f = -3.8095417022705078125f0
c1f =  2.2241256237030029296875f0

global @inline cbrt_kernel(x::Float64) = @horner x c1d c2d c3d c4d c5d c6d
global @inline cbrt_kernel(x::Float32) = @horner x c1f c2f c3f c4f c5f c6f

"""
    cbrt_fast(x)

Return `x^{1/3}`.
"""
function cbrt_fast(d::T) where {T<:Union{Float32,Float64}}
    e  = ilogbk(abs(d)) + 1
    d  = ldexp2k(d, -e)
    r  = (e + 6144) % 3
    q  = r == 1 ? T(M2P13) : T(1)
    q  = r == 2 ? T(M2P23) : q
    q  = ldexp2k(q, (e + 6144) ÷ 3 - 2048)
    q  = flipsign(q, d)
    d  = abs(d)
    x  = cbrt_kernel(d)
    y  = x * x
    y  = y * y
    x -= (d * y - x) * T(1 / 3)
    y  = d * x * x
    y  = (y - T(2 / 3) * y * (y * x - 1)) * q
end

"""
    cbrt(x)

Return `x^{1/3}`. The prefix operator `∛` is equivalent to `cbrt`.
"""
function cbrt(d::T) where {T<:Union{Float32,Float64}}
    e  = ilogbk(abs(d)) + 1
    d  = ldexp2k(d, -e)
    r  = (e + 6144) % 3
    q2 = r == 1 ? MD2P13(T) : Double(T(1))
    q2 = r == 2 ? MD2P23(T) : q2
    q2 = flipsign(q2, d)
    d  = abs(d)
    x  = cbrt_kernel(d)
    y  = x * x
    y  = y * y
    x -= (d * y - x) * T(1 / 3)
    z  = x
    u  = dsqu(x)
    u  = dsqu(u)
    u  = dmul(u, d)
    u  = dsub(u, x)
    y  = T(u)
    y  = -T(2 / 3) * y * z
    v  = dadd(dsqu(z), y)
    v  = dmul(v, d)
    v  = dmul(v, q2)
    z  = ldexp2k(T(v), (e + 6144) ÷ 3 - 2048)
    isinf(d) && (z = flipsign(T(Inf), q2.hi))
    d == 0   && (z = flipsign(T(0), q2.hi))
    return z
end
end

"""
    hypot(x,y)

Compute the hypotenuse `\\sqrt{x^2+y^2}` avoiding overflow and underflow.
"""
function hypot(x::T, y::T) where {T<:IEEEFloat}
    x = abs(x)
    y = abs(y)
    if x < y
       x, y = y, x
    end
    r = (x == 0) ? y : y / x
    x * sqrt(T(1.0) + r * r)
end


__precompile__()

"""
A toy Julia implementation of [Bailey-Borwein-Plouffe (BBP) alogithm](http://www.davidhbailey.com/dhbpapers/bbp-alg.pdf) for computation
of hexadecimal digits of Pi.
"""
module PiBBP

export digpi

include("bbp.jl")

end # module


"""
    FiniteDifferenceGrids

A simple 1-dimensional uniform grid
for finite difference method.
"""
module FiniteDifferenceGrids

export Grid, UniformGrid, over_elems, over_elems_real, over_elems_ghost
export ZBoundary, Zmin, Zmax, n_hat, binary, ghost_vec, ghost_dual
export first_interior, boundary, second_interior, first_ghost, boundary_points

abstract type Grid{FT <: AbstractFloat, IT <: Integer} end

"""
    UniformGrid{T, I}

A simple 1-dimensional uniform grid of
type `T` for finite difference method.
"""
struct UniformGrid{FT, IT} <: Grid{FT, IT}
    """ Number of physical elements """
    n_elem::IT

    """ Number of real elements """
    n_elem_real::IT

    """ Number of ghost points per side """
    n_ghost::IT

    """ Element size """
    Δz::FT

    """ Inverse of element size squared """
    Δzi::FT

    """ Inverse of element size squared """
    Δzi2::FT

    """ z-coordinate at cell edges """
    ze::Vector{FT}

    """ z-coordinate at cell centers """
    zc::Vector{FT}

    """ z-coordinate at bottom of boundary node """
    zn_min::FT

    """ z-coordinate at top of boundary node """
    zn_max::FT
end

"""
    UniformGrid(z_min::T, z_max::T, n_elem_real::Int, n_ghost::Int = 1)

A simple grid implementation that accepts the domain interval
`z_min` and `z_max`, the number of elements `n_elem_real` and
the number of ghost points per side `n_ghost`.
"""
function UniformGrid(
    z_min::T,
    z_max::T,
    n_elem_real::I,
    n_ghost::I = 1,
) where {T, I}
    n_elem = n_elem_real + 2 * n_ghost
    Δz = (z_max - z_min) / n_elem_real
    Δzi = inv(Δz)
    Δzi2 = Δzi^2
    zc = [(i - n_ghost) * Δz - Δz / 2 for i in 1:n_elem]
    ze = [(i - n_ghost) * Δz - Δz for i in 1:(n_elem + 1)]
    zn_min = (zc[n_ghost] + zc[1 + n_ghost]) / 2
    zn_max = (zc[end - n_ghost] + zc[end - n_ghost + 1]) / 2
    return UniformGrid{T, I}(
        n_elem,
        n_elem_real,
        n_ghost,
        Δz,
        Δzi,
        Δzi2,
        ze,
        zc,
        zn_min,
        zn_max,
    )
end

function Base.show(io::IO, grid::Grid)
    println(io, "----------------- Grid")
    println(io, "zc      = ", grid.zc)
    println(io, "ze      = ", grid.ze)
    println(io, "n_elem  = ", grid.n_elem)
    println(io, "n_ghost = ", grid.n_ghost)
    println(io, "zn_min  = ", grid.zn_min)
    println(io, "zn_max  = ", grid.zn_max)
    println(io, "-----------------")
end

abstract type ZBoundary end

"""
    Zmax
Type to dispatch function to particular location
"""
struct Zmax <: ZBoundary end

"""
    Zmin
Type to dispatch function to particular location
"""
struct Zmin <: ZBoundary end

"""
    n_hat(::ZBoundary)

The outward normal vector to the boundary
"""
n_hat(::Zmin) = -1
n_hat(::Zmax) = 1

"""
    binary(::ZBoundary)

Returns 0 for Zmin and 1 for Zmax
"""
binary(::Zmin) = 0
binary(::Zmax) = 1

"""
    ghost_vec(::ZBoundary)

A 3-element vector near the boundary with 1's on ghost cells and 0's on interior cells
"""
ghost_vec(::Zmin) = [1, 0, 0]
ghost_vec(::Zmax) = [0, 0, 1]

"""
    ghost_dual(::ZBoundary)

A 2-element vector near the boundary with 1's on ghost cells and 0's on interior cells
"""
ghost_dual(::Zmin) = [1, 0]
ghost_dual(::Zmax) = [0, 1]

"""
    over_elems(grid::Grid)

Get the range of indexes to traverse real and ghost grid elements
"""
@inline over_elems(grid::Grid) = 1:(grid.n_elem)

"""
    over_elems_real(grid::Grid)

Get the range of indexes to traverse only real grid elements
"""
@inline over_elems_real(grid::Grid) =
    (1 + grid.n_ghost):(grid.n_elem - grid.n_ghost)

"""
    first_interior(grid::Grid, ::ZBoundary)

Get the first element index in the physical domain
"""
@inline first_interior(grid::Grid, ::Zmax) = grid.n_elem - grid.n_ghost
@inline first_interior(grid::Grid, ::Zmin) = 1 + grid.n_ghost

"""
    boundary(grid::Grid, ::ZBoundary)

Get the element index for edge data at the boundary
"""
@inline boundary(grid::Grid, ::Zmax) = grid.n_elem - grid.n_ghost
@inline boundary(grid::Grid, ::Zmin) = 1 + grid.n_ghost

"""
    second_interior(grid::Grid, ::ZBoundary)

Get the second element index in the physical domain
"""
@inline second_interior(grid::Grid, ::Zmax) = first_interior(grid, Zmax()) - 1
@inline second_interior(grid::Grid, ::Zmin) = first_interior(grid, Zmin()) + 1

"""
    first_ghost(grid::Grid, ::ZBoundary)

Get the first element index in the ghost domain
"""
@inline first_ghost(grid::Grid, ::Zmax) = first_interior(grid, Zmax()) + 1
@inline first_ghost(grid::Grid, ::Zmin) = first_interior(grid, Zmin()) - 1

"""
    boundary_points(grid::Grid, ::ZBoundary)

Get the first element index in the ghost domain
"""
@inline boundary_points(grid::Grid, b::ZBoundary) =
    first_ghost(grid, b), first_interior(grid, b), second_interior(grid, b)

"""
    over_elems_ghost(grid::Grid)

Get the range of indexes to traverse only ghost grid elements
"""
@inline over_elems_ghost(grid::Grid) =
    setdiff(over_elems(grid), over_elems_real(grid))

"""
    over_elems_ghost(grid::Grid, ::ZBoundary)

Get the range of indexes to traverse only ghost grid elements below ``z_{min}`` and above ``z_{max}``.
"""
@inline over_elems_ghost(grid::Grid, ::Zmin) = 1:(grid.n_ghost)
@inline over_elems_ghost(grid::Grid, ::Zmax) =
    (grid.n_elem - grid.n_ghost + 1):(grid.n_elem)

Base.eltype(::Grid{T}) where {T} = T

include("discrete_operators.jl")

end


"""
AllEqual(dimension::Int)

The set corresponding to an all-equal constraint.

All expressions of a vector-valued function are enforced to take the same value
in the solution.

## Example

    [x, y, z] in AllEqual(3)
    # enforces `x == y` AND `x == z`.
"""
struct AllEqual <: MOI.AbstractVectorSet
    dimension::Int
end

"""
    AllDifferent(dimension::Int)

The set corresponding to an all-different constraint.

All expressions of a vector-valued function are enforced to take distinct
values in the solution: for all pairs of expressions, their values must
differ.

This constraint is sometimes called `distinct`.

## Example

    [x, y, z] in AllDifferent(3)
    # enforces `x != y` AND `x != z` AND `y != z`.
"""
struct AllDifferent <: MOI.AbstractVectorSet
    dimension::Int
end

# https://sofdem.github.io/gccat/gccat/Calldifferent_cst.html should be modelled
# with AllDifferent, because it is just shifting all expressions by constants,
# i.e. scalar affine expressions.

"""
    AllDifferentExceptConstants{T <: Number}(dimension::Int, k::Set{T})

All expressions of a vector-valued function are enforced to take distinct
values in the solution, but values equal to any value in `k` are not
considered: for all pairs of expressions, either their values must differ or
at least one of the two variables has a value in `k`.

This constraint is sometimes called `distinct`.

## Example

    [x, y] in AllDifferentExceptConstant(2, 0)
    # enforces `x != y` OR `x == 0` OR `y == 0`.

    [x, y] in AllDifferentExceptConstant(2, Set([0, 1]))
    # enforces `x != y` OR `x == 0` OR `y == 0` OR `x == 1` OR `y == 1`.
"""
struct AllDifferentExceptConstants{T <: Number} <: MOI.AbstractVectorSet
    dimension::Int
    k::Set{T}
end

function copy(set::AllDifferentExceptConstants{T}) where {T}
    return AllDifferentExceptConstants(set.dimension, copy(set.k))
end
function Base.:(==)(
    x::AllDifferentExceptConstants{T},
    y::AllDifferentExceptConstants{T},
) where {T}
    return x.dimension == y.dimension && x.k == y.k
end

"""
Special case of [`AllDifferentExceptConstants`](@ref) where only one value is
ignored.
"""
function AllDifferentExceptConstant(dimension::Int, value::T) where {T <: Number}
    return AllDifferentExceptConstants(dimension, Set(value))
end

"""
    SymmetricAllDifferent(dimension::Int)

The set corresponding to an all-different constraint, with the additional
requirement that the array must be symmetric.

All expressions of a vector-valued function are enforced to take distinct
values in the solution: for all pairs of expressions, their values must
differ. Symmetry means that, if ``x[i]=j``, then ``x[j]=i``.

This constraint is sometimes called [`symmetric_alldifferent`](https://sofdem.github.io/gccat/gccat/Csymmetric_alldifferent.html).

## Example

    [x, y, z] in SymmetricAllDifferent(3)
    # enforces `x != y` AND `x != z` AND `y != z` AND `(x == 2 => y == 1)` AND
    # `(x == 3 => z = 1)` AND `(y == 1 => x == 2)` AND `(y == 3 => z == 2)` AND
    # `(z == 1 => x == 3)` AND `(z == 2 => y == 3)`.
"""
struct SymmetricAllDifferent <: MOI.AbstractVectorSet
    dimension::Int
end

"""
    Domain{T <: Number}(values::Set{T})

The set corresponding to an enumeration of constant values.

The value of a scalar function is enforced to take a value from this set of
values.

This constraint is sometimes called `in`, `member` or `allowed_assignments`.
https://sofdem.github.io/gccat/gccat/Cdomain.html

## Example

    x in Domain(1:3)
    # enforces `x == 1` OR `x == 2` OR `x == 3`.
"""
struct Domain{T <: Number} <: MOI.AbstractScalarSet
    values::Set{T}
end

copy(set::Domain{T}) where {T} = Domain(copy(set.values))
Base.:(==)(x::Domain{T}, y::Domain{T}) where {T} = x.values == y.values

"""
    VectorDomain{T <: Number}(dimension::Int, values::Set{Vector{T}})

The set corresponding to an enumeration of constant values.

The value of a vector function is enforced to take a value from this set of
vector values.

This constraint is sometimes called `in`, `member` or `allowed_assignments`.
https://sofdem.github.io/gccat/gccat/Cdomain.html

## Example

    [x, y] in Domain(2, Set([[1, 2], [2, 3]]))
    # enforces (`x == 1` AND `y == 2`) OR (`x == 2` AND `y == 3`).
"""
struct VectorDomain{T <: Number} <: MOI.AbstractVectorSet
    dimension::Int
    values::Set{Vector{T}}
end

copy(set::VectorDomain{T}) where {T} = VectorDomain(set.dimension, copy(set.values))
Base.:(==)(x::VectorDomain{T}, y::VectorDomain{T}) where {T} = x.dimension == y.dimension && x.values == y.values

"""
    AntiDomain{T <: Number}(values::Set{T})

The set corresponding to an enumeration of constant values that are excluded.

The value of a scalar function is enforced to take a value that is not from
this set of values.

This constraint is sometimes called (`not_in`)[https://sofdem.github.io/gccat/gccat/Cnot_in.html],
`not_member`, `rel`, `forbidden_assignments`, or `no_good`.

## Example

    x in AntiDomain(1:3)
    # enforces `x != 1` AND `x != 2` AND `x != 3`.
"""
struct AntiDomain{T <: Number} <: MOI.AbstractScalarSet
    values::Set{T}
end

copy(set::AntiDomain{T}) where {T} = AntiDomain(copy(set.values))
Base.:(==)(x::AntiDomain{T}, y::AntiDomain{T}) where {T} = x.values == y.values

"""
    VectorAntiDomain{T <: Number}(values::Set{T})

The set corresponding to an enumeration of constant values that are excluded.

The value of a vector function is enforced to take a value that is not from
this set of vector values.

This constraint is sometimes called (`not_in`)[https://sofdem.github.io/gccat/gccat/Cnot_in.html],
`not_member`, `rel`, `forbidden_assignments`, or `no_good`.

## Example

    [x, y] in VectorAntiDomain(2, Set([[1, 2], [2, 3]]))
    # enforces (`x != 1` AND `y != 2`) OR (`x != 2` AND `y != 3`).
"""
struct VectorAntiDomain{T <: Number} <: MOI.AbstractScalarSet
    dimension::Int
    values::Set{Vector{T}}
end

copy(set::VectorAntiDomain{T}) where {T} = VectorAntiDomain(set.dimension, copy(set.values))
Base.:(==)(x::VectorAntiDomain{T}, y::VectorAntiDomain{T}) where {T} = x.values == y.values
MOI.dimension(set::VectorAntiDomain{T}) where {T} = set.dimension # TODO: why is this needed?

"""
    Membership(dimension)

The first element of a function of dimension `dimension` must equal at least
one of the following `dimension - 1` elements of the function.

This constraint is sometimes called `in_set`.

## Example

    [x, y, z] in Membership(3)
    # enforces `x == y` OR `x == z`.
"""
struct Membership <: MOI.AbstractVectorSet
    dimension::Int
end

"""
    DifferentFrom{T <: Number}(value::T)

The set excluding the single point ``x \\in \\mathbb{R}`` where ``x`` is given
by `value`.
"""
struct DifferentFrom{T <: Number} <: MOI.AbstractScalarSet
    value::T
end

MOI.constant(set::DifferentFrom{T}) where {T} = set.value
copy(set::DifferentFrom{T}) where {T} = DifferentFrom(copy(set.value))
function MOIU.shift_constant(set::DifferentFrom{T}, offset::T) where {T}
    return typeof(set)(MOI.constant(set) + offset)
end

"""
    Element{T <: Real}(values::Vector{T})

``\\{(x, i) \\in \\mathbb{R} \\times \\mathbb{N} | x = values[i]\\}``

Less formally, the first element constrained in this set will take the value of
`values` at the index given by the second element.

Also called [`indexing`](https://sofdem.github.io/gccat/gccat/Celement.html)
or `nth`.

## Examples

    [x, 3] in Element([4, 5, 6])
    # Enforces that x = 6, because 6 is the 3rd element from the array.

    [y, j] in Element([4, 5, 6])
    # Enforces that y = array[j], depending on the value of j (an integer
    # between 1 and 3).
"""
struct Element{T <: Real} <: MOI.AbstractVectorSet
    values::Vector{T}
end

MOI.dimension(set::Element{T}) where {T} = 2
copy(set::Element{T}) where {T} = Element(copy(set.values))
Base.:(==)(x::Element{T}, y::Element{T}) where {T} = x.values == y.values

"""
    ElementVariableArray(dimension::Int)

``\\{(x, i, values) \\in \\mathbb{R} \\times \\mathbb{N} \\times \\mathbb{R}^{\\mathtt{dimension}} | x = values[i]\\}``

Less formally, the first element constrained in this set will take the value of
`values` at the index given by the second element in the array given by the
remaining elements constrained in the set.

## Examples

    [x, 3, a, b, c] in ElementVariableArray(3)
    # Enforces that x = c, because 6 is the 3rd element from the array [a, b, c].

    [y, j, a, b, c] in ElementVariableArray(3)
    # Enforces that y = array[j], depending on the value of j (an integer
    # between 1 and 3), from the array [a, b, c].
"""
struct ElementVariableArray <: MOI.AbstractVectorSet
    dimension::Int
end

MOI.dimension(set::ElementVariableArray) = 2 + set.dimension
copy(set::ElementVariableArray) = ElementVariableArray(set.dimension)
function Base.:(==)(x::ElementVariableArray, y::ElementVariableArray)
    return x.dimension == y.dimension
end

"""
    MinimumDistance{T <: Real}(dimension::Int, k::T)

Ensures that all the `dimension` expressions in this set are at least `k`
apart, in absolute value:

``\\Big\\{x \\in \\mathbb{S}^{\\mathtt{dimension}} \\Big| |x_i - x_j| \\geq k, \\forall i \\neq j \\in \\{1, 2\\dots \\mathtt{dimension}\\} \\Big\\}``

Also called [`all_min_dist`](https://sofdem.github.io/gccat/gccat/Call_min_dist.html)
or `inter_distance`.
"""
struct MinimumDistance{T <: Real} <: MOI.AbstractVectorSet
    dimension::Int
    k::T
end

function copy(set::MinimumDistance{T}) where {T}
    return MinimumDistance(set.dimension, copy(set.k))
end
function Base.:(==)(x::MinimumDistance{T}, y::MinimumDistance{T}) where {T}
    return x.dimension == y.dimension && x.k == y.k
end

"""
    MaximumDistance{T <: Real}(dimension::Int, k::T)

Ensures that all the `dimension` expressions in this set are at most `k`
apart, in absolute value:

``\\Big\\{x \\in \\mathbb{S}^{\\mathtt{dimension}} \\Big| |x_i - x_j| \\leq k, \\forall i \\neq j \\in \\{1, 2\\dots \\mathtt{dimension}\\} \\Big\\}``
"""
struct MaximumDistance{T <: Real} <: MOI.AbstractVectorSet
    dimension::Int
    k::T
end

function copy(set::MaximumDistance{T}) where {T}
    return MaximumDistance(set.dimension, copy(set.k))
end
function Base.:(==)(x::MaximumDistance{T}, y::MaximumDistance{T}) where {T}
    return x.dimension == y.dimension && x.k == y.k
end

"""
    Inverse(dimension::Int)

Ensures that the two arrays of variables of size `dimension` are the inverse
one of the other.

``\\Big\\{(x, y) \\in \\mathbb{R}^{\\mathtt{dimension}} \\times \\mathbb{R}^{dimension} \\Big| x_i = j \\iff y_j = i, \\forall i, j \\in \\{1, 2 \\dots \\mathtt{dimension}\\} \\Big\\}``

Indices start at 1, like Julia.

Also called [`channel`](https://sofdem.github.io/gccat/gccat/Cinverse.html),
`inverse_channeling`, or `assignment`.
"""
struct Inverse <: MOI.AbstractVectorSet
    dimension::Int
end

"""
    SlidingSum{T}(low::T, high::T, length::Int, dimension::Int)

Ensures that the sum of all sequences of size `length` have a value between
`low` and `high`.

``\\{x \\in \\mathbb{R}^{\\mathtt{dimension}} | \\mathtt{low} \\leq \\sum_{j=i}^{i+\\mathtt{length}-1} x_i \\leq \\mathtt{high}, \\forall i \\in \\{ 0, 1 \\dots \\mathtt{dimension} - \\mathtt{length} \\} \\}``

https://sofdem.github.io/gccat/gccat/Csliding_sum.html
"""
struct SlidingSum{T <: Real} <: MOI.AbstractVectorSet
    low::T
    high::T
    length::Int
    dimension::Int
end

"""
    ValuePrecedence(before::T, value::T, dimension::Int)

Ensures that the value `before` happens before `value` in the array of size
`dimension`.

``\\{x \\in \\mathbb{R}^{\\mathtt{dimension}} | \\exists i < j: x_i = \\mathtt{before}, x_j = \\mathtt{value} \\}``

Also called `precede` or `value_precede`.

https://sofdem.github.io/gccat/gccat/Cint_value_precede.html
"""
struct ValuePrecedence{T <: Real} <: MOI.AbstractVectorSet
    before::T
    value::T
    dimension::Int
end

function copy(set::ValuePrecedence{T}) where {T}
    return ValuePrecedence(copy(set.before), copy(set.value), set.dimension)
end
function Base.:(==)(x::ValuePrecedence{T}, y::ValuePrecedence{T}) where {T}
    return x.dimension == y.dimension && x.before == y.before && x.value == y.value
end

# Solvers tend not to agree on the name of this one... "Inverse" is the most common one, though.
#     https://sofdem.github.io/gccat/gccat/Cinverse.html
# - inverse: CPLEX, MiniZinc, GCC, CHIP
# - inverseChanneling: Choco
# - channel: Gecode
# - assignment: SICStus

MOI.dimension(set::Inverse) = 2 * set.dimension

# isbits types, nothing to copy
function copy(
    set::Union{AllEqual, AllDifferent, SymmetricAllDifferent, Membership, Inverse, SlidingSum,},
)
    return set
end


## import packages
using MAT,Plots,Dates,TimerOutputs,WriteVTK,DataFrames,CSV,
Statistics,DelimitedFiles,ProgressMeter

const USE_GPU=false;  # Use GPU? If this is set false, then no GPU needs to be available
using ParallelStencil
using ParallelStencil.FiniteDifferences3D
@static if USE_GPU
    @init_parallel_stencil(CUDA,Float64,3);
else
    @init_parallel_stencil(Threads,Float64,3);
end
## 8th-order derivatives
@parallel function Dx_inn(in,out)
    @inn(out)=@d_xi_8(in);
    return nothing
end

@parallel function Dy_inn(in,out)
    @inn(out)=@d_yi_8(in);
    return nothing
end

@parallel function Dz_inn(in,out)
    @inn(out)=@d_zi_8(in);
    return nothing
end

@parallel_indices (iy,iz) function u_1_plus(in,out)
out[4:end-2,iy,iz]=in[:,iy,iz];
return nothing
end

@parallel_indices (iy,iz) function u_1_minus(in,out)
out[3:end-3,iy,iz]=in[:,iy,iz];
return nothing
end

@parallel_indices (ix,iz) function u_2_plus(in,out)
out[ix,4:end-2,iz]=in[ix,:,iz];
return nothing
end

@parallel_indices (ix,iz) function u_2_minus(in,out)
out[ix,3:end-3,iz]=in[ix,:,iz];
return nothing
end

@parallel_indices (ix,iy) function u_3_plus(in,out)
out[ix,iy,4:end-2]=in[ix,iy,:];
return nothing
end

@parallel_indices (ix,iy) function u_3_minus(in,out)
out[ix,iy,3:end-3]=in[ix,iy,:];
return nothing
end
##
nx=12;
ny=12;
nz=12;
A=@zeros(nx,ny,nz);
dtt1=@zeros(nx-5,ny,nz);
dtt2=@zeros(nx,ny-5,nz);
dtt3=@zeros(nx,ny,nz-5);
C=@zeros(nx,ny,nz);
A[5,5,5]=1;
## Dx_plus: place derivative in the later point
@parallel Dx_inn(A,dtt1);
@parallel (2:ny-1,2:nz-1) u_1_plus(dtt1,C);
## Dx_minus: place derivative in the previous point
@parallel Dx_inn(A,dtt1);
@parallel (2:ny-1,2:nz-1) u_1_minus(dtt1,C);
## Dy_plus: place derivative in the later point
@parallel Dy_inn(A,dtt2);
@parallel (2:nx-1,2:nz-1) u_2_plus(dtt2,C);
## Dy_minus: place derivative in the previous point
@parallel Dy_inn(A,dtt2);
@parallel (2:nx-1,2:nz-1) u_2_minus(dtt2,C);
## Dz_plus: place derivative in the later point
@parallel Dz_inn(A,dtt3);
@parallel (2:nx-1,2:ny-1) u_3_plus(dtt3,C);
## Dz_minus: place derivative in the previous point
@parallel Dz_inn(A,dtt3);
@parallel (2:nx-1,2:nz-1) u_3_minus(dtt3,C);
## 2nd-order derivatives
@parallel function Dx_inn(in,out)
    @inn(out)=@d_xi_1(in);
    return nothing
end

@parallel_indices (iy,iz) function u_1_plus(in,out)
out[2:end-1,iy,iz]=in[:,iy,iz];
return nothing
end

@parallel_indices (iy,iz) function u_1_minus(in,out)
out[1:end-2,iy,iz]=in[:,iy,iz];
return nothing
end
##
nx=12;
ny=12;
nz=12;
IND_accuracy=2;
A=@zeros(nx,ny,nz);
dtt1=@zeros(nx-IND_accuracy,ny,nz);
dtt2=@zeros(nx,ny-IND_accuracy,nz);
dtt3=@zeros(nx,ny,nz-IND_accuracy);
C=@zeros(nx,ny,nz);
A[5,5,5]=1;
## Dx_plus: place derivative in the later point
@parallel Dx_inn(A,dtt1);
@parallel (2:ny-1,2:nz-1) u_1_plus(dtt1,C);
## Dx_minus: place derivative in the previous point
@parallel Dx_inn(A,dtt1);
@parallel (2:ny-1,2:nz-1) u_1_minus(dtt1,C);
## 12th-order derivatives
@parallel function Dx_inn(in,out)
    @inn(out)=@d_xi_12(in);
    return nothing
end

@parallel function Dy_inn(in,out)
    @inn(out)=@d_yi_12(in);
    return nothing
end

@parallel function Dz_inn(in,out)
    @inn(out)=@d_zi_12(in);
    return nothing
end

@parallel_indices (iy,iz) function u_1_plus(in,out)
out[6:end-4,iy,iz]=in[:,iy,iz];
return nothing
end

@parallel_indices (iy,iz) function u_1_minus(in,out)
out[5:end-5,iy,iz]=in[:,iy,iz];
return nothing
end

@parallel_indices (ix,iz) function u_2_plus(in,out)
out[ix,6:end-4,iz]=in[ix,:,iz];
return nothing
end

@parallel_indices (ix,iz) function u_2_minus(in,out)
out[ix,5:end-5,iz]=in[ix,:,iz];
return nothing
end

@parallel_indices (ix,iy) function u_3_plus(in,out)
out[ix,iy,6:end-4]=in[ix,iy,:];
return nothing
end

@parallel_indices (ix,iy) function u_3_minus(in,out)
out[ix,iy,5:end-5]=in[ix,iy,:];
return nothing
end
##
nx=21;
ny=21;
nz=21;
A=@zeros(nx,ny,nz);
dtt1=@zeros(nx-9,ny,nz);
dtt2=@zeros(nx,ny-9,nz);
dtt3=@zeros(nx,ny,nz-9);
C=@zeros(nx,ny,nz);
A[11,11,11]=1;
## Dx_plus: place derivative in the later point
@parallel Dx_inn(A,dtt1);
@parallel (2:ny-1,2:nz-1) u_1_plus(dtt1,C);
## Dx_minus: place derivative in the previous point
@parallel Dx_inn(A,dtt1);
@parallel (2:ny-1,2:nz-1) u_1_minus(dtt1,C);


#=

amss_figures.jl

@author: Shunsuke Hori

=#

include("CES.jl")
include("BGP.jl")
using CES
using BGP

#initialize mugrid for value function iteration
muvec = linspace(-0.7,0.01,200)

#Time Varying Example

CES.M_time_example.transfers = true #Government can use transfers
PP_seq_time = LS.Planners_Allocation_Sequential(CES.M_time_example) #solve sequential problem

PP_im_time = Planners_Allocation_Bellman(CES.M_time_example,muvec)

sHist_h = [1,2,3,4,6,6,6]
sHist_l = [1,2,3,5,6,6,6]

sim_seq_h = LS.simulate(PP_seq_time,1.,1,7,sHist_h)
sim_im_h = simulate(PP_im_time,1.,1,7,sHist_h)
sim_seq_l = LS.simulate(PP_seq_time,1.,1,7,sHist_l)
sim_im_l = simulate(PP_im_time,1.,1,7,sHist_l)

using Plots
pyplot()
p=plot(size = (700,500),layout =(3,2),
        xaxis=(0:6), grid=false, titlefont=Plots.font("sans-serif", 10))
title!(p[1],"Consumption")
plot!(p[1],0:6,sim_seq_l[1],marker=:circle,color=:black,legend=false)
plot!(p[1],0:6,sim_im_l[1],marker=:circle,color=:red,legend=false)
plot!(p[1],0:6,sim_seq_h[1],marker=:utriangle,color=:black,legend=false)
plot!(p[1],0:6,sim_im_h[1],marker=:utriangle,color=:red,legend=false)
title!(p[2],"Labor")
plot!(p[2],0:6,sim_seq_l[2],marker=:circle,color=:black,lab="Complete Markets")
plot!(p[2],0:6,sim_im_l[2],marker=:circle,color=:red,lab="Incomplete Markets")
plot!(p[2],0:6,sim_seq_h[2],marker=:utriangle,color=:black,lab="")
plot!(p[2],0:6,sim_im_h[2],marker=:utriangle,color=:red,lab="")
title!(p[3],"Government Debt")
plot!(p[3],0:6,sim_seq_l[3],marker=:circle,color=:black,legend=false)
plot!(p[3],0:6,sim_im_l[3],marker=:circle,color=:red,legend=false)
plot!(p[3],0:6,sim_seq_h[3],marker=:utriangle,color=:black,legend=false)
plot!(p[3],0:6,sim_im_h[3],marker=:utriangle,color=:red,legend=false)
title!(p[4],"Tax Rate")
plot!(p[4],0:6,sim_seq_l[4],marker=:circle,color=:black,legend=false)
plot!(p[4],0:6,sim_im_l[5],marker=:circle,color=:red,legend=false)
plot!(p[4],0:6,sim_seq_h[4],marker=:utriangle,color=:black,legend=false)
plot!(p[4],0:6,sim_im_h[5],marker=:utriangle,color=:red,legend=false)
title!(p[5],"Government Spending", ylims=(0.05,0.25))
plot!(p[5],0:6,CES.M_time_example.G[sHist_l],marker=:circle,color=:black,legend=false)
plot!(p[5],0:6,CES.M_time_example.G[sHist_l],marker=:circle,color=:red,legend=false)
plot!(p[5],0:6,CES.M_time_example.G[sHist_h],marker=:utriangle,color=:black,legend=false)
plot!(p[5],0:6,CES.M_time_example.G[sHist_h],marker=:utriangle,color=:red,legend=false)
title!(p[6],"Output")
plot!(p[6],0:6,CES.M_time_example.Theta[sHist_l].*sim_seq_l[2],marker=:circle,color=:black,legend=false)
plot!(p[6],0:6,CES.M_time_example.Theta[sHist_l].*sim_im_l[2],marker=:circle,color=:red,legend=false)
plot!(p[6],0:6,CES.M_time_example.Theta[sHist_h].*sim_seq_h[2],marker=:utriangle,color=:black,legend=false)
plot!(p[6],0:6,CES.M_time_example.Theta[sHist_h].*sim_im_h[2],marker=:utriangle,color=:red,legend=false)
savefig("TaxSequence_time_varying_AMSS.png")
p1=p

#=
BGP Example
=#
BGP.M1.transfers = false   #Government can use transfers
PP_seq = LS.Planners_Allocation_Sequential(BGP.M1) #solve sequential problem
PP_bel = LS.Planners_Allocation_Bellman(BGP.M1,muvec) #solve recursive problem
PP_im = Planners_Allocation_Bellman(BGP.M1,muvec)

T = 20
#sHist = utilities.simulate_markov(M1.Pi,0,T)
sHist = [1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 1, 1, 1, 2, 2, 2, 2, 2, 2, 1]

#simulate
sim_seq = LS.simulate(PP_seq,0.5,1,T,sHist)
#sim_bel = PP_bel.simulate(0.5,0,T,sHist)
sim_im = simulate(PP_im,0.5,1,T,sHist)

#plot policies
p=plot(size = (700,500), layout = grid(3,2),
        xaxis=(0:T), grid=false, titlefont=Plots.font("sans-serif", 10))
title!(p[1], "Consumption")
plot!(p[1], sim_seq[1], marker=:circle, color=:black, lab="Complete Markets")
#plt[:plot](sim_bel[0],"-xk")
plot!(p[1], sim_im[1], marker=:utriangle, color=:black, lab="Incomplete Markets")
title!(p[2], "Labor")
plot!(p[2], sim_seq[1], marker=:circle, color=:black, legend=false)
#plt.plot(sim_bel[1],"-xk")
plot!(p[2], sim_im[1], marker=:utriangle, color=:black, legend=false)
title!(p[3], "Government Debt")
plot!(p[3], sim_seq[2], marker=:circle, color=:black, legend=false)
#plt.plot(sim_bel[2],"-xk")
plot!(p[3], sim_im[2], marker=:utriangle, color=:black, legend=false)
title!(p[4], "Tax Rate")
plot!(p[4], sim_seq[4], marker=:circle, color=:black, legend=false)
#plt.plot(sim_bel[3],"-xk")
plot!(p[4], sim_im[5], marker=:utriangle, color=:black, legend=false)
title!(p[5], "Government Spending", ylims=(0.05,0.25))
plot!(p[5], BGP.M1.G[sHist], marker=:circle, color=:black, legend=false)
#plt.plot(M1.G[sHist],"-^k")
title!(p[6], "Output")
plot!(p[6], BGP.M1.Theta[sHist].*sim_seq[2], marker=:circle,
        color=:black, legend=false)
#plt.plot(M1.Theta[sHist]*sim_bel[1],"-xk")
plot!(p[6], BGP.M1.Theta[sHist].*sim_im[2], marker=:utriangle,
        color=:black, legend=false)
savefig("TaxSequence_AMSS.png")
p2=p

#Now long simulations
T_long = 200
sim_seq_long = LS.simulate(PP_seq, 0.5,1,T_long)
sHist_long = sim_seq_long[end-2]
sim_im_long = simulate(PP_im, 0.5,1,T_long,sHist_long)

p=plot(size = (700,500), layout = (3,2), xaxis=(0:50:T_long), grid=false,
        titlefont=Plots.font("sans-serif", 10))
title!(p[1],"Consumption")
plot!(p[1], sim_seq_long[1], color=:black, linestyle=:solid, lab="Complete Markets")
plot!(p[1], sim_im_long[1], color=:black, linestyle=:dot, lab="Incomplete Markets")
title!(p[2],"Labor")
plot!(p[2], sim_seq_long[1], color=:black, linestyle=:solid, legend=false)
plot!(p[2], sim_im_long[1], color=:black, linestyle=:dot, legend=false)
title!(p[3],"Government Debt")
plot!(p[3], sim_seq_long[2], color=:black, linestyle=:solid, legend=false)
plot!(p[3], sim_im_long[2], color=:black, linestyle=:dot, legend=false)
title!(p[4],"Tax Rate")
plot!(p[4], sim_seq_long[3], color=:black, linestyle=:solid, legend=false)
plot!(p[4], sim_im_long[4], color=:black, linestyle=:dot, legend=false)
title!(p[5],"Government Spending",ylims=(0.05,0.25))
plot!(p[5], BGP.M1.G[sHist_long], color=:black, linestyle=:solid, legend=false)
plot!(p[5], BGP.M1.G[sHist_long], color=:black, linestyle=:dot, legend=false)
title!(p[6],"Output")
plot!(p[6],BGP.M1.Theta[sHist_long].*sim_seq_long[2],
    color=:black, linestyle=:solid, legend=false)
plot!(p[6],BGP.M1.Theta[sHist_long].*sim_im_long[2],
    color=:black, linestyle=:dot, legend=false)
# plt[:tight_layout]()
savefig("Long_SimulationAMSS.png")
p3=p

#=
Show Convergence example
=#
M_convergence = CES.M1
muvec = linspace(-0.15,0.0,100) #change
PP_C = Planners_Allocation_Bellman(M_convergence,muvec)
xgrid = PP_C.xgrid
xf = PP_C.policies[end-1] #get x policies
p= plot()
for s in 1:2
    plot!(p,xgrid,xf[1,s].(xgrid)-xgrid)
end
p4=p

sim_seq_convergence = simulate(PP_C,0.5,1,2000)
sHist_long = sim_seq_convergence[end]

p=plot(size = (700,500),layout = grid(3,2), xaxis=(0:200:2000),
        grid=false, titlefont=Plots.font("sans-serif", 10))
title!("Consumption")
plot!(sim_seq_convergence[1],color=:black,
    lab=["Complete Markets" "Incomplete Markets"])
title!(p[2],"Labor")
plot!(p[2],sim_seq_convergence[2],color=:black,legend=false)
title!(p[3],"Government Debt")
plot!(p[3],sim_seq_convergence[3],color=:black,legend=false)
title!(p[4],"Tax Rate")
plot!(p[4],sim_seq_convergence[4],color=:black,legend=false)
title!(p[5],"Government Spending",ylims=(0.05,0.25))
plot!(p[5],M_convergence.G[sHist_long],color=:black,legend=false)
title!(p[6],"Output")
plot!(p[6],M_convergence.Theta[sHist_long].*sim_seq_convergence[2],color=:black,legend=false)
savefig("Convergence_SimulationAMSS.png")
p5=p


# check the Bernoulli numbers and polynomials
#    this is mostly just a check that code is correct
#    most errors are arising because of errors in the functions being used
using Polylogarithms
using BenchmarkTools
using DataFrames, CSV
using PyPlot
using SpecialFunctions
import Base: parse
using Printf

Q = Polylogarithms.Q
c = Polylogarithms.c
# b = Polylogarithms.b
# f = Polylogarithms.f
# g = Polylogarithms.g

# from https://github.com/JuliaLang/julia/issues/18328
function parse(::Type{Complex{T}}, s::AbstractString) where {T<:Real}
    s = replace(s, "*^" => "e") # Mathematica seems to export in forms like "3.061616997868383*^-18"
    real_r = r"([+-]?\d*\.\d*(e[+-]\d+)?)([\s+-]|$)"
    imag_r = r"([+-]?\d*\.\d*(e[+-]\d+)?)\s*\*\s*I"

    z = zero(Complex{T})

    m = match( imag_r, s )
    if m != nothing
        z += im*parse(T, m[1])
    end

    m = match( real_r, s )
    if m != nothing
        z += parse(T, m[1])
    end
    return z
end

# input data from Mathematica and reparse into complex numbers
data1 = CSV.read("../data/Q_test_data_1.csv"; delim=",", type=String)
#    has trouble reading in numbers like "2." so read all into strings, and parse
data1[!,:n] = Int.(parse.(Float64, data1[!,:n] ))
data1[!,:s] = parse.(Complex{Float64}, data1[!,:s] )
data1[!,:r] = parse.(Float64, data1[!,:r] )
data1[!,:z] = parse.(Complex{Float64}, data1[!,:z] )
data1[!,:Q] = parse.(Complex{Float64}, data1[!,:Q] )
n = data1[!,:n]
m = size(data1,1)
s = data1[!,:s]
r = data1[!,:r]
μ = log.( data1[!,:z] )
ℒ = log.(complex.(-μ))  # '\u2112'

# figure(1)
# semilogx( data1[!,:r], real( data1[!,:Q] ), "b" )
# semilogx( data1[!,:r], imag( data1[!,:Q] ), "r--" )

# figure(2)
# loglog( data1[!,:r], abs.(real( data1[!,:Q] .- data1[1,:Q] )), "b" )
# loglog( data1[!,:r], abs.(imag( data1[!,:Q] .- data1[1,:Q] )), "r--" )

n_terms = 7

result = zeros(Complex{Float64}, m, n_terms)
error = zeros(Float64, m, n_terms)
rel_error = zeros(Float64, m, n_terms)

result2 = zeros(Complex{Float64}, m)
error2 = zeros(Float64, m)
rel_error2 = zeros(Float64, m)

for i=1:m
    for j=1:n_terms
        print("$i.")
        result[i,j] = Q(n[i], r[i], ℒ[i]; n_terms = j)
        error[i,j] = abs( result[i,j] - data1[i,:Q]  )
        rel_error[i,j] = error[i,j]/abs( data1[i,:Q] )

        result2[i] = (-1)^n[i] * factorial(n[i]) * gamma(-n[i] .- r[i]) * (-μ[i])^r[i]  + zeta(1 + r[i])
        error2[i] = abs( result2[i] - data1[i,:Q]  )
        rel_error2[i] = error2[i]/abs( data1[i,:Q] )
    end
end
println("")

for i=1:5
    @printf(" r1 = %.20f, r2 = %.20f, r3 = %.20f\n", abs(result[i,1]), abs(result[i,2]) , abs(result[i,3])  )
end

fig = figure("Q_bench", figsize=(5,5))
clf()
d = 0.00
for j=1:n_terms
    loglog( data1[!,:r] .+ d, rel_error[:,j], "-"; label=@sprintf("%d terms",j) )
end
loglog( data1[!,:r] .+ d, rel_error2[:], "r-"; label="direct")
legend()
xlabel("tau")
ylabel("relative absolute error")


using Test
using NL2sol
using Glob

print_steps = false
verbose = false
include("util.jl")

for fn in glob("./optests/*.jl")
    include(fn)
end

@testset "NL2sol tests" begin
    @test rosenbrock(scale=3)
    @test helix(scale=3)
    @test singular(scale=3)
    @test woods(scale=3)
    @test zangwill(scale=3)

    @test engvall(scale=3)
    @test branin(scale=3)
    @test beale(scale=2)
    @test cragg_levy(scale=2)
    @test box(scale=2)
    #@test davidon(scale 1, verbose=true)  # false convergence
    @test freudenstein_roth(scale=3)
    @test watson6(scale=1)
    @test watson9(scale=1)
    @test watson12(scale=1)
    #@test watson20(scale=1, verbose=true) # false convergence
    @test chebyquad(scale=2)
    @test brown_dennis(scale=3)
    @test bard(scale=3)
    @test jennrich_sampson(scale=3)
    @test kowalik_osborne(scale=3)
    @test osborne1(scale=3)
    @test osborne2(scale=2)
    @test madsen(scale=3)
    #@test meyer(verbose=true)             # false convergence
    @test brown5()
    @test brown10()
    @test brown30()
    @test brown40()
end


# Test for tree-based analysis tools found in `AnalysisTools.jl`.
using Test
using IncrementalInference

@testset "Number of non-zero calculation for frontals." begin
    # Alternative way of calculating number of upper triangular matrix elements.
    nnzFrontalsRecursive(dim) = dim==1 ? 1 : dim + nnzFrontalsRecursive(dim-1)
    # Both must agree for any integer number dimension.
    for dim in 1:100
        @test nnzFrontalsRecursive(dim) == nnzFrontals(dim)
    end
end

@testset "Number of non-zero calculation for full cliques." begin
    fg = generateGraph_Kaess()
    vo = [:l1, :l2, :x1, :x2, :x3]
    tree = buildTreeReset!(fg, vo)
    # Must agree with hand-calculated values, iSAM2 paper.
    @test nnzClique(tree.cliques[1]) == 3
    @test nnzClique(tree.cliques[2]) == 5
    @test nnzClique(tree.cliques[3]) == 2
end

@testset "Number of non-zero calculation for full trees." begin
    fg = generateGraph_Kaess()
    vo = [:l1, :l2, :x1, :x2, :x3]
    tree = buildTreeReset!(fg, vo)
    # Must agree with hand-calculated values, iSAM2 paper.
    @test nnzTree(tree) == 10
end

@testset "Test drawTree" begin
    fg = generateGraph_Kaess(graphinit=false)
    vo = [:l1, :l2, :x1, :x2, :x3]
    tree = buildTreeReset!(fg, vo)

    drawTree(tree, show=false)
end

##