###########################################################################################################
###########################################################################################################
###########################################################################################################
###########################################################################################################
###########################################################################################################
###########################################################################################################
#                         SLATER-TYPE ORBİTALS ONE ELECTRON MOLECULAR INTEGRALS                           #
###########################################################################################################
###########################################################################################################
###########################################################################################################
###########################################################################################################
###########################################################################################################
###########################################################################################################
export TwoCenterOverlap, CTwoCenterOverlap, VTwoCenterOverlap, STwoCenterOverlap, TwoCenterOverlapRec,
TwoCenterNucAttractABA, CTwoCenterNucAttractABA, VTwoCenterNucAttractABA, STwoCenterNucAttractABA,
TwoCenterNucAttractAAB, CTwoCenterNucAttractAAB, VTwoCenterNucAttractAAB, VTwoCenterNucAttractAAB,
TwoCenterKinEnergy, CTwoCenterKinEnergy, VTwoCenterKinEnergy, STwoCenterKinEnergy
###########################################################################################################
###########################################################################################################
####################################### TWO CENTER OVERLAP INTEGRALS ######################################(43)
################################################### ALIGNED MOLECULAR COORDINATE SYSTEM
function TwoCenterOverlap(n1::arb, l1::Int, n2::arb, l2::Int, λ::Int, ρ::arb, τ::arb,lim::Int)
    Overlap = RF(0)
    for s1 in (EvenQ(l1-λ) ? -λ : -λ + 1)  : 2 : l1
        for s2 in (EvenQ(l2+λ) ? λ : λ + 1)  : 2 : l2
            for s3 in 0 : s1 + s2
                Overlap += SphPCG(s3, s1, s2, l1, λ, l2, λ, λ)*
                AuxiliaryG(RF(10//10), s3, RF(n1 - s1), RF(n2 - s2), RF(10//10), ρ, ρ * τ, lim)
            end
        end
    end
    return Power(-1,l2 + λ) * SlaterON(n1, n2, ρ, τ) * Overlap
end

function CTwoCenterOverlap(n1::arb, l1::Int, n2::arb, l2::Int, λ::Int, ρ::arb, τ::arb,lim::Float64)
    Overlap = RF(0)
    for s1 in (EvenQ(l1-λ) ? -λ : -λ + 1)  : 2 : l1
        for s2 in (EvenQ(l2+λ) ? λ : λ + 1)  : 2 : l2
            for s3 in 0 : s1 + s2
                Overlap += SphPCG(s3, s1, s2, l1, λ, l2, λ, λ)*
                CuhreAuxiliaryG(10//10, s3, n1 - s1, n2 - s2, 10//10, ρ, ρ * τ, lim)[1]
            end
        end
    end
    return Power(-1,l2 + λ) * SlaterON(n1, n2, ρ, τ) * Overlap
end

function VTwoCenterOverlap(n1::arb, l1::Int, n2::arb, l2::Int, λ::Int, ρ::arb, τ::arb,lim::Float64)
    Overlap = RF(0)
    for s1 in (EvenQ(l1-λ) ? -λ : -λ + 1)  : 2 : l1
        for s2 in (EvenQ(l2+λ) ? λ : λ + 1)  : 2 : l2
            for s3 in 0 : s1 + s2
                Overlap += SphPCG(s3, s1, s2, l1, λ, l2, λ, λ)*
                VegasAuxiliaryG(10//10, s3, n1 - s1, n2 - s2, 10//10, ρ, ρ * τ, lim)[1]
            end
        end
    end
    return Power(-1,l2 + λ) * SlaterON(n1, n2, ρ, τ) * Overlap
end

function STwoCenterOverlap(n1::arb, l1::Int, n2::arb, l2::Int, λ::Int, ρ::arb, τ::arb,lim::Float64)
    Overlap = RF(0)
    for s1 in (EvenQ(l1-λ) ? -λ : -λ + 1)  : 2 : l1
        for s2 in (EvenQ(l2+λ) ? λ : λ + 1)  : 2 : l2
            for s3 in 0 : s1 + s2
                Overlap += SphPCG(s3, s1, s2, l1, λ, l2, λ, λ)*
                SuaveAuxiliaryG(10//10, s3, n1 - s1, n2 - s2, 10//10, ρ, ρ * τ, lim)[1]
            end
        end
    end
    return Power(-1,l2 + λ) * SlaterON(n1, n2, ρ, τ) * Overlap
end

function TwoCenterOverlapRec(n1::arb, l1::Int, n2::arb, l2::Int, λ::Int, ρ::arb, τ::arb,lim::Int)
    OverlapRec = RF(0)
    for s1 in (EvenQ(l1-λ) ? -λ : -λ + 1)  : 2 : l1
        for s2 in (EvenQ(l2+λ) ? λ : λ + 1)  : 2 : l2
            for s3 in 0 : s1 + s2
                OverlapRec += SphPCG(s3, s1, s2, l1, λ, l2, λ, λ)*
                AuxiliaryGr(RF(10//10), s3, RF(n1 - s1), RF(n2 - s2), RF(10//10), ρ, ρ * τ, lim)
            end
        end
    end
    return SlaterON(n1, n2, ρ, τ)*OverlapRec
end
###########################################################################################################
function TwoCenterOverlap(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Int)

    TwoCenterOverlap(n1, l1, n2, l2, λ, (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2), lim)
end

function CTwoCenterOverlap(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64)

    CTwoCenterOverlap(n1, l1, n2, l2, λ, (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2), lim)
end

function VTwoCenterOverlap(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64)

    VTwoCenterOverlap(n1, l1, n2, l2, λ, (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2), lim)
end

function STwoCenterOverlap(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64)

    STwoCenterOverlap(n1, l1, n2, l2, λ, (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2), lim)
end

function TwoCenterOverlapRec(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Int)

    TwoCenterOverlapRec(n1, l1, n2, l2, λ, (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2), lim)
end
###########################################################################################################
###########################################################################################################
###########################################################################################################
###########################################################################################################
################################################### NON-ALIGNED MOLECULAR COORDINATE SYSTEM
function  TwoCenterOverlap(
n1::arb, l1::Int, m1::Int,
n2::arb, l2::Int, m2::Int,
ρ::arb, τ::arb, theta::arb, phi::arb,
x::Int, lim::Int)

    NOverlap = RF(0)

    for s in 0 : min(l1,l2)
        NOverlap += (RotaT(s, l1, m1, l2, m2, theta, phi, x) *
        TwoCenterOverlap(n1, l1, n2, l2, s, ρ, τ,lim))
    end
    NOverlap
end

function  CTwoCenterOverlap(
    n1::arb, l1::Int, m1::Int,
    n2::arb, l2::Int, m2::Int,
    ρ::arb, τ::arb, theta::arb, phi::arb,
    x::Int, lim::Float64)

    NOverlap = RF(0)

    for s in 0 : min(l1,l2)
        NOverlap += (RotaT(s, l1, m1, l2, m2, theta, phi, x) *
        CTwoCenterOverlap(n1, l1, n2, l2, s, ρ, τ,lim))
    end
    NOverlap
end

function  VTwoCenterOverlap(
    n1::arb, l1::Int, m1::Int,
    n2::arb, l2::Int, m2::Int,
    ρ::arb, τ::arb, theta::arb, phi::arb,
    x::Int, lim::Float64)

    NOverlap = RF(0)

    for s in 0 : min(l1,l2)
        NOverlap += (RotaT(s, l1, m1, l2, m2, theta, phi, x) *
        VTwoCenterOverlap(n1, l1, n2, l2, s, ρ, τ,lim))
    end
     NOverlap
end

function  STwoCenterOverlap(
    n1::arb, l1::Int, m1::Int,
    n2::arb, l2::Int, m2::Int,
    ρ::arb, τ::arb, theta::arb, phi::arb,
    x::Int, lim::Float64)

    NOverlap = RF(0)

    for s in 0 : min(l1,l2)
        NOverlap += (RotaT(s, l1, m1, l2, m2, theta, phi, x) *
        STwoCenterOverlap(n1, l1, n2, l2, s, ρ, τ,lim))
    end
    NOverlap
end

function  TwoCenterOverlapRec(
    n1::arb, l1::Int, m1::Int,
    n2::arb, l2::Int, m2::Int,
    ρ::arb, τ::arb, theta::arb, phi::arb,
    x::Int, lim::Int)
        NOverlap = RF(0)

    for s in 0 : min(l1,l2)
        NOverlap += (RotaT(s, l1, m1, l2, m2, theta, phi, x) *
        TwoCenterOverlapRec(n1, l1, n2, l2, s, ρ, τ,lim))
    end
        NOverlap
end

function TwoCenterOverlapRec(
    n1::arb, l1::Int, m1:: Int, ζ1::arb,
    n2::arb, l2::Int, m2::Int, ζ2::arb,
    R::arb, theta::arb, phi::arb, x::Int, lim::Int)

    TwoCenterOverlapRec(n1, l1, m1, n2, l2, m2, (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2), theta, phi, x, lim)
end
###########################################################################################################
###########################################################################################################
################################# TWO CENTER NUCLEAR ATTRACTION INTEGRALS #################################(43)
################################################### ALIGNED MOLECULAR COORDINATE SYSTEM
function  TwoCenterNucAttractABA(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Int)

    coef = (2 * ζ1) // (Sqrt(2 * n1 * (2 * n1 - 1)))
    restco = TwoCenterOverlap(n1 - RF(1), l1, n2, l2, λ, (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2),lim)

    return coef * restco
end

function  CTwoCenterNucAttractABA(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64)

    coef = (2 * ζ1) // (Sqrt(2 * n1 * (2 * n1 - 1)))
    restco = CTwoCenterOverlap(n1 - RF(1), l1, n2, l2, λ, (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2),lim)

    return coef * restco
end

function  VTwoCenterNucAttractABA(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64)

    coef = (2 * ζ1) // (Sqrt(2 * n1 * (2 * n1 - 1)))
    restco = VTwoCenterOverlap(n1 - RF(1), l1, n2, l2, λ, (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2),lim)

    return coef * restco
end

function  STwoCenterNucAttractABA(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64)

    coef = (2 * ζ1) // (Sqrt(2 * n1 * (2 * n1 - 1)))
    restco = STwoCenterOverlap(n1 - RF(1), l1, n2, l2, λ, (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2),lim)

    return coef * restco
end
###########################################################################################################
###########################################################################################################
###########################################################################################################
################################################### NON-ALIGNED MOLECULAR COORDINATE SYSTEM
function  TwoCenterNucAttractABA(
    n1::arb, l1::Int, m1::Int, ζ1::arb,
    n2::arb, l2::Int, m2::Int, ζ2::arb,
    R::arb, theta::arb, phi::arb,
    x::Int, lim::Int)

    coef = (2 * ζ1) // (Sqrt(2 * n1 * (2 * n1 - 1)))
    restco = TwoCenterOverlap(
    n1 - RF(1), l1, m1,
    n2, l2, m2,
    (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2), theta, phi, x, lim)

    return coef * restco
end

function  CTwoCenterNucAttractABA(
    n1::arb, l1::Int, m1::Int, ζ1::arb,
    n2::arb, l2::Int, m2::Int, ζ2::arb,
    R::arb, theta::arb, phi::arb,
    x::Int, lim::Float64)

    coef = (2 * ζ1) // (Sqrt(2 * n1 * (2 * n1 - 1)))
    restco = CTwoCenterOverlap(
    n1 - RF(1), l1, m1,
    n2, l2, m2,
    (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2), theta, phi, x, lim)

    return coef * restco
end

function  VTwoCenterNucAttractABA(
    n1::arb, l1::Int, m1::Int, ζ1::arb,
    n2::arb, l2::Int, m2::Int, ζ2::arb,
    R::arb, theta::arb, phi::arb,
    x::Int, lim::Float64)

    coef = (2 * ζ1) // (Sqrt(2 * n1 * (2 * n1 - 1)))
    restco = VTwoCenterOverlap(
    n1 - RF(1), l1, m1,
    n2, l2, m2,
    (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2), theta, phi, x, lim)

    return coef * restco
end

function  STwoCenterNucAttractABA(
    n1::arb, l1::Int, m1::Int, ζ1::arb,
    n2::arb, l2::Int, m2::Int, ζ2::arb,
    R::arb, theta::arb, phi::arb,
    x::Int, lim::Float64)

    coef = (2 * ζ1) // (Sqrt(2 * n1 * (2 * n1 - 1)))
    restco = STwoCenterOverlap(
    n1 - RF(1), l1, m1,
    n2, l2, m2,
    (R//2)*(ζ1+ζ2), (ζ1-ζ2)//(ζ1+ζ2), theta, phi, x, lim)

    return coef * restco
end
###########################################################################################################
function TwoCenterNucAttractAAB(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Int
)
    ρ = (R // 2) * (ζ1 + ζ2)
    τ = (ζ1 - ζ2) // (ζ1 + ζ2)
    n2z = RF(0.000000000000000000000000000000000000000000000001)

    coeff = (R // 2) * SlaterON(n1, n2, ρ, τ)

    sresaab = RF(0)
    for s1 in abs(l1 - l2) : l1 + l2
        for s2 in (EvenQ(l1 + l2 - λ) ? -λ : -λ + 1) : 2 : l1 + l2
            for s3 in 0 : s2
                sresaab1 = Sqrt(2 * s1 + 1)
                sresaab2 = GauntGF(l1, λ, l2, λ, s1)
                sresaab3 = SphPCG(s3, s2, 0, s1, 0, 0, 0, 0)
                sresaab4 = AuxiliaryG(RF(10//10), s3, n1 + n2 - s2 - 1, n2z, RF(10//10), ρ, ρ, lim)
                sresaab += sresaab1 * sresaab2 * sresaab3 * sresaab4
            end
        end
    end

    resaab = coeff * sresaab
    return resaab
end

function CTwoCenterNucAttractAAB(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64
)
    ρ = (R // 2) * (ζ1 + ζ2)
    τ = (ζ1 - ζ2) // (ζ1 + ζ2)
    n2z = RF(0.000000000000000000000000000000000000000000000001)

    coeff = (R // 2) * SlaterON(n1, n2, ρ, τ)

    sresaab = RF(0)
    for s1 in abs(l1 - l2) : l1 + l2
        for s2 in (EvenQ(l1 + l2 - λ) ? -λ : -λ + 1) : 2 : l1 + l2
            for s3 in 0 : s2
                sresaab1 = Sqrt(2 * s1 + 1)
                sresaab2 = GauntGF(l1, λ, l2, λ, s1)
                sresaab3 = SphPCG(s3, s2, 0, s1, 0, 0, 0, 0)
                sresaab4 = CuhreAuxiliaryG(RF(10//10), s3, n1 + n2 - s2 - 1, n2z, RF(10//10), ρ, ρ, lim)[1]
                sresaab += sresaab1 * sresaab2 * sresaab3 * sresaab4
            end
        end
    end

    resaab = coeff * sresaab
    return resaab
end

function VTwoCenterNucAttractAAB(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64
)
    ρ = (R // 2) * (ζ1 + ζ2)
    τ = (ζ1 - ζ2) // (ζ1 + ζ2)
    n2z = RF(0.000000000000000000000000000000000000000000000001)

    coeff = (R // 2) * SlaterON(n1, n2, ρ, τ)

    sresaab = RF(0)
    for s1 in abs(l1 - l2) : l1 + l2
        for s2 in (EvenQ(l1 + l2 - λ) ? -λ : -λ + 1) : 2 : l1 + l2
            for s3 in 0 : s2
                sresaab1 = Sqrt(2 * s1 + 1)
                sresaab2 = GauntGF(l1, λ, l2, λ, s1)
                sresaab3 = SphPCG(s3, s2, 0, s1, 0, 0, 0, 0)
                sresaab4 = VegasAuxiliaryG(RF(10//10), s3, n1 + n2 - s2 - 1, n2z, RF(10//10), ρ, ρ, lim)[1]
                sresaab += sresaab1 * sresaab2 * sresaab3 * sresaab4
            end
        end
    end

    resaab = coeff * sresaab
    return resaab
end

function STwoCenterNucAttractAAB(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64
)
    ρ = (R // 2) * (ζ1 + ζ2)
    τ = (ζ1 - ζ2) // (ζ1 + ζ2)
    n2z = RF(0.000000000000000000000000000000000000000000000001)

    coeff = (R // 2) * SlaterON(n1, n2, ρ, τ)

    sresaab = RF(0)
    for s1 in abs(l1 - l2) : l1 + l2
        for s2 in (EvenQ(l1 + l2 - λ) ? -λ : -λ + 1) : 2 : l1 + l2
            for s3 in 0 : s2
                sresaab1 = Sqrt(2 * s1 + 1)
                sresaab2 = GauntGF(l1, λ, l2, λ, s1)
                sresaab3 = SphPCG(s3, s2, 0, s1, 0, 0, 0, 0)
                sresaab4 = SuaveAuxiliaryG(RF(10//10), s3, n1 + n2 - s2 - 1, n2z, RF(10//10), ρ, ρ, lim)[1]
                sresaab += sresaab1 * sresaab2 * sresaab3 * sresaab4
            end
        end
    end

    resaab = coeff * sresaab
    return resaab
end
###########################################################################################################
#function  TwoCenterNucAttractAAB(
#    n1::arb, l1::Int, m1::Int, ζ1::arb,
#    n2::arb, l2::Int, m2::Int, ζ2::arb,
#    R::arb, theta::arb, phi::arb,
#    x::Int, lim::Int
#    )

#    lowL = abs(l1-l2)
#    upL = l1 + l2
#    n2z = RF(0.000000000000000000000000000000000000000000000001)
#    ρ = (R//2)*(ζ1+ζ2)
#    τ = (ζ1-ζ2)//(ζ1+ζ2)

#    resaab = RF(0)
#    for s1 in lowL : 2 : upL
#        for s2 in -s1 : s1
#            resaab += ChargeDOneC(n1,l1,m1,ζ1,n2,l2,m2,ζ2,n1+n2-1,s1,s2) *
#            TwoCenterOverlap(n1+n2-1,s1,s2,n2z,0,0,ρ,τ,theta,phi,1,lim)
#        end
#    end

#    return resaab
#end

function  TwoCenterNucAttractAAB(
    n1::arb, l1::Int, m1::Int, ζ1::arb,
    n2::arb, l2::Int, m2::Int, ζ2::arb,
    R::arb, theta::arb, phi::arb, lim::Int
    )

    lowL = abs(l1-l2)
    upL = l1 + l2

    ρ = (R // 2) * (ζ1 + ζ2)
    τ = (ζ1 - ζ2) // (ζ1 + ζ2)

    resaab = RF(0)
    for s1 in lowL : 2 : upL
        for s2 in -s1 : s1
            resaab1 = Sqrt((4 * Pi(RF)) // (2 * s1 + 1))
            resaab += (
            GGauntG(l1, m1, l2, m2, s1, s2) * RSphCA(s2, m1, m2) *
            OneCenterP(s1, n1, n2, ζ1, ζ2, R) * resaab1 * SphericalHarmonicsS(s1, s2, theta, phi)
            )
        end
    end

    return resaab//2
end
###########################################################################################################
###########################################################################################################
###########################################################################################################
################################# TWO CENTER KINETIC ENERGY INTEGRALS #####################################(43)
################################################### ALIGNED MOLECULAR COORDINATE SYSTEM
###########################################################################################################
###########################################################################################################
###########################################################################################################
################################# TWO CENTER KINETIC ENERGY INTEGRALS #####################################(43)
################################################### ALIGNED MOLECULAR COORDINATE SYSTEM
#function  TwoCenterKinEnergy(
#    n1::arb, l1::Int, ζ1::arb,
#    n2::arb, l2::Int, ζ2::arb,
#    λ::Int, R::arb, lim::Int
#    )

#    ρ = (R // 2) * (ζ1 + ζ2)
#    τ = (ζ1 - ζ2) // (ζ1 + ζ2)

#    coeff1 = -Power(ζ2, 2) // 2
#    coeff2 = 4 * n2 * Sqrt(Gamma(2*n2 - 1) // Gamma(2*n2 + 1))

#    res = coeff1 * (
#        TwoCenterOverlap(n1, l1, ζ1, n2, l2, ζ2, λ, R, lim) -
#        coeff2 * TwoCenterOverlap(n1, l1, ζ1, n2 - 1, l2, ζ2, λ, R, lim) +
#     	if 2 * n2 - 3 < 0
#	0
#    else
#        coeff3 = 4* (n2 + l2) * (n2 - l2 - 1) * Sqrt(Gamma(2*n2 - 3) // Gamma(2*n2 + 1))
#        overlap3 = TwoCenterOverlap(n1, l1, ζ1, n2 - 2, l2, ζ2, λ, R, lim)
#        coeff3 * overlap3
#    end
#    )

#    return res

#end

function  TwoCenterKinEnergy(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Int
    )

    ρ = (R // 2) * (ζ1 + ζ2)
    τ = (ζ1 - ζ2) // (ζ1 + ζ2)

    coeff1 = -Power(ζ2, 2) // 2
    coeff2 = 4 * n2 * Sqrt(Gamma(2*n2 - 1) // Gamma(2*n2 + 1))

#    if 2 * n2 - 3 < 0 || l2 > n2 - 2
#        coeff3 = 0
#        overlap3 = 0
#    else
#        coeff3 = 4* (n2 + l2) * (n2 - l2 - 1) * Sqrt(Gamma(2*n2 - 3) // Gamma(2*n2 + 1))
#        overlap3 = TwoCenterOverlap(n1, l1, ζ1, n2 - 2, l2, ζ2, λ, R, lim)
#    end

    if 2 * n2 - 3 < 0
        coeff3 = 0
        overlap3 = 0
    else
        coeff3 = 4* (n2 + l2) * (n2 - l2 - 1) * Sqrt(Gamma(2*n2 - 3) // Gamma(2*n2 + 1))
        overlap3 = TwoCenterOverlap(n1, l1, ζ1, n2 - 2, l2, ζ2, λ, R, lim)
    end

    overlap1 = TwoCenterOverlap(n1, l1, ζ1, n2, l2, ζ2, λ, R, lim)
    overlap2 = TwoCenterOverlap(n1, l1, ζ1, n2 - 1, l2, ζ2, λ, R, lim)
    #if l2 > n2-1
    #    overlap2 = 0
    #else
    #    overlap2 = TwoCenterOverlap(n1, l1, ζ1, n2 - 1, l2, ζ2, λ, R, lim)
    #end

    res = coeff1 * (
        overlap1 - coeff2 * overlap2 + coeff3 * overlap3
    )

    return res

end

function  CTwoCenterKinEnergy(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64
    )

    ρ = (R // 2) * (ζ1 + ζ2)
    τ = (ζ1 - ζ2) // (ζ1 + ζ2)

    coeff1 = -Power(ζ2, 2) // 2
    coeff2 = 4 * n2 * Sqrt(Gamma(2*n2 - 1) // Gamma(2*n2 + 1))

#    if 2 * n2 - 3 < 0 || l2 > n2 - 2
#        coeff3 = 0
#        overlap3 = 0
#    else
#        coeff3 = 4* (n2 + l2) * (n2 - l2 - 1) * Sqrt(Gamma(2*n2 - 3) // Gamma(2*n2 + 1))
#        overlap3 = CTwoCenterOverlap(n1, l1, ζ1, n2 - 2, l2, ζ2, λ, R, lim)
#    end

    if 2 * n2 - 3 < 0
        coeff3 = 0
        overlap3 = 0
    else
        coeff3 = 4* (n2 + l2) * (n2 - l2 - 1) * Sqrt(Gamma(2*n2 - 3) // Gamma(2*n2 + 1))
        overlap3 = CTwoCenterOverlap(n1, l1, ζ1, n2 - 2, l2, ζ2, λ, R, lim)
    end

    overlap1 = CTwoCenterOverlap(n1, l1, ζ1, n2, l2, ζ2, λ, R, lim)
    overlap2 = CTwoCenterOverlap(n1, l1, ζ1, n2 - 1, l2, ζ2, λ, R, lim)
    #if l2 > n2-1
    #    overlap2 = 0
    #else
    #    overlap2 = CTwoCenterOverlap(n1, l1, ζ1, n2 - 1, l2, ζ2, λ, R, lim)
    #end

    res = coeff1 * (
        overlap1 - coeff2 * overlap2 + coeff3 * overlap3
    )

    return res

end

function  VTwoCenterKinEnergy(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64
    )

    ρ = (R // 2) * (ζ1 + ζ2)
    τ = (ζ1 - ζ2) // (ζ1 + ζ2)

    coeff1 = -Power(ζ2, 2) // 2
    coeff2 = 4 * n2 * Sqrt(Gamma(2*n2 - 1) // Gamma(2*n2 + 1))

    #    if 2 * n2 - 3 < 0 || l2 > n2 - 2
    #        coeff3 = 0
    #        overlap3 = 0
    #    else
    #        coeff3 = 4* (n2 + l2) * (n2 - l2 - 1) * Sqrt(Gamma(2*n2 - 3) // Gamma(2*n2 + 1))
    #        overlap3 = VTwoCenterOverlap(n1, l1, ζ1, n2 - 2, l2, ζ2, λ, R, lim)
    #    end

    if 2 * n2 - 3 < 0
        coeff3 = 0
        overlap3 = 0
    else
        coeff3 = 4* (n2 + l2) * (n2 - l2 - 1) * Sqrt(Gamma(2*n2 - 3) // Gamma(2*n2 + 1))
        overlap3 = VTwoCenterOverlap(n1, l1, ζ1, n2 - 2, l2, ζ2, λ, R, lim)
    end

    overlap1 = VTwoCenterOverlap(n1, l1, ζ1, n2, l2, ζ2, λ, R, lim)
    overlap2 = VTwoCenterOverlap(n1, l1, ζ1, n2 - 1, l2, ζ2, λ, R, lim)

    #if l2 > n2-1
    #    overlap2 = 0
    #else
    #    overlap2 = VTwoCenterOverlap(n1, l1, ζ1, n2 - 1, l2, ζ2, λ, R, lim)
    #end

    res = coeff1 * (
        overlap1 - coeff2 * overlap2 + coeff3 * overlap3
    )

    return res

end

function  STwoCenterKinEnergy(
    n1::arb, l1::Int, ζ1::arb,
    n2::arb, l2::Int, ζ2::arb,
    λ::Int, R::arb, lim::Float64
    )

    ρ = (R // 2) * (ζ1 + ζ2)
    τ = (ζ1 - ζ2) // (ζ1 + ζ2)

    coeff1 = -Power(ζ2, 2) // 2
    coeff2 = 4 * n2 * Sqrt(Gamma(2*n2 - 1) // Gamma(2*n2 + 1))

    #    if 2 * n2 - 3 < 0 || l2 > n2 - 2
    #        coeff3 = 0
    #        overlap3 = 0
    #    else
    #        coeff3 = 4* (n2 + l2) * (n2 - l2 - 1) * Sqrt(Gamma(2*n2 - 3) // Gamma(2*n2 + 1))
    #        overlap3 = STwoCenterOverlap(n1, l1, ζ1, n2 - 2, l2, ζ2, λ, R, lim)
    #    end

    if 2 * n2 - 3 < 0
        coeff3 = 0
        overlap3 = 0
    else
        coeff3 = 4* (n2 + l2) * (n2 - l2 - 1) * Sqrt(Gamma(2*n2 - 3) // Gamma(2*n2 + 1))
        overlap3 = STwoCenterOverlap(n1, l1, ζ1, n2 - 2, l2, ζ2, λ, R, lim)
    end

    overlap1 = STwoCenterOverlap(n1, l1, ζ1, n2, l2, ζ2, λ, R, lim)
    overlap2 = STwoCenterOverlap(n1, l1, ζ1, n2 - 1, l2, ζ2, λ, R, lim)

    #if l2 > n2-1
    #    overlap2 = 0
    #else
    #    overlap2 = STwoCenterOverlap(n1, l1, ζ1, n2 - 1, l2, ζ2, λ, R, lim)
    #end

    res = coeff1 * (
        overlap1 - coeff2 * overlap2 + coeff3 * overlap3
    )

    return res

end
###########################################################################################################
###########################################################################################################
###########################################################################################################
################################################### NON-ALIGNED MOLECULAR COORDINATE SYSTEM
function  TwoCenterKinEnergy(
    n1::arb, l1::Int, m1:: Int, ζ1::arb,
    n2::arb, l2::Int, m2::Int, ζ2::arb,
    R::arb, theta::arb, phi::arb, x::Int, lim::Int
    )

    NKinEnergy = RF(0)

    for s in 0 : min(l1,l2)
        NKinEnergy += (RotaT(s, l1, m1, l2, m2, theta, phi, x) *
        TwoCenterKinEnergy(n1, l1, ζ1, n2, l2, ζ2, s, R, lim))
    end
    NKinEnergy
end

function  CTwoCenterKinEnergy(
    n1::arb, l1::Int, m1:: Int, ζ1::arb,
    n2::arb, l2::Int, m2::Int, ζ2::arb,
    R::arb, theta::arb, phi::arb, x::Int, lim::Float64
    )

    NKinEnergy = RF(0)

    for s in 0 : min(l1,l2)
        NKinEnergy += (RotaT(s, l1, m1, l2, m2, theta, phi, x) *
        CTwoCenterKinEnergy(n1, l1, ζ1, n2, l2, ζ2, s, R, lim))
    end
    NKinEnergy
end

function  VTwoCenterKinEnergy(
    n1::arb, l1::Int, m1:: Int, ζ1::arb,
    n2::arb, l2::Int, m2::Int, ζ2::arb,
    R::arb, theta::arb, phi::arb, x::Int, lim::Float64
    )

    NKinEnergy = RF(0)

    for s in 0 : min(l1,l2)
        NKinEnergy += (RotaT(s, l1, m1, l2, m2, theta, phi, x) *
        VTwoCenterKinEnergy(n1, l1, ζ1, n2, l2, ζ2, s, R, lim))
    end
    NKinEnergy
end

function  STwoCenterKinEnergy(
    n1::arb, l1::Int, m1:: Int, ζ1::arb,
    n2::arb, l2::Int, m2::Int, ζ2::arb,
    R::arb, theta::arb, phi::arb, x::Int, lim::Float64
    )

    NKinEnergy = RF(0)

    for s in 0 : min(l1,l2)
        NKinEnergy += (RotaT(s, l1, m1, l2, m2, theta, phi, x) *
        STwoCenterKinEnergy(n1, l1, ζ1, n2, l2, ζ2, s, R, lim))
    end
    NKinEnergy
end


function Legendre(n,x)
    if n == 0
        1
    elseif n == 1
        x
    elseif n == 2
        (1/2)*(3x^2-1)
    elseif n == 3
        (1/2)*(5x^3-3x)
    elseif n == 4
        (1/8)*(35x^4-30x^2 + 3)
    elseif n == 5
        (1/8)*(63x^5 - 70x^3 + 15)
    elseif n == 6
        (1/16)*(231x^6 - 315x^4 + 105x^2 - 5)
    elseif n == 7
        (1/16)*(-35x + 315x^3 - 693x^5 + 429x^7)
    elseif n == 8
        (1/128)*(35 - 1260x^2 + 6930x^4 - 12012x^6 + 6435x^8)
    end
end


# by lazarusA # HIDE
# using GLMakie # HIDE
using CairoMakie
CairoMakie.activate!() # HIDE
let
    x = LinRange(0,2π,50)
    fig = Figure(resolution = (700, 450), font =:sans)
    ax = Axis(fig, xlabel = "x", ylabel = "")
    line1 = lines!(x, sin.(x), color = :black)
    pnts1 = scatter!(x, sin.(x), color = :black, marker = '◆', markersize = 10)
    line2 = lines!(x, cos.(x), color = :black)
    pnts2 = scatter!(x, cos.(x), color = :red, strokecolor = :red, marker = '■',
                    markersize = 10)
    leg = Legend(fig, [[line1, pnts1], [line2, pnts2]], ["sin(x)", "cos(x)"],
        halign = :center, valign = :center, markersize = 7,tellheight = false,
        tellwidth = false, bgcolor = :white, framecolor=:white)
    fig[1, 1] = ax
    fig[1, 1] = leg
    fig
    #save("FigLineScatter.png", fig, px_per_unit = 2)
    save(joinpath(@__DIR__, "output", "FigLinesScatters.png"), fig, px_per_unit = 2) # HIDE
end
using Pkg # HIDE
Pkg.status("CairoMakie") # HIDE


module TypesTests

using TensorValues
using Test
using StaticArrays

# Constructors (MultiValue)

a = MArray{Tuple{3,2}}((1,2,3,4,5,6))
v = MultiValue(a)
@test isa(v,MultiValue{Tuple{3,2},Int})
@test v.array.data === a.data

a = SArray{Tuple{3,2}}((1,2,3,4,5,6))

v = MultiValue(a)
@test isa(v,MultiValue{Tuple{3,2},Int})
@test v.array === a

v = MultiValue{Tuple{3,2}}((1,2,3,4,5,6))
@test isa(v,MultiValue{Tuple{3,2},Int})
@test v.array == a

v = MultiValue{Tuple{3,2}}(1,2,3,4,5,6)
@test isa(v,MultiValue{Tuple{3,2},Int})
@test v.array == a

v = MultiValue{Tuple{3,2},Float64}((1,2,3,4,5,6))
@test isa(v,MultiValue{Tuple{3,2},Float64})
@test v.array == a

v = MultiValue{Tuple{3,2},Float64}(1,2,3,4,5,6)
@test isa(v,MultiValue{Tuple{3,2},Float64})
@test v.array == a

a = SVector(1)
v = MultiValue{Tuple{1}}((1,))
@test isa(v,MultiValue{Tuple{1},Int})
@test v.array == a

v = MultiValue{Tuple{1}}(1)
@test isa(v,MultiValue{Tuple{1},Int})
@test v.array == a

a = SMatrix{1,1}(1)
v = MultiValue{Tuple{1,1}}(1)
@test isa(v,MultiValue{Tuple{1,1},Int})
@test v.array == a

a = SVector{0,Int}()
v = MultiValue{Tuple{0},Int}(())
@test isa(v,MultiValue{Tuple{0},Int})
@test v.array == a

a = SMatrix{0,0,Int}()
v = MultiValue{Tuple{0,0},Int}()
@test isa(v,MultiValue{Tuple{0,0},Int})
@test v.array == a

# Constructors (TensorValue)

a = SMatrix{2,2}(1,2,3,4)
t = TensorValue(a)
@test isa(t,TensorValue{2,Int})
@test t.array == [1 3;2 4]

a = MMatrix{2,2}(1,2,3,4)
t = TensorValue(a)
@test isa(t,TensorValue{2,Int})
@test t.array == [1 3;2 4]

t = TensorValue{2}((1,2,3,4))
@test isa(t,TensorValue{2,Int})
@test t.array == [1 3;2 4]

t = TensorValue{2,Float64}((1,2,3,4))
@test isa(t,TensorValue{2,Float64})
@test t.array == [1 3;2 4]

t = TensorValue{2}(1,2,3,4)
@test isa(t,TensorValue{2,Int})
@test t.array == [1 3;2 4]

t = TensorValue{2,Float64}(1,2,3,4)
@test isa(t,TensorValue{2,Float64})
@test t.array == [1 3;2 4]

t = TensorValue(1,2,3,4)
@test isa(t,TensorValue{2,Int})
@test t.array == [1 3;2 4]

t = TensorValue((1,2,3,4))
@test isa(t,TensorValue{2,Int})
@test t.array == [1 3;2 4]

t = TensorValue{0,Int}()
@test isa(t,TensorValue{0,Int})
@test t.array == zeros(0,0)

t = TensorValue{1}(10)
@test isa(t,TensorValue{1,Int})
@test t.array == 10*ones(1,1)

t = TensorValue{1}((10,))
@test isa(t,TensorValue{1,Int})
@test t.array == 10*ones(1,1)

t = TensorValue{1,Float64}(10)
@test isa(t,TensorValue{1,Float64})
@test t.array == 10*ones(1,1)

t = TensorValue{1,Float64}((10,))
@test isa(t,TensorValue{1,Float64})
@test t.array == 10*ones(1,1)

# Constructors (VectorValue)

a = SVector(1)
g = VectorValue(a)
@test isa(g,VectorValue{1,Int})
@test g.array == [1,]

a = SVector(1,2,3,4)
g = VectorValue(a)
@test isa(g,VectorValue{4,Int})
@test g.array == [1,2,3,4]

a = MVector(1,2,3,4)
g = VectorValue(a)
@test isa(g,VectorValue{4,Int})
@test g.array == [1,2,3,4]

g = VectorValue{4}((1,2,3,4))
@test isa(g,VectorValue{4,Int})
@test g.array == [1,2,3,4]

g = VectorValue{1}((1,))
@test isa(g,VectorValue{1,Int})
@test g.array == [1,]

g = VectorValue{0,Int}(())
@test isa(g,VectorValue{0,Int})
@test g.array == []

g = VectorValue{4}(1,2,3,4)
@test isa(g,VectorValue{4,Int})
@test g.array == [1,2,3,4]

g = VectorValue{1}(1)
@test isa(g,VectorValue{1,Int})
@test g.array == [1,]

g = VectorValue{1,Float64}(1)
@test isa(g,VectorValue{1,Float64})
@test g.array == [1,]

g = VectorValue{1,Float64}((1,))
@test isa(g,VectorValue{1,Float64})
@test g.array == [1,]

g = VectorValue{0,Int}()
@test isa(g,VectorValue{0,Int})
@test g.array == []

g = VectorValue{4,Float64}((1,2,3,4))
@test isa(g,VectorValue{4,Float64})
@test g.array == [1,2,3,4]

g = VectorValue{4}(1,2,3,4)
@test isa(g,VectorValue{4,Int})
@test g.array == [1,2,3,4]

g = VectorValue{4,Float64}(1,2,3,4)
@test isa(g,VectorValue{4,Float64})
@test g.array == [1,2,3,4]

g = VectorValue(1,2,3,4)
@test isa(g,VectorValue{4,Int})
@test g.array == [1,2,3,4]

g = VectorValue((1,2,3,4))
@test isa(g,VectorValue{4,Int})
@test g.array == [1,2,3,4]

g = VectorValue(1)
@test isa(g,VectorValue{1,Int})
@test g.array == [1,]

# Initializers

z = zero(MultiValue{Tuple{3,2},Int,2,6})
@test isa(z,MultiValue{Tuple{3,2},Int,2,6})
@test z.array == zeros(Int,(3,2))
s = zero(z)
@test s.array == zeros(Int,(3,2))

z = zero(TensorValue{3,Int,9})
@test isa(z,TensorValue{3,Int,9})
@test z.array == zeros(Int,(3,3))

z = zero(VectorValue{3,Int})
@test isa(z,VectorValue{3,Int})
@test z.array == zeros(Int,3)

z = one(TensorValue{3,Int,9})
@test isa(z,TensorValue{3,Int,9})
@test z.array == [1 0 0; 0 1 0; 0 0 1]
s = one(z)
@test s.array == [1 0 0; 0 1 0; 0 0 1]

# Conversions

a = @SVector ones(Int,3)
b = convert(VectorValue{3,Int},a)
@test isa(b,VectorValue{3,Int})

a = ones(Int,3)
b = convert(VectorValue{3,Int},a)
@test isa(b,VectorValue{3,Int})

a = ones(Int,1)
b = convert(VectorValue{1,Int},a)
@test isa(b,VectorValue{1,Int})

a = (1,2,2,1,3,2)
V = MultiValue{Tuple{3,2},Int,2,6}
b = convert(V,a)
@test isa(b,V)
b = V[a,a,a,]
@test isa(b,Vector{V})

# Misc operations on the type itself

V = VectorValue{3,Int}
@test length(V) == 3
@test size(V) == (3,)

V = VectorValue{3}
@test length(V) == 3
@test size(V) == (3,)

# Custom type printing

v = MultiValue{Tuple{3,2},Float64}(1,2,3,4,5,6)
s = "(1.0, 2.0, 3.0, 4.0, 5.0, 6.0)"
@test string(v) == s

# Misc

M = mutable(VectorValue{3,Int})
@test M == MVector{3,Int}
m = zero(M)
v = VectorValue(m)
@test isa(v,VectorValue{3,Int})

end # module TypesTests


using OrdinaryDiffEq, DiffEqProblemLibrary,Base.Test, DiffEqBase

bools = Vector{Bool}(0)
prob = prob_ode_linear

sol =solve(prob,Euler(),dt=1//2^(2),dense=true)

interpd_1d = sol(0:1//2^(4):1)

sol2 =solve(prob,Euler(),dt=1//2^(4),dense=true)

sol3 =solve(prob,Euler(),dt=1//2^(5),dense=true)

prob = prob_ode_2Dlinear
sol =solve(prob,Euler(),dt=1//2^(2),dense=true)

interpd = sol(0:1//2^(4):1)

interpd_idxs = sol(0:1//2^(4):1,idxs=1:2:5)

@test minimum([interpd_idxs[i] == interpd[i][1:2:5] for i in 1:length(interpd)])

interpd_single = sol(0:1//2^(4):1,idxs=1)

@test typeof(interpd_single) <: Vector{Float64}

@test typeof(sol(0.5,idxs=1)) <: Float64

A = rand(4,2)
sol(A,0.777)
A == sol(0.777)
sol2 =solve(prob,Euler(),dt=1//2^(4),dense=true)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < .2

sol(interpd,0:1//2^(4):1)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < .2

sol =solve(prob,Euler(),dt=1//2^(2),dense=false)

@test_throws BoundsError sol(0.5)

prob = prob_ode_linear

sol =solve(prob,Midpoint(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,Midpoint(),dt=1//2^(4),dense=true)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 2e-2

prob = prob_ode_2Dlinear

sol =solve(prob,Midpoint(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,Midpoint(),dt=1//2^(4),dense=true)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 2.3e-2

prob = prob_ode_linear

sol =solve(prob,RK4(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,RK4(),dt=1//2^(4),dense=true)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 5e-5

prob = prob_ode_2Dlinear

sol =solve(prob,RK4(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,RK4(),dt=1//2^(4),dense=true)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 1e-4

prob = prob_ode_linear

sol =solve(prob,DP5(),dense=true)

sol2 =solve(prob,DP5(),dt=1//2^(4),dense=true,adaptive=false)

sol(interpd_1d,0:1//2^(4):1)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 5e-6

prob = prob_ode_2Dlinear

sol =solve(prob,DP5(),dense=true)

sol2 =solve(prob,DP5(),dt=1//2^(4),dense=true,adaptive=false)

sol(interpd,0:1//2^(4):1)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 1e-5

prob = prob_ode_linear

sol =solve(prob,BS3(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,BS3(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 1e-3

prob = prob_ode_2Dlinear

sol =solve(prob,BS3(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,BS3(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 1e-3

prob = prob_ode_linear

sol =solve(prob,Tsit5(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,Tsit5(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 1e-5

prob = prob_ode_2Dlinear

sol =solve(prob,Tsit5(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,Tsit5(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 1e-5

prob = prob_ode_linear

sol =solve(prob,TanYam7(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,TanYam7(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 1e-3

prob = prob_ode_2Dlinear

sol =solve(prob,TanYam7(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,TanYam7(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 1e-3

prob = prob_ode_linear

sol =solve(prob,TsitPap8(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,TsitPap8(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 2e-2

prob = prob_ode_2Dlinear

sol =solve(prob,TsitPap8(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,TsitPap8(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 1e-2

prob = prob_ode_linear

sol =solve(prob,Feagin10(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,Feagin10(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 1e-3

prob = prob_ode_2Dlinear

sol =solve(prob,Feagin10(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,Feagin10(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 1e-3

### Vern6()
const linear_bigα = parse(BigFloat,"1.01")
f = (t,u) -> (linear_bigα*u)
prob_ode_bigfloatlinear = ODEProblem(f,parse(BigFloat,"0.5"),(0.0,1.0))
prob = prob_ode_bigfloatlinear

sol =solve(prob,Vern6(),dt=1//2^(2),dense=true)

interpd_1d_big = sol(0:1//2^(7):1)

sol2 =solve(prob,Vern6(),dt=1//2^(7),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d_big)) < 1e-7

#plot(sol2.t,interpd)
#plot!(sol.t,sol[:])
#scatter!(sol.t,sol[:])

prob_ode_bigfloatveclinear = ODEProblem(f,[parse(BigFloat,"0.5")],(0.0,1.0))
prob = prob_ode_bigfloatveclinear

sol =solve(prob,Vern6(),dt=1//2^(2),dense=true)

interpd_big = sol(0:1//2^(4):1)

sol2 =solve(prob,Vern6(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_big)) < 1e-7

### BS5()

prob = prob_ode_linear

sol =solve(prob,BS5(),dt=1//2^(1),dense=true,adaptive=false)

interpd_1d_long = sol(0:1//2^(7):1)

sol2 =solve(prob,BS5(),dt=1//2^(7),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d_long)) < 2e-7

# plot(sol2.t,interpd)

prob = prob_ode_2Dlinear

sol =solve(prob,BS5(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,BS5(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 2e-7

### Vern7()

prob = prob_ode_linear

sol =solve(prob,Vern7(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,Vern7(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 3e-9

prob = prob_ode_2Dlinear

sol =solve(prob,Vern7(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,Vern7(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 5e-9

### Vern8()

prob = prob_ode_linear

sol =solve(prob,Vern8(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,Vern8(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 1e-7

# plot(sol2.t,interpd)

prob = prob_ode_2Dlinear

sol =solve(prob,Vern8(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,Vern8(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 1e-7

### Vern9()

prob = prob_ode_linear

sol =solve(prob,Vern9(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,Vern9(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 1e-9

# plot(sol2.t,interpd)

prob = prob_ode_2Dlinear

sol =solve(prob,Vern9(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,Vern9(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 2e-9

### Rosenbrock23()

prob = prob_ode_linear

sol =solve(prob,Rosenbrock23(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,Rosenbrock23(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 3e-3

# plot(sol2.t,interpd)

prob = prob_ode_2Dlinear

sol =solve(prob,Rosenbrock23(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,Rosenbrock23(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 6e-3

### Rosenbrock32()

prob = prob_ode_linear

sol =solve(prob,Rosenbrock32(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,Rosenbrock32(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 3e-4

# plot(sol2.t,interpd)

prob = prob_ode_2Dlinear

sol =solve(prob,Rosenbrock32(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,Rosenbrock32(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 6e-4

### ImplicitEuler()

prob = prob_ode_linear

sol =solve(prob,ImplicitEuler(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,ImplicitEuler(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 0.2

# plot(sol2.t,interpd)

prob = prob_ode_2Dlinear

sol =solve(prob,ImplicitEuler(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,ImplicitEuler(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < .354

### Trapezoid()

prob = prob_ode_linear

sol =solve(prob,Trapezoid(),dt=1//2^(2),dense=true)

sol(interpd_1d,0:1//2^(4):1)

sol2 =solve(prob,Trapezoid(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d)) < 7e-3

# plot(sol2.t,interpd)

prob = prob_ode_2Dlinear

sol =solve(prob,Trapezoid(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,Trapezoid(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 1.4e-2

### DP8()

prob = prob_ode_linear

sol =solve(prob,DP8(),dt=1//2^(2),dense=true)

sol(interpd_1d_long,0:1//2^(7):1)

sol2 =solve(prob,DP8(),dt=1//2^(7),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d_long)) < 2e-7

prob = prob_ode_2Dlinear

sol =solve(prob,DP8(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,DP8(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 2.01e-7

### ExplicitRK()

prob = prob_ode_linear

sol =solve(prob,ExplicitRK(),dt=1//2^(2),dense=true)

sol(interpd_1d_long,0:1//2^(7):1)

sol2 =solve(prob,ExplicitRK(),dt=1//2^(7),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd_1d_long)) < 6e-5

prob = prob_ode_2Dlinear

sol =solve(prob,ExplicitRK(),dt=1//2^(2),dense=true)

sol(interpd,0:1//2^(4):1)

sol2 =solve(prob,ExplicitRK(),dt=1//2^(4),dense=true,adaptive=false)

@test maximum(map((x)->maximum(abs.(x)),sol2[:] - interpd)) < 1e-3


# This file is pat of RLinearAlgebra.jl
# 1. Specifies type
# 2. Implements sample function
# 3. Exports Type

# using Random

"""
    LinSysVecRowRandCyclic <: LinSysVecRowSelect

A mutable structure with a field to store a cycling order. Randomly specifies a cycling
order the equations of a linear system. Once this ordering is exhausted by the solver,
a new random ordering is specified. This process is repeated

# Fields
- `order::Union{Vector{Int64},Nothing}`

Calling `LinSysVecRowOneRandCyclic()` defaults to setting `order` to `nothing`. The `sample`
function will handle the re-initialization of the fields once the system is provided.
"""
mutable struct LinSysVecRowRandCyclic <: LinSysVecRowSelect
    order::Union{Vector{Int64},Nothing}
end
LinSysVecRowRandCyclic() = LinSysVecRowRandCyclic(nothing)

# Common sample interface for linear systems
function sample(
    type::LinSysVecRowRandCyclic,
    A::AbstractArray,
    b::AbstractVector,
    x::AbstractVector,
    iter::Int64
)

    mod_ind = mod(iter, 1:length(b))
    if mod_ind == 1
        type.order = randperm(length(b))
    end

    eqn_ind = type.order[mod_ind]
    return A[eqn_ind, :], b[eqn_ind]
end

#export LinSysVecRowRandCyclic


using Winston

x = linspace(0., 2pi, 30)
y = sin(x)

p = FramedArray(2, 2,
        title="title",
        aspect_ratio=0.75,
        xlabel="x label",
        ylabel="y label",
        uniform_limits=true,
        cellspacing=1.)

add(p, LineY(0, kind="dot"))

add(p[1,1], Curve(x, .25*y))
add(p[1,2], Curve(x, .50*y))
add(p[2,1], Curve(x, .75*y))
add(p[2,2], Curve(x, y))

file(p, "example5.png")


# ---
# title: 977. Squares of a Sorted Array
# id: problem977
# author: zhwang
# date: 2022-01-19
# difficulty: Easy
# categories: Array, Two Pointers
# link: <https://leetcode.com/problems/squares-of-a-sorted-array/description/>
# hidden: true
# ---
#
# Given an integer array `nums` sorted in **non-decreasing** order, return _an
# array of **the squares of each number**  sorted in non-decreasing order_.
#
#
#
# **Example 1:**
#
#
#
#     Input: nums = [-4,-1,0,3,10]
#     Output: [0,1,9,16,100]
#     Explanation: After squaring, the array becomes [16,1,0,9,100].
#     After sorting, it becomes [0,1,9,16,100].
#
#
# **Example 2:**
#
#
#
#     Input: nums = [-7,-3,2,3,11]
#     Output: [4,9,9,49,121]
#
#
#
#
# **Constraints:**
#
#   * `1 <= nums.length <= 104`
#   * `-104 <= nums[i] <= 104`
#   * `nums` is sorted in **non-decreasing** order.
#
#
## @lc code=start
using LeetCode

## binary search + merge sort
function squares_of_a_sorted_array(nums::Vector{Int})::Vector{Int}
    n = length(nums)
    ## binary search
    left, right = 1, n
    while left <= right
        mid = (right + left) >> 1
        if nums[mid] < 0
            left = mid + 1
        else
            right = mid - 1
        end
    end
    ## merge sort
    i, j, res = right, left, Int[]
    while i >= 1 && j <= n
        if nums[i]^2 < nums[j]^2
            push!(res, nums[i]^2)
            i -= 1
        else
            push!(res, nums[j]^2)
            j += 1
        end
    end
    return vcat(res, (i == 0) ? nums[j:end] .^ 2 : nums[i:-1:1] .^ 2)
end

## @lc code=end


#=
This model is taken from [1]. The theoretical derivations of the analytic solution can be found in [2].

[1] Malakiyeh, Mohammad Mahdi, Saeed Shojaee, and Klaus-Jürgen Bathe. "The Bathe time integration method revisited for prescribing desired numerical dissipation." Computers & Structures 212 (2019): 289-298.

[2] Mechanical Vibrations, Gerardin et al, page 250-251.
=#

using ReachabilityAnalysis, LinearAlgebra, LazySets, SparseArrays
using StructuralDynamicsODESolvers
using LazySets.Arrays

include("../krylov_concept.jl")
using FastExpm

LazySets.set_ztol(Float64, 1e-15)
LazySets.set_atol(Float64, 1e-15)
LazySets.set_rtol(Float64, 1e-15)

function clamped_bar(; N=1000, E=30e6, ρ=7.3e-4, A=1, L=200)
    ℓ = L / N
    K = (E * A / ℓ) * SymTridiagonal(fill(2.0, N), fill(-1.0, N))
    K[end, end] = E * A / ℓ

    M = (ρ * A * ℓ / 2) * Diagonal(vcat(fill(2.0, N-1), 1.0))
    M[end, end] = ρ * A * ℓ / 2

    return M, K
end

"""
    clamped_free(; N)

Free (no forcing term) instance of the clamped bar.
"""
function clamped_free(; N)
    M, K = clamped_bar(N=N)
    C = spzeros(N, N) # no damping
    sys = SecondOrderLinearContinuousSystem(M, C, K)
end

"""
    clamped_forced(; N, F=10e3, E=30e6, A=1)

Forced (constant force `F`) instance of the clamped bar.
"""
function clamped_forced(; N, F=10e3, E=30e6, A=1)
    M, K = clamped_bar(N=N)
    C = spzeros(N, N) # no damping
    F = vcat(zeros(N-1), F) # the right-most node is excited
    sys = SecondOrderAffineContinuousSystem(M, C, K, F)
end

# "nominal" step size
const dtn = 9.88e-7

# problem instances
s5 = clamped_forced(N=5);
s10 = clamped_forced(N=10);
s20 = clamped_forced(N=20);
s30 = clamped_forced(N=30);
s50 = clamped_forced(N=50);
s100 = clamped_forced(N=100);
s200 = clamped_forced(N=200);
s500 = clamped_forced(N=500);
s1000 = clamped_forced(N=1000);

# box initial condition
function prob_box(s, e)
    two_n = 2statedim(s)
    return InitialValueProblem(s, X0(two_n) + BallInf(two_n, e))
end

# solve with GLGM06
function solve_glg(s; δ=dtn, NSTEPS=10, model=Forward())
    # singleton initial condition
    prob = InitialValueProblem(s, Singleton(zeros(2statedim(s))))
    alg = GLGM06(δ=δ, approx_model=model)
    sol = solve(ivp, NSTEPS=NSTEPS, alg=alg, homogeneize=true)
end

# solve with LGG09 for nodeidx
function solve_lgg(s; δ=dtn, NSTEPS=100, nodeidx, model=Forward())
    # singleton initial condition
    prob = InitialValueProblem(s, Singleton(zeros(2statedim(s))))

    n = statedim(s)
    eplus = SingleEntryVector(nodeidx, 2n+1, 1.0)
    eminus = SingleEntryVector(nodeidx, 2n+1, -1.0)

    dirs = [eminus, eplus]

    alg = LGG09(δ=δ, template=dirs, approx_model=model)
    sol = solve(ivp, NSTEPS=NSTEPS, alg=alg, homogeneize=true)
end

# solve with BOX
# usage: sol = solve_box(s10, NSTEPS=100)
function solve_box(s; δ=dtn, NSTEPS=100, model=Forward())

    # ph = homogeneize(normalize(p))
    # @time pd = discretize(ph, dt, model) # ~35 sec for 1000 nodes

    alg = BOX(δ=δ, approx_model=model)

    # singleton initial condition
    prob = InitialValueProblem(s, Singleton(zeros(2statedim(s))))

    sbox = solve(p, alg=alg, NSTEPS=NSTEPS, homogeneize=true)
    return sbox
end

# Ec. (4.181) libro Geradin & Rixen (2015)
function _sol_analitica_clamped(x, t, Ncorte)

    F = 10E3
    E = 30E6
    dens = 7.3E-4
    L = 200
    A = 1

    # reescalar
    nMasses = 1000
    x = x*L/nMasses

    m = A*dens
    c = sqrt(E*A/m)

    α = 8*F*L/π^2/E/A

    β = π/2/L
    u = sin.( β * x ) * ( 1.0 .- cos.( β * c * t ) )
    v = sin.( β * x ) * β * c * sin.( β * c * t )

    for s in 2:Ncorte
        u .+= (-1)^(s-1)/(2*s-1)^2 * sin.( (2*s-1)* β * x ) * ( 1.0 .- cos.( (2*s-1)* β * c * t ) )
        v .+= (-1)^(s-1)/(2*s-1)^2 * sin.( (2*s-1)* β * x ) * ( (2*s-1)* β * c * sin.( (2*s-1)* β * c * t ) )
    end

    return u*α, v*α

end

function _sol_reachability_clamped(p; δt=dtn, NSTEPS=NSTEPS,
                                      nodeidx=[PosTwoThirds, VelTwoThirds],
                                      mk=150, tol=1e-12)

    # ==============================
    # Homogeneize
    # ==============================
    m = size(p.K, 1)
    Shom = _homogeneize(p)
    Aext = Shom.A
    @assert size(Aext, 1) == 2m+1

    # la condicion inicial es nula y se agrega la fuerza al final
    X0 = Vector{Float64}(undef, 2m+1)
    X0 = zeros(2m+1)
    X0[end] = 1.0

    # ==============================
    # Propagacion a tiempo continuo
    # ==============================
    #Aᵀδ = copy(transpose(Aext .* δt))
    num_nodes = isa(nodeidx, Int) ? 1 : length(nodeidx)

    #println("Calculando factor de bloating...")

    Eplus = _build_Eplus_optim(Aext, X0, δt; m=mk, tol=tol, cutoff=eps(Float64))

    #println("Propagando a tiempo continuo...")
    _resolver_clamped_expm(Aext, X0, m, δt, mk, tol, Eplus, num_nodes, NSTEPS, nodeidx)
end

function _resolver_clamped_expm(Aext, X0, m, δt, mk, tol, Eplus, num_nodes, NSTEPS, nodeidx)
    fastexpm = true

    if fastexpm
        Φᵀ = real.( fastExpm(transpose(Aext) .* δt, threshold=eps(Float64), nonzero_tol=eps(Float64)) )
    else
        Φᵀ = exp(Matrix(transpose(Aext) .* δt))
        Φᵀ = sparse(Φᵀ)
        droptol!(Φᵀ, eps(Float64))
    end

    q0 = transpose(transpose(X0) * Φᵀ) # para no calcular Φ y Φ^T

    # build Ω0 plus
    q0_msum_Eplus = Hyperrectangle(q0, Eplus.radius) # q0 ⊕ Eplus
    Ω0 = CH(Singleton(X0), q0_msum_Eplus)

    step_intersect = true
    if step_intersect
        Eplusstep = _build_Eplus_optim(Aext, q0, δt; m=mk, tol=tol, cutoff=eps(Float64))
        Ω0minus = CH(Singleton(q0), Singleton(X0) ⊕ Eplusstep)
        Ω0 = Ω0 ∩ Ω0minus
    end

    # Ω0box = box_approximation(Ω0, Hyperrectangle)

    Mout = Matrix{Float64}(undef, 2*num_nodes, NSTEPS)

    d = zeros(2m+1)
    @inbounds for j in 1:num_nodes
        node = nodeidx[j]
        d[node] = 1.0
    #    println("Propagando indice $node ...")
        idx = (2(j-1) + 1):2j
        reach_homog_dir_LGG09_modif!(view(Mout, idx, :), Ω0, Φᵀ, d, NSTEPS)
        d[node] = 0.0
    end
    δt0 = interval(0, δt)
    return Mout
end

function reach_homog_dir_LGG09_modif!(Mout, Ω₀, Φᵀ, d, NSTEPS)
    rᵢ = copy(d)
    rᵢ₊₁ = similar(rᵢ)

    @inbounds for i in 1:NSTEPS
        Mout[1, i] = ρ(rᵢ, Ω₀)
        Mout[2, i] = ρ(-rᵢ, Ω₀)

        # update cache for the next iteration
        mul!(rᵢ₊₁, Φᵀ, rᵢ)
        copy!(rᵢ, rᵢ₊₁)
    end
    return nothing
end


function makealltrans(N,n,Ω,basis="Hermite")
dim=length(N)
if dim==1
  Nx = N[1]
  ωx = Ω[1]
  #n-field transforms for PGPE
  x,wx,Tx = nfieldtrans(Nx,n,ω=ωx,basis=basis)
  return x,wx,Tx
elseif dim==2
  Nx,Ny = N
  ωx,ωy = Ω
  #n-field transforms for PGPE
  x,wx,Tx = nfieldtrans(Nx,n,ω=ωx,basis=basis)
  y,wy,Ty = nfieldtrans(Ny,n,ω=ωy,basis=basis)
  return x,wx,Tx,y,wy,Ty
elseif dim==3
  Nx,Ny,Nz = N
  ωx,ωy,ωz = Ω
  #n-field transforms for PGPE
  x,wx,Tx = nfieldtrans(Nx,n,ω=ωx,basis=basis)
  y,wy,Ty = nfieldtrans(Ny,n,ω=ωy,basis=basis)
  z,wz,Tz = nfieldtrans(Nz,n,ω=ωz,basis=basis)
  return x,wx,Tx,y,wy,Ty,z,wz,Tz
end
end


using Printf

function middle3(n::Integer)
    l = ndigits(n)
    iseven(l) && error("n must have an odd number of digits")
    l < 3 && error("n must have 3 digits or more")
    mid = (l + 1) ÷ 2
    abs(n) ÷ 10^(mid-2) % 10^3
end

for n = [123, 12345, 1234567, 987654321, 10001, -10001, -123,
         -100, 100, -12345, 1, 2, -1, -10, 2002, -2002, 0]
    @printf("%10d -> %s\n", n, try middle3(n) catch e e.msg end)
end


using StructuredSolvers
using Test
using Random

@testset "StructuredSolvers.jl" begin
    @test 1 == 1
    # Write your tests here.

    @testset "Conjugate gradient" begin
        n = 5

        M = l1Manifold(ones(n))
        x = zeros(n)

        Random.seed!(1435)
        A = rand(n, n)
        A += A'
        A += Diagonal((1-minimum(eigvals(A))) .* ones(n))
        b = A * ones(n)

        Aoperator(η) = A * η

        ## Solves A * η + b = 0
        d, j, d_type = StructuredSolvers.solve_tCG(M, x, b, Aoperator; ν=1e-13, ϵ_residual = 1e-13, maxiter=1e5, safe_projection=false, printlev=0)

        @test isapprox(d, -ones(n))
        @test d_type == :Solved
    end

end


function sliwak_heuristic(clique::AbstractVector, edge; merge_kwargs)
    clique1 = clique[edge[1]]
    clique2 = clique[edge[2]]
    size_intersect = length(intersect(clique1, clique2))
    size_clique1 = length(clique1)
    size_clique2 = length(clique2)
    size_fusion_clique = size_clique1 + size_clique2 - size_intersect
    nb_lc_new = merge_kwargs["nb_lc"] - size_intersect * (2 * size_intersect + 1)
    q2 = (merge_kwargs["m"] + nb_lc_new)^3
    cost = merge_kwargs["alpha"] * (- size_clique1^3 - size_clique2^3 + size_fusion_clique^3)
           + merge_kwargs["beta"] * q2
    return cost
end

function update_sliwak(;clique, edge, merge_kwargs)
    size_intersect = length(intersect(clique[edge[1]], clique[edge[2]]))
    merge_kwargs["nb_lc"] -= size_intersect * (2 * size_intersect + 1)
end


using JuMP
using CPLEX
include("parser.jl")

export Static

function Static(fileName :: String, silent=false)
	cout_ouverture, Fct_commod, func_cost, func_capacity, nb_nodes, nb_arcs, nb_commodities, latency, node_capacity, commodity, nb_func, exclusion = read_instance(fileName)
	m = Model(CPLEX.Optimizer)
	if silent
		set_silent(m)
	end
	# Var
	@variable(m, x_i[1:nb_nodes], Bin) #activation d'une fonction au sommet i

	@variable(m, x_fi[1:nb_func,1:nb_nodes], Int) #activation de la fonction f au sommet i

	@variable(m, x_ikf[1:nb_nodes,1:nb_commodities,1:nb_func], Bin) #activation de la fonction f au sommet i pour le traitement de la commodité k

	@variable(m, e[1:nb_nodes,1:nb_nodes,1:nb_commodities,1:nb_func+1], Bin) #passage par l'arc (i,j) par la commodité k lpour traitement par la fonction f, ou en direction du puit si on considère e[i,j,k,end]

	#Constraint on states
	for i in 1:nb_nodes
		@constraint(m,[f in 1:nb_func],x_fi[f,i]*func_capacity[f] >= sum(x_ikf[i,k,f]*commodity[k,3] for k in 1:nb_commodities), base_name = "n_"*string(i)) #Nombre de fonctions f à placer en i
		@constraint(m,sum(x_fi[f,i] for f in 1:nb_func) <= node_capacity[i], base_name = "cap_"*string(i) ) #capacité de noeud
		for k in 1:nb_commodities
			@constraint(m,[f in 1:nb_func],x_i[i] >= x_ikf[i,k,f], base_name = "ouverture_"*string(i)) #ouverture du noeud en i
			if size(exclusion[k,:])[1]>0
				@constraint(m,sum(x_ikf[i,k,w] for w in exclusion[k,:] if w!=0) <= 1, base_name = "exclusion_"*string(k)*"_"*string(i)) #exclusion
			end
			for j in 1:nb_nodes
				if latency[i,j] == 0
					@constraint(m,[f in 1:nb_func+1],e[i,j,k,f]==0, base_name = "no_arc_"*string(i)*"_"*string(j)) #absence d'arcs
				end
			end
		end
	end

	#Contrainte de flots successifs pour chaque commodité
	for k in 1:nb_commodities
		size_fk=length( findall( y -> y > 0, Fct_commod[k,:]))

		tab_fk=sortperm( Fct_commod[k,:])[1:size_fk]

		@constraint(m,sum(e[commodity[k,1],j,k,tab_fk[1]] - e[j,commodity[k,1],k,tab_fk[1]] for j in 1:nb_nodes)-x_ikf[commodity[k,1],k,tab_fk[1]]==1, base_name = "init_flot_"*string(k)) #initialisation du flot à la source
		@constraint(m,[i in 1:nb_nodes ; i!=commodity[k,1]],sum(e[i,j,k,tab_fk[1]] - e[j,i,k,tab_fk[1]]  for j in 1:nb_nodes)+x_ikf[i,k,tab_fk[1]]==0, base_name = "1er_flot_"*string(k)) #flot pour traiter la première fonction de puis la source
		for t in 2:size_fk
			@constraint(m,[i in 1:nb_nodes],sum(e[i,j,k,tab_fk[t]] - e[j,i,k,tab_fk[t]] for j in 1:nb_nodes)-x_ikf[i,k,tab_fk[t-1]]+x_ikf[i,k,tab_fk[t]]==0, base_name = "mid_flow_"*string(t)*"_"*string(k)) #flot d'un traitement au suivant
		end
		@constraint(m,[i in 1:nb_nodes ; i!=commodity[k,2]],sum(e[i,j,k,nb_func+1] - e[j,i,k,nb_func+1] for j in 1:nb_nodes)-x_ikf[i,k,tab_fk[end]]==0, base_name = "last_flow_"*string(k)) #flot du dernier traitement vers le puit
		@constraint(m,sum(e[j,commodity[k,2],k,end] for j in 1:nb_nodes)+x_ikf[commodity[k,2],k,tab_fk[end]]==1, base_name = "end_flow_"*string(k)) #fin de flot sur le puit
	end

	for k in 1:nb_commodities
		for f in 1:nb_func
			@constraint(m,sum(x_ikf[i,k,f] for i in 1:nb_nodes)<=1, base_name = "concatenation_"*string(k)*"_"*string(f)) #concatenation des problemes de flot
		end
	end

	#Contraintes de latence
	@constraint(m, [k in 1:nb_commodities], sum( sum( sum(e[i,j,k,f]*latency[i,j] for f in 1:nb_func+1) for j in 1:nb_nodes) for i in 1:nb_nodes) <= commodity[k,4], base_name = "latence")

	#Objective
	@objective(m, Min, sum(x_i[i]*cout_ouverture + sum(x_fi[f,i]*func_cost[f,i] for f in 1:nb_func) for i in 1:nb_nodes) )

	optimize!(m)

	if !silent
		println(solution_summary(m))

		e=JuMP.value.(e)
		x_ikf=JuMP.value.(x_ikf)

		write_results(fileName,e,x_ikf)
	end
	status = termination_status(m)
	isOptimal = status == MOI.OPTIMAL
	return isOptimal, JuMP.value.(x_i), JuMP.value.(x_fi), JuMP.value.(x_ikf), JuMP.value.(e)
end

Static("di-yuan/di-yuan_3/")


#####################
## model parameters
#####################

p = (
    γ = 0.5,    # labor share
    θ = 8.,     # trade elasticity
    damp = 0.1, # function iteration damping
    tol = 1e-3  # convergence tolerance
    )

#####################
## initial data
#####################

λ = [.3 .7;
     .4 .6]         # trade shares
Y = [100., 100.]    # output
X = λ .* p.γ .* Y   # bilateral expenditures
π = [.8 .2;
     .3 .7]         # migration shares
L = [1.5, 1.5]        # labor distribution

#####################
## counterfactual set up
#####################

T_hat = [5., 1.]  # counterfactual change in productivity

#####################
## initialize model
#####################

w_guess = ones(2) # initial guess

data = (λ = λ, π = π, L = L, Y = Y, X = X) # set up data tuple

# make sure shares sum to 1
sum(data.λ, dims = 2)
sum(data.π, dims = 2)

#####################
## solver functions
#####################

# solve P as function of wages and data
function solve_P_hat(p, data, w_hat, T_hat)
    ( data.λ * ( T_hat .* w_hat.^(-p.γ*p.θ) ) ).^(-1/p.θ)
end

# solve λ as function of wages and P
function solve_λ_hat(p, w_hat, T_hat, P_hat)
    T_hat .* ( w_hat.^p.γ ./ P_hat ).^-p.θ
end

# solve for migration shares as function of real wages and data
function solve_π_hat(p, data, w_hat, P_hat)
    (w_hat ./ P_hat) ./ (data.π * ((w_hat ./ P_hat)))
end

# solve for labor as function of migration shares and data
function solve_L_hat(p, data, π_hat)
    (1 ./ data.L) .* (( π_hat .* data.π ) * data.L)
end

# solve for change in wages (and other variables)
function solve_w_hat(p, data, w_hat)

    L_hat = similar(w_hat)
    π_hat = similar(w_hat)
    λ_hat = similar(w_hat)
    P_hat = similar(w_hat)
    error = Inf
    iters = 1

    while error > p.tol && iters < 1e3

        P_hat = solve_P_hat(p, data, w_hat, T_hat)  # given wage guess, solve for P
        λ_hat = solve_λ_hat(p, w_hat, T_hat, P_hat) # given P, solve for trade shares
        π_hat = solve_π_hat(p, data, w_hat, P_hat)  # given real wages, solve for migration shares
        L_hat = solve_L_hat(p, data, π_hat)         # given migration shares, solve for labor

        # wage fixed point we are iterating on until convergence
        w_hat_new = (1/p.γ) * (1 ./ L_hat) .* (1 ./ data.Y) .* ( ( data.X .* λ_hat ) * ( w_hat .* L_hat ) )

        # error checking
        error = maximum(abs.((w_hat_new .- w_hat)./w_hat))
        w_hat = p.damp .* w_hat_new .+ (1 - p.damp) .* w_hat
        iters += 1

    end

    return w_hat, L_hat, π_hat, λ_hat, P_hat

end

#####################
## SOLVE MODEL
#####################

w_hat, L_hat, π_hat, λ_hat, P_hat = solve_w_hat(p, data, w_guess)


# using IntervalArithmetic
using DifferentialEquations
using LinearAlgebra

function Flo(du,u,p,t)
    h=BigFloat(".125")
    A11=(-1) *(1+4. *h^2+4. *h*cos(2. *t)).^(-1/2).*sin(2. *t)
    A12=(1+4. *h^2+(1+4. *h^2+4. *h*cos(2. *t)).^(1/2)+2. *h*cos(2. *t).*(2+(1+4. *h^2+ 4. *h*cos(2. *t)).^(1/2))).^(-1) *((-1) *cos(2. *t).*(1+(-8).*h^2+(1+4. *h^2+4. *h*cos(2. *t)).^(1/2))+h*(1+(-4).*h+16. *h^2+2. *(1+ 4. *h^2+4. *h*cos(2. *t)).^(1/2)+(-8).*h*(1+4. *h^2+4. *h*cos(2. * t)).^(1/2)+(-1) *cos(4. *t)))
    A21=(1+4. *h^2+(1+4. *h^2+4. *h*cos(2. *t) ).^(1/2)+2. *h*cos(2. *t).*(2+(1+4. *h^2+4. *h*cos(2. *t)).^(1/2))).^(-1) *(h+4. *h^2. *(1+4. *h)+2. *h*(1+4. *h).*(1+4. *h^2+4. *h*cos(2. *t)).^(1/2)+(-1) *cos(2. *t).*(1+(-8).*h^2+(1+4. *h^2+4. *h*cos( 2. *t)).^(1/2))+(-1) *h*cos(4. *t))
    A22=(1+4. *h^2+4. *h*cos(2. *t)).^( .-1/2).*sin(2. *t)

    du[1]=A11*u[1]+A12*u[2]
    du[2]=A21*u[1]+A22*u[2]
end
#

T_end=BigFloat(pi)
tspan=(BigFloat("0"),T_end)
relT=BigFloat("1e-72")
absT=relT*1e-2

u0=[BigFloat("1"),BigFloat("0")]
prob = ODEProblem(Flo,u0,tspan)
t,u1=solve(prob, Feagin14(),reltol=relT,abstol=absT,maxiters=1e20)
diff=sqrt((u1[1,end]-u1[1,1])^2+(u1[2,end]-u1[2,1])^2)
println(diff)
#
# u0=[BigFloat("0"),BigFloat("1")]
# prob = ODEProblem(Flo,u0,tspan)
# t,u2=solve(prob, RK4(),reltol=relT,abstol=absT,maxiters=1e15)
#
# monodromy_matrix=[u1[1,end] u2[1,end]; u1[2,end] u2[2,end]]
# discriminant=abs(tr(monodromy_matrix))-2
# println(discriminant)
# Flo1=eigvals(monodromy_matrix)[1]
# Flo2=eigvals(monodromy_matrix)[2]


@testset "Quadratic and hermitian forms" begin
  @time include("QuadForm/Basic.jl")
  @time include("QuadForm/Lattices.jl")
  @time include("QuadForm/Genus.jl")
  @time include("QuadForm/Morphism.jl")
  @time include("QuadForm/Enumeration.jl")
  @time include("QuadForm/MassQuad.jl")
  @time include("QuadForm/MassHerm.jl")
  @time include("QuadForm/Quad.jl")
  @time include("QuadForm/QuadBin.jl")
end


module Bri17

using LinearAlgebra
using Scapin.Grid
using Scapin.Elasticity

"""
    modal_strain_displacement!(b, n, N, h)

Compute the modal strain-displacement vector `b` in place, for the spatial frequency `n`.

This function returns `b`.

The grid is defined by its size, `N` and spacing, `h`. The spatial frequency is
defined by `n ∈ CartesianIndices(1:N[1], …, 1:N[d])`.

"""
function modal_strain_displacement!(
    b::AbstractVector{Complex{T}},
    n::CartesianIndex{d},
    N::NTuple{d,Int},
    h::NTuple{d,T},
) where {d,T<:Number}
    ∑α = zero(T)
    c = Array{T}(undef, d)
    s = Array{T}(undef, d)
    for i = 1:d
        α = π * (n[i] - 1) / N[i]
        ∑α += α
        c[i] = cos(α)
        s[i] = sin(α) / h[i]
    end

    b₀ = -2sin(∑α) + 2im * cos(∑α)
    if d == 2
        b[1] = b₀ * s[1] * c[2]
        b[2] = b₀ * c[1] * s[2]
    elseif d == 3
        b[1] = b₀ * s[1] * c[2] * c[3]
        b[2] = b₀ * c[1] * s[2] * c[3]
        b[3] = b₀ * c[1] * c[2] * s[3]
    else
        throw(ArgumentError("not implemented"))
    end
    return b
end

"""
    modal_strain_displacement(n, N, h)

Return the modal strain-displacement vector `b`.

See [`modal_strain_displacement!`](@ref) for a description of the parameters.

"""
function modal_strain_displacement(
    n::CartesianIndex{d},
    N::NTuple{d,Int},
    h::NTuple{d,T},
) where {d,T<:Number}
    return modal_strain_displacement!(Array{Complex{T}}(undef, d), n, N, h)
end

"""
    modal_stiffness!(K, n, N, h, C)

Compute the modal stiffness matrix `K` in place, for the spatial frequency `n`.

This function returns `K`.

The grid is defined by its size, `N` and spacing, `h`. The spatial frequency is
defined by `n ∈ CartesianIndices(1:N[1], …, 1:N[d])`. The (homogeneous)
constitutive material is specified by `C`.

!!! danger "Scaling of the modal stiffness matrix"

    The modal stiffness matrix introduced above differs from the matrix initially
    introduced in Ref. [DOI:10.1002/nme.5263](https://doi.org/10.1002/nme.5263)
    by a factor `h[1] * … * h[d]`. More precisely,

    ```
    K̂{Scapin} = h[1] * … * h[d] * K̂{10.1002/nme.5263}
    ```

    Such rescaling makes the relation between modal and nodal stiffness operators
    more natural (the former is the DFT of the latter).

"""
function modal_stiffness!(
    K::AbstractMatrix{Complex{T}},
    n::CartesianIndex{d},
    N::NTuple{d,Int},
    h::NTuple{d,T},
    C::Hooke{d,T},
) where {d,T<:Number}
    # In the notation of [Bri17, see Eq. (B.17)]
    #
    # φ[i] = φ(zᵢ) * hᵢ
    # χ[i] = χ(zᵢ) * hᵢ²
    # ψ[i] = ψ(zᵢ) * hᵢ
    #
    # Which simplifies the expression of Hₖ and allows to express the
    # nodal stiffness matrix as the inverse DFT of the modal stiffness
    # matrix.

    φ = Array{T}(undef, d)
    χ = Array{T}(undef, d)
    ψ = Array{T}(undef, d)
    for i = 1:d
        β = 2π * (n[i] - 1) / N[i]
        φ[i] = 2 * (1 - cos(β)) / h[i]
        χ[i] = (2 + cos(β)) / 3 * h[i]
        ψ[i] = sin(β)
    end

    scaling = C.μ / (1 - 2 * C.ν)
    if d == 2
        H₁₁ = φ[1] * χ[2]
        H₂₂ = χ[1] * φ[2]
        K_diag = C.μ * (H₁₁ + H₂₂)
        K[1, 1] = scaling * H₁₁ + K_diag
        K[1, 2] = scaling * ψ[1] * ψ[2]
        K[2, 1] = K[1, 2]
        K[2, 2] = scaling * H₂₂ + K_diag
    elseif d == 3
        H₁₁ = φ[1] * χ[2] * χ[3]
        H₂₂ = χ[1] * φ[2] * χ[3]
        H₃₃ = χ[1] * χ[2] * φ[3]
        K_diag = C.μ * (H₁₁ + H₂₂ + H₃₃)
        K[1, 1] = scaling * H₁₁ + K_diag
        K[1, 2] = scaling * ψ[1] * ψ[2] * χ[3]
        K[1, 3] = scaling * ψ[1] * χ[2] * ψ[3]
        K[2, 1] = K[1, 2]
        K[2, 2] = scaling * H₂₂ + K_diag
        K[2, 3] = scaling * χ[1] * ψ[2] * ψ[3]
        K[3, 1] = K[1, 3]
        K[3, 2] = K[2, 3]
        K[3, 3] = scaling * H₃₃ + K_diag
    else
        throw(ArgumentError("not implemented"))
    end
    return K
end

"""
    modal_stiffness(n, N, h, C)

Return the modal stiffness matrix `K`.

See [`modal_stiffness!`](@ref) for a description of the parameters.

"""
function modal_stiffness(
    n::CartesianIndex{d},
    N::NTuple{d,Int},
    h::NTuple{d,T},
    C::Hooke{d,T},
) where {T<:Number,d}
    return modal_stiffness!(Array{Complex{T}}(undef, d, d), n, N, h, C)
end

struct DiscreteGreenOperatorBri17{d,T}
    C::Hooke{d,T}
    N::NTuple{d,Int}
    h::NTuple{d,T}
end

Base.size(::DiscreteGreenOperatorBri17{d,T}) where {d,T} =
    ((d * (d + 1)) ÷ 2, (d * (d + 1)) ÷ 2)
Base.eltype(::Type{DiscreteGreenOperatorBri17{d,T}}) where {d,T} = T

function apply!(
    ε̂::AbstractVector{Complex{T}},
    Γ̂::DiscreteGreenOperatorBri17{d,T},
    n::CartesianIndex{d},
    τ̂::AbstractVector{Complex{T}},
) where {T<:Number,d}
    if all(Tuple(n) .== 1)
        ε̂ .= zero(eltype(ε̂))
        return
    end
    s = one(T) / √(2 * one(T))
    b̂ = modal_strain_displacement(n, Γ̂.N, Γ̂.h)
    conj_b̂ = conj.(b̂)
    K̂ = modal_stiffness(n, Γ̂.N, Γ̂.h, Γ̂.C)
    # TODO: use static arrays here!
    τ̂_conj_b̂ = Vector{Complex{T}}(undef, d)
    if d == 2
        #     ┌               ┐
        # τ̂ = │ τ̂₁       τ̂₃/√2│
        #     │ τ̂₃/√2    τ̂₂   │
        #     └               ┘
        τ̂_conj_b̂[1] = τ̂[1] * conj_b̂[1] + s * τ̂[3] * conj_b̂[2]
        τ̂_conj_b̂[2] = τ̂[2] * conj_b̂[2] + s * τ̂[3] * conj_b̂[1]
    elseif d == 3
        #     ┌                        ┐
        #     │ τ̂₁       τ̂₆/√2    τ̂₅/√2│
        # τ̂ = │ τ̂₆/√2    τ̂₂       τ̂₄/√2│
        #     │ τ̂₅/√2    τ̂₄/√2    τ̂₃/√2│
        #     └                        ┘
        τ̂_conj_b̂[1] = τ̂[1] * conj_b̂[1] + s * (τ̂[6] * conj_b̂[2] + τ̂[5] * conj_b̂[3])
        τ̂_conj_b̂[2] = τ̂[2] * conj_b̂[2] + s * (τ̂[6] * conj_b̂[1] + τ̂[4] * conj_b̂[3])
        τ̂_conj_b̂[3] = τ̂[3] * conj_b̂[3] + s * (τ̂[5] * conj_b̂[1] + τ̂[4] * conj_b̂[2])
    end
    # Do not include the `-` sign!
    û = prod(Γ̂.h) .* (K̂ \ τ̂_conj_b̂)
    if d == 2
        ε̂[1] = û[1] * b̂[1]
        ε̂[2] = û[2] * b̂[2]
        ε̂[3] = s * (û[1] * b̂[2] + û[2] * b̂[1])
    elseif d == 3
        ε̂[1] = û[1] * b̂[1]
        ε̂[2] = û[2] * b̂[2]
        ε̂[3] = û[3] * b̂[3]
        ε̂[4] = s * (û[2] * b̂[3] + û[3] * b̂[2])
        ε̂[5] = s * (û[3] * b̂[1] + û[1] * b̂[3])
        ε̂[6] = s * (û[1] * b̂[2] + û[2] * b̂[1])
    end
    return ε̂
end

function apply(
    Γ̂::DiscreteGreenOperatorBri17{d,T},
    n::CartesianIndex{d},
    τ̂::AbstractVector{Complex{T}},
) where {T<:Number,d}
    return apply!(Array{Complex{T}}(undef, size(Γ̂, 1)), n, τ̂)
end

export modal_strain_displacement!,
    modal_strain_displacement,
    modal_stiffness!,
    modal_stiffness,
    DiscreteGreenOperatorBri17,
    apply!,
    apply
end


"""
Shunt constraint using linearized voltage magnitude difference phi

```
sum(p) + sum(pconv_grid_ac)  == sum(pg) - sum(pd) - sum(gs*(1.0 + 2*phi)
```
"""
function constraint_kcl_shunt(pm::AbstractLPACModel, n::Int, cnd::Int, i::Int, bus_arcs, bus_arcs_dc, bus_gens, bus_convs_ac, bus_loads, bus_shunts, pd, qd, gs, bs)
    phi = PowerModels.var(pm, n, cnd, :phi, i)
    p = PowerModels.var(pm, n, cnd, :p)
    q = PowerModels.var(pm, n, cnd, :q)
    pg = PowerModels.var(pm, n, cnd, :pg)
    qg = PowerModels.var(pm, n, cnd, :qg)
    pconv_grid_ac = PowerModels.var(pm, n, cnd, :pconv_tf_fr)
    qconv_grid_ac = PowerModels.var(pm, n, cnd, :qconv_tf_fr)

    PowerModels.con(pm, n, cnd, :kcl_p)[i] = @constraint(pm.model, sum(p[a] for a in bus_arcs) + sum(pconv_grid_ac[c] for c in bus_convs_ac)  == sum(pg[g] for g in bus_gens)   - sum(pd[d] for d in bus_loads) - sum(gs[s] for s in bus_shunts)*(1.0 + 2*phi))
    PowerModels.con(pm, n, cnd, :kcl_q)[i] = @constraint(pm.model, sum(q[a] for a in bus_arcs) + sum(qconv_grid_ac[c] for c in bus_convs_ac)  == sum(qg[g] for g in bus_gens)   - sum(qd[d] for d in bus_loads) + sum(bs[s] for s in bus_shunts)*(1.0 + 2*phi))
end
"""
DC branch power flow using linearized voltage magnitude difference phi

```
p_dc_fr == p * g *  (phi_fr - phi_to)
p_dc_to == p * g *  (phi_to - phi_fr)
```
"""
function constraint_ohms_dc_branch(pm::AbstractLPACModel, n::Int, cnd::Int, f_bus, t_bus, f_idx, t_idx, r, p)
    p_dc_fr = PowerModels.var(pm, n, cnd, :p_dcgrid, f_idx)
    p_dc_to = PowerModels.var(pm, n, cnd, :p_dcgrid, t_idx)
    phi_fr = PowerModels.var(pm, n, cnd, :phi_vdcm, f_bus)
    phi_to = PowerModels.var(pm, n, cnd, :phi_vdcm, t_bus)
    phi_fr_ub = JuMP.UpperBoundRef(phi_to)
    phi_fr_lb = JuMP.LowerBoundRef(phi_to)

    if r == 0
        @constraint(pm.model, p_dc_fr + p_dc_to == 0)
    else
        g = 1 / r
        @constraint(pm.model, p_dc_fr == p * g *  (phi_fr - phi_to))
        @constraint(pm.model, p_dc_to == p * g *  (phi_to - phi_fr))
    end
end

function constraint_dc_branch_current(pm::AbstractLPACModel, n::Int, cnd::Int, f_bus, f_idx, ccm_max, p)

end

function add_dc_bus_voltage_setpoint(sol, pm::AbstractLPACModel)
    PowerModels.add_setpoint!(sol, pm, "busdc", "vm", :phi_vdcm, status_name="Vdc", inactive_status_value = 4, scale = (x,item,cnd) -> 1.0+x)
end


include("HW7.jl")
l_0 = 0.3
l_1 = 0.6
l_2 = 0.4
## Answer for 3.b
twist_1 = [0 0 1 0 0 0]';
twist_2 = [0 0 1 l_1 0 0]';
twist_3 = [0 0 1 l_1+l_2 0 0]';
twist_4 = [0 0 0 0 0 1]';
twists = [twist_1 twist_2 twist_3 twist_4];
R = Matrix(I,3,3)
t = [0 l_1+l_2 l_0];
T=RpToTrans(R,t)
angles=[pi/3 -pi/2 pi/4 -0.35]
## Answer for 3.b
g = FwdKin(T,twists,angles)
## Answer for 3.c
Js = JacobianSpace(twists,angles)
## Answer for 3.d
l_rod = 0.15;
P_rod = [0 l_1+l_2+l_rod l_0 1]';
Q_dot = [-0.2 1 0.5 -2]';

V_s = JacobianSpace(twists,angles)*Q_dot;
V_rod = VecTose3( V_s ) * P_rod;


"""
For an initial guess from the index of a Fold bifurcation point located in ContResult.bifpoint, returns a point which will be refined using `newtonFold`.
"""
function FoldPoint(br::ContResult, index::Int64)
	@assert br.foldpoint[index].type == :fold "The provided index does not refer to a Fold point"
	bifpoint = br.foldpoint[index]
	return BorderedArray(_copy(bifpoint.x), bifpoint.param)
end

####################################################################################################
# Method using Minimally Augmented formulation
@with_kw struct FoldProblemMinimallyAugmented{TF, TJ, TJa, Td2f, Tl <: Lens, vectype, S <: AbstractLinearSolver, Sa <: AbstractLinearSolver, Sbd <: AbstractBorderedLinearSolver}
	F::TF								# Function F(x, p) = 0
	J::TJ								# Jacobian of F wrt x
	Jadjoint::TJa						# Adjoint of the Jacobian of F
	d2F::Td2f = nothing					# Hessian of F
	lens::Tl							# parameter axis for the Fold point
	a::vectype							# close to null vector of J^T
	b::vectype							# close to null vector of J
	linsolver::S						# linear solver
	linsolverAdjoint::Sa = linsolver	# linear solver for the jacobian adjoint
	linbdsolver::Sbd					# bordered linear solver
end

hasHessian(pb::FoldProblemMinimallyAugmented{TF, TJ, TJa, Td2f, Tl, vectype, S, Sa, Sbd}) where {TF, TJ, TJa, Td2f, Tp, Tl, vectype, S, Sa, Sbd} = Td2f != Nothing

hasAdjoint(pb::FoldProblemMinimallyAugmented{TF, TJ, TJa, Td2f, Tl, vectype, S, Sa, Sbd}) where {TF, TJ, TJa, Td2f, Tl, vectype, S, Sa, Sbd} = TJa != Nothing

FoldProblemMinimallyAugmented(F, J, Ja, d2F, lens::Lens, a, b, linsolve) = FoldProblemMinimallyAugmented(F, J, Ja, d2F, lens, a, b, linsolve, linsolve, BorderingBLS(linsolve))

FoldProblemMinimallyAugmented(F, J, Ja, lens::Lens, a, b, linsolve) = FoldProblemMinimallyAugmented(F, J, Ja, nothing, lens, a, b, linsolve)

function (fp::FoldProblemMinimallyAugmented)(x::vectype, p::T, par) where {vectype, T}
	# These are the equations of the minimally augmented (MA) formulation of the Fold bifurcation point
	# input:
	# - x guess for the point at which the jacobian is singular
	# - p guess for the parameter at which the jacobian is singular
	# The jacobian of the MA problem is solved with a bordering method
	a = fp.a
	b = fp.b

	# we solve Jv + a σ1 = 0 with <b, v> = n
	# the solution is v = -σ1 J\a with σ1 = -n/<b, J^{-1}a>
	n = T(1)
	v, _, _ = fp.linsolver(fp.J(x, set(par, fp.lens, p)), a)
	bv = dot(b, v)
	bv == T(0) && @error "Error when using Minimally Augmented formulation for Fold bifurcation. The dot product should non-zero."

	σ1 = -n / bv
	return fp.F(x, set(par, fp.lens, p)), σ1
end

# this function is for the functional
function (foldpb::FoldProblemMinimallyAugmented)(x::BorderedArray, params)
	res = foldpb(x.u, x.p, params)
	return BorderedArray(res[1], res[2])
end

# Struct to invert the jacobian of the fold MA problem.
struct FoldLinearSolverMinAug <: AbstractLinearSolver; end

function foldMALinearSolver(x, p::T, pbMA::FoldProblemMinimallyAugmented, par,
							rhsu, rhsp;
							debug_::Bool = false) where T
	# The jacobian should be passed as a tuple as Jac_fold_MA(u0, pb::FoldProblemMinimallyAugmented) = (return (u0, pb, d2F::Bool))
	# The Jacobian J of the vector field is expressed at (x, p)
	# We solve here Jfold⋅res = rhs := [rhsu, rhsp]
	# The Jacobian expression of the Fold problem is Jfold = [J dpF ; σx σp] where σx := ∂_xσ
	# We recall the expression of σx = -< w, d2F(x,p)[v, x2]> where (w, σ2) is solution of J'w + b σ2 = 0 with <a, w> = n
	############### Extraction of function names #################
	# N = length(du) - 1

	F = pbMA.F
	J = pbMA.J

	d2F = pbMA.d2F
	a = pbMA.a
	b = pbMA.b

	# parameter axis
	lens = pbMA.lens

	# we define the following jacobian. It is used at least 3 times below. This avoid doing 3 times the possibly costly building of J(x, p)
	J_at_xp = J(x, set(par, lens, p))

	# we do the following in order to avoid computing J_at_xp twice in case pbMA.Jadjoint is not provided
	if hasAdjoint(pbMA)
		JAd_at_xp = pbMA.Jadjoint(x, set(par, lens, p))
	else
		JAd_at_xp = transpose(J_at_xp)
	end

	n = T(1)
	# we solve Jv + a σ1 = 0 with <b, v> = n
	# the solution is v = -σ1 J\a with σ1 = -n/<b, J\a>
	v = pbMA.linsolver(J_at_xp, a)[1]
	σ1 = -n / dot(b, v)
	rmul!(v, -σ1)

	# we solve J'w + b σ2 = 0 with <a, w> = n
	# the solution is w = -σ2 J'\b with σ2 = -n/<a, J'\b>
	w = pbMA.linsolverAdjoint(JAd_at_xp, b)[1]
	σ2 = -n / dot(a, w)
	rmul!(w, -σ2)

	δ = T(1e-8)
	ϵ1, ϵ2, ϵ3 = T(δ), T(δ), T(δ)
	################### computation of σx σp ####################
	dpF = minus(F(x, set(par, lens, p + ϵ1)), F(x, set(par, lens, p - ϵ1))); rmul!(dpF, T(1) / T(2ϵ1))
	dJvdp = minus(apply(J(x, set(par, lens, p + ϵ3)), v), apply(J(x, set(par, lens, p - ϵ3)), v)); rmul!(dJvdp, T(1) / T(2ϵ3))
	σp = -dot(w, dJvdp) / n

	if hasHessian(pbMA) == false
		# We invert the jacobian of the Fold problem when the Hessian of x -> F(x, p) is not known analytically. We thus rely on finite differences which can be slow for large dimensions
		prod(size(x)) > 1e4 && @warn "You might want to pass the Hessian, you are using finite differences with $(prod(size(x))) unknowns to compute a gradient"
		# we first need to compute the value of ∂_xσ written σx
		σx = zero(x)
		e  = zero(x)

		# this is the part which does not work if x is not AbstractArray. We use CartesianIndices to support AbstractArray as a type for the solution we are looking for
		for ii in CartesianIndices(x)
			e[ii] = T(1)
			# d2Fve := d2F(x,p)[v,e]
			d2Fve = (apply(J(x + ϵ2 * e, set(par, lens, p)), v) - apply(J(x - ϵ2 * e, set(par, lens, p)), v)) / T(2ϵ2)
			σx[ii] = -dot(w, d2Fve) / n
			e[ii] = T(0)
		end

		########## Resolution of the bordered linear system ########
		dX, dsig, flag, it = pbMA.linbdsolver(J_at_xp, dpF, σx, σp, rhsu, rhsp)

	else
		# We invert the jacobian of the Fold problem when the Hessian of x -> F(x, p) is known analytically. Much faster than the previous case

		# we solve it here instead of calling linearBorderedSolver because this removes the need to pass the linear form associated to σx
		x1, x2, _, it = pbMA.linsolver(J_at_xp, rhsu, dpF)

		d2Fv = d2F(x, set(par, lens, p), x1, v)
		σx1 = -dot(w, d2Fv ) / n

		copyto!(d2Fv, d2F(x, set(par, lens, p), x2, v))
		σx2 = -dot(w, d2Fv ) / n

		dsig = (rhsp - σx1) / (σp - σx2)

		# dX = x1 .- dsig .* x2
		dX = copyto!(similar(x1), x1); axpy!(-dsig, x2, dX)
	end

	if debug_
		return dX, dsig, true, sum(it), 0., [J(x, set(par, lens, p)) dpF ; σx' σp]
	else
		return dX, dsig, true, sum(it), 0.
	end
end

function (foldl::FoldLinearSolverMinAug)(Jfold, du::BorderedArray{vectype, T}, debug_::Bool = false) where {vectype, T}
	out =  foldMALinearSolver((Jfold.x).u,
				 (Jfold.x).p,
				 Jfold.fldpb,
				 Jfold.param,
				 du.u, du.p;
				 debug_ = debug_)

	if debug_ == false
		return BorderedArray(out[1], out[2]), out[3], out[4], out[5]
	else
		return BorderedArray(out[1], out[2]), out[3], out[4], out[5], out[6]
	end
end

################################################################################################### Newton / Continuation functions
"""
	newtonFold(F, J, foldpointguess, par, lens::Lens, eigenvec, options::NewtonPar; Jt = nothing, d2F = nothing, normN = norm, kwargs...)

This function turns an initial guess for a Fold point into a solution to the Fold problem based on a Minimally Augmented formulation. The arguments are as follows
- `F   = (x, p) -> F(x, p)` where `p` is a set of parameters.
- `dF  = (x, p) -> d_xF(x, p)` associated jacobian
- `foldpointguess` initial guess (x_0, p_0) for the Fold point. It should be a `BorderedArray` as returned by the function `FoldPoint`
- `par` parameters used for the vector field
- `lens` parameter axis used to locate the Fold point.
- `eigenvec` guess for the 0 eigenvector
- `options::NewtonPar` options for the Newton-Krylov algorithm, see [`NewtonPar`](@ref).

# Optional arguments:
- `Jt = (x, p) -> transpose(d_xF(x, p))` jacobian adjoint, it should be implemented in an efficient manner. For matrix-free methods, `transpose` is not readily available and the user must provide a dedicated method. In the case of sparse based jacobian, `Jt` should not be passed as it is computed internally more efficiently, i.e. it avoid recomputing the jacobian as it would be if you pass `Jt = (x, p) -> transpose(dF(x, p))`
- `d2F = (x, p, v1, v2) ->  d2F(x, p, v1, v2)` a bilinear operator representing the hessian of `F`. It has to provide an expression for `d2F(x,p)[v1,v2]`.
- `normN = norm`
- `kwargs` keywords arguments to be passed to the regular Newton-Krylov solver

# Simplified call
Simplified call to refine an initial guess for a Fold point. More precisely, the call is as follows

	newtonFold(F, J, br::ContResult, ind_fold::Int64, par, lens::Lens; Jt = nothing, d2F = nothing, options = br.contparams.newtonOptions, kwargs...)

where the optional argument `Jt` is the jacobian transpose and the Hessian is `d2F`. The parameters / options are as usual except that you have to pass the branch `br` from the result of a call to `continuation` with detection of bifurcations enabled and `index` is the index of bifurcation point in `br` you want to refine. You can pass newton parameters different from the ones stored in `br` by using the argument `options`.

!!! tip "Jacobian tranpose"
    The adjoint of the jacobian `J` is computed internally when `Jt = nothing` by using `tranpose(J)` which works fine when `J` is an `AbstractArray`. In this case, do not pass the jacobian adjoint like `Jt = (x, p) -> transpose(d_xF(x, p))` otherwise the jacobian will be computed twice!

!!! warning "Hessian"
    The hessian of `F`, when `d2F` is not passed, is computed with Finite differences. This can be slow for many variables, e.g. ~1e6
"""
function newtonFold(F, J, foldpointguess, par, lens::Lens, eigenvec, options::NewtonPar; Jt = nothing, d2F = nothing, normN = norm, kwargs...) where {T, vectype}
	foldproblem = FoldProblemMinimallyAugmented(
		F, J, Jt, d2F, lens,
		_copy(eigenvec), #copy(eigenvec),
		_copy(eigenvec), #copy(eigenvec),
		options.linsolver)

	# Jacobian for the Fold problem
	Jac_fold_MA = (x, param) -> (x = x, param = param, fldpb = foldproblem)

	opt_fold = @set options.linsolver = FoldLinearSolverMinAug()

	# solve the Fold equations
	return newton(foldproblem, Jac_fold_MA, foldpointguess, par, opt_fold; normN = normN, kwargs...)
end

function newtonFold(F, J, br::ContResult, ind_fold::Int64, par, lens::Lens; Jt = nothing, d2F = nothing, options = br.contparams.newtonOptions, kwargs...)
	foldpointguess = FoldPoint(br, ind_fold)
	bifpt = br.foldpoint[ind_fold]
	eigenvec = bifpt.tau

	# solve the Fold equations
	return newtonFold(F, J, foldpointguess, par, lens, eigenvec, options; Jt = Jt, d2F = d2F, kwargs...)
end

"""
$(SIGNATURES)

Codim 2 continuation of Fold points. This function turns an initial guess for a Fold point into a curve of Fold points based on a Minimally Augmented formulation. The arguments are as follows
- `F = (x, p) ->	F(x, p)` where `p` is a set of parameters
- `J = (x, p) -> d_xF(x, p)` associated jacobian
- `foldpointguess` initial guess (x_0, p1_0) for the Fold point. It should be a `BorderedArray` as returned by the function `FoldPoint`
- `par` set of parameters
- `lens1` parameter axis for parameter 1
- `lens2` parameter axis for parameter 2
- `eigenvec` guess for the 0 eigenvector at p1_0
- `options_cont` arguments to be passed to the regular [`continuation`](@ref)

# Optional arguments:

- `Jt = (x, p) -> transpose(d_xF(x, p))` associated jacobian transpose
- `d2F = p -> ((x, p, v1, v2) -> d2F(x, p, v1, v2))` this is the hessian of `F` computed at `(x, p)` and evaluated at `(v1, v2)`.
- `kwargs` keywords arguments to be passed to the regular [`continuation`](@ref)

# Simplified call
The call is as follows

	continuationFold(F, J, br::ContResult, ind_fold::Int64, par, lens1::Lens, lens2::Lens, options_cont::ContinuationPar ; Jt = nothing, d2F = nothing, kwargs...)

where the parameters are as above except that you have to pass the branch `br` from the result of a call to `continuation` with detection of bifurcations enabled and `index` is the index of Fold point in `br` you want to continue.

!!! tip "Jacobian tranpose"
    The adjoint of the jacobian `J` is computed internally when `Jt = nothing` by using `tranpose(J)` which works fine when `J` is an `AbstractArray`. In this case, do not pass the jacobian adjoint like `Jt = (x, p) -> transpose(d_xF(x, p))` otherwise the jacobian would be computed twice!

!!! warning "Hessian"
    The hessian of `F`, when `d2F` is not passed, is computed with Finite differences. This can be slow for many variables, e.g. ~1e6
"""
function continuationFold(F, J, foldpointguess::BorderedArray{vectype, T}, par, lens1::Lens, lens2::Lens, eigenvec, options_cont::ContinuationPar ; Jt = nothing, d2F = nothing, kwargs...) where {T,vectype}

	# options for the Newton Solver inheritated from the ones the user provided
	options_newton = options_cont.newtonOptions

	foldPb = FoldProblemMinimallyAugmented(
			F, J, Jt, d2F,
			lens1,
			_copy(eigenvec), #copy(eigenvec),
			_copy(eigenvec), #copy(eigenvec),
			options_newton.linsolver)

	# Jacobian for the Fold problem
	Jac_fold_MA = (x, param) -> (x = x, param = param, fldpb = foldPb)

	opt_fold_cont = @set options_cont.newtonOptions.linsolver = FoldLinearSolverMinAug()

	# solve the Fold equations
	branch, u, tau = continuation(
		foldPb, Jac_fold_MA,
		foldpointguess, par, lens2,
		opt_fold_cont,
		printSolution = (u, p) -> u.p,
		plotSolution = (x, p; kwargs...) -> (xlabel!("p2", subplot=1); ylabel!("p1", subplot=1)  ); kwargs...)
	return setproperties(branch; type = :FoldCodim2, functional = foldPb), u, tau
end

function continuationFold(F, J, br::ContResult, ind_fold::Int64, par, lens1::Lens, lens2::Lens, options_cont::ContinuationPar ; Jt = nothing, d2F = nothing, kwargs...)
	foldpointguess = FoldPoint(br, ind_fold)
	bifpt = br.foldpoint[ind_fold]
	eigenvec = bifpt.tau
	return continuationFold(F, J, foldpointguess, par, lens1, lens2, eigenvec, options_cont ; Jt = Jt, d2F = d2F, kwargs...)
end


using ModelingToolkit, OrdinaryDiffEq
using ModelingToolkitStandardLibrary.Blocks
using ModelingToolkitStandardLibrary.Blocks: Dₜ, t

#=
Testing strategy:
The general strategy is to test systems using simple intputs where the solution is known on closed form. For algebraic systems (without differential variables), an integrator with a constant input is often used together with the system under test.
=#

@testset "Gain" begin
    @info "Testing Gain"
    @named c = Gain(42)
    @named int = Integrator(; k=1)
    @named iosys = ODESystem([int.u~c.y, c.u~1], t, systems=[c, int])
    sys = structural_simplify(iosys)
    prob = ODEProblem(sys, Pair[], (0.0, 1.0))
    sol = solve(prob, Rosenbrock23(), saveat=0:0.1:1)
    @test sol[int.y] ≈ 42 .* (0:0.1:1)

    # Matrix gain
    @named c = Gain([2 0; 0 3])
    ints = [Integrator(; k=1, name=Symbol("int$i")) for i in 1:2]
    @named iosys = ODESystem([
        ints[1].u~c.y[1],
        ints[2].u~c.y[2],
        c.u[1]~1,
        c.u[2]~2,
        ], t, systems=[c; ints])
    sys = structural_simplify(iosys)
    prob = ODEProblem(sys, Pair[], (0.0, 1.0))
    sol = solve(prob, Rosenbrock23(), saveat=0:0.1:1)
    @test sol[ints[1].y] ≈ 2 .* (0:0.1:1) # 2 * 1
    @test sol[ints[2].y] ≈ 6 .* (0:0.1:1) # 3 * 2
end

@testset "Sum" begin
    @info "Testing Sum"
    @named s = Sum(2)
    ints = [Integrator(; k=1, name=Symbol("int$i")) for i in 1:2]
    @named iosys = ODESystem([
        ints[1].u~1,
        ints[2].u~2,
        ints[1].y~s.u[1],
        ints[2].y~s.u[2],
        ], t, systems=[s; ints])
    sys = structural_simplify(iosys)
    prob = ODEProblem(sys, Pair[], (0.0, 1.0))
    sol = solve(prob, Rosenbrock23(), saveat=0:0.1:1)
    @test sol[s.y] ≈ 3 .* (0:0.1:1)

    @named s = Sum([1, -2])
    ints = [Integrator(; k=1, name=Symbol("int$i")) for i in 1:2]
    @named iosys = ODESystem([
        ints[1].u~1,
        ints[2].u~1,
        ints[1].y~s.u[1],
        ints[2].y~s.u[2],
        ], t, systems=[s; ints])
    sys = structural_simplify(iosys)
    prob = ODEProblem(sys, Pair[], (0.0, 1.0))
    sol = solve(prob, Rosenbrock23(), saveat=0:0.1:1)
    @test sol[s.y] ≈ (1 + (-2)) .* (0:0.1:1)
end


import Base: print
export latex

using Printf

print(io::IO,A::Union{BosonDestroy,FermionDestroy}) = print(io,"$(A.name)($(A.inds...))")
print(io::IO,A::Union{BosonCreate,FermionCreate}) = print(io,"$(A.name)†($(A.inds...))")
print(io::IO,A::σ) = print(io,"σ$(A.a)($(A.inds...))")
print(io::IO,A::σminus) = print(io,"σ-($(A.inds...))")
print(io::IO,A::σplus) = print(io,"σ+($(A.inds...))")
print(io::IO,A::scal) = print(io,imag(A.v)==0 ? @sprintf("%g",real(A.v)) : (real(A.v)==0 ? @sprintf("%gi",imag(A.v)) : @sprintf("(%g%+gi)",real(x),imag(x))))
print(io::IO,A::param) = print(io,string(A.name, A.state=='c' ? "'" : "",length(A.inds)==0 ? "" : "($(A.inds...))"))
print(io::IO,A::OpProd) = print(io,"$(A.A) $(A.B)")
print(io::IO,A::OpSum) = print(io,"$(A.A) + $(A.B)")
print(io::IO,A::ExpVal) = print(io,"⟨$(A.A)⟩")
print(io::IO,A::Corr) = print(io,"⟨$(A.A)⟩c")
print(io::IO,A::OpSumAnalytic) = print(io,"∑_$(A.ind) $(A.A)")

mystring(x::Number) = @sprintf "%g" x
mystring(x::Rational) = denominator(x)==1 ? "$(numerator(x))" : "\\frac{$(numerator(x))}{$(denominator(x))}"
mystring(x::Complex) = @sprintf "(%g%+gi)" real(x) imag(x)
mystring(x::Complex{Rational{T}}) where T = @sprintf "\\left(%s%s%si\\right)" mystring(real(x)) (imag(x)>=0 ? "+" : "-") mystring(abs(imag(x)))

Base.show(io::IO, ::MIME"text/latex", A::Operator) = print(io,"\$",latex(A),"\$")
latex(A::σ) = string("\\sigma_{$(A.a)",length(A.inds)>0 ? ",$(A.inds...)}" : "}")
latexindstr(inds::OpIndices) = length(inds)==0 ? "" : "_{$(inds...)}"
latex(A::δ) = "δ" * latexindstr(A.inds)
latex(A::Union{BosonDestroy,FermionDestroy}) = "{$(A.name)}$(latexindstr(A.inds))"
latex(A::Union{BosonCreate,FermionCreate}) = "{$(A.name)}$(latexindstr(A.inds))^\\dagger"
latex(A::σminus) = "\\sigma^-" * latexindstr(A.inds)
latex(A::σplus) = "\\sigma^+" * latexindstr(A.inds)
latex(A::scal) = imag(A.v)==0 ? mystring(real(A.v)) : (real(A.v)==0 ? mystring(imag(A.v))*"i" : mystring(A.v))
latex(A::param)  = string(A.name, latexindstr(A.inds), A.state=='c' ? "^*" : "")
latex(A::OpProd) = string(latex(A.A)," ",latex(A.B))
latex(A::OpSum) = string(latex(A.A)," + ",latex(A.B))
latex(A::ExpVal) = "\\langle $(latex(A.A)) \\rangle"
latex(A::Corr) = "\\langle $(latex(A.A)) \\rangle_{c}"
latex(A::OpSumAnalytic) = string("\\sum_{$(A.ind)}",latex(A.A))


using NumCME
using BenchmarkTools
using Catalyst
using Sundials: CVODE_BDF
using StaticArrays: @MVector

fspalgorithm = AdaptiveFspSparse(
    ode_method=CVODE_BDF(linear_solver=:GMRES),
    space_adapter=RStepAdapter(5, 10, true)
)

# Bursting gene model definition using basic Julia
𝕊 = [[-1, 1, 0] [1, -1, 0] [0, 0, 1] [0, 0, -1]]
x₀ = [1, 0, 0]

a1 = propensity() do x, p
    p[1] * x[1]
end
a2 = propensity() do x, p
    p[2] * x[2]
end
a3 = propensity() do x, p
    p[3] * x[2]
end
a4 = propensity() do x, p
    p[4] * x[3]
end

k₀₁ = 0.05
k₁₀ = 0.1
λ = 5.0
γ = 0.5
θ = [k₀₁, k₁₀, λ, γ]

model = CmeModel(𝕊, [a1, a2, a3, a4], θ)

p0 = FspVectorSparse([@MVector [1, 0, 0]], [1.0])
tspan = (0.0, 300.0)

fspsol1 = solve(model, p0, tspan, fspalgorithm)

# Bursting model definition using Catalyst
@parameters k₀₁ k₁₀ λ γ
bursting_rn = @reaction_network begin
    k₀₁, G0 --> G1
    k₁₀, G1 --> G0
    λ, G1 --> G1 + mRNA
    γ, mRNA --> ∅
end k₀₁ k₁₀ λ γ

parameter_values = [k₀₁ => 0.05, k₁₀ => 0.1, λ => 5.0, γ => 0.5]
model_from_catalyst = CmeModel(bursting_rn, parameter_values)

fspsol2 = solve(model_from_catalyst, p0, tspan, fspalgorithm)

# Check that the two ways to code the model lead to the same numerical outputs
@assert length(fspsol1) == length(fspsol2)
for i ∈ 1:length(fspsol1)
    @assert get_states(fspsol1[i].p) == get_states(fspsol2[i].p)
    @assert get_values(fspsol1[i].p) == get_values(fspsol2[i].p)
end

# Which method is faster?
@btime solve(model, p0, tspan, fspalgorithm);
@btime solve(model_from_catalyst, p0, tspan, fspalgorithm);


using Distributions, OrdinaryDiffEq, GpABC, Distances, PyPlot

# GaussianProcesses/Bioinformatics paper/l2_norm_two_params/...

idx1 = 1
idx2 = 9
n_design_point = 200
n_plot = 100
contour_colors = ["white", "#FFE9EC", "#FFBBC5", "#FF8B9C", "#FF5D75", "#FF2F4E", "#D0001F", "#A20018", "#990017", "#800013"]

true_params =  [2.0, 2.0, 15.0, 1.0, 1.0, 1.0, 100.0, 1.0, 1.0, 1.0]
priors = [Uniform(0.2, 5.), Uniform(0.2, 5.), Uniform(10., 20.),
            Uniform(0.2, 2.), Uniform(0.2, 2.), Uniform(0.2, 2.),
            Uniform(75., 125.),
            Uniform(0.2, 2.), Uniform(0.2, 2.), Uniform(0.2, 2.)]
param_indices = [idx1, idx2]
Tspan = (0.0, 10.0)
x0 = [3.0, 2.0, 1.0]
solver = RK4()
saveat = 0.1

function ODE_3GeneReg(dx, x, pars, t)
    dx[1] = pars[1]/(1+pars[7]*x[3]) - pars[4]*x[1]
    dx[2] = pars[2]*pars[8]*x[1]./(1+pars[8]*x[1]) - pars[5]*x[2]
    dx[3] = pars[3]*pars[9]*x[1]*pars[10]*x[2]./(1+pars[9]*x[1])./(1+pars[10]*x[2]) - pars[6]*x[3]
end

function simulate_particle(param1::T, param2::T) where {T<:Real}
    params = copy(true_params)
    params[idx1] = param1
    params[idx2] = param2

    prob = ODEProblem(ODE_3GeneReg, x0, Tspan, params)
    Obs = solve(prob, solver, saveat=saveat)

    return Obs
end

function simulate_particles_product(param1::AbstractArray{T, 1}, param2::AbstractArray{T, 1}) where {T<:Real}
    map(x->simulate_particle(x...), Iterators.product(param1, param2))
end

function emulate_particles_product(param1::AbstractArray{T, 1}, param2::AbstractArray{T, 1}, gpem::GPModel) where {T<:Real}
    test_x = vcat(map(x->[x...]',
        reshape(collect(Iterators.product(param1, param2)),
            (length(param1) * length(param2),) ))...)
    ret = gp_regression(test_x, gpem)
    return reshape(ret[1], (length(param1), length(param2)))
end

function simulate_particles(params::AbstractArray{T, 2}) where {T<:Real}
    map(i->simulate_particle(params[i, :]...), range(1, size(params, 1)))
end

function build_distances(model_results, ref_data::AbstractArray{T,2},
    summary_stats_func::Function=GpABC.keep_all_summary_statistic,
    distance_func::Function=Distances.euclidean) where {T<:Real}
    ref_stats = summary_stats_func(ref_data)
    return map(x->distance_func(x, ref_stats), summary_stats_func.(model_results))
end

reference_data = simulate_particle(true_params[idx1], true_params[idx2])
contour_x = linspace(params(priors[idx1])..., n_plot)
contour_y = linspace(params(priors[idx2])..., n_plot)
sim_result = build_distances(
    simulate_particles_product(contour_x, contour_y),
    reference_data)

training_x = hcat(rand(priors[idx1], n_design_point), rand(priors[idx2], n_design_point))
training_y = build_distances(simulate_particles(training_x), reference_data)
gpm = GPModel(training_x=training_x, training_y=training_y, kernel=SquaredExponentialArdKernel())
gp_train(gpm)
em_result = emulate_particles_product(contour_x, contour_y, gpm)

subplot(121)
subplots_adjust(wspace  =  0.5)
contourf(contour_x, contour_y, sim_result, 8, colors=contour_colors)
# scatter3D(repmat(contour_x, 1, n_plot), repmat(contour_y', n_plot, 1), sim_result)
title("L2 norm - Simulation")
xlabel("Parameter $(idx1)")
ylabel("Parameter $(idx2)")
subplot(122)
contourf(contour_x, contour_y, em_result, 8, colors=contour_colors)
# scatter3D(repmat(contour_x, 1, n_plot), repmat(contour_y', n_plot, 1), em_result)
scatter(training_x[:, 1], training_x[:,2], marker="x", label="training points")
legend()
xlim(params(priors[idx1])...)
ylim(params(priors[idx2])...)
title("L2 norm - Emulation")
xlabel("Parameter $(idx1)")
ylabel("Parameter $(idx2)")
# x = simulate_prior_distances(5, 6, reference_data)