"""
    PI_NewtonGS

Uses the interval arithmetic presented in ValidatedNumerics to bound unique
implicit functions x:P->X defined by a system of equations h(z,p)=0 via
Gauss-Siedel Newton. Inputs:
* `X0::Vector{Interval{T}}`: Bounds for dependent variables
* `P::Vector{Interval{T}}`: Bounds for independent variables
* `hj::Function`: Jacobian of h(x,p) with respect to x
* `h::Function`: Equations defining potential implicit function
* `opt::Vector{Any}`: `opt[1]::Int64` is the number of iterations, `opt[2]::Float64`
                       is the equality tolerance for intervals, `opt[3]::Float64`
                       is the amount added to M[i,i] when processing extended
                       interval division.
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
* `eDflag::Bool`: True if extended division occured in iteration
Returns a tuple `(X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh)` where
* `X::Vector{Interval{T}}`: Output interval box
* `Xtemp::Vector{Interval{T}}`: Extra box formed from extended division
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
* `inclusionLow::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from lower bound
* `inclusionHigh::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from upper bound
"""
function PI_NewtonGS(X0::Vector{Interval{T}},P::Vector{Interval{T}},
                     hj::Function,h::Function,
                     opt::Vector{Any},Eflag::Bool,
                     Iflag::Bool,eDflag::Bool) where {T<:AbstractFloat}
  # unpacks option file
  kmax::Int64 = opt[1]
  etol::Float64 = opt[2]
  rtol::Float64 = opt[3]

  # Initializes variables
  nx::Int64 = length(X0)
  S1::Interval{T} = Interval(0.0)
  S2::Interval{T} = Interval(0.0)
  inclusion::Vector{Bool} = [false for i=1:nx]
  inclusionHigh::Vector{Bool} = [false for i=1:nx]
  inclusionLow::Vector{Bool} = [false for i=1:nx]
  eD::Int64 = 0
  exclusion::Bool = false
  Iflag::Bool = false
  Eflag::Bool = false
  eDflag::Bool = false

  X::Vector{Interval{T}} = copy(X0)
  Xi::Vector{Interval{T}} = copy(X0)
  N::Vector{Interval{T}} = copy(X)
  x_mid::Vector{T} = mid.(X)
  k::Int64 = 1
  H::Union{Array{Interval{T},2},Vector{Interval{T}}} = h(x_mid,P)
  J::Union{Array{Interval{T},2},Vector{Interval{T}}} = hj(X,P)
  Y::Union{Array{T,2},Vector{T}} = Preconditioner(hj,X,P,jac="User")

  if (nx == 1)
    B::Vector{Interval{T}} = Y.*H
    M::Union{Array{Interval{T},2},Vector{Interval{T}}} = Y.*J
  else
    B = Y*H
    M = Y*J
  end

  for i=1:nx
    S1 = Interval(0.0)
    S2 = Interval(0.0)
    for j=1:nx
      if (j<i)
        S1 += M[i,j]*(X[j]-x_mid[j])
      elseif (j>i)
        S2 += M[i,j]*(X[j]-x_mid[j])
      end
    end
    if M[i,i].lo*M[i,i].hi > 0.0

      N[i] = x_mid[i] - (B[i]+S1+S2)/M[i,i]

    else
      Ntemp = copy(N)

      eD,N[i],Ntemp[i] = extProcess(N[i],X[i],M[i,i],S1,S2,B[i],rtol)

      if eD == 1
        eDflag = true
        Xtemp = copy(X)
        Xtemp[i] = Ntemp[i] ∩ X[i]
        X[i] = N[i] ∩ X[i]
        return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
      end
    end
    if Strict_XinY(N[i],X[i])
      inclusion[i] = true
      inclusionHigh[i] = true
      inclusionLow[i] = true
    else
      inclusion[i] = false
      inclusionLow[i] = false
      inclusionHigh[i] = false
      if (N[i].lo>X[i].lo)
        inclusionLow[i] = true
      elseif (N[i].hi<X[i].hi)
        inclusionHigh[i] = true
      end
    end
    if ~isdisjoint(N[i],X[i])
      X[i] = N[i] ∩ X[i]
    else
      exclusion = true
      break
    end
  end
  k += 1

  # checks if all components are included
  if (Iflag == false)
    for i=1:nx
      if (inclusion[i] == true)
        Iflag = true
        continue
      elseif (inclusion[i] == false)
        Iflag = false
        break
      end
    end
  end

  #
  while ((k<kmax) && isEqual(X,Xi,etol) == false)

    Xi = copy(X0)
    x_mid = mid.(X)
    H = h(x_mid,P)
    J = hj(X,P)
    Y = Preconditioner(hj,X,P,jac="User")

    if (nx == 1)
      B = copy(Y.*H)
      M = copy(Y.*J)
    else
      B = copy(Y*H)
      M = copy(Y*J)
    end

    for i=1:nx
      S1 = Interval(0.0)
      S2 = Interval(0.0)
      for j=1:nx
        if (j<i)
          S1 += M[i,j]*(X[j]-x_mid[j])
        elseif (j>i)
          S2 += M[i,j]*(X[j]-x_mid[j])
        end
      end
      if M[i,i].lo*M[i,i].hi > 0.0
        N[i] = x_mid[i] - (B[i]+S1+S2)/M[i,i]
      else
        Ntemp = copy(N)
        eD,N[i],Ntemp[i] = extProcess(N[i],X[i],M[i,i],S1,S2,B[i],rtol)
        if eD == 1
          eDflag = true
          Xtemp = copy(X)
          Xtemp[i] = Ntemp[i] ∩ X[i]
          X[i] = N[i] ∩ X[i]
          return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
        end
      end
      if Strict_XinY(N[i],X[i])
        inclusion[i] = true
        inclusionHigh[i] = true
        inclusionLow[i] = true
      else
        inclusion[i] = false
        inclusionLow[i] = false
        inclusionHigh[i] = false
        if (N[i].lo>X[i].lo)
          inclusionLow[i] = true
        elseif (N[i].hi<X[i].hi)
          inclusionHigh[i] = true
        end
      end
      if ~isdisjoint(N[i],X[i])
        X[i] = N[i] ∩ X[i]
      else
        exclusion = true
        break
      end
    end
    k += 1

    # checks if all components are included
    if (Iflag == false)
      for i=1:nx
        if (inclusion[i] == true)
          Iflag = true
          continue
        elseif (inclusion[i] == false)
          Iflag = false
          break
        end
      end
    end

  end

  if exclusion
    Eflag = true
  end

  Xtemp = copy(X)

  return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
end

"""
    PI_KrawczykCW

Uses the interval arithmetic presented in ValidatedNumerics to bound unique
implicit functions x:P->X defined by a system of equations h(z,p)=0 via
a component-wise Krawczyk method. Inputs:
* `X0::Vector{Interval{T}}`: Bounds for dependent variables
* `P::Vector{Interval{T}}`: Bounds for independent variables
* `hj::Function`: Jacobian of h(x,p) with respect to x
* `h::Function`: Equations defining potential implicit function
* `opt::Vector{Any}`: `opt[1]::Int64` is the number of iterations, `opt[2]::Float64`
                       is the equality tolerance for intervals, `opt[3]::Float64`
                       is the amount added to M[i,i] when processing extended
                       interval division.
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
Returns a tuple `(X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh)` where
* `X::Vector{Interval{T}}`: Output interval box
* `Xtemp::Vector{Interval{T}}`: Extra box formed from extended division
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
* `inclusionLow::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from lower bound
* `inclusionHigh::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from upper bound
"""
function PI_KrawczykCW(X0::Vector{Interval{T}},P::Vector{Interval{T}},
                     hj::Function,h::Function,
                     opt::Vector{Any},Eflag::Bool,Iflag::Bool) where {T<:AbstractFloat}
  #println(" ---------- begin out place kraw ------------- ")
  #println("X0: $X0")
  #println("P: $P")
  # unpacks option file
  kmax::Int64 = opt[1]
  etol::Float64 = opt[2]

  # Initializes variables
  nx::Int64 = length(X0)
  S1::Interval{T} = Interval(0.0)
  S2::Interval{T} = Interval(0.0)
  inclusion::Vector{Bool} = [false for i=1:nx]
  inclusionHigh::Vector{Bool} = [false for i=1:nx]
  inclusionLow::Vector{Bool} = [false for i=1:nx]
  exclusion::Bool = false
  Iflag::Bool = false
  Eflag::Bool = false
  eDflag::Bool = false
  X::Vector{Interval{T}} = copy(X0)
  Xi::Vector{Interval{T}} = copy(X)
  N::Vector{Interval{T}} = copy(X)
  x_mid::Vector{T} = mid.(X)
  k::Int64 = 1

  H::Union{Array{Interval{T},2},Vector{Interval{T}}} = h(x_mid,P)
  J::Union{Array{Interval{T},2},Vector{Interval{T}}} = hj(X,P)
  #println("H, J calced")
  #println("H: $H")
  #println("J: $J")
  Y::Union{Array{T,2},Vector{T}} = Preconditioner(hj,X,P,jac="User")
  #println("Y calced")
  #println("Y: $Y")
  if (nx == 1)
    B::Vector{Interval{T}} = Y.*H
    M::Union{Array{Interval{T},2},Vector{Interval{T}}} = eye(nx)-Y.*J
  else
    B = Y*H
    M = eye(nx)-Y*J
  end
  #println("B M calced")

  for i=1:nx
    S1 = Interval(0.0)
    S2 = Interval(0.0)
    for j=1:nx
      if (j<i)
        S1 += M[i,j]*(X[j]-x_mid[j])
      elseif (j>=i)
        S2 += M[i,j]*(X[j]-x_mid[j])
      end
    end
    N[i] = x_mid[i] - B[i] + S1 + S2

    if Strict_XinY(N[i],X[i])
      inclusion[i] = true
      inclusionHigh[i] = true
      inclusionLow[i] = true
    else
      inclusion[i] = false
      inclusionLow[i] = false
      inclusionHigh[i] = false
      if (N[i].lo>X[i].lo)
        inclusionLow[i] = true
      elseif (N[i].hi<X[i].hi)
        inclusionHigh[i] = true
      end
    end
    if ~isdisjoint(N[i],X[i])
      X[i] = N[i] ∩ X[i]
    else
      exclusion = true
      break
    end
  end
  k += 1

  if (Iflag == false)
    for i=1:nx
      if (inclusion[i] == true)
        Iflag = true
        continue
      elseif (inclusion[i] == false)
        Iflag = false
        break
      end
    end
  end

  #
  while ((k<kmax) && (isEqual(X,Xi,etol) == false))
    #println("iteration $k")
    Xi = X0
    x_mid = mid.(X)
    H = h(x_mid,P)
    J = hj(X,P)
    #println("H, J calced")
    Y = Preconditioner(hj,X,P,jac="User")
    #println("Y calced")
    #println("Y: $Y")
    #println("H: $H")
    #println("J: $J")
    if (nx == 1)
      B = Y.*H
      M = eye(nx)-Y.*J
    else
      B = Y*H
      M = eye(nx)-Y*J
    end
    #println("B, M calced")

    for i=1:nx
      S1 = Interval(0.0)
      S2 = Interval(0.0)
      for j=1:nx
        if (j<i)
          S1 += M[i,j]*(X[j]-x_mid[j])
        elseif (j>=i)
          S2 += M[i,j]*(X[j]-x_mid[j])
        end
      end
      N[i] = x_mid[i] - B[i]+S1+S2
      if Strict_XinY(N[i],X[i])
        inclusion[i] = true
        inclusionHigh[i] = true
        inclusionLow[i] = true
      else
        inclusion[i] = false
        inclusionLow[i] = false
        inclusionHigh[i] = false
        if (N[i].lo>X[i].lo)
          inclusionLow[i] = true
        elseif (N[i].hi<X[i].hi)
          inclusionHigh[i] = true
        end
      end
      if ~isdisjoint(N[i],X[i])
        X[i] = N[i] ∩ X[i]
      else
        exclusion = true
        break
      end
    end
    k += 1

    # checks if all components are included
    if (Iflag == false)
      for i=1:nx
        if (inclusion[i] == true)
          Iflag = true
          continue
        elseif (inclusion[i] == false)
          Iflag = false
          break
        end
      end
    end
  end

  if exclusion
    Eflag = true
  end
  return X,Eflag,Iflag,inclusionLow,inclusionHigh
end

"""
    PIn_NewtonGS

Uses the interval arithmetic presented in ValidatedNumerics to bound unique
implicit functions x:P->X defined by a system of equations h(z,p)=0 via
Gauss-Siedel Newton. The jacobian of h w.r.t. z is calculated in place. Inputs:
* `X0::Vector{Interval{T}}`: Bounds for dependent variables
* `P::Vector{Interval{T}}`: Bounds for independent variables
* `hj!::Function`: Jacobian of h(x,p) with respect to x (takes input J,x,p)
* `h!::Function`: Equations defining potential implicit function (takes inputs H,x,p)
* `opt::Vector{Any}`: `opt[1]::Int64` is the number of iterations, `opt[2]::Float64`
                       is the equality tolerance for intervals, `opt[3]::Float64`
                       is the amount added to M[i,i] when processing extended
                       interval division.
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
* `eDflag::Bool`: True if extended division occured in iteration
Returns a tuple `(X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh)` where
* `X::Vector{Interval{T}}`: Output interval box
* `Xtemp::Vector{Interval{T}}`: Extra box formed from extended division
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
* `inclusionLow::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from lower bound
* `inclusionHigh::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from upper bound
"""
function PIn_NewtonGS(X0::Vector{Interval{T}},P::Vector{Interval{T}},
                     hj!::Function,h!::Function,
                     opt::Vector{Any},Eflag::Bool,
                     Iflag::Bool,eDflag::Bool) where {T<:AbstractFloat}
  # unpacks option file
  kmax::Int64 = opt[1]
  etol::Float64 = opt[2]
  rtol::Float64 = opt[3]

  # Initializes variables
  nx::Int64 = length(X0)
  S1::Interval{T} = Interval(0.0)
  S2::Interval{T} = Interval(0.0)
  S3::Interval{T} = Interval(0.0)
  inclusion::Vector{Bool} = [false for i=1:nx]
  inclusionHigh::Vector{Bool} = [false for i=1:nx]
  inclusionLow::Vector{Bool} = [false for i=1:nx]
  eD::Int64 = 0
  exclusion::Bool = false
  Iflag::Bool = false
  Eflag::Bool = false
  eDflag::Bool = false
  k::Int64 = 1
  X::Vector{Interval{T}} = copy(X0)
  Xi::Vector{Interval{T}} = copy(X0)
  N::Vector{Interval{T}} = copy(X)

  # creates storage for h(xm,P), J_x(X,P), and preconditioner Y
  H::Vector{Interval{T}} = [Interval(0.0) for i=1:nx]
  J::SparseMatrixCSC{Interval{T},Int64} = spzeros(Interval{T},nx,nx)

  # initalizes storage object for in-place sparse calculations
  SSto = SparseInSto()
  SSto.Xh = copy(H)
  SSto.Yh = copy(H)
  SSto.Zh = copy(H)
  SSto.Xj = spzeros(Interval{T},nx,nx)
  SSto.Yj = spzeros(Interval{T},nx,nx)
  SSto.Zj = spzeros(Interval{T},nx,nx)
  SSto.nx = copy(nx)

  # calculates extension of h(xm,P) and J_x(X,P)
  x_mid::Vector{T} = mid.(X)
  h!(H,x_mid,P)
  hj!(J,X,P)
  Sparse_Precondition!(H,J,mid.(J),SSto)
  Mt = transpose(J)
  for i=1:nx
    S1 = Interval(0.0)
    S2 = Interval(0.0)
    S3 = Interval(0.0)
    for q=(Mt.colptr[i]):(Mt.colptr[i+1]-1)
      if (i < Mt.rowval[q])
        S1 += Mt.nzval[q]*(X[Mt.rowval[q]]-x_mid[Mt.rowval[q]])
      elseif (i > Mt.rowval[q])
        S2 += Mt.nzval[q]*(X[Mt.rowval[q]]-x_mid[Mt.rowval[q]])
      else
        S3 = Mt.nzval[q]
      end
    end
    if S3.lo*S3.hi > 0.0
      N[i] = x_mid[i] - (H[i]+S1+S2)/S3
    else
      Ntemp = copy(N)
      eD,N[i],Ntemp[i] = extProcess(N[i],X[i],S3,S1,S2,H[i],rtol)
      if eD == 1
        eDflag = true
        Xtemp = copy(X)
        Xtemp[i] = Ntemp[i] ∩ X[i]
        X[i] = N[i] ∩ X[i]
        return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
      end
    end
    if Strict_XinY(N[i],X[i])
      inclusion[i] = true
      inclusionHigh[i] = true
      inclusionLow[i] = true
    else
      inclusion[i] = false
      inclusionLow[i] = false
      inclusionHigh[i] = false
      if (N[i].lo>X[i].lo)
        inclusionLow[i] = true
      elseif (N[i].hi<X[i].hi)
        inclusionHigh[i] = true
      end
    end
    if ~isdisjoint(N[i],X[i])
      X[i] = N[i] ∩ X[i]
    else
      exclusion = true
      break
    end
  end
  k += 1

  # checks if all components are included
  if (Iflag == false)
    for i=1:nx
      if (inclusion[i] == true)
        Iflag = true
        continue
      elseif (inclusion[i] == false)
        Iflag = false
        break
      end
    end
  end

  #
  while ((k<kmax) && isEqual(X,Xi,etol) == false)

    x_mid = mid.(X)
    h!(H,x_mid,P)
    hj!(J,X,P)
    Sparse_Precondition!(H,J,mid.(J),SSto)
    Mt = transpose(J)

    for i=1:nx
      S1 = Interval(0.0)
      S2 = Interval(0.0)
      S3 = Interval(0.0)
      for q=(Mt.colptr[i]):(Mt.colptr[i+1]-1)
        if (i < Mt.rowval[q])
          S1 += Mt.nzval[q]*(X[Mt.rowval[q]]-x_mid[Mt.rowval[q]])
        elseif (i > Mt.rowval[q])
          S2 += Mt.nzval[q]*(X[Mt.rowval[q]]-x_mid[Mt.rowval[q]])
        else
          S3 = Mt.nzval[q]
        end
      end
      if S3.lo*S3.hi > 0.0
        N[i] = x_mid[i] - (H[i]+S1+S2)/S3
      else
        Ntemp = copy(N)
        eD,N[i],Ntemp[i] = extProcess(N[i],X[i],S3,S1,S2,H[i],rtol)
        if eD == 1
          eDflag = true
          Xtemp = copy(X)
          Xtemp[i] = Ntemp[i] ∩ X[i]
          X[i] = N[i] ∩ X[i]
          return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
        end
      end
      if Strict_XinY(N[i],X[i])
        inclusion[i] = true
        inclusionHigh[i] = true
        inclusionLow[i] = true
      else
        inclusion[i] = false
        inclusionLow[i] = false
        inclusionHigh[i] = false
        if (N[i].lo>X[i].lo)
          inclusionLow[i] = true
        elseif (N[i].hi<X[i].hi)
          inclusionHigh[i] = true
        end
      end
      if ~isdisjoint(N[i],X[i])
        X[i] = N[i] ∩ X[i]
      else
        exclusion = true
        break
      end
    end
    k += 1

    # checks if all components are included
    if (Iflag == false)
      for i=1:nx
        if (inclusion[i] == true)
          Iflag = true
          continue
        elseif (inclusion[i] == false)
          Iflag = false
          break
        end
      end
    end
  end

  if exclusion
    Eflag = true
  end
  Xtemp = copy(X)
  return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
end

"""
    PIn_KrawczykCW

Uses the interval arithmetic presented in ValidatedNumerics to bound unique
implicit functions x:P->X defined by a system of equations h(z,p)=0 via
a component-wise Krawczyk method. Inputs:
* `X0::Vector{Interval{T}}`: Bounds for dependent variables
* `P::Vector{Interval{T}}`: Bounds for independent variables
* `hj!::Function`: Jacobian of h(x,p) with respect to x (takes input J,x,p)
* `h!::Function`: Equations defining potential implicit function (takes inputs H,x,p)
* `opt::Vector{Any}`: `opt[1]::Int64` is the number of iterations, `opt[2]::Float64`
                       is the equality tolerance for intervals, `opt[3]::Float64`
                       is the amount added to M[i,i] when processing extended
                       interval division.
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
Returns a tuple `(X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh)` where
* `X::Vector{Interval{T}}`: Output interval box
* `Xtemp::Vector{Interval{T}}`: Extra box formed from extended division
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
* `inclusionLow::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from lower bound
* `inclusionHigh::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from upper bound
"""
function PIn_KrawczykCW(X0::Vector{Interval{T}},P::Vector{Interval{T}},
                     hj!::Function,h!::Function,
                     opt::Vector{Any},Eflag::Bool,Iflag::Bool) where {T<:AbstractFloat}

  # unpacks option file
  kmax::Int64 = opt[1]
  etol::Float64 = opt[2]

  # Initializes variables
  nx::Int64 = length(X0)
  S::Vector{Interval{T}} = [Interval(0.0) for i=1:nx]
  inclusion::Vector{Bool} = [false for i=1:nx]
  inclusionHigh::Vector{Bool} = [false for i=1:nx]
  inclusionLow::Vector{Bool} = [false for i=1:nx]
  exclusion::Bool = false
  Iflag::Bool = false
  Eflag::Bool = false
  eDflag::Bool = false
  X::Vector{Interval{T}} = copy(X0)
  Xi::Vector{Interval{T}} = copy(X)
  N::Vector{Interval{T}} = [Interval(0.0) for i=1:nx]
  x_mid::Vector{T} = mid.(X)
  k::Int64 = 1
  # creates storage for h(xm,P), J_x(X,P), and preconditioner Y
  H::Vector{Interval{T}} = [Interval(0.0) for i=1:nx]
  J::SparseMatrixCSC{Interval{T},Int64} = spzeros(Interval{T},nx,nx)

  # initalizes storage object for in-place sparse calculations
  SSto = SparseInSto()
  SSto.Xh = copy(H)
  SSto.Yh = copy(H)
  SSto.Zh = copy(H)
  SSto.Xj = spzeros(Interval{T},nx,nx)
  SSto.Yj = spzeros(Interval{T},nx,nx)
  SSto.Zj = spzeros(Interval{T},nx,nx)
  SSto.nx = copy(nx)

  # calculates extension of h(xm,P) and J_x(X,P)
  h!(H,x_mid,P)
  hj!(J,X,P)
  Sparse_Precondition!(H,J,mid.(J),SSto)
  Mt = transpose(J)
  for i=1:nx
    for q=(Mt.colptr[i]):(Mt.colptr[i+1]-1)
      if (i == Mt.rowval[q])
        N[i] += (1.0-Mt.nzval[q])*(X[Mt.rowval[q]]-x_mid[Mt.rowval[q]])
      else
        N[i] += -Mt.nzval[q]*(X[Mt.rowval[q]]-x_mid[Mt.rowval[q]])
      end
    end
    N[i] = x_mid[i] - H[i] + N[i]

    if Strict_XinY(N[i],X[i])
      inclusion[i] = true
      inclusionHigh[i] = true
      inclusionLow[i] = true
    else
      inclusion[i] = false
      inclusionLow[i] = false
      inclusionHigh[i] = false
      if (N[i].lo>X[i].lo)
        inclusionLow[i] = true
      elseif (N[i].hi<X[i].hi)
        inclusionHigh[i] = true
      end
    end
    if ~isdisjoint(N[i],X[i])
      X[i] = N[i] ∩ X[i]
    else
      exclusion = true
      break
    end
  end
  k += 1

  if (Iflag == false)
    for i=1:nx
      if (inclusion[i] == true)
        Iflag = true
        continue
      elseif (inclusion[i] == false)
        Iflag = false
        break
      end
    end
  end
  #
  while ((k<kmax) && (isEqual(X,Xi,etol) == false))
    Xi = X0
    x_mid = mid.(X)
    h!(H,x_mid,P)
    hj!(J,X,P)
    Sparse_Precondition!(H,J,mid.(J),SSto)
    Mt = transpose(J)
    N = [Interval(0.0) for i=1:nx]
    for i=1:nx
      for q=(Mt.colptr[i]):(Mt.colptr[i+1]-1)
        if (i == Mt.rowval[q])
          N[i] += (1.0-Mt.nzval[q])*(X[Mt.rowval[q]]-x_mid[Mt.rowval[q]])
        else
          N[i] += -Mt.nzval[q]*(X[Mt.rowval[q]]-x_mid[Mt.rowval[q]])
        end
      end
      N[i] = x_mid[i] - H[i] + N[i]

      if Strict_XinY(N[i],X[i])
        inclusion[i] = true
        inclusionHigh[i] = true
        inclusionLow[i] = true
      else
        inclusion[i] = false
        inclusionLow[i] = false
        inclusionHigh[i] = false
        if (N[i].lo>X[i].lo)
          inclusionLow[i] = true
        elseif (N[i].hi<X[i].hi)
          inclusionHigh[i] = true
        end
      end
      if ~isdisjoint(N[i],X[i])
        X[i] = N[i] ∩ X[i]
      else
        exclusion = true
        break
      end
    end
    k += 1

    # checks if all components are included
    if (Iflag == false)
      for i=1:nx
        if (inclusion[i] == true)
          Iflag = true
          continue
        elseif (inclusion[i] == false)
          Iflag = false
          break
        end
      end
    end
  end

  if exclusion
    Eflag = true
  end
  return X,Eflag,Iflag,inclusionLow,inclusionHigh
end

"""
    PId_NewtonGS

Uses the interval arithmetic presented in ValidatedNumerics to bound unique
implicit functions x:P->X defined by a system of equations h(z,p)=0 via
Gauss-Siedel Newton. The jacobian of h w.r.t. z is calculated in place. Inputs:
* `X0::Vector{Interval{T}}`: Bounds for dependent variables
* `P::Vector{Interval{T}}`: Bounds for independent variables
* `hj!::Function`: Jacobian of h(x,p) with respect to x (takes input J,x,p)
* `h!::Function`: Equations defining potential implicit function (takes inputs H,x,p)
* `opt::Vector{Any}`: `opt[1]::Int64` is the number of iterations, `opt[2]::Float64`
                       is the equality tolerance for intervals, `opt[3]::Float64`
                       is the amount added to M[i,i] when processing extended
                       interval division.
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
* `eDflag::Bool`: True if extended division occured in iteration
Returns a tuple `(X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh)` where
* `X::Vector{Interval{T}}`: Output interval box
* `Xtemp::Vector{Interval{T}}`: Extra box formed from extended division
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
* `inclusionLow::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from lower bound
* `inclusionHigh::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from upper bound
"""
function PId_NewtonGS(X0::Vector{Interval{T}},P::Vector{Interval{T}},
                     hj!::Function,h!::Function,
                     opt::Vector{Any},Eflag::Bool,
                     Iflag::Bool,eDflag::Bool) where {T<:AbstractFloat}
  # unpacks option file
  kmax::Int64 = opt[1]
  etol::Float64 = opt[2]
  rtol::Float64 = opt[3]

  # Initializes variables
  nx::Int64 = length(X0)
  S1::Interval{T} = Interval(0.0)
  S2::Interval{T} = Interval(0.0)
  S3::Interval{T} = Interval(0.0)
  inclusion::Vector{Bool} = [false for i=1:nx]
  inclusionHigh::Vector{Bool} = [false for i=1:nx]
  inclusionLow::Vector{Bool} = [false for i=1:nx]
  eD::Int64 = 0
  exclusion::Bool = false
  Iflag::Bool = false
  Eflag::Bool = false
  eDflag::Bool = false
  k::Int64 = 1
  X::Vector{Interval{T}} = copy(X0)
  Xi::Vector{Interval{T}} = copy(X0)
  N::Vector{Interval{T}} = copy(X)

  # creates storage for h(xm,P), J_x(X,P), and preconditioner Y
  H::Vector{Interval{T}} = [Interval(0.0) for i=1:nx]
  J::Array{Interval{T},2} = zeros(Interval{T},nx,nx)

  # calculates extension of h(xm,P) and J_x(X,P)
  x_mid::Vector{T} = mid.(X)
  h!(H,x_mid,P)
  hj!(J,X,P)
  Dense_Precondition!(H,J,mid.(J),nx)

  for i=1:nx
    S1 = Interval(0.0)
    S2 = Interval(0.0)
    for j=1:nx
      if (j<i)
        S1 += J[i,j]*(X[j]-x_mid[j])
      elseif (j>i)
        S2 += J[i,j]*(X[j]-x_mid[j])
      end
    end
    if J[i,i].lo*J[i,i].hi > 0.0
      N[i] = x_mid[i] - (H[i]+S1+S2)/J[i,i]
    else
      Ntemp = copy(N)
      eD,N[i],Ntemp[i] = extProcess(N[i],X[i],J[i,i],S1,S2,H[i],rtol)
      if eD == 1
        eDflag = true
        Xtemp = copy(X)
        Xtemp[i] = Ntemp[i] ∩ X[i]
        X[i] = N[i] ∩ X[i]
        return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
      end
    end
    if Strict_XinY(N[i],X[i])
      inclusion[i] = true
      inclusionHigh[i] = true
      inclusionLow[i] = true
    else
      inclusion[i] = false
      inclusionLow[i] = false
      inclusionHigh[i] = false
      if (N[i].lo>X[i].lo)
        inclusionLow[i] = true
      elseif (N[i].hi<X[i].hi)
        inclusionHigh[i] = true
      end
    end
    if ~isdisjoint(N[i],X[i])
      X[i] = N[i] ∩ X[i]
    else
      exclusion = true
      break
    end
  end
  k += 1

  # checks if all components are included
  if (Iflag == false)
    for i=1:nx
      if (inclusion[i] == true)
        Iflag = true
        continue
      elseif (inclusion[i] == false)
        Iflag = false
        break
      end
    end
  end

  #
  while ((k<kmax) && isEqual(X,Xi,etol) == false)

    x_mid = mid.(X)
    h!(H,x_mid,P)
    hj!(J,X,P)
    Dense_Precondition!(H,J,mid.(J),nx)

    for i=1:nx
      S1 = Interval(0.0)
      S2 = Interval(0.0)
      for j=1:nx
        if (j<i)
          S1 += J[i,j]*(X[j]-x_mid[j])
        elseif (j>i)
          S2 += J[i,j]*(X[j]-x_mid[j])
        end
      end
      if J[i,i].lo*J[i,i].hi > 0.0
        N[i] = x_mid[i] - (H[i]+S1+S2)/J[i,i]
      else
        Ntemp = copy(N)
        eD,N[i],Ntemp[i] = extProcess(N[i],X[i],J[i,i],S1,S2,H[i],rtol)
        if eD == 1
          eDflag = true
          Xtemp = copy(X)
          Xtemp[i] = Ntemp[i] ∩ X[i]
          X[i] = N[i] ∩ X[i]
          return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
        end
      end
      if Strict_XinY(N[i],X[i])
        inclusion[i] = true
        inclusionHigh[i] = true
        inclusionLow[i] = true
      else
        inclusion[i] = false
        inclusionLow[i] = false
        inclusionHigh[i] = false
        if (N[i].lo>X[i].lo)
          inclusionLow[i] = true
        elseif (N[i].hi<X[i].hi)
          inclusionHigh[i] = true
        end
      end
      if ~isdisjoint(N[i],X[i])
        X[i] = N[i] ∩ X[i]
      else
        exclusion = true
        break
      end
    end
    k += 1

    # checks if all components are included
    if (Iflag == false)
      for i=1:nx
        if (inclusion[i] == true)
          Iflag = true
          continue
        elseif (inclusion[i] == false)
          Iflag = false
          break
        end
      end
    end
  end

  if exclusion
    Eflag = true
  end
  Xtemp = copy(X)
  return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
end

function PId_NewtonGS(X0::Vector{MCInterval{T}},P::Vector{MCInterval{T}},
                     hj!::Function,h!::Function,
                     opt::Vector{Any},Eflag::Bool,
                     Iflag::Bool,eDflag::Bool) where {T<:AbstractFloat}
  # unpacks option file
  kmax::Int64 = opt[1]
  etol::Float64 = opt[2]
  rtol::Float64 = opt[3]
  # Initializes variables
  nx::Int64 = length(X0)
  S1::MCInterval{T} = zero(MCInterval{T})
  S2::MCInterval{T} = zero(MCInterval{T})
  S3::MCInterval{T} = zero(MCInterval{T})
  inclusion::Vector{Bool} = [false for i=1:nx]
  inclusionHigh::Vector{Bool} = [false for i=1:nx]
  inclusionLow::Vector{Bool} = [false for i=1:nx]
  eD::Int64 = 0
  exclusion::Bool = false
  Iflag::Bool = false
  Eflag::Bool = false
  eDflag::Bool = false
  k::Int64 = 1
  X::Vector{MCInterval{T}} = copy(X0)
  Xi::Vector{MCInterval{T}} = copy(X0)
  N::Vector{MCInterval{T}} = copy(X)
  # creates storage for h(xm,P), J_x(X,P), and preconditioner Y
  H::Vector{MCInterval{T}} = [MCInterval(0.0) for i=1:nx]
  J::Array{MCInterval{T},2} = zeros(MCInterval{T},nx,nx)
  # calculates extension of h(xm,P) and J_x(X,P)
  x_mid::Vector{T} = mid.(X)
  h!(H,x_mid,P)
  hj!(J,X,P)
  Dense_Precondition!(H,J,mid.(J),nx)
  for i=1:nx
    S1 = zero(MCInterval{T})
    S2 = zero(MCInterval{T})
    for j=1:nx
      if (j<i)
        S1 += J[i,j]*(X[j]-x_mid[j])
      elseif (j>i)
        S2 += J[i,j]*(X[j]-x_mid[j])
      end
    end
    if J[i,i].lo*J[i,i].hi > zero(T)
      N[i] = x_mid[i] - (H[i]+S1+S2)/J[i,i]
    else
      Ntemp = copy(N)
      eD,N[i],Ntemp[i] = extProcess(N[i],X[i],J[i,i],S1,S2,H[i],rtol)
      if eD == 1
        eDflag = true
        Xtemp = copy(X)
        Xtemp[i] = Ntemp[i] ∩ X[i]
        X[i] = N[i] ∩ X[i]
        return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
      end
    end
    if Strict_XinY(N[i],X[i])
      inclusion[i] = true
      inclusionHigh[i] = true
      inclusionLow[i] = true
    else
      inclusion[i] = false
      inclusionLow[i] = false
      inclusionHigh[i] = false
      if (N[i].lo>X[i].lo)
        inclusionLow[i] = true
      elseif (N[i].hi<X[i].hi)
        inclusionHigh[i] = true
      end
    end
    if ~isdisjoint(N[i],X[i])
      X[i] = N[i] ∩ X[i]
    else
      exclusion = true
      break
    end
  end
  k += 1
  # checks if all components are included
  if (Iflag == false)
    for i=1:nx
      if (inclusion[i] == true)
        Iflag = true
        continue
      elseif (inclusion[i] == false)
        Iflag = false
        break
      end
    end
  end
  #
  while ((k<kmax) && isEqual(X,Xi,etol) == false)

    x_mid = mid.(X)
    h!(H,x_mid,P)
    hj!(J,X,P)
    Dense_Precondition!(H,J,mid.(J),nx)
    for i=1:nx
      S1 = zero(MCInterval{T})
      S2 = zero(MCInterval{T})
      for j=1:nx
        if (j<i)
          S1 += J[i,j]*(X[j]-x_mid[j])
        elseif (j>i)
          S2 += J[i,j]*(X[j]-x_mid[j])
        end
      end
      if J[i,i].lo*J[i,i].hi > zero(T)
        N[i] = x_mid[i] - (H[i]+S1+S2)/J[i,i]
      else
        Ntemp = copy(N)
        eD,N[i],Ntemp[i] = extProcess(N[i],X[i],J[i,i],S1,S2,H[i],rtol)
        if eD == 1
          eDflag = true
          Xtemp = copy(X)
          Xtemp[i] = Ntemp[i] ∩ X[i]
          X[i] = N[i] ∩ X[i]
          return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
        end
      end
      if Strict_XinY(N[i],X[i])
        inclusion[i] = true
        inclusionHigh[i] = true
        inclusionLow[i] = true
      else
        inclusion[i] = false
        inclusionLow[i] = false
        inclusionHigh[i] = false
        if (N[i].lo>X[i].lo)
          inclusionLow[i] = true
        elseif (N[i].hi<X[i].hi)
          inclusionHigh[i] = true
        end
      end
      if ~isdisjoint(N[i],X[i])
        X[i] = N[i] ∩ X[i]
      else
        exclusion = true
        break
      end
    end
    k += 1

    # checks if all components are included
    if (Iflag == false)
      for i=1:nx
        if (inclusion[i] == true)
          Iflag = true
          continue
        elseif (inclusion[i] == false)
          Iflag = false
          break
        end
      end
    end
  end

  if exclusion
    Eflag = true
  end
  Xtemp = copy(X)
  return X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh
end

"""
    PId_KrawczykCW

Uses the interval arithmetic presented in ValidatedNumerics to bound unique
implicit functions x:P->X defined by a system of equations h(z,p)=0 via
a component-wise Krawczyk method. Inputs:
* `X0::Vector{Interval{T}}`: Bounds for dependent variables
* `P::Vector{Interval{T}}`: Bounds for independent variables
* `hj!::Function`: Jacobian of h(x,p) with respect to x (takes input J,x,p)
* `h!::Function`: Equations defining potential implicit function (takes inputs H,x,p)
* `opt::Vector{Any}`: `opt[1]::Int64` is the number of iterations, `opt[2]::Float64`
                       is the equality tolerance for intervals, `opt[3]::Float64`
                       is the amount added to M[i,i] when processing extended
                       interval division.
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
Returns a tuple `(X,Xtemp,Eflag,Iflag,eDflag,inclusionLow,inclusionHigh)` where
* `X::Vector{Interval{T}}`: Output interval box
* `Xtemp::Vector{Interval{T}}`: Extra box formed from extended division
* `Eflag::Bool`: True if no implicit function exists in box
* `Iflag::Bool`: True if unique implicit function exists in box
* `inclusionLow::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from lower bound
* `inclusionHigh::Vector{Bool}`: Each component is true if corresponding variable
                                contracted from upper bound
"""
function PId_KrawczykCW(X0::Vector{Interval{T}},P::Vector{Interval{T}},
                     hj!::Function,h!::Function,
                     opt::Vector{Any},Eflag::Bool,Iflag::Bool) where {T<:AbstractFloat}

  # unpacks option file
  kmax::Int64 = opt[1]
  etol::Float64 = opt[2]

  # Initializes variables
  nx::Int64 = length(X0)
  S::Vector{Interval{T}} = [Interval(0.0) for i=1:nx]
  inclusion::Vector{Bool} = [false for i=1:nx]
  inclusionHigh::Vector{Bool} = [false for i=1:nx]
  inclusionLow::Vector{Bool} = [false for i=1:nx]
  exclusion::Bool = false
  Iflag::Bool = false
  Eflag::Bool = false
  eDflag::Bool = false
  X::Vector{Interval{T}} = copy(X0)
  Xi::Vector{Interval{T}} = copy(X)
  N::Vector{Interval{T}} = [Interval(0.0) for i=1:nx]
  x_mid::Vector{T} = mid.(X)
  k::Int64 = 1
  # creates storage for h(xm,P), J_x(X,P), and preconditioner Y
  H::Vector{Interval{T}} = [Interval(0.0) for i=1:nx]
  J::Array{Interval{T},2} = zeros(Interval{T},nx,nx)

  # calculates extension of h(xm,P) and J_x(X,P)
  h!(H,x_mid,P)
  hj!(J,X,P)
  Dense_Precondition!(H,J,mid.(J),nx)

  for i=1:nx
    S1 = Interval(0.0)
    S2 = Interval(0.0)
    S3 = Interval(0.0)
    for j=1:nx
      if (j<i)
        S1 += -J[i,j]*(X[j]-x_mid[j])
      elseif (j>i)
        S2 += -J[i,j]*(X[j]-x_mid[j])
      else
        S3 += (1.0-J[i,j])*(X[j]-x_mid[j])
      end
    end
    N[i] = x_mid[i] - H[i] + S1 + S2 +S3

    if Strict_XinY(N[i],X[i])
      inclusion[i] = true
      inclusionHigh[i] = true
      inclusionLow[i] = true
    else
      inclusion[i] = false
      inclusionLow[i] = false
      inclusionHigh[i] = false
      if (N[i].lo>X[i].lo)
        inclusionLow[i] = true
      elseif (N[i].hi<X[i].hi)
        inclusionHigh[i] = true
      end
    end
    if ~isdisjoint(N[i],X[i])
      X[i] = N[i] ∩ X[i]
    else
      exclusion = true
      break
    end
  end
  k += 1

  if (Iflag == false)
    for i=1:nx
      if (inclusion[i] == true)
        Iflag = true
        continue
      elseif (inclusion[i] == false)
        Iflag = false
        break
      end
    end
  end
  #
  while ((k<kmax) && (isEqual(X,Xi,etol) == false))
    Xi = X0
    x_mid = mid.(X)
    h!(H,x_mid,P)
    hj!(J,X,P)
    Dense_Precondition!(H,J,mid.(J),nx)

    for i=1:nx
      S1 = Interval(0.0)
      S2 = Interval(0.0)
      S3 = Interval(0.0)
      for j=1:nx
        if (j<i)
          S1 += -J[i,j]*(X[j]-x_mid[j])
        elseif (j>i)
          S2 += -J[i,j]*(X[j]-x_mid[j])
        else
          S3 += (one(T)-J[i,j])*(X[j]-x_mid[j])
        end
      end
      N[i] = x_mid[i] - H[i] + S1 + S2 +S3

      if Strict_XinY(N[i],X[i])
        inclusion[i] = true
        inclusionHigh[i] = true
        inclusionLow[i] = true
      else
        inclusion[i] = false
        inclusionLow[i] = false
        inclusionHigh[i] = false
        if (N[i].lo>X[i].lo)
          inclusionLow[i] = true
        elseif (N[i].hi<X[i].hi)
          inclusionHigh[i] = true
        end
      end
      if ~isdisjoint(N[i],X[i])
        X[i] = N[i] ∩ X[i]
      else
        exclusion = true
        break
      end
    end
    k += 1

    # checks if all components are included
    if (Iflag == false)
      for i=1:nx
        if (inclusion[i] == true)
          Iflag = true
          continue
        elseif (inclusion[i] == false)
          Iflag = false
          break
        end
      end
    end
  end

  if exclusion
    Eflag = true
  end
  return X,Eflag,Iflag,inclusionLow,inclusionHigh
end

function PId_KrawczykCW(X0::Vector{MCInterval{T}},P::Vector{MCInterval{T}},
                     hj!::Function,h!::Function,
                     opt::Vector{Any},Eflag::Bool,Iflag::Bool) where {T<:AbstractFloat}

  # unpacks option file
  kmax::Int64 = opt[1]
  etol::Float64 = opt[2]

  # Initializes variables
  nx::Int64 = length(X0)
  S::Vector{MCInterval{T}} = [MCInterval(0.0) for i=1:nx]
  inclusion::Vector{Bool} = [false for i=1:nx]
  inclusionHigh::Vector{Bool} = [false for i=1:nx]
  inclusionLow::Vector{Bool} = [false for i=1:nx]
  exclusion::Bool = false
  Iflag::Bool = false
  Eflag::Bool = false
  eDflag::Bool = false
  X::Vector{MCInterval{T}} = copy(X0)
  Xi::Vector{MCInterval{T}} = copy(X)
  N::Vector{MCInterval{T}} = [MCInterval(0.0) for i=1:nx]
  x_mid::Vector{T} = mid.(X)
  k::Int64 = 1
  # creates storage for h(xm,P), J_x(X,P), and preconditioner Y
  H::Vector{MCInterval{T}} = [MCInterval(0.0) for i=1:nx]
  J::Array{MCInterval{T},2} = zeros(MCInterval{T},nx,nx)

  # calculates extension of h(xm,P) and J_x(X,P)
  h!(H,x_mid,P)
  hj!(J,X,P)
  Dense_Precondition!(H,J,mid.(J),nx)

  for i=1:nx
    S1 = zero(MCInterval{T})
    S2 = zero(MCInterval{T})
    S3 = zero(MCInterval{T})
    for j=1:nx
      if (j<i)
        S1 += -J[i,j]*(X[j]-x_mid[j])
      elseif (j>i)
        S2 += -J[i,j]*(X[j]-x_mid[j])
      else
        S3 += (one(T)-J[i,j])*(X[j]-x_mid[j])
      end
    end
    N[i] = x_mid[i] - H[i] + S1 + S2 +S3

    if Strict_XinY(N[i],X[i])
      inclusion[i] = true
      inclusionHigh[i] = true
      inclusionLow[i] = true
    else
      inclusion[i] = false
      inclusionLow[i] = false
      inclusionHigh[i] = false
      if (N[i].lo>X[i].lo)
        inclusionLow[i] = true
      elseif (N[i].hi<X[i].hi)
        inclusionHigh[i] = true
      end
    end
    if ~isdisjoint(N[i],X[i])
      X[i] = N[i] ∩ X[i]
    else
      exclusion = true
      break
    end
  end
  k += 1

  if (Iflag == false)
    for i=1:nx
      if (inclusion[i] == true)
        Iflag = true
        continue
      elseif (inclusion[i] == false)
        Iflag = false
        break
      end
    end
  end
  #
  while ((k<kmax) && (isEqual(X,Xi,etol) == false))
    Xi = X0
    x_mid = mid.(X)
    h!(H,x_mid,P)
    hj!(J,X,P)
    Dense_Precondition!(H,J,mid.(J),nx)

    for i=1:nx
      S1 = zero(MCInterval{T})
      S2 = zero(MCInterval{T})
      S3 = zero(MCInterval{T})
      for j=1:nx
        if (j<i)
          S1 += -J[i,j]*(X[j]-x_mid[j])
        elseif (j>i)
          S2 += -J[i,j]*(X[j]-x_mid[j])
        else
          S3 += (one(T)-J[i,j])*(X[j]-x_mid[j])
        end
      end
      N[i] = x_mid[i] - H[i] + S1 + S2 +S3

      if Strict_XinY(N[i],X[i])
        inclusion[i] = true
        inclusionHigh[i] = true
        inclusionLow[i] = true
      else
        inclusion[i] = false
        inclusionLow[i] = false
        inclusionHigh[i] = false
        if (N[i].lo>X[i].lo)
          inclusionLow[i] = true
        elseif (N[i].hi<X[i].hi)
          inclusionHigh[i] = true
        end
      end
      if ~isdisjoint(N[i],X[i])
        X[i] = N[i] ∩ X[i]
      else
        exclusion = true
        break
      end
    end
    k += 1

    # checks if all components are included
    if (Iflag == false)
      for i=1:nx
        if (inclusion[i] == true)
          Iflag = true
          continue
        elseif (inclusion[i] == false)
          Iflag = false
          break
        end
      end
    end
  end

  if exclusion
    Eflag = true
  end
  return X,Eflag,Iflag,inclusionLow,inclusionHigh
end


CONIC_OPTIMIZERS = [SCS.Optimizer, ProxSDP.Optimizer, COSMO.Optimizer]

@testset "MOI to MatOI conversion 1" begin
    # _psd1test: https://github.com/jump-dev/MathOptInterface.jl/blob/master/src/Test/contconic.jl#L2417

    for optimizer in CONIC_OPTIMIZERS
        model = MOI.instantiate(optimizer, with_bridge_type=Float64)
        δ = √(1 + (3*√2+2)*√(-116*√2+166) / 14) / 2
        ε = √((1 - 2*(√2-1)*δ^2) / (2-√2))
        y2 = 1 - ε*δ
        y1 = 1 - √2*y2
        obj = y1 + y2/2
        k = -2*δ/ε
        x2 = ((3-2obj)*(2+k^2)-4) / (4*(2+k^2)-4*√2)
        α = √(3-2obj-4x2)/2
        β = k*α

        X = MOI.add_variables(model, 6)
        x = MOI.add_variables(model, 3)

        vov = MOI.VectorOfVariables(X)

        cX = MOI.add_constraint(
            model,
            MOI.VectorAffineFunction{Float64}(vov), MOI.PositiveSemidefiniteConeTriangle(3)
        )

        cx = MOI.add_constraint(
            model,
            MOI.VectorAffineFunction{Float64}(MOI.VectorOfVariables(x)), MOI.SecondOrderCone(3)
        )

        c1 = MOI.add_constraint(
            model,
            MOI.VectorAffineFunction(
                MOI.VectorAffineTerm.(1:1, MOI.ScalarAffineTerm.([1., 1., 1., 1.], [X[1], X[3], X[end], x[1]])),
                [-1.0]
            ),
            MOI.Zeros(1)
        )

        c2 = MOI.add_constraint(
            model,
            MOI.VectorAffineFunction(
                MOI.VectorAffineTerm.(1:1, MOI.ScalarAffineTerm.([1., 2, 1, 2, 2, 1, 1, 1], [X; x[2]; x[3]])),
                [-0.5]
            ),
            MOI.Zeros(1)
        )

        objXidx = [1:3; 5:6]
        objXcoefs = 2*ones(5)
        MOI.set(model, MOI.ObjectiveFunction{MOI.ScalarAffineFunction{Float64}}(),
        MOI.ScalarAffineFunction(MOI.ScalarAffineTerm.([objXcoefs; 1.0], [X[objXidx]; x[1]]), 0.0))
        MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE)

        MatModel = MatOI.get_conic_form(Float64, model, [cX; cx; c1; c2])

        @test MatModel.c' ≈ [2. 2. 2. 0. 2. 2. 1. 0. 0.]
        @test MatModel.b' ≈ [0.   0.   0.   0.   0.   0.   0.   0.   0.  -1.  -0.5 ]
        @test MatModel.A.nzval ≈ [-1.0, -1.0, -1.0, -1.0, -2.0, -1.0, -1.0, -1.0, -1.0, -2.0, -1.0, -2.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0] atol=ATOL rtol=RTOL
    end
end

@testset "MOI to MatOI conversion 2" begin
    # find equivalent diffcp program here - https://github.com/AKS1996/jump-gsoc-2020/blob/master/diffcp_sdp_3_py.ipynb

    for optimizer in CONIC_OPTIMIZERS
        model = MOI.instantiate(optimizer, with_bridge_type=Float64)

        x = MOI.add_variables(model, 7)
        @test MOI.get(model, MOI.NumberOfVariables()) == 7

        η = 10.0

        c1  = MOI.add_constraint(
            model,
            MOI.VectorAffineFunction(
                MOI.VectorAffineTerm.(1, MOI.ScalarAffineTerm.(-1.0, x[1:6])),
                [η]
            ),
            MOI.Nonnegatives(1)
        )
        c2 = MOI.add_constraint(model, MOI.VectorAffineFunction(MOI.VectorAffineTerm.(1:6, MOI.ScalarAffineTerm.(1.0, x[1:6])), zeros(6)), MOI.Nonnegatives(6))
        α = 0.8
        δ = 0.9
        c3 = MOI.add_constraint(model, MOI.VectorAffineFunction(MOI.VectorAffineTerm.([fill(1, 7); fill(2, 5);     fill(3, 6)],
                                                                MOI.ScalarAffineTerm.(
                                                                [ δ/2,       α,   δ, δ/4, δ/8,      0.0, -1.0,
                                                                    -δ/(2*√2), -δ/4, 0,     -δ/(8*√2), 0.0,
                                                                    δ/2,     δ-α,   0,      δ/8,      δ/4, -1.0],
                                                                [x[1:7];     x[1:3]; x[5:6]; x[1:3]; x[5:7]])),
                                                                zeros(3)), MOI.PositiveSemidefiniteConeTriangle(2))
        c4 = MOI.add_constraint(
            model,
            MOI.VectorAffineFunction(
                MOI.VectorAffineTerm.(1, MOI.ScalarAffineTerm.(0.0, [x[1:3]; x[5:6]])),
                [0.0]
            ),
            MOI.Zeros(1)
        )

        MOI.set(model, MOI.ObjectiveFunction{MOI.ScalarAffineFunction{Float64}}(), MOI.ScalarAffineFunction([MOI.ScalarAffineTerm(1.0, x[7])], 0.0))
        MOI.set(model, MOI.ObjectiveSense(), MOI.MAX_SENSE)

        MatModel = MatOI.get_conic_form(Float64, model, [c1; c2; c3; c4])

        @test MatModel.c ≈ [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -1.0]
        @test MatModel.b ≈ [10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        @test MatModel.A.nzval ≈ [1.0, -1.0, -0.45, 0.318198, -0.45, 1.0, -1.0, -0.8, 0.225, -0.1, 1.0, -1.0, -0.9, 1.0, -1.0, -0.225, 1.0, -1.0, -0.1125, 0.0795495, -0.1125, 1.0, -1.0, -0.225, 1.0, 1.0] atol=ATOL rtol=RTOL
    end
end

@testset "Testing minor utilities" begin
    # testing offsets
    n = rand(Int) % 100
    @test MatOI.cons_offset(MOI.Zeros(n)) == n
    @test MatOI.cons_offset(MOI.Nonnegatives(n)) == n
    @test MatOI.cons_offset(MOI.SecondOrderCone(n)) == n
    @test MatOI.cons_offset(MOI.PositiveSemidefiniteConeTriangle(n)) == ((n+1)*n)/2

    model = MOI.instantiate(SCS.Optimizer, with_bridge_type=Float64)
    cf = MatOI.get_conic_form(Float64,model,[])
    @test MOI.is_empty(cf) == false
    MOI.empty!(cf)
    @test MOI.is_empty(cf) == true
end


using ClusterTrees

"""
    Sort objects to lie on a space filling curve
"""
function sort_sfc(points)

    ct, sz = CompScienceMeshes.boundingbox(points)
    tree = ClusterTrees.LevelledTrees.LevelledTree(ct, sz, Int[])

    smb = sz / 2^log(length(points)+1)
    for (i,pt) = enumerate(points)
        dest = (smallest_box_size=smb, target_point=pt)
        state = ClusterTrees.LevelledTrees.rootstate(tree, dest)
        ClusterTrees.update!(tree, state, i, dest) do tree, node, i
            push!(data(tree, node).values, i)
        end
    end

    sorted = Vector{Int}()
    for node in ClusterTrees.DepthFirstIterator(tree, root(tree))
        append!(sorted, data(tree,node).values)
    end

    return sorted
end


#Demo 1: Transmission line test
#-------------------------------------------------------------------------------

using CMDimCircuits
CMDimCircuits.@using_CData()

#Get a demo display:
include(CMDimCircuits.demoplotcfgscript)
#Normally use something like:
#CMDimData.@includepkg EasyPlotInspect

#==Constants
===============================================================================#
LBLAX_MAGDB = "Magnitude (dB)"
LBLAX_FREQ = "Frequency (Hz)"
color1 = cons(:a, line = set(color=:red, width=2))
color2 = cons(:a, line = set(color=:blue, width=2))

#==Input data
===============================================================================#
z0 = 50 #Ohms (reference impedance)
ℓ = 5e-3 #Meters
zC = 200 #Ohms (line impedance)
μ0 = 4pi*1e-7 #F/m
ϵ0 = 8.854e-12 #H/m
f = collect(1:.1:100)*1e9

#==Computations
===============================================================================#
ω = 2pi*f
α = 0
β = ω*sqrt(μ0*ϵ0)
γ = α .+ im*β

#Convert γ type to DataF1, function of 1 argument (leverage C-Data toolkit):
γ = DataF1(f, γ)

#ABCD matrix:
A = cosh(γ*ℓ)      ;B = sinh(γ*ℓ)*zC;
C = sinh(γ*ℓ)/zC   ;D = cosh(γ*ℓ);

T = Network(:ABCD, [A B; C D])
S = Network(:S, T, z0=z0)
(s11, s12, s21, s22) = mx2elem(S)

#==Generate plot
===============================================================================#
plot = cons(:plot, nstrips=2,
	ystrip1 = set(axislabel=LBLAX_MAGDB, striplabel="Reflection Coefficients (𝛤)"),
	ystrip2 = set(axislabel=LBLAX_MAGDB, striplabel="Transmission Coefficients (𝛵)"),
	xaxis = set(label=LBLAX_FREQ),
)
push!(plot,
	cons(:wfrm, dB20(s11), color1, label="s11", strip=1),
	cons(:wfrm, dB20(s22), color2, label="s22", strip=1),
	cons(:wfrm, dB20(s12), color1, label="s12", strip=2),
	cons(:wfrm, dB20(s21), color2, label="s21", strip=2),
)

pcoll = push!(cons(:plotcoll, title="Transmission Line Test"), plot)
	pcoll.ncolumns = 1

#==Display results in pcoll
===============================================================================#
EasyPlot.displaygui(pcoll)

#==Return pcoll to user (call evalfile(...))
===============================================================================#
pcoll


using ApproxFun, SingularIntegralEquations, Test
import Base.MathConstants: γ, catalan

@testset "Convolution ProductFun" begin
    @testset "Chebyshev addition" begin
        d = Segment(-1.,1.)
        fK(x,y) = exp(abs2(y-x))
        FLR = LowRankFun(fK,Chebyshev(d),Chebyshev(d))
        FPF = ProductFun(fK,Chebyshev(d),Chebyshev(d))
        G = convolutionProductFun(fK,Chebyshev(d),Chebyshev(d))
        G1 = convolutionProductFun(fK,Ultraspherical(1,d),Chebyshev(d))
        G2 = convolutionProductFun(fK,Ultraspherical(1,d),ChebyshevDirichlet{1,1}(d))

        @test norm(pad(coefficients(FLR),size(coefficients(G),1),size(coefficients(G),2))-coefficients(G))<100000eps()
        @test norm(pad(coefficients(FPF),size(coefficients(G),1),size(coefficients(G),2))-coefficients(G))<100000eps()
        @test norm(fK(.123,.456)-G(.123,.456))≤10000eps()
        @test norm(G(.123,.456)-G1(.123,.456))≤10000eps()
        @test norm(G(.123,.456)-G2(.123,.456))≤10000eps()
    end

    @testset "Fourier on PeriodicSegment" begin
        f2 = Fun(θ->exp(sin(θ))+sin(cos(θ)),Fourier())
        FLR = LowRankFun((θ,ϕ)->f2(ϕ-θ),Fourier(),Fourier())
        FPF = ProductFun((θ,ϕ)->f2(ϕ-θ),Fourier(),Fourier())
        G = convolutionProductFun(f2,Fourier(),Fourier())

        @test norm(pad(coefficients(FLR),size(coefficients(G),1),size(coefficients(G),2))-coefficients(G))<400eps()
        @test norm(pad(coefficients(FPF),size(coefficients(G),1),size(coefficients(G),2))-coefficients(G))<400eps()
        @test norm(f2(.456-.123)-G(.123,.456))≤400eps()

        f2 = Fun(θ->1+sin(cos(θ)),CosSpace())
        G = convolutionProductFun(f2,Fourier(),Fourier())
        @test norm(f2(.456-.123)-G(.123,.456))≤400eps()

        f2 = Fun(θ->sin(sin(θ)),SinSpace())
        G = convolutionProductFun(f2,Fourier(),Fourier())
        @test norm(f2(.456-.123)-G(.123,.456))≤400eps()
    end

    @testset "Laurent on PeriodicSegment" begin
        f2 = Fun(θ->exp(sin(θ))+sin(cos(θ)),Laurent())
        G = convolutionProductFun(f2,Laurent(),Laurent())
        @test norm(f2(.456-.123)-G(.123,.456))≤400eps()

        f2 = Fun(θ->π+ℯ*exp(im*θ)+sqrt(2)*exp(im*2θ)+catalan*exp(im*3θ)+γ*exp(im*4θ),Taylor(PeriodicSegment()))
        G = convolutionProductFun(f2,Laurent(),Laurent())
        @test norm(f2(.456-.123)-G(.123,.456))≤100eps()

        f2 = Fun(θ->ℯ*exp(-im*θ)+sqrt(2)*exp(-im*2θ)+catalan*exp(-im*3θ)+γ*exp(-im*4θ),Hardy{false}(PeriodicSegment()))
        G = convolutionProductFun(f2,Laurent(),Laurent())
        @test norm(f2(.456-.123)-G(.123,.456))≤100eps()
    end
    @testset "Timing" begin
        d = Segment(-2.5,-.5)
        fK2(x,y) = besselj0(100(y-x))
        convolutionProductFun(fK2,Chebyshev(d),Chebyshev(d))
        @time G = convolutionProductFun(fK2,Chebyshev(d),Chebyshev(d))
        convolutionProductFun(fK2,Ultraspherical(1,d),ChebyshevDirichlet{1,1}(d))
        @time G = convolutionProductFun(fK2,Ultraspherical(1,d),ChebyshevDirichlet{1,1}(d))
        println("Chebyshev addition: Time should be ~0.01 seconds.")

        f2 = Fun(θ->besselj0(500*abs(2sin(θ/2))),CosSpace())
        convolutionProductFun(f2,Fourier(),Fourier())
        @time G = convolutionProductFun(f2,Fourier(),Fourier())
        println("CosSpace addition: Time should be ~0.05 seconds.")

        f2 = Fun(θ->besselj0(500*abs(2sin(θ/2))),Laurent())
        convolutionProductFun(f2,Laurent(),Laurent())
        @time G = convolutionProductFun(f2,Laurent(),Laurent())
        println("Laurent addition: Time should be ~0.14 seconds.")
    end
end


function mass(u, q, w, v, x, t, mu, eta)
    m = 1.0;
    return m;
end
function flux(u, q, w, v, x, t, mu, eta)
    f = mu[1]*q;
    return f;
end
function source(u, q, w, v, x, t, mu, eta)
    s = 0.0;
    return s;
end
function ubou(u, q, w, v, x, t, mu, eta, uhat, n, tau)
    ub = [u u];
    return ub;
end
function fbou(u, q, w, v, x, t, mu, eta, uhat, n, tau)
    #ui = sin(mu[2]*x[1]+mu[3]*x[2]-mu[4]*sqrt(mu[2]*mu[2]+mu[3]*mu[3])*t);
    uix = mu[2]*cos(mu[2]*x[1]+mu[3]*x[2]-mu[4]*sqrt(mu[2]*mu[2]+mu[3]*mu[3])*t);
    uiy = mu[3]*cos(mu[2]*x[1]+mu[3]*x[2]-mu[4]*sqrt(mu[2]*mu[2]+mu[3]*mu[3])*t);
    fb = [u uix*n[1]+uiy*n[2]]; # Neumann conditions
    return fb;
end
function initu(x, mu, eta)
    u0 = 0.0;
    return u0;
end
function initq(x, mu, eta)
    q0 = [0.0 0.0];
end
function initw(x, mu, eta)
    w0 = 0.0;
    return w0;
end


using Soss

m = @model begin
    x ~ Normal() |> iid(2)
    xxt = x * x'
end;

s = rand(m(), 1000)


export discretize,timedirichlet

include("KroneckerOperator.jl")

## PDE

lap(d::Space) = Laplacian(d)
lap(d::Domain) = Laplacian(d)
lap(f::Fun) = Laplacian()*f

function Laplacian(d::BivariateSpace,k::Integer)
    Dx2=Derivative(d,[2,0])
    Dy2=Derivative(d,[0,2])
    if k==1
        LaplacianWrapper(Dx2+Dy2,k)
    else
        @assert k > 0
        Δ=Laplacian(d,1)
        LaplacianWrapper(TimesOperator(Laplacian(rangespace(Δ),k-1),Δ),k)
    end
end

Laplacian(d::Domain2d, k::Integer) = Laplacian(Space(d),k)
grad(d::ProductDomain) = [Derivative(d,[1,0]),Derivative(d,[0,1])]

function tensor_Dirichlet(d::Union{ProductDomain,TensorSpace},k)
    @assert nfactors(d)==2

    DirichletWrapper(
        if isempty(∂(factor(d,1)))
            I ⊗ Dirichlet(factor(d,2),k)
        elseif isempty(∂(factor(d,2)))
            Dirichlet(factor(d,1),k) ⊗ I
        else
            [Dirichlet(factor(d,1),k) ⊗ I;I ⊗ Dirichlet(factor(d,2),k)]
        end, k)
end

Dirichlet(d::Union{ProductDomain,TensorSpace},k) = tensor_Dirichlet(d,k)

function timedirichlet(d::Union{ProductDomain,TensorSpace})
    @assert nfactors(d)==2
    Bx=Dirichlet(factor(d,1))
    Bt=ldirichlet(factor(d,2))
    [I⊗Bt;Bx⊗I]
end

function *(B::Operator,f::ProductFun)
    if isafunctional(B)
        Fun(factor(space(f),2),map(c->Number(B*c),f.coefficients))
    else
        ProductFun(space(f),map(c->B*c,f.coefficients))
    end
end

*(f::ProductFun,B::Operator) = transpose(B*(transpose(f)))


@testset "nearest_rotation" begin
    @testset "$(N)-dim" for N in [2,3,4]
        for _ in 1:100
            M = randn(N,N)
            R = nearest_rotation(M)
            V = M/R
            U = R\M

            # R is rotation matrix
            @test isrotation(R)
            # U, V are (approximately) symmetric matrices
            # See [polar decomposition](https://en.wikipedia.org/wiki/Polar_decomposition)
            @test U ≈ U'
            @test V ≈ V'
            # Eigen values are non-negative, except for the first value.
            # We just need sort for Julia 1.0
            @test all(sort(eigvals(U))[2:end] .≥ 0)
            @test all(sort(eigvals(V))[2:end] .≥ 0)
        end

        for _ in 1:100
            M = @SMatrix randn(N,N)
            R = nearest_rotation(M)
            V = M/R
            U = R\M

            @test isrotation(R)
            @test U ≈ U'
            @test V ≈ V'
            # Can't calculate eigen values of general SMatrix.
            @test all(sort(eigvals(Symmetric(U)))[2:end] .≥ 0)
            @test all(sort(eigvals(Symmetric(V)))[2:end] .≥ 0)
        end
    end
end


using SamplingHS3

# inverted egg crate function
f_ec = (x::Vector{Float64}) -> ( sin(x[1])^2 + sin(x[2])^2 ) / ( 0.1 + x[1]*x[1] + x[2]*x[2] )^(1/2.5)

# polytope
xlim = [-20.,20.]; ylim = [-20.,20.]
bounding_box_G =[eye(2);-eye(2)]
bounding_box_h =[-xlim[1];xlim[2];-ylim[1];ylim[2]]

x0 = [5.0;5.0]

# pure mcmc sampling using hr and mh:
conf_only_hr = SamplingHS3.DecorrelationConfig( 10000 , 1.0*eye(2) , 0.5 , 0.5 )
f_density    = f_ec
# this is quite slow.. most time is spent for the generation of the multivar. normal random numbers..
tic()
mcmc_result_01 = SamplingHS3.run_mcmc( x0 , f_density(x0) , f_density , (x) -> 0 , 1.0 , bounding_box_G  , bounding_box_h , conf_only_hr )
toc()

# run with adaptive mcmc
tic()
config_mcmc = SamplingHS3.MCMCConfig()
mcmc_result_adaptive_01 = SamplingHS3.run_adaptive_mcmc( 10000 , x0 , f_density(x0) , f_density , (x) -> 0 , 1.0 , bounding_box_G  , bounding_box_h , config_mcmc )
toc()

# Plot results


#include("../examples/grad.jl")
include("../examples/gravity.jl")
include("../examples/LQG.jl")
include("../examples/tensors.jl")


include("../src/RandomField.jl")
using .RandomField

# choose parameters:
## remember, we need that 4|n_i for i=1,...,d
N = [400,400]		# number pixels per dimension
L = [20.0,80.0]		# real space box is [-l,l]^d
f(V::Vector{T} where T<:Real) = gaus_rf(V)
					# spectral density
n_real = 100		# number of realizations

# derived parameters
d = length(N) 		# dimension
H = (pi/2) .* N ./L
var_y(V::Vector{T} where T<:Real) = 2*(2pi)^(d/2)*prod(L)/pi^d*f(V)

# allocate memory
V = Cloud(d,N)
X = Cloud(d,N)
Y = Cscalar(V)
Z = Cscalar(X)
C_est = Cscalar(X)	# estimated stationary covariance
C_tru = Cscalar(X)	# true stationary covariance

# initialize grids
init!(X,L)
init!(V,H)

# estimate stationary covariance
## (we assume here that the mean is 0 to save time)
@. C_est.values = 0
i_o = get_originindex(X)
for i=1:n_real
	draw_realization!(Y,var_y)
	ift!(Z,Y)	# Y ↦ Z
	@. C_est.values += 0.5 * Z.values * conj(Z.values[i_o])
end
@. C_est.values /= n_real

sample!(C_tru,gaus_rf)

# print:
tovtk(C_est, "../data/x.c.vtk")
#tocsv(slice(Z,1), "../data/x.z1.csv")
tocsv(slice(C_est,1), "../data/x.c1.csv")
tocsv(slice(C_tru,1), "../data/x.ctru1.csv")


#!/usr/bin/env julia

using Colors
using Compose

function golden_rect(n::Int)
    if n == 0 return context() end
    c = compose(context(), rectangle(), fill(LCHab(90, 80, 70 - 15n)))
    compose(c, context(units=UnitBox(0, -1/φ, 1h, 1/φ), rotation=Rotation(π/2, 0, 1)),  golden_rect(n - 1))
end

draw(SVG("golden_rect.svg", φ * 3inch, 3inch),
    compose(golden_rect(10), fill(nothing), stroke(colorant"white"), linewidth(0.2mm)))


KILOGRAM_CHART = Dict{String,Float64}(
    "kilogram" => 1,
    "gram" => 10^3,
    "milligram" => 10^6,
    "metric-ton" => 10^-3,
    "long-ton" => 0.0009842073,
    "short-ton" => 0.0011023122,
    "pound" => 2.2046244202,
    "ounce" => 35.273990723,
    "carrat" => 5000,
    "atomic-mass-unit" => 6.022136652e26,
)

WEIGHT_TYPE_CHART = Dict{String,Float64}(
    "kilogram" => 1,
    "gram" => 10^-3,
    "milligram" => 10^-6,
    "metric-ton" => 10^3,
    "long-ton" => 1016.04608,
    "short-ton" => 907.184,
    "pound" => 0.453592,
    "ounce" => 0.0283495,
    "carrat" => 0.0002,
    "atomic-mass-unit" => 1.660540199e-27,
)

function weight_conversion(from_type, to_type, value)
    if !haskey(KILOGRAM_CHART, to_type) || !haskey(WEIGHT_TYPE_CHART, from_type)
        throw(
            error(
                "Invalid 'from_type' or 'to_type' value: $(from_type), $(to_type)\n",
                "Supported values are: $(WEIGHT_TYPE_CHART)",
            ),
        )
    end
    return value * KILOGRAM_CHART[to_type] * WEIGHT_TYPE_CHART[from_type]
end


function HDA(g, threshold=sqrt(nv(g)))
	attack_nodes = high_degree_adaptive(g)
	present = fill(true, nv(g))
	present[attack_nodes] .= false
	reinsert_nodes = reverse_greedy!(present, g, threshold)
	return [i for i in attack_nodes if !present[i]]
end

function CoreHD(g, threshold=sqrt(nv(g)))
	decycling_nodes = decore(g)
	present = fill(true, nv(g))
	present[decycling_nodes] .= false
	treebreak_nodes, _ = tree_break!(present, g, threshold)
	reinsert_nodes = reverse_greedy!(present, g, threshold)
	return [i for i in [decycling_nodes;treebreak_nodes] if !present[i]]
end

function WeakNei(g, threshold=sqrt(nv(g)))
	decycling_nodes = weak_neighbor_decore(g)
	present = fill(true, nv(g))
	present[decycling_nodes] .= false
	treebreak_nodes, _ = tree_break!(present, g, threshold)
	reinsert_nodes = reverse_greedy!(present, g, threshold)
	return [i for i in [decycling_nodes;treebreak_nodes] if !present[i]]
end


using StatsBase
using Random
using Memoize

"""
    weights(state::Vector{ComplexF64})

extended StatsBase weights to allow complex values
"""
function StatsBase.weights(state::Vector{ComplexF64})
    weights(norm.(state).^2)
end

"""
    measure(state:Vector{ComplexF64}, n::Int)

sample 'n' possible quantum state after measurement.
"""
function measure(state::Vector{ComplexF64}, shots::Int=1)

    (shots > 0) || error("Shots has to be a natural integer above 0.")
    ispow2(length(state)) || error("Statevector is not a viable quantum state (length $(length(state) )")
    norm(state) ≈ 1 || error("Statevector is not a viable statevector (norm is $(norm(state))")
    N = Int(log(2, length(state)))

    # generate samples and count number of measurements of each basis state
    s = sample(Random.GLOBAL_RNG, [0:(2^N)-1 ... ], weights(state), shots)
    u=unique(s)
    d=Dict([(i,count(x->x==i,s)) for i in u])

    return d
end


#jacobian2by2.jl
# Code 8.1 of Random Eigenvalues by Alan Edelman

#Experiment:    Compute the Jacobian of a 2x2 matrix function
#Comment:       Symbolic tools are not perfect. The author
#               exercised care in choosing the variables

warn("Julia doesn't currently have a good symbolic math package")
warn("This may use features of PyCall and/or SymPy which are not publicly available yet")
using PyCall
pyinitialize("/opt/local/bin/python2")
using SymPy
jacobian(Y,X) = Sym(Y[:reshape](1,length(Y))[:jacobian](X[:reshape](1,length(X))))

p,q,r,s, a,b,c,d, t, e1,e2 = Sym(:p,:q,:r,:s, :a,:b,:c,:d, :t, :e1,:e2)
X = SymMatrix([p q; r s])
A = SymMatrix([a b; c d])

Y = X^2; J = jacobian(Y,X); JAC_square = Sym(J[:det]()[:factor]()); println("square:\n", J, "\n\n", JAC_square, "\n\n")
Y = X^3; J = jacobian(Y,X); JAC_cube = Sym(J[:det]()[:factor]()); println("cube:\n", J, "\n\n", JAC_cube, "\n\n")
Y = X[:inv](); J = jacobian(Y,X); JAC_inv = Sym(J[:det]()[:factor]()); println("inv:\n", J, "\n\n", JAC_inv, "\n\n")
Y = A*X; J = jacobian(Y,X); JAC_linear = Sym(J[:det]()[:factor]()); println("linear:\n", J, "\n\n", JAC_linear, "\n\n")
Y = SymMatrix([p q; r/p X[:det]()/p]); J = jacobian(Y,X); JAC_lu = Sym(J[:det]()[:factor]()); println("lu:\n", J, "\n\n", JAC_lu, "\n\n")

x = SymMatrix([p, s, r])
y = SymMatrix([sqrt(p), sqrt(s), r/(sqrt(p)*sqrt(s))])
J = jacobian(y,x)
JAC_DMD = Sym(J[:det]()[:factor]())
println("DMD:\n", J, "\n\n", JAC_DMD, "\n\n")

x = SymMatrix([p, s])
y = SymMatrix([sqrt(p^2 + s^2), atan(s/p)])
J = jacobian(y,x)
JAC_notrace = Sym(J[:det]()[:factor]())
println("notrace:\n", J, "\n\n", JAC_notrace, "\n\n")

Q = SymMatrix([cos(t)  -sin(t); sin(t)  cos(t)])
D = SymMatrix([e1 0; 0 e2])
Y = Q*D*Q[:transpose]()
y = SymMatrix([Y[1,1], Y[2,2], Y[1,2]])
x = SymMatrix([t, e1, e2])
J = jacobian(Y,X)
JAC_symeig = Sym(J[:det]()[:factor]())
println("symeig:\n", J, "\n\n", JAC_symeig, "\n\n")

X = SymMatrix([p s; s r])
Y = Sym(A[:transpose]())*X*A
y = SymMatrix([Y[1,1], Y[2,2], Y[1,2]])
x = SymMatrix([p, r, s])
J = jacobian(Y,X)
JAC_symcong = Sym(J[:det]()[:factor]())
println("symcong:\n", J, "\n\n", JAC_symcong, "\n\n")


using Gadfly, Distributions

function Q(x) 0.5 * erfc(x / sqrt(2)) end

# генерируем все кодовые слова
function getAllCodeWords(k, n, G)
  C = Array(Int64, 2^k, n)
  ind = 1
  for i in 1:k # перебираем все сочетания строк, чтобы получить кодовые слова
    for comb in combinations(1:k, i)
      res = Array(Int64, n)'
      fill!(res, 0)
      for c in comb
        res += G[c, :]
      end

      res = map(x -> x % 2, res)
      C[ind, :] = res

      ind += 1
    end
  end
  for i = 1:n
    C[2^k, i] = 0
  end

  return C
end

function addErrors(n, c, errProb)
  e = rand(Uniform(0, 1), n)
  for i = 1:n
    if e[i] <= errProb  # вносим ошибку согласно вероятности
      c[i] = 1
    end
  end
end

# функция декодирования по МАВ
function guessCode(y, C, errProb)
  codeNum = length(C[:, 1])
  bestProb = -1
  guessIndex = -1

  for i in 1:codeNum
    curProb = 1
    for j in 1:length(y)
      if y[j] == C[i, j]
        curProb *= (1 - errProb)
      else
        curProb *= errProb
      end
    end

    if curProb > bestProb
      bestProb = curProb
      guessIndex = i
    end
  end

  return guessIndex
end

function errorProbOnBit(errProb, n, k, m, C, matrNum)
  result = 0

  println("num: $matrNum, errProb = $errProb: start")
  c = Array(Int64, n) # кодовое слово длины n
  for i = 1:m
    fill!(c, 0) # берём нулевое кодовое слово
    addErrors(n, c, errProb) # передаём, внося ошибки
    wordNum = guessCode(c, C, errProb) # декодируем по МАВ

    oneBits = 0
    for j = 1:n # считаем число ошибочныx бит
      if C[wordNum, j] == 1
        oneBits += 1
      end
    end

    result += oneBits / k # вероятность ошибки на бит
  end
  println("done")

  return result / m # усредняем по числу экспериментов
end

# вариант 68
GK = [[1 0 0 0 0 0 1 0 1 1],
      [0 1 0 0 0 0 1 1 1 1],
      [0 0 1 0 0 0 1 1 1 0],
      [0 0 0 1 0 0 0 1 1 1],
      [0 0 0 0 1 0 1 0 0 1],
      [0 0 0 0 0 1 0 1 0 1]]

n = length(GK[1, :]) # n – число столбцов
k = length(GK[:, 1]) # k – длина блока (число строк)
m = 100000 # число экспериментов

sdb = [0:0.1:10] # SNR, dB per bit
sb = 10 .^ (sdb / 10) # SNR per bit
sig = sqrt(0.5 ./ sb)
sig1 = 1 ./ sig
PE = Q(sig1)

C = getAllCodeWords(k, n, GK)
errPerBit1 = map(pe -> errorProbOnBit(pe, n, k, m, C, 1), PE)

peplot = plot(layer(x=sdb, y=errPerBit1, Theme(default_color=colorant"blue"), Geom.line),
              Guide.xticks(ticks=[0:1:10]), Guide.title("m = 10^5"), Scale.y_log10,
              Guide.xlabel("E/N0 (dB)"), Guide.ylabel("Ошибка на бит"))

draw(PDF("m100000_log.pdf", 4inch, 3inch), peplot)


module TestInteractionATE

include("helper_fns.jl")

using Test
using TMLE
using MLJ
using Distributions
using Random
using StableRNGs
using Tables
using StatsBase

mutable struct InteractionTransformer <: Static end

function MLJ.transform(a::InteractionTransformer, _, X)
    Xmatrix = MLJ.matrix(X)
    nrows, ncols = size(Xmatrix)
    ninter = Int(ncols*(ncols-1)/2)
    Xinteracts = Matrix{Float64}(undef, nrows, ninter)
    i = 0
    for col₁ in 1:(ncols-1)
        for col₂ in (col₁+1):ncols
            i += 1
            Xinteracts[:, i] = Xmatrix[:, col₁] .* Xmatrix[:, col₂]
        end
    end
    return MLJ.table(hcat(Xmatrix, Xinteracts))
end

LogisticClassifier = @load LogisticClassifier pkg=MLJLinearModels verbosity=0
LinearRegressor = @load LinearRegressor pkg=MLJLinearModels verbosity = 0

cont_interacter = InteractionTransformer |> LinearRegressor
cat_interacter = InteractionTransformer |> LogisticClassifier
Ns = [100, 1000, 10000, 100000]

function binary_target_binary_treatment_pb(rng;n=100)
    μy_fn(W, T₁, T₂) = TMLE.expit(2W[:, 1] .+ 1W[:, 2] .- 2W[:, 3] .- T₁ .+ T₂ .+ 2*T₁ .* T₂)
    # Sampling W: Bernoulli
    W = rand(rng, Bernoulli(0.5), n, 3)

    # Sampling T₁, T₂ from W: Softmax
    θ = rand(rng, 3, 4)
    softmax = exp.(W*θ) ./ sum(exp.(W*θ), dims=2)
    T = [sample(rng, [1, 2, 3, 4], Weights(softmax[i, :])) for i in 1:n]
    T₁ = [t in (1,2) ? true : false for t in T]
    T₂ = [t in (1,3) ? true : false for t in T]

    # Sampling y from T₁, T₂, W: Logistic
    μy = μy_fn(W, T₁, T₂)
    y = [rand(rng, Bernoulli(μy[i])) for i in 1:n]

    # Respect the Tables.jl interface and convert types
    W = MLJ.table(float(W))
    T = (T₁ = categorical(T₁), T₂ = categorical(T₂))
    y = categorical(y)

    # Compute the theoretical ATE
    Wcomb = [1 1 1;
            1 1 0;
            1 0 0;
            1 0 1;
            0 1 0;
            0 0 0;
            0 0 1;
            0 1 1]
    ATE = 0
    for i in 1:8
        w = reshape(Wcomb[i, :], 1, 3)
        temp = μy_fn(w, [1], [1])[1]
        temp += μy_fn(w, [0], [0])[1]
        temp -= μy_fn(w, [1], [0])[1]
        temp -= μy_fn(w, [0], [1])[1]
        ATE += temp*0.5*0.5*0.5
    end
    return T, W, y, ATE
end

function binary_target_categorical_treatment_pb(rng;n=100)
    function μy_fn(W, T, Hmach)
        Thot = transform(Hmach, T)
        TMLE.expit(2W[:, 1] .+ 1W[:, 2] .- 2W[:, 3]
                    .- Thot[1] .+ Thot[2] .+ 2Thot[3] .- 3Thot[4]
                    .+ 2*Thot[1].*Thot[2]
                    .+ 1*Thot[1].*Thot[3]
                    .- 4*Thot[1].*Thot[4]
                    .- 3*Thot[2].*Thot[3]
                    .+ 1.5*Thot[2].*Thot[4]
                    .- 2.5*Thot[3].*Thot[4]
                    )
    end
    # Sampling W:
    W = rand(rng, Bernoulli(0.5), n, 3)

    # Sampling T from W:
    # T₁, T₂ will have 3 categories each
    # This is embodied by a 9 dimensional full joint
    θ = rand(rng, 3, 9)
    softmax = exp.(W*θ) ./ sum(exp.(W*θ), dims=2)
    encoding = collect(Iterators.product(["CC", "GG", "CG"], ["TT", "AA", "AT"]))
    T = [sample(rng, encoding, Weights(softmax[i, :])) for i in 1:n]
    T = (T₁=categorical([t[1] for t in T]), T₂=categorical([t[2] for t in T]))

    Hmach = machine(OneHotEncoder(drop_last=true), T)
    fit!(Hmach, verbosity=0)

    # Sampling y from T, W:
    μy = μy_fn(W, T, Hmach)
    y = [rand(rng, Bernoulli(μy[i])) for i in 1:n]

    # Compute the theoretical ATE for the query
    # (CC, AT) against (CG, AA)
    Wcomb = [1 1 1;
            1 1 0;
            1 0 0;
            1 0 1;
            0 1 0;
            0 0 0;
            0 0 1;
            0 1 1]
    ATE = 0
    levels₁ = levels(T.T₁)
    levels₂ = levels(T.T₂)
    for i in 1:8
        w = reshape(Wcomb[i, :], 1, 3)
        temp = μy_fn(w, (T₁=categorical(["CC"], levels=levels₁), T₂=categorical(["AT"], levels=levels₂)), Hmach)[1]
        temp += μy_fn(w, (T₁=categorical(["CG"], levels=levels₁), T₂=categorical(["AA"], levels=levels₂)), Hmach)[1]
        temp -= μy_fn(w, (T₁=categorical(["CC"], levels=levels₁), T₂=categorical(["AA"], levels=levels₂)), Hmach)[1]
        temp -= μy_fn(w, (T₁=categorical(["CG"], levels=levels₁), T₂=categorical(["AT"], levels=levels₂)), Hmach)[1]
        ATE += temp*0.5*0.5*0.5
    end
    return T, MLJ.table(float(W)), categorical(y), ATE
end

function continuous_target_binary_treatment_pb(rng;n=100)
    μy_fn(W, T₁, T₂) = 2W[:, 1] .+ 1W[:, 2] .- 2W[:, 3] .- T₁ .+ T₂ .+ 2*T₁ .* T₂
    # Sampling W: Bernoulli
    W = rand(rng, Bernoulli(0.5), n, 3)

    # Sampling T₁, T₂ from W: Softmax
    θ = rand(rng, 3, 4)
    softmax = exp.(W*θ) ./ sum(exp.(W*θ), dims=2)
    T = [sample(rng, [1, 2, 3, 4], Weights(softmax[i, :])) for i in 1:n]
    T₁ = [t in (1,2) ? true : false for t in T]
    T₂ = [t in (1,3) ? true : false for t in T]

    # Sampling y from T₁, T₂, W: Logistic
    μy = μy_fn(W, T₁, T₂)
    y = μy + rand(rng, Normal(0, 0.1), n)

    # Respect the Tables.jl interface and convert types
    W = MLJ.table(float(W))
    T = (T₁ = categorical(T₁), T₂ = categorical(T₂))

    # Compute the theoretical ATE
    Wcomb = [1 1 1;
            1 1 0;
            1 0 0;
            1 0 1;
            0 1 0;
            0 0 0;
            0 0 1;
            0 1 1]
    ATE = 0
    for i in 1:8
        w = reshape(Wcomb[i, :], 1, 3)
        temp = μy_fn(w, [1], [1])[1]
        temp += μy_fn(w, [0], [0])[1]
        temp -= μy_fn(w, [1], [0])[1]
        temp -= μy_fn(w, [0], [1])[1]
        ATE += temp*0.5*0.5*0.5
    end
    return T, W, y, ATE
end

@testset "Test Double Robustness IATE on binary_target_binary_treatment_pb" begin
    # When Q̅ is misspecified but G is well specified
    query = (T₁=[true, false], T₂=[true, false])
    Q̅ = ConstantClassifier()
    G = FullCategoricalJoint(LogisticClassifier())
    tmle = TMLEstimator(Q̅, G, query)

    abs_mean_rel_errors, abs_vars = asymptotics(
            tmle,
            binary_target_binary_treatment_pb,
            StableRNG(123),
            Ns
            )
    @test all(abs_mean_rel_errors .< [69, 14, 5, 1.5])
    @test all(abs_vars .< [0.05, 0.002, 0.0003, 3.5e-5])

    # When Q̅ is well specified  but G is misspecified
    query = (T₁=[true, false], T₂=[true, false])
    Q̅ = cat_interacter
    G = FullCategoricalJoint(ConstantClassifier())
    tmle = TMLEstimator(Q̅, G, query)

    abs_mean_rel_errors, abs_vars = asymptotics(
            tmle,
            binary_target_binary_treatment_pb,
            StableRNG(123),
            Ns
            )
    @test all(abs_mean_rel_errors .< [53, 13, 4.5, 1.4])
    @test all(abs_vars .< [0.03, 0.002, 0.0003, 2.7e-5])

end

@testset "Test Double Robustness IATE on continuous_target_binary_treatment_pb" begin
    # When Q̅ is misspecified but G is well specified

    # If the order of the variables in the query do no match
    # the order in T it shouldn't matter
    query = (T₂=[true, false], T₁=[true, false])
    Q̅ = MLJ.DeterministicConstantRegressor()
    G = FullCategoricalJoint(LogisticClassifier())
    tmle = TMLEstimator(Q̅, G, query)

    abs_mean_rel_errors, abs_vars = asymptotics(
            tmle,
            continuous_target_binary_treatment_pb,
            StableRNG(123),
            Ns
            )
    @test all(abs_mean_rel_errors .< [18, 1.6, 0.5, 0.2])
    @test all(abs_vars .< [0.08, 0.002, 0.0002, 1.5e-5])

    # When Q̅ is well specified  but G is misspecified
    query = (T₁=[true, false], T₂=[true, false])
    Q̅ = cont_interacter
    G = FullCategoricalJoint(ConstantClassifier())
    tmle = TMLEstimator(Q̅, G, query)

    abs_mean_rel_errors, abs_vars = asymptotics(
            tmle,
            continuous_target_binary_treatment_pb,
            StableRNG(123),
            Ns
            )
    @test all(abs_mean_rel_errors .< [2.3, 0.6, 0.3, 0.06])
    @test all(abs_vars .< [0.003, 0.0003, 2.5e-5, 2e-6])

end

@testset "Test Double Robustness IATE on binary_target_categorical_treatment_pb" begin
    # When Q̅ is misspecified but G is well specified
    query = (T₁=["CC", "CG"], T₂=["AT", "AA"])
    Q̅ = ConstantClassifier()
    G = FullCategoricalJoint(LogisticClassifier())
    tmle = TMLEstimator(Q̅, G, query)

    abs_mean_rel_errors, abs_vars = asymptotics(
            tmle,
            binary_target_categorical_treatment_pb,
            StableRNG(123),
            Ns
            )
    @test all(abs_mean_rel_errors .< [31, 8, 2, 0.6])
    @test all(abs_vars .< [0.03, 0.006, 0.0004, 2.7e-5])

    # When Q̅ is well specified but G is misspecified
    query = (T₁=["CC", "CG"], T₂=["AT", "AA"])
    Q̅ = cat_interacter
    G = FullCategoricalJoint(ConstantClassifier())
    tmle = TMLEstimator(Q̅, G, query)

    abs_mean_rel_errors, abs_vars = asymptotics(
            tmle,
            binary_target_categorical_treatment_pb,
            StableRNG(123),
            Ns
            )
    @test all(abs_mean_rel_errors .< [22, 7, 2.1, 0.7])
    @test all(abs_vars .< [0.04, 0.005, 0.0004, 3.5e-5])
end

end;

true


using Distributions
using Random
using StaticArrays

struct Kronecker_sampler{T, K} <: Random.Sampler{NTuple{K, T}} #TODO make edge type abstract
    power::Int
    dist::AliasTable{NTuple{K, T}, Vector{NTuple{K, T}}}
    initializer_size::T
    remainder::T
end

"""
    Kronecker_sampler(initializer, power; T=Int, space=1)

Create an efficient edge sampler whose probability distribution follows the specified
kroneker model.

Giving the sampler an adequite (but not excessive) amount of space may speed up generation
by up to 2x. `space = edge_count ÷ 100` may be a good heuristic. The sampler will take up
O(`sizeof(t)*ndims(initailizer)*space`) space with a constant factor around 4 bytes.

Draw a hypergraph from the sampler with `rand(sampler, edges)`.
"""
function Kronecker_sampler(initializer::AbstractArray, power::Integer; Type::Type{T}=Int, space=1,
    inner_power = max(1, min(power, Int(log(max(1,space)) ÷ log(max(2,length(initializer)))))),
    outer_power = cld(power, inner_power)) where T <: Integer

    Base.require_one_based_indexing(initializer)
    initializer_size = first(size(initializer))
    all(size(initializer) .== initializer_size) || ArgumentError("initializer must be square")

    inner_power = cld(power, outer_power)
    remainder = initializer_size ^ (power - inner_power*(outer_power-1))

    initializer = kronecker_power(initializer, inner_power)
    is = [Tuple(i).-one(T) for i in vec(eachindex(IndexCartesian(), initializer))]
    dist = AliasTable(vec(initializer), is)

    K = ndims(initializer)

    Kronecker_sampler{T, K}(outer_power, dist, initializer_size, remainder)
end

"""
    rand(s::Kronecker_sampler)
    rand(s::Kronecker_sampler, e::Integer)

Draw an edge or a hypergraph with `e` edges from the sampler `s`.
"""
function Base.rand(rng::AbstractRNG, s::Kronecker_sampler{T, K}) where {T, K}
    mult = s.remainder
    edge = MVector{K}(rand(rng, s.dist)) .% mult
    for _ in 1:(s.power-1)
        edge .+= rand(rng, s.dist) .* mult
        mult *= s.initializer_size
    end
    NTuple{K}(edge)
end


using SurfaceGeometry
using LinearAlgebra
using Test

# Pkg.test("SurfaceGeometry")
# write your own tests here
# @test 1 == 2

# Loading of spherical mesh

function SphereError(points)
    error = 0
    for xkey in 1:size(points,2)
        pos = points[:,xkey]
        n = pos/norm(pos)
        err = norm(n - pos)
        if err>error
            error = err
        end
    end
    return error
end

### Loading of mesh for other tests
@info "Mesh loading test"
using JLD
data = load(joinpath(dirname(@__FILE__),"sphere.jld"))
points = data["points"]
faces = data["faces"]

@test SphereError(points) < 0.01

@info "Topology function tests"

t = [5,7,10,7,5,6,4,0,3,0,4,6,4,7,6,4,9,10,7,4,10,0,2,1,2,0,6,2,5,1,5,2,6,8,4,3,4,11,9,8,11,4,9,11,3,11,8,3]
faces = reshape(t,(3,div(length(t),3))) .+ 1

triangles = []
for i in FaceVRing(5,faces)
    #println("i")
    push!(triangles,i)
end

@test sort(triangles)==[3,4,5,6,7,12,13,14]

triverticies = []
for i in DoubleVertexVRing(5,faces)
    #println("i")
    push!(triverticies,i)
end

@test (1,4) in triverticies

verticies = []
for i in VertexVRing(5,faces)
    push!(verticies,i)
end

@test sort(verticies)==[1,4,7,8,9,10,11,12] #[1,2,6,7]

@info "Testing Complex DS"

points = zero(faces)
fb = FaceBasedDS(faces)

triangles = []
for i in FaceVRing(5,fb)
    push!(triangles,i)
end
@test sort(triangles)==[3,4,5,6,7,12,13,14]

triverticies = []
for i in DoubleVertexVRing(5,fb)
    push!(triverticies,i)
end

@test (1,4) in triverticies

verticies = []
for i in VertexVRing(5,fb)
     push!(verticies,i)
end

@test sort(verticies)==[1,4,7,8,9,10,11,12]

@info "Topology tests for Connectivity table"

using JLD
data = load(joinpath(dirname(@__FILE__),"sphere.jld"))
#data = load("sphere.jld")
faces = data["faces"]

con = ConnectivityDS(faces,10)

verticies = []
for i in VertexVRing(1,con)
    push!(verticies,i)
end

@test sort(verticies)==[2,4,5,14,15]

triverticies = []
for i in DoubleVertexVRing(1,con)
    push!(triverticies,i)
end

@test (2,14) in triverticies

# Here I will also have tests for connectivity table

##### Surface Properties ######

@info "Tests for surface properties"

using JLD
data = load(joinpath(dirname(@__FILE__),"sphere.jld"))
#data = load("sphere.jld")
points = data["points"]
faces = data["faces"]

n = Array{Float64}(undef,size(points)...)
NormalVectors!(n,points,faces,i->FaceVRing(i,faces))
curvatures = Array{Float64}(undef,size(points,2))
MeanCurvatures!(curvatures,points,faces,n,i->FaceVRing(i,faces))

angle_ = 0
curvaturer = 0

for xkey in 1:size(points,2)
    iter = FaceVRing(xkey,faces)
    ncalc = n[:,xkey]
    ccalc = curvatures[xkey]

    nT = points[:,xkey]/norm(points[:,xkey])
    angl = acos(dot(ncalc,nT))
    if angl>angle_
        global angle_ = angl
    end

    cT = 1.
    N = size(points,2)
    global curvaturer += abs((ccalc - cT)/cT)/N
end

@test angle_*180/pi < 1.3
@test curvaturer < 0.01

@test isapprox(volume(points,faces),4/3*pi,atol=0.05)
@test isapprox(*(FitEllipsoid(points)...),1)

### And now integrators
@info "Testing pasive mesh stabilisation"

function velocity(t,pos)
    x,y,z = pos

    x = x*0.15 + 0.35
    y = y*0.15 + 0.35
    z = z*0.15 + 0.35

    u = 2*sin(pi*x)^2 * sin(2*pi*y) * sin(2*pi*z) * sin(2/3*pi*t)
    v = - sin(2*pi*x) * sin(pi*y)^2 * sin(2*pi*z) * sin(2/3*pi*t)
    w = - sin(2*pi*x) * sin(2*pi*y) * sin(pi*z)^2 * sin(2/3*pi*t)

    [u,v,w] /0.15
end

v = zero(points)
t = 1
for i in 1:size(points,2)
    v[:,i] = velocity(t,points[:,i])
end

res =  stabilise(points,faces,v)
println("Energy before minimization Finit=$(res.Finit) after Fres=$(res.Fres)")


"""
SamplerState for the multidimensional stochastic gradient Nosé-Hoover thermostat algorithm following

Ding, N., Fang, Y., Babbush, R., Chen, C., Skeel, R. D., & Neven, H. (2014). Bayesian Sampling Using Stochastic Gradient Thermostats. In Advances in Neural Information Processing Systems (pp. 3203–3211).

and

Leimkuhler, B., & Shang, X. (2016). Adaptive Thermostats for Noisy Gradient Systems. SIAM Journal on Scientific Computing, 38(2), A712–A736.

In contrast to SGNHT this sampler has a thermostat variable per dimension.

Fields:

    - x: the current state
    - p: current momentum
    - xi: thermostat variable (initialised to be D*ones)
    - t: iterations counter
    - stepsize: s.stepsize(s.t) gives the stepsize to be used in iteration s.t
    - mu: Constant in the thermostat equation
    - D: friction parameters
Constructors:
    - construct using `mSGNHTState(x,p,stepsize::Function;mu=1.,D=1.0)` or `mSGNHTState(x,p,stepsize::Float64;mu=1.,D=1.0)` for a fixed stepsize if p is not specified then          p = randn(length(x))

"""
type mSGNHTState <: SamplerState
    x::Array{Float64,1}
    p::Array{Float64,1}
    xi::Array{Float64,1}

    t::Int
    stepsize::Function
    mu::Float64
    D::Float64
    function mSGNHTState(x,p,stepsize::Function;mu=1.,D=1.0)
        @assert size(x) = size(p)
        xi = ones(length(x))*D
        new(x,p,xi,0,stepsize,mu,D)
    end
    function mSGNHTState(x,p,stepsize::Float64;mu=1.,D=1.0)
        @assert size(x) = size(p)
        xi = ones(length(x))*D
        s(niters) = stepsize
        new(x,p,xi,0,s,mu,D)
    end
    function mSGNHTState(x,stepsize::Function;mu=1.,D=1.0)
        p = randn(length(x))
        xi = ones(length(x))*D
        new(x,p,xi,0,stepsize,mu,D)
    end
    function mSGNHTState(x,stepsize::Float64;mu=1.,D=1.0)
        p = randn(length(x))
        xi = ones(length(x))*D
        s(niters) = stepsize
        new(x,p,xi,0,s,mu,D)
    end

end

"""
sample! performs one mSGNHTState update and returns the updated mSGNHTState.
"""
function sample!(s::mSGNHTState,grad::Function)
    # increment iterations counter
    s.t += 1

    # improve readability
    nparams = length(s.x)
    stepsize = s.stepsize(s.t)
    x = s.x
    p = s.p
    xi = s.xi
    D = s.D
    mu = s.mu

    # second order symmetric splitting scheme from Leimkuhler et al.
    add_inplace!(.5*stepsize,grad(x),p)
    add_inplace!(0.5*stepsize,p,x)
    add_inplace!(.5*stepsize/mu, (p.*p - 1),xi)
    ids = abs(stepsize*xi) .> 1e-6
    p[ids] = exp(-stepsize*xi[ids]).*p[ids] + sqrt(.5.*(1-exp(-2.0*stepsize*xi[ids]))./xi[ids] * D) .* randn(sum(ids))
    p[!ids] += sqrt(stepsize*D)*randn(sum(!ids))
    add_inplace!(.5*stepsize/mu, (p.*p - 1),xi)
    add_inplace!(.5*stepsize,p,x)
    add_inplace!(.5*stepsize,grad(x),p)
    s
end


function getAnalyticalConstGrad2D(n::Array{Int64,1},h::Array{Float64,1})
src = [1,div(n[2],2)];

source1 = (src[1]-1)*h[1];
source2 = (src[2]-1)*h[2];

(X1,X2) = ndgrid((0:(n[1]-1))*h[1],(0:(n[2]-1))*h[2]);

a  = -0.4;
s0 = 2.0;

kappaSquared = s0^2 + 2*a*(X1-source1);

SBarSquared = s0^2 + a*(X1-source1);
rSquared = ((X1-source1).^2 + (X2-source2).^2);
sigmaSquarred = 2*rSquared./(SBarSquared + sqrt(SBarSquared.^2 - (a^2).*rSquared));
sigma = sqrt(sigmaSquarred);
T_exact =  SBarSquared.*sigma - (a^2).*(sigma.^3)./6;

return kappaSquared,src,T_exact;
end

function getAnalyticalConstGrad3D(n::Array{Int64,1},h::Array{Float64,1})
src = [div(n[1],2),div(n[2],2),1];

source1 = (src[1]-1)*h[1];
source2 = (src[2]-1)*h[2];
source3 = (src[3]-1)*h[3];

(X1,X2,X3) = ndgrid((0:(n[1]-1))*h[1],(0:(n[2]-1))*h[2],(0:(n[3]-1))*h[3]);

# a  = -1.75;
a  = -1.65;
s0 = 2.0;

kappaSquared = s0^2 + 2*a*(X3-source3);
SBarSquared = s0^2 + a*(X3-source3);
rSquared = (X1-source1).^2 + (X2-source2).^2 + (X3-source3).^2;
sigmaSquarred = 2*rSquared./(SBarSquared + sqrt(SBarSquared.^2 - (a^2).*rSquared));
sigma = sqrt(sigmaSquarred);
T_exact =  SBarSquared.*sigma - (a^2).*(sigma.^3)./6;

return kappaSquared,src,T_exact;
end

function runAnalyticalConstGradExperiment3D(h::Array{Float64,1},WU::Float64)
I = [1.6,1.6,0.8];

n = zeros(Int64,3);
n[1] = round(Int64,(I[1]/h[1])+1);
n[2] = round(Int64,(I[2]/h[2])+1);
n[3] = round(Int64,(I[3]/h[3])+1);

# src = [div(n[1],2),div(n[2],2),div(n[3],2)];
src = [div(n[1],2),div(n[2],2),1];

source1 = (src[1]-1)*h[1];
source2 = (src[2]-1)*h[2];
source3 = (src[3]-1)*h[3];

(X1,X2,X3) = ndgrid((0:(n[1]-1))*h[1],(0:(n[2]-1))*h[2],(0:(n[3]-1))*h[3]);

a  = -1.75;
s0 = 2.0;

kappaSquared = s0^2 + 2*a*(X3-source3);
SBarSquared = s0^2 + a*(X3-source3);
rSquared = (X1-source1).^2 + (X2-source2).^2 + (X3-source3).^2;
sigmaSquarred = 2*rSquared./(SBarSquared + sqrt(SBarSquared.^2 - (a^2).*rSquared));
sigma = sqrt(sigmaSquarred);
T_exact =  SBarSquared.*sigma - (a^2).*(sigma.^3)./6;

sigmaSquarred = sigma = SBarSquared = X1 = X2 = X3 = rSquared = 0.0;

return T_exact,src,kappaSquared;
end

function getAnalyticalConstGradInv2D(n::Array{Int64,1},h::Array{Float64,1})

src = [1,div(n[2],2)];

source1 = (src[1]-1)*h[1];
source2 = (src[2]-1)*h[2];

(X1,X2) = ndgrid((0:(n[1]-1))*h[1],(0:(n[2]-1))*h[2]);

a  = 1.0;
s0 = 2.0;

Kappa = 1./(1./s0 + a*(X1-source1));
T_exact = (1/a)*acosh.(1+(0.5*s0*a*a).*Kappa.*((X1-source1).^2 + (X2-source2).^2));
G2_exact = (1/a)*(1./sqrt.((1+(0.5*s0*a*a).*Kappa.*((X1-source1).^2 + (X2-source2).^2)).^2 - 1)).*(0.5*s0*a*a).*Kappa.*2.*(X2-source2);
G1_exact = (1/a)*(1./sqrt.((1+(0.5*s0*a*a).*Kappa.*((X1-source1).^2 + (X2-source2).^2)).^2 - 1)).*((0.5*s0*a*a).*Kappa.*2.*(X1-source1)
			+ (0.5*s0*a*a).*((X1-source1).^2 + (X2-source2).^2).*(-(Kappa.^2)).*a);
kappaSquared = Kappa.^2;

return kappaSquared,src,T_exact;

end

function getAnalyticalConstGradInv3D(n::Array{Int64,1},h::Array{Float64,1})

src = [div(n[1],2),div(n[1],2),1];

source1 = (src[1]-1)*h[1];
source2 = (src[2]-1)*h[2];
source3 = (src[3]-1)*h[3];

(X1,X2,X3) = ndgrid((0:(n[1]-1))*h[1],(0:(n[2]-1))*h[2],(0:(n[3]-1))*h[3]);

a  = 1.0;
s0 = 2.0;
Kappa = 1./(1./s0 + a*(X3-source3));
Rsquared = (X1-source1).^2 + (X2-source2).^2 + (X3-source3).^2;
T_exact = (1/a)*acosh.(1+(0.5*s0*a*a).*Kappa.*(Rsquared));
G1_exact = (1/a)*(1./sqrt.((1+(0.5*s0*a*a).*Kappa.*Rsquared).^2 - 1)).*(0.5*s0*a*a).*Kappa.*2.*(X1-source1);
G2_exact = (1/a)*(1./sqrt.((1+(0.5*s0*a*a).*Kappa.*Rsquared).^2 - 1)).*(0.5*s0*a*a).*Kappa.*2.*(X2-source2);
G3_exact = (1/a)*(1./sqrt.((1+(0.5*s0*a*a).*Kappa.*Rsquared).^2 - 1)).*((0.5*s0*a*a).*Kappa.*2.*(X3-source3)
			+ (0.5*s0*a*a).*(Rsquared).*(-(Kappa.^2)).*a);
kappaSquared = Kappa.^2;

return kappaSquared,src,T_exact;

end

function getSmoothGaussianMedium(n::Array{Int64,1},h::Array{Float64,1})
src = div(n,4);
kappaSquared = [];
T_exact = [];
if length(n)==2
	(T1_exact,G11_exact,G12_exact) = getSmoothFactoredModel(n,h);
	(T0,G01,G02,L0) = getAnalytic2DeikonalSolutionAll(n,h,src);
	G1_exact = T0.*G11_exact + G01.*T1_exact;
	G2_exact = T0.*G12_exact + G02.*T1_exact;
	kappaSquared = G1_exact.*G1_exact + G2_exact.*G2_exact;
	T_exact = T0.*T1_exact;
else

	(T1_exact,G11_exact,G12_exact,G13_exact) = getSmoothFactoredModel3D(n,h);

	(T0,G01,G02,G03) = getAnalytic3DeikonalSolutionAll(n,h,src);

	G1_exact = T0.*G11_exact + G01.*T1_exact;
	G2_exact = T0.*G12_exact + G02.*T1_exact;
	G3_exact = T0.*G13_exact + G03.*T1_exact;
	kappaSquared = G1_exact.*G1_exact + G2_exact.*G2_exact + G3_exact.*G3_exact;
	T_exact = T0.*T1_exact;

end

return kappaSquared,src,T_exact;
end

function getSmoothFactoredModel(n::Array{Int64,1},h::Array{Float64,1})
xsrc = zeros(2);
src_kappa = zeros(Int64,2);
src_kappa[1] = div(n[1],3);
src_kappa[2] = div(n[2],4);

xsrc[1] = (src_kappa[1]-1)*h[1];
xsrc[2] = (src_kappa[2]-1)*h[2];

X1,X2 = ndgrid((0:(n[1]-1))*h[1],(0:(n[2]-1))*h[2]);

sigma = 0.1;
T1_exact = (exp( - (sigma*((X1 - xsrc[1]).^2) + 4*sigma*((X2-xsrc[2]).^2))) + 1)/2;
G11_exact = -2*sigma*(X1 - xsrc[1]).*exp.( - (sigma*((X1 - xsrc[1]).^2) + 4*sigma*((X2-xsrc[2]).^2)))/2;
G12_exact = -8*sigma*(X2 - xsrc[2]).*exp.( - (sigma*((X1 - xsrc[1]).^2) + 4*sigma*((X2-xsrc[2]).^2)))/2;

return T1_exact,G11_exact,G12_exact;
end

function getSmoothFactoredModel3D(n::Array{Int64,1},h::Array{Float64,1})
xsrc = zeros(3);
src_kappa = zeros(Int64,3);
src_kappa[1] = div(n[1],3);
src_kappa[2] = div(n[2],4);
src_kappa[3] = div(n[3],2);

xsrc[1] = (src_kappa[1]-1)*h[1];
xsrc[2] = (src_kappa[2]-1)*h[2];
xsrc[3] = (src_kappa[3]-1)*h[3];

X1,X2,X3 = ndgrid((0:(n[1]-1))*h[1],(0:(n[2]-1))*h[2],(0:(n[3]-1))*h[3]);

Sigma = [0.1,0.4,0.2];
EXPRSkewedSquared = exp.(-Sigma[1]*((X1 - xsrc[1]).^2) - Sigma[2]*((X2-xsrc[2]).^2) - Sigma[3]*((X3-xsrc[3]).^2));

T1_exact = EXPRSkewedSquared/2 + 1/2;
G11_exact = -Sigma[1]*(X1 - xsrc[1]).*EXPRSkewedSquared;
G12_exact = -Sigma[2]*(X2 - xsrc[2]).*EXPRSkewedSquared;
G13_exact = -Sigma[3]*(X3 - xsrc[3]).*EXPRSkewedSquared;

return T1_exact,G11_exact,G12_exact,G13_exact;
end


# Infer population model parameters from control group

using Pumas, MCMCChains, StatsPlots, CSV, Random
plotly()

# Import data
path = joinpath(
    @__DIR__,
    "../../..", "python/lung_cancer/data",
    "lxf_control_growth.csv")
data = DataFrame(CSV.File(path))

# Filter Body weight information
data = filter(x -> !(occursin.("Body weight", x.Biomarker)), data)
pumas_data = read_pumas(data,
    id           = :ID,
    time         = :Time,
    observations = [:Measurement],
    event_data = false)

# Visualise tumour volume
@df data scatter(
    :Time,
    :Measurement,
    group = :ID)
yaxis!("Tumour volume in cm^3")
xaxis!("Time in day")

# Build model
tumour_growth_model = @model begin
    @param   begin
        mean_initial_volume ~ Constrained(
            Normal(0.2, 1), lower=0.001, upper=Inf, init=0.2)
        mean_critical_volume ~ Constrained(
            Normal(0.2, 1), lower=0.001, upper=Inf, init=0.2)
        mean_growth_rate ~ Constrained(
            Normal(0.2, 1), lower=0.001, upper=Inf, init=0.2)
        std ~ Constrained(
            MvNormal([1 , 1, 1]),
            lower=[0, 0, 0],
            upper=[Inf, Inf, Inf],
            init=[0.09,0.09, 0.09])
        σ_base ~ Constrained(
            Normal(0.2, 1), lower=0.001, upper=Inf, init=0.2)
        σ_rel ~ Constrained(
            Normal(0.2, 1), lower=0.001, upper=Inf, init=0.2)
        # mean_initial_volume ∈ RealDomain(lower=0, init = 0.2)
        # mean_critical_volume ∈ RealDomain(lower=0, init = 1)
        # mean_growth_rate ∈ RealDomain(lower = 0, init= 0.1)
        # std ∈ PDiagDomain(init=[0.09,0.09, 0.09])
        # σ_base ∈ RealDomain(lower=0, init=0.04)
        # σ_rel ∈ RealDomain(lower=0, init=0.04)
    end

    @random begin
        η ~ MvNormal(std)
    end

    @pre begin
        initial_volume = mean_initial_volume * exp(η[1])
        critical_volume = mean_critical_volume * exp(η[2])
        growth_rate = mean_growth_rate * exp(η[3])
    end

    @init begin
        volume = initial_volume
    end

    @dynamics begin
       volume' = (growth_rate * critical_volume * volume) / (volume + critical_volume)
    end

  @derived begin
      σ_tot := σ_base .+ volume * σ_rel
      Measurement ~ @. Normal(volume, σ_tot)
    end
end

# Infer parameters
param = init_param(tumour_growth_model)
result = fit(tumour_growth_model, pumas_data, param, Pumas.BayesMCMC();
  nsamples=2000, nadapts=1000)

# Visualise population parameters
chains = Pumas.Chains(result)
plot(chains[1000:end])


function getInteractionHamIPicture(
    Ham_0::Array,
    Ham_I::Array,
    t::AbstractFloat,
)::Array
    U_array = evolutionOperator(Ham_0, t)
    return adjoint(U_array) * Ham_I * U_array
end

function getInteractionHamIPicture(
    Ham_0::Operator,
    Ham_I::Operator,
    t::AbstractFloat,
)::Operator
    U_op = evolutionOperator(Ham_0, t)
    return U_op' * Ham_I * U_op
end

function getInteractionHamIPictureA(
    Ham_0::Operator,
    Ham_I::Operator,
    t::AbstractFloat,
)::Array
    U_op = evolutionOperator(Ham_0, t)
    return (U_op' * Ham_I * U_op).data
end

function getInteractionHamIPictureA(
    Ham_int::Array,
    H_lambda::Array,
    H_S::Array,
    H_Sinv::Array,
    t::AbstractFloat,
)
    U_op = evolutionOperatorA(H_lambda, H_S, H_Sinv, t)
    U_opd = adjoint(U_op)
    return U_opd * Ham_int * U_op
end


# this is basically log2_ext with a slightly modified range reduction.

function log21p_ext(x)
    u = 1.0 + x
    sig,n = frexp(u)
    au = 2sig; n -= 1

    # reduce to
    # k = 0, sqrt(3)/2 = 0.866 < au < sqrt(3/2) = 1.22
    # k = 1, sqrt(3/2) = 1.22 < au < sqrt(3) = 1.73

    if au < sqrt(3/2)
        bp = 1.0
        dp_h = 0.0
        dp_l = 0.0
    elseif au < sqrt(3)
        bp = 1.5
        # log2(1.5)
        dp_h = 5.84962487220764160156e-01
        dp_l = 1.35003920212974897128e-08
    else
        bp = 1.0
        dp_h = 0.0
        dp_l = 0.0

        n += 1
        au *= 0.5
    end

    # exact 1.0 + x = u + c
    if x >= 1.0
        # u <= 2x
        c = 1.0-(u-x)
    else
        # 1) x > -0.5: 0.5 <= u <= 2.0
        # 2) x <= -0.5: u = x+1.0 exact, hence so is u-1.0
        c = x-(u-1.0)
    end

    # x <= -0.5: c = 0
    # x < 0.5: |c| < eps(1.0)
    # x < 0x1p53: |c| in {-eps(x), 0, eps(x)}
    # otherwise: c = 1

    # scale to au:  1 + x = (au + ac)*2^n
    ac = ldexp(c,-n)
    # log(x) = 2s + 2/3 s^3 + 2/5 s^5...
    f = au - bp # usual f, reduced to -0.28 < u < 0.23
    f1 = f + ac
    f2 = ac + (f-f1)
    g = au + bp
    g_h = trunc32(g) # according to comment
    g_l = ac + (au + (bp - g_h))
    rg = 1.0/g

    ss = f1*rg # ss = (x-1)/(x+1) or (x-1.5)/(x+1.5) < 0.125 = 2^-3
    s_h = trunc32(ss)
    s_l = (((f1-s_h*g_h)-s_h*g_l) + f2)*rg # division correction?

    z = ss*ss # < 2^-6
    # log(au) = 2s + 2/3*(s^3 + r) = 2/3s (3 + s^2 + r)
    r = z*z*@horner(z,
                    5.99999999999994648725e-01,
                    4.28571428578550184252e-01,
                    3.33333329818377432918e-01,
                    2.72728123808534006489e-01,
                    2.30660745775561754067e-01,
                    2.06975017800338417784e-01) # < 2^-12
    # (s_h + s_l)^2 = s_h^2 + 2 s_l s_h + s_l s_l
    #   = s_h^2 + s_l (s_h + ss)
    r += s_l*(s_h+ss)
    s2 = s_h*s_h
    t_h = 3.0 + s2 + r
    t_h = trunc32(t_h)
    t_l = r-((t_h-3.0)-s2)
    # u+v = ss*(1+...)
    u = s_h*t_h
    v = s_l*t_h+t_l*ss

    # 2/(3log2)
    cp    =  9.61796693925975554329e-01 # /* 0x3FEEC709, 0xDC3A03FD =2/(3ln2) */
    cp_h  =  9.61796700954437255859e-01 # /* 0x3FEEC709, 0xE0000000 =(float)cp */
    cp_l  = -7.02846165095275826516e-09 # /* 0xBE3E2FE0, 0x145B01F5 =tail of cp_h*/

    # 2/(3log2)*(ss+...)
    p_h = trunc32(u+v)
    p_l = v-(p_h-u)
    z_h = cp_h*p_h
    z_l = cp_l*p_h + p_l*cp + dp_l

    t = float64(n)
    t1 = trunc32(((z_h+z_l) + dp_h) + t)
    t2 = z_l - (((t1 - t) - dp_h) - z_h)
    Double64(t1,t2)
end

function exp2m1(p::Double64)
    z = p.lo + p.hi
    # check for under/overflow

    # 2^(p_l+p_h)

    zr = round(z)
    n = int(zr)
    p_h = p.hi - zr
    p_l = p.lo

    lg2  =  6.93147180559945286227e-01 # /* 0x3FE62E42, 0xFEFA39EF */
    lg2_h  =  6.93147182464599609375e-01 # /* 0x3FE62E43, 0x00000000 */
    lg2_l  = -1.90465429995776804525e-09 # /* 0xBE205C61, 0x0CA86C39 */

    pt = trunc32(p_l + p_h)
    u = pt*lg2_h
    v = (p_l-(pt-p_h))*lg2+pt*lg2_l
    z = u+v
    w = v-(z-u)

    expm1_kernel(n,Double64(z,w))
end

# 2^n - 1

# |z| < 0.5*log(2) = 0.35
# u ~ z
# r ~ -z^2/2

pow1p(x,y) = exp2(mul_ext(log21p_ext(x),y))
powm1(x,y) = exp2m1(mul_ext(log2_ext(x),y))
pow1pm1(x,y) = exp2m1(mul_ext(log21p_ext(x),y))