#HGPgen - code for hand-written and auto-generated routines

"""
vrr_ss(aexpn,bexpn,cexpn,dexpn, A,B,C,D)

Use Head-Gordon/Pople's vertical recurrence relations to compute
an array of two-electron integrals.

`A`, `B`, `C`, `D` are the centers of four Gaussian functions.
`aexpn`, `bexpn`, `cexpn`, `dexpn` are their exponents.

This is a hand-written routine specific to when the A and C
shells both have s-type angular momentum. It is meant to be
a model for machine generated angular momentum specific code.
"""
function vrr_ss(aexpn,bexpn,cexpn,dexpn, A,B,C,D)
    vrrs = zeros(Float64,1,1)

    P = gaussian_product_center(aexpn,A,bexpn,B)
    Q = gaussian_product_center(cexpn,C,dexpn,D)
    zeta,eta = aexpn+bexpn,cexpn+dexpn
    ze = zeta+eta
    W = gaussian_product_center(zeta,P,eta,Q)
    rab2 = dist2(A-B)
    rcd2 = dist2(C-D)
    rpq2 = dist2(P-Q)
    T = zeta*eta*rpq2/ze
    Kab = sqrt(2)pi^1.25/zeta*exp(-aexpn*bexpn*rab2/zeta)
    Kcd = sqrt(2)pi^1.25/eta*exp(-cexpn*dexpn*rcd2/eta)

    vrrs[1,1] = Kab*Kcd*Fgamma(0,T)/sqrt(ze)
    return vrrs
end

"""
vrr_ps(aexpn,bexpn,cexpn,dexpn, A,B,C,D)

Use Head-Gordon/Pople's vertical recurrence relations to compute
an array of two-electron integrals.

`A`, `B`, `C`, `D` are the centers of four Gaussian functions.
`aexpn`, `bexpn`, `cexpn`, `dexpn` are their exponents.

This is a hand-written routine specific to when the A shell has
p-type angular momentum and the C shell has s-type a.m. It is meant to be
a model for machine generated angular momentum specific code.
"""
function vrr_ps(aexpn,bexpn,cexpn,dexpn, A,B,C,D)
    mmax = 2
    vrrs = zeros(Float64,4,1,mmax)

    P = gaussian_product_center(aexpn,A,bexpn,B)
    Q = gaussian_product_center(cexpn,C,dexpn,D)
    zeta,eta = aexpn+bexpn,cexpn+dexpn
    ze = zeta+eta
    W = gaussian_product_center(zeta,P,eta,Q)
    rab2 = dist2(A-B)
    rcd2 = dist2(C-D)
    rpq2 = dist2(P-Q)
    T = zeta*eta*rpq2/ze
    Kab = sqrt(2)pi^1.25/zeta*exp(-aexpn*bexpn*rab2/zeta)
    Kcd = sqrt(2)pi^1.25/eta*exp(-cexpn*dexpn*rcd2/eta)

    PA = P-A
    WP = W-P

    vrrs[1,1, 1] = Kab*Kcd*Fgamma(0,T)/sqrt(ze)
    vrrs[1,1, 2] = Kab*Kcd*Fgamma(1,T)/sqrt(ze)

    vrrs[2,1,1] = (PA[1])*vrrs[1,1,1] + (WP[1])*vrrs[1,1,2]
    vrrs[3,1,1] = (PA[2])*vrrs[1,1,1] + (WP[2])*vrrs[1,1,2]
    vrrs[4,1,1] = (PA[3])*vrrs[1,1,1] + (WP[3])*vrrs[1,1,2]

    return vrrs[:,:,1]
end

"""
vrr_sp(aexpn,bexpn,cexpn,dexpn, A,B,C,D)

Use Head-Gordon/Pople's vertical recurrence relations to compute
an array of two-electron integrals.

`A`, `B`, `C`, `D` are the centers of four Gaussian functions.
`aexpn`, `bexpn`, `cexpn`, `dexpn` are their exponents.

This is a hand-written routine specific to when the A shell has
s-type angular momentum and the C shell has p-type a.m. It is meant to be
a model for machine generated angular momentum specific code.
"""
function vrr_sp(aexpn,bexpn,cexpn,dexpn, A,B,C,D)
    mmax = 2
    vrrs = zeros(Float64,1,4,mmax)

    P = gaussian_product_center(aexpn,A,bexpn,B)
    Q = gaussian_product_center(cexpn,C,dexpn,D)
    zeta,eta = aexpn+bexpn,cexpn+dexpn
    ze = zeta+eta
    W = gaussian_product_center(zeta,P,eta,Q)
    rab2 = dist2(A-B)
    rcd2 = dist2(C-D)
    rpq2 = dist2(P-Q)
    T = zeta*eta*rpq2/ze
    Kab = sqrt(2)pi^1.25/zeta*exp(-aexpn*bexpn*rab2/zeta)
    Kcd = sqrt(2)pi^1.25/eta*exp(-cexpn*dexpn*rcd2/eta)

    QC = Q-C
    WQ = W-Q

    vrrs[1,1, 1] = Kab*Kcd*Fgamma(0,T)/sqrt(ze)
    vrrs[1,1, 2] = Kab*Kcd*Fgamma(1,T)/sqrt(ze)

    vrrs[1,2,1] = (QC[1])*vrrs[1,1,1] + (WQ[1])*vrrs[1,1,2]
    vrrs[1,3,1] = (QC[2])*vrrs[1,1,1] + (WQ[2])*vrrs[1,1,2]
    vrrs[1,4,1] = (QC[3])*vrrs[1,1,1] + (WQ[3])*vrrs[1,1,2]

    return vrrs[:,:,1]
end

"""
vrr_pp(aexpn,bexpn,cexpn,dexpn, A,B,C,D)

Use Head-Gordon/Pople's vertical recurrence relations to compute
an array of two-electron integrals.

`A`, `B`, `C`, `D` are the centers of four Gaussian functions.
`aexpn`, `bexpn`, `cexpn`, `dexpn` are their exponents.

This is a hand-written routine specific to when the A and C
shells both have p-type angular momentum. It is meant to be
a model for machine generated angular momentum specific code.
"""
function vrr_pp(aexpn,bexpn,cexpn,dexpn, A,B,C,D)
    mmax = 3
    vrrs = zeros(Float64,4,4,mmax)

    P = gaussian_product_center(aexpn,A,bexpn,B)
    Q = gaussian_product_center(cexpn,C,dexpn,D)
    zeta,eta = aexpn+bexpn,cexpn+dexpn
    ze = zeta+eta
    W = gaussian_product_center(zeta,P,eta,Q)
    rab2 = dist2(A-B)
    rcd2 = dist2(C-D)
    rpq2 = dist2(P-Q)
    T = zeta*eta*rpq2/ze
    Kab = sqrt(2)pi^1.25/zeta*exp(-aexpn*bexpn*rab2/zeta)
    Kcd = sqrt(2)pi^1.25/eta*exp(-cexpn*dexpn*rcd2/eta)
    PA = P-A
    WP = W-P
    QC = Q-C
    WQ = W-Q

    vrrs[1,1, 1] = Kab*Kcd*Fgamma(0,T)/sqrt(ze)
    vrrs[1,1, 2] = Kab*Kcd*Fgamma(1,T)/sqrt(ze)
    vrrs[1,1, 3] = Kab*Kcd*Fgamma(2,T)/sqrt(ze)

    vrrs[2,1,1] = (PA[1])*vrrs[1,1,1] + (WP[1])*vrrs[1,1,2]
    vrrs[3,1,1] = (PA[2])*vrrs[1,1,1] + (WP[2])*vrrs[1,1,2]
    vrrs[4,1,1] = (PA[3])*vrrs[1,1,1] + (WP[3])*vrrs[1,1,2]
    vrrs[1,2,1] = (QC[1])*vrrs[1,1,1] + (WQ[1])*vrrs[1,1,2]
    vrrs[1,3,1] = (QC[2])*vrrs[1,1,1] + (WQ[2])*vrrs[1,1,2]
    vrrs[1,4,1] = (QC[3])*vrrs[1,1,1] + (WQ[3])*vrrs[1,1,2]
    vrrs[2,1,2] = (PA[1])*vrrs[1,1,2] + (WP[1])*vrrs[1,1,3]
    vrrs[3,1,2] = (PA[2])*vrrs[1,1,2] + (WP[2])*vrrs[1,1,3]
    vrrs[4,1,2] = (PA[3])*vrrs[1,1,2] + (WP[3])*vrrs[1,1,3]
    vrrs[1,2,2] = (QC[1])*vrrs[1,1,2] + (WQ[1])*vrrs[1,1,3]
    vrrs[1,3,2] = (QC[2])*vrrs[1,1,2] + (WQ[2])*vrrs[1,1,3]
    vrrs[1,4,2] = (QC[3])*vrrs[1,1,2] + (WQ[3])*vrrs[1,1,3]

    vrrs[2,2,1] = (QC[1])*vrrs[2,1,1] + (WQ[1])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[2,3,1] = (QC[2])*vrrs[2,1,1] + (WQ[2])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[2,4,1] = (QC[3])*vrrs[2,1,1] + (WQ[3])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]

    vrrs[3,2,1] = (QC[1])*vrrs[3,1,1] + (WQ[1])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[3,3,1] = (QC[2])*vrrs[3,1,1] + (WQ[2])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[3,4,1] = (QC[3])*vrrs[3,1,1] + (WQ[3])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]

    vrrs[4,2,1] = (QC[1])*vrrs[4,1,1] + (WQ[1])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[4,3,1] = (QC[2])*vrrs[4,1,1] + (WQ[2])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[4,4,1] = (QC[3])*vrrs[4,1,1] + (WQ[3])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]
    return vrrs[:,:,1]
end

"""
vrr_sd(aexpn,bexpn,cexpn,dexpn, A,B,C,D)

Use Head-Gordon/Pople's vertical recurrence relations to compute
an array of two-electron integrals.

`A`, `B`, `C`, `D` are the centers of four Gaussian functions.
`aexpn`, `bexpn`, `cexpn`, `dexpn` are their exponents.

This is a hand-written routine specific to when the A shell has
s-type angular momentum and the C shell has d-type a.m. It is meant to be
a model for machine generated angular momentum specific code.
"""
function vrr_sd(aexpn,bexpn,cexpn,dexpn, A,B,C,D)
    mmax = 3
    vrrs = zeros(Float64,1,10,mmax)

    P = gaussian_product_center(aexpn,A,bexpn,B)
    Q = gaussian_product_center(cexpn,C,dexpn,D)
    zeta,eta = aexpn+bexpn,cexpn+dexpn
    ze = zeta+eta
    W = gaussian_product_center(zeta,P,eta,Q)
    rab2 = dist2(A-B)
    rcd2 = dist2(C-D)
    rpq2 = dist2(P-Q)
    T = zeta*eta*rpq2/ze
    Kab = sqrt(2)pi^1.25/zeta*exp(-aexpn*bexpn*rab2/zeta)
    Kcd = sqrt(2)pi^1.25/eta*exp(-cexpn*dexpn*rcd2/eta)

    QC = Q-C
    WQ = W-Q

    vrrs[1,2,1] = (QC[1])*vrrs[1,1,1] + (WQ[1])*vrrs[1,1,2]
    vrrs[1,3,1] = (QC[2])*vrrs[1,1,1] + (WQ[2])*vrrs[1,1,2]
    vrrs[1,4,1] = (QC[3])*vrrs[1,1,1] + (WQ[3])*vrrs[1,1,2]

    vrrs[1,2,2] = (QC[1])*vrrs[1,1,2] + (WQ[1])*vrrs[1,1,3]
    vrrs[1,3,2] = (QC[2])*vrrs[1,1,2] + (WQ[2])*vrrs[1,1,3]
    vrrs[1,4,2] = (QC[3])*vrrs[1,1,2] + (WQ[3])*vrrs[1,1,3]

    # [MVector(2,0,0),MVector(1,1,0),MVector(1,0,1),MVector(0,2,0),MVector(0,1,1),MVector(0,0,2)],

    vrrs[1,5,1] = (QC[1])*vrrs[1,2,1] + (WQ[1])*vrrs[1,2,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,6,1] = (QC[2])*vrrs[1,2,1] + (WQ[2])*vrrs[1,2,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,7,1] = (QC[3])*vrrs[1,2,1] + (WQ[3])*vrrs[1,2,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,8,1] = (QC[2])*vrrs[1,3,1] + (WQ[2])*vrrs[1,3,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,9,1] = (QC[3])*vrrs[1,3,1] + (WQ[3])*vrrs[1,3,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,10,1] =(QC[3])*vrrs[1,4,1] + (WQ[3])*vrrs[1,4,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])

    return vrrs[:,:,1]
end

"""
vrr_ds(aexpn,bexpn,cexpn,dexpn, A,B,C,D)

Use Head-Gordon/Pople's vertical recurrence relations to compute
an array of two-electron integrals.

`A`, `B`, `C`, `D` are the centers of four Gaussian functions.
`aexpn`, `bexpn`, `cexpn`, `dexpn` are their exponents.

This is a hand-written routine specific to when the A shell has
c-type angular momentum and the C shell has s-type a.m. It is meant to be
a model for machine generated angular momentum specific code.
"""
function vrr_ds(aexpn,bexpn,cexpn,dexpn, A,B,C,D)
    mmax = 3
    vrrs = zeros(Float64,10,1,mmax)

    P = gaussian_product_center(aexpn,A,bexpn,B)
    Q = gaussian_product_center(cexpn,C,dexpn,D)
    zeta,eta = aexpn+bexpn,cexpn+dexpn
    ze = zeta+eta
    W = gaussian_product_center(zeta,P,eta,Q)
    rab2 = dist2(A-B)
    rcd2 = dist2(C-D)
    rpq2 = dist2(P-Q)
    T = zeta*eta*rpq2/ze
    Kab = sqrt(2)pi^1.25/zeta*exp(-aexpn*bexpn*rab2/zeta)
    Kcd = sqrt(2)pi^1.25/eta*exp(-cexpn*dexpn*rcd2/eta)
    PA = P-A
    WP = W-P

    vrrs[1,1, 1] = Kab*Kcd*Fgamma(0,T)/sqrt(ze)
    vrrs[1,1, 2] = Kab*Kcd*Fgamma(1,T)/sqrt(ze)
    vrrs[1,1, 3] = Kab*Kcd*Fgamma(2,T)/sqrt(ze)

    vrrs[2,1,1] = (PA[1])*vrrs[1,1,1] + (WP[1])*vrrs[1,1,2]
    vrrs[3,1,1] = (PA[2])*vrrs[1,1,1] + (WP[2])*vrrs[1,1,2]
    vrrs[4,1,1] = (PA[3])*vrrs[1,1,1] + (WP[3])*vrrs[1,1,2]

    vrrs[2,1,2] = (PA[1])*vrrs[1,1,2] + (WP[1])*vrrs[1,1,3]
    vrrs[3,1,2] = (PA[2])*vrrs[1,1,2] + (WP[2])*vrrs[1,1,3]
    vrrs[4,1,2] = (PA[3])*vrrs[1,1,2] + (WP[3])*vrrs[1,1,3]

    # [MVector(2,0,0),MVector(1,1,0),MVector(1,0,1),MVector(0,2,0),MVector(0,1,1),MVector(0,0,2)],

    vrrs[5,1,1] = (PA[1])*vrrs[2,1,1] + (WP[1])*vrrs[2,1,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[6,1,1] = (PA[2])*vrrs[2,1,1] + (WP[2])*vrrs[2,1,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[7,1,1] = (PA[3])*vrrs[2,1,1] + (WP[3])*vrrs[2,1,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[8,1,1] = (PA[2])*vrrs[3,1,1] + (WP[2])*vrrs[3,1,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[9,1,1] = (PA[3])*vrrs[3,1,1] + (WP[3])*vrrs[3,1,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[10,1,1] =(PA[3])*vrrs[4,1,1] + (WP[3])*vrrs[4,1,2] +  1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])

    return vrrs[:,:,1]
end

"""
vrr_dp(aexpn,bexpn,cexpn,dexpn, A,B,C,D)

Use Head-Gordon/Pople's vertical recurrence relations to compute
an array of two-electron integrals.

`A`, `B`, `C`, `D` are the centers of four Gaussian functions.
`aexpn`, `bexpn`, `cexpn`, `dexpn` are their exponents.

This is a hand-written routine specific to when the A shell has
c-type angular momentum and the C shell has s-type a.m. It is meant to be
a model for machine generated angular momentum specific code.
"""
function vrr_dp(aexpn,bexpn,cexpn,dexpn, A,B,C,D)
    mmax = 4
    vrrs = zeros(Float64,10,4,mmax)

    P = gaussian_product_center(aexpn,A,bexpn,B)
    Q = gaussian_product_center(cexpn,C,dexpn,D)
    zeta,eta = aexpn+bexpn,cexpn+dexpn
    ze = zeta+eta
    W = gaussian_product_center(zeta,P,eta,Q)
    rab2 = dist2(A-B)
    rcd2 = dist2(C-D)
    rpq2 = dist2(P-Q)
    T = zeta*eta*rpq2/ze
    Kab = sqrt(2)pi^1.25/zeta*exp(-aexpn*bexpn*rab2/zeta)
    Kcd = sqrt(2)pi^1.25/eta*exp(-cexpn*dexpn*rcd2/eta)

    PA = P-A
    WP = W-P
    QC = Q-C
    WQ = W-Q

    vrrs[1,1, 1] = Kab*Kcd*Fgamma(0,T)/sqrt(ze)
    vrrs[1,1, 2] = Kab*Kcd*Fgamma(1,T)/sqrt(ze)
    vrrs[1,1, 3] = Kab*Kcd*Fgamma(2,T)/sqrt(ze)
    vrrs[1,1, 4] = Kab*Kcd*Fgamma(3,T)/sqrt(ze)

    vrrs[2,1,1] = (PA[1])*vrrs[1,1,1] + (WP[1])*vrrs[1,1,2]
    vrrs[3,1,1] = (PA[2])*vrrs[1,1,1] + (WP[2])*vrrs[1,1,2]
    vrrs[4,1,1] = (PA[3])*vrrs[1,1,1] + (WP[3])*vrrs[1,1,2]

    vrrs[2,1,2] = (PA[1])*vrrs[1,1,2] + (WP[1])*vrrs[1,1,3]
    vrrs[3,1,2] = (PA[2])*vrrs[1,1,2] + (WP[2])*vrrs[1,1,3]
    vrrs[4,1,2] = (PA[3])*vrrs[1,1,2] + (WP[3])*vrrs[1,1,3]

    vrrs[1,2,1] = (QC[1])*vrrs[1,1,1] + (WQ[1])*vrrs[1,1,2]
    vrrs[1,3,1] = (QC[2])*vrrs[1,1,1] + (WQ[2])*vrrs[1,1,2]
    vrrs[1,4,1] = (QC[3])*vrrs[1,1,1] + (WQ[3])*vrrs[1,1,2]

    vrrs[1,2,2] = (QC[1])*vrrs[1,1,2] + (WQ[1])*vrrs[1,1,3]
    vrrs[1,3,2] = (QC[2])*vrrs[1,1,2] + (WQ[2])*vrrs[1,1,3]
    vrrs[1,4,2] = (QC[3])*vrrs[1,1,2] + (WQ[3])*vrrs[1,1,3]

    vrrs[2,2,1] = (QC[1])*vrrs[2,1,1] + (WQ[1])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[2,3,1] = (QC[2])*vrrs[2,1,1] + (WQ[2])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[2,4,1] = (QC[3])*vrrs[2,1,1] + (WQ[3])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]

    vrrs[3,2,1] = (QC[1])*vrrs[3,1,1] + (WQ[1])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[3,3,1] = (QC[2])*vrrs[3,1,1] + (WQ[2])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[3,4,1] = (QC[3])*vrrs[3,1,1] + (WQ[3])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]

    vrrs[4,2,1] = (QC[1])*vrrs[4,1,1] + (WQ[1])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[4,3,1] = (QC[2])*vrrs[4,1,1] + (WQ[2])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[4,4,1] = (QC[3])*vrrs[4,1,1] + (WQ[3])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]

    # [MVector(2,0,0),MVector(1,1,0),MVector(1,0,1),MVector(0,2,0),MVector(0,1,1),MVector(0,0,2)],

    vrrs[5,1,1] = (PA[1])*vrrs[2,1,1] + (WP[1])*vrrs[2,1,2] +
            1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[6,1,1] = (PA[2])*vrrs[2,1,1] + (WP[2])*vrrs[2,1,2] +
            1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[7,1,1] = (PA[3])*vrrs[2,1,1] + (WP[3])*vrrs[2,1,2] +
            1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[8,1,1] = (PA[2])*vrrs[3,1,1] + (WP[2])*vrrs[3,1,2] +
            1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[9,1,1] = (PA[3])*vrrs[3,1,1] + (WP[3])*vrrs[3,1,2] +
            1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[10,1,1] =(PA[3])*vrrs[4,1,1] + (WP[3])*vrrs[4,1,2] +
            1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])

    vrrs[5,2,1] = (PA[1])*vrrs[2,2,1] + (WP[1])*vrrs[2,2,2] +
            1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[6,2,1] = (PA[2])*vrrs[2,2,1] + (WP[2])*vrrs[2,2,2] +
            1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[7,2,1] = (PA[3])*vrrs[2,2,1] + (WP[3])*vrrs[2,2,2] +
            1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[8,2,1] = (PA[2])*vrrs[3,2,1] + (WP[2])*vrrs[3,2,2] +
            1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[9,2,1] = (PA[3])*vrrs[3,2,1] + (WP[3])*vrrs[3,2,2] +
            1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[10,2,1] =(PA[3])*vrrs[4,2,1] + (WP[3])*vrrs[4,2,2] +
            1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]

    vrrs[5,3,1] = (PA[1])*vrrs[2,3,1] + (WP[1])*vrrs[2,3,2] +
            1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[6,3,1] = (PA[2])*vrrs[2,3,1] + (WP[2])*vrrs[2,3,2] +
            1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[7,3,1] = (PA[3])*vrrs[2,3,1] + (WP[3])*vrrs[2,3,2] +
            1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[8,3,1] = (PA[2])*vrrs[3,3,1] + (WP[2])*vrrs[3,3,2] +
            1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[9,3,1] = (PA[3])*vrrs[3,3,1] + (WP[3])*vrrs[3,3,2] +
            1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[10,3,1] =(PA[3])*vrrs[4,3,1] + (WP[3])*vrrs[4,3,2] +
            1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]

    vrrs[5,4,1] = (PA[1])*vrrs[2,4,1] + (WP[1])*vrrs[2,4,2] +
            1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[6,4,1] = (PA[2])*vrrs[2,4,1] + (WP[2])*vrrs[2,4,2] +
            1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[7,4,1] = (PA[3])*vrrs[2,4,1] + (WP[3])*vrrs[2,4,2] +
            1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[8,4,1] = (PA[2])*vrrs[3,4,1] + (WP[2])*vrrs[3,4,2] +
            1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[9,4,1] = (PA[3])*vrrs[3,4,1] + (WP[3])*vrrs[3,4,2] +
            1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[10,4,1] =(PA[3])*vrrs[4,4,1] + (WP[3])*vrrs[4,4,2] +
            1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]

    return vrrs[:,:,1]
end

"""
vrr_pd(aexpn,bexpn,cexpn,dexpn, A,B,C,D)

Use Head-Gordon/Pople's vertical recurrence relations to compute
an array of two-electron integrals.

`A`, `B`, `C`, `D` are the centers of four Gaussian functions.
`aexpn`, `bexpn`, `cexpn`, `dexpn` are their exponents.

This is a hand-written routine specific to when the A shell has
c-type angular momentum and the C shell has s-type a.m. It is meant to be
a model for machine generated angular momentum specific code.
"""
function vrr_pd(aexpn,bexpn,cexpn,dexpn, A,B,C,D)
    mmax = 4
    vrrs = zeros(Float64,4,10,mmax)

    P = gaussian_product_center(aexpn,A,bexpn,B)
    Q = gaussian_product_center(cexpn,C,dexpn,D)
    zeta,eta = aexpn+bexpn,cexpn+dexpn
    ze = zeta+eta
    W = gaussian_product_center(zeta,P,eta,Q)
    rab2 = dist2(A-B)
    rcd2 = dist2(C-D)
    rpq2 = dist2(P-Q)
    T = zeta*eta*rpq2/ze
    Kab = sqrt(2)pi^1.25/zeta*exp(-aexpn*bexpn*rab2/zeta)
    Kcd = sqrt(2)pi^1.25/eta*exp(-cexpn*dexpn*rcd2/eta)
    PA = P-A
    WP = W-P
    QC = Q-C
    WQ = W-Q

    vrrs[1,1, 1] = Kab*Kcd*Fgamma(0,T)/sqrt(ze)
    vrrs[1,1, 2] = Kab*Kcd*Fgamma(1,T)/sqrt(ze)
    vrrs[1,1, 3] = Kab*Kcd*Fgamma(2,T)/sqrt(ze)
    vrrs[1,1, 4] = Kab*Kcd*Fgamma(3,T)/sqrt(ze)

    vrrs[2,1,1] = (PA[1])*vrrs[1,1,1] + (WP[1])*vrrs[1,1,2]
    vrrs[3,1,1] = (PA[2])*vrrs[1,1,1] + (WP[2])*vrrs[1,1,2]
    vrrs[4,1,1] = (PA[3])*vrrs[1,1,1] + (WP[3])*vrrs[1,1,2]

    vrrs[2,1,2] = (PA[1])*vrrs[1,1,2] + (WP[1])*vrrs[1,1,3]
    vrrs[3,1,2] = (PA[2])*vrrs[1,1,2] + (WP[2])*vrrs[1,1,3]
    vrrs[4,1,2] = (PA[3])*vrrs[1,1,2] + (WP[3])*vrrs[1,1,3]

    vrrs[1,2,1] = (QC[1])*vrrs[1,1,1] + (WQ[1])*vrrs[1,1,2]
    vrrs[1,3,1] = (QC[2])*vrrs[1,1,1] + (WQ[2])*vrrs[1,1,2]
    vrrs[1,4,1] = (QC[3])*vrrs[1,1,1] + (WQ[3])*vrrs[1,1,2]

    vrrs[1,2,2] = (QC[1])*vrrs[1,1,2] + (WQ[1])*vrrs[1,1,3]
    vrrs[1,3,2] = (QC[2])*vrrs[1,1,2] + (WQ[2])*vrrs[1,1,3]
    vrrs[1,4,2] = (QC[3])*vrrs[1,1,2] + (WQ[3])*vrrs[1,1,3]

    vrrs[2,2,1] = (QC[1])*vrrs[2,1,1] + (WQ[1])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[2,3,1] = (QC[2])*vrrs[2,1,1] + (WQ[2])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[2,4,1] = (QC[3])*vrrs[2,1,1] + (WQ[3])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]

    vrrs[3,2,1] = (QC[1])*vrrs[3,1,1] + (WQ[1])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[3,3,1] = (QC[2])*vrrs[3,1,1] + (WQ[2])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[3,4,1] = (QC[3])*vrrs[3,1,1] + (WQ[3])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]

    vrrs[4,2,1] = (QC[1])*vrrs[4,1,1] + (WQ[1])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[4,3,1] = (QC[2])*vrrs[4,1,1] + (WQ[2])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[4,4,1] = (QC[3])*vrrs[4,1,1] + (WQ[3])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]

    # [MVector(2,0,0),MVector(1,1,0),MVector(1,0,1),MVector(0,2,0),MVector(0,1,1),MVector(0,0,2)],
    vrrs[1,5,1] = (QC[1])*vrrs[1,2,1] + (WQ[1])*vrrs[1,2,2] +
          1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,6,1] = (QC[2])*vrrs[1,2,1] + (WQ[2])*vrrs[1,2,2] +
          1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,7,1] = (QC[3])*vrrs[1,2,1] + (WQ[3])*vrrs[1,2,2] +
          1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,8,1] = (QC[2])*vrrs[1,3,1] + (WQ[2])*vrrs[1,3,2] +
          1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,9,1] = (QC[3])*vrrs[1,3,1] + (WQ[3])*vrrs[1,3,2] +
          1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,10,1] =(QC[3])*vrrs[1,4,1] + (WQ[3])*vrrs[1,4,2] +
         1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])

    vrrs[2,5,1] = (QC[1])*vrrs[2,2,1] + (WQ[1])*vrrs[2,2,2] +
          1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[2,6,1] = (QC[2])*vrrs[2,2,1] + (WQ[2])*vrrs[2,2,2] +
          1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[2,7,1] = (QC[3])*vrrs[2,2,1] + (WQ[3])*vrrs[2,2,2] +
          1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[2,8,1] = (QC[2])*vrrs[2,3,1] + (WQ[2])*vrrs[2,3,2] +
          1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[2,9,1] = (QC[3])*vrrs[2,3,1] + (WQ[3])*vrrs[2,3,2] +
          1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[2,10,1] =(QC[3])*vrrs[2,4,1] + (WQ[3])*vrrs[2,4,2] +
         1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]

    vrrs[3,5,1] = (QC[1])*vrrs[3,2,1] + (WQ[1])*vrrs[3,2,2] +
          1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[3,6,1] = (QC[2])*vrrs[3,2,1] + (WQ[2])*vrrs[3,2,2] +
          1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[3,7,1] = (QC[3])*vrrs[3,2,1] + (WQ[3])*vrrs[3,2,2] +
          1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[3,8,1] = (QC[2])*vrrs[3,3,1] + (WQ[2])*vrrs[3,3,2] +
          1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[3,9,1] = (QC[3])*vrrs[3,3,1] + (WQ[3])*vrrs[3,3,2] +
          1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[3,10,1] =(QC[3])*vrrs[3,4,1] + (WQ[3])*vrrs[3,4,2] +
         1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]

    vrrs[4,5,1] = (QC[1])*vrrs[4,2,1] + (WQ[1])*vrrs[4,2,2] +
          1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[4,6,1] = (QC[2])*vrrs[4,2,1] + (WQ[2])*vrrs[4,2,2] +
          1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[4,7,1] = (QC[3])*vrrs[4,2,1] + (WQ[3])*vrrs[4,2,2] +
          1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[4,8,1] = (QC[2])*vrrs[4,3,1] + (WQ[2])*vrrs[4,3,2] +
          1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[4,9,1] = (QC[3])*vrrs[4,3,1] + (WQ[3])*vrrs[4,3,2] +
          1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[4,10,1] =(QC[3])*vrrs[4,4,1] + (WQ[3])*vrrs[4,4,2] +
         1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]

    return vrrs[:,:,1]
end

"""
vrr_dd(aexpn,bexpn,cexpn,dexpn, A,B,C,D)

Use Head-Gordon/Pople's vertical recurrence relations to compute
an array of two-electron integrals.

`A`, `B`, `C`, `D` are the centers of four Gaussian functions.
`aexpn`, `bexpn`, `cexpn`, `dexpn` are their exponents.

This is a hand-written routine specific to when the A shell has
d-type angular momentum and the C shell has d-type a.m. It is meant to be
a model for machine generated angular momentum specific code.

This function is still incomplete.
"""
function vrr_dd(aexpn,bexpn,cexpn,dexpn, A,B,C,D)
    mmax = 5
    vrrs = zeros(Float64,10,10,mmax)

    P = gaussian_product_center(aexpn,A,bexpn,B)
    Q = gaussian_product_center(cexpn,C,dexpn,D)
    zeta,eta = aexpn+bexpn,cexpn+dexpn
    ze = zeta+eta
    W = gaussian_product_center(zeta,P,eta,Q)
    rab2 = dist2(A-B)
    rcd2 = dist2(C-D)
    rpq2 = dist2(P-Q)
    T = zeta*eta*rpq2/ze
    Kab = sqrt(2)pi^1.25/zeta*exp(-aexpn*bexpn*rab2/zeta)
    Kcd = sqrt(2)pi^1.25/eta*exp(-cexpn*dexpn*rcd2/eta)

    PA = P-A
    WP = W-P
    QC = Q-C
    WQ = W-Q

    vrrs[1,1, 1] = Kab*Kcd*Fgamma(0,T)/sqrt(ze)
    vrrs[1,1, 2] = Kab*Kcd*Fgamma(1,T)/sqrt(ze)
    vrrs[1,1, 3] = Kab*Kcd*Fgamma(2,T)/sqrt(ze)
    vrrs[1,1, 4] = Kab*Kcd*Fgamma(3,T)/sqrt(ze)
    vrrs[1,1, 5] = Kab*Kcd*Fgamma(4,T)/sqrt(ze)

    vrrs[2,1,1] = (PA[1])*vrrs[1,1,1] + (WP[1])*vrrs[1,1,2]
    vrrs[3,1,1] = (PA[2])*vrrs[1,1,1] + (WP[2])*vrrs[1,1,2]
    vrrs[4,1,1] = (PA[3])*vrrs[1,1,1] + (WP[3])*vrrs[1,1,2]

    vrrs[2,1,2] = (PA[1])*vrrs[1,1,2] + (WP[1])*vrrs[1,1,3]
    vrrs[3,1,2] = (PA[2])*vrrs[1,1,2] + (WP[2])*vrrs[1,1,3]
    vrrs[4,1,2] = (PA[3])*vrrs[1,1,2] + (WP[3])*vrrs[1,1,3]

    vrrs[1,2,1] = (QC[1])*vrrs[1,1,1] + (WQ[1])*vrrs[1,1,2]
    vrrs[1,3,1] = (QC[2])*vrrs[1,1,1] + (WQ[2])*vrrs[1,1,2]
    vrrs[1,4,1] = (QC[3])*vrrs[1,1,1] + (WQ[3])*vrrs[1,1,2]

    vrrs[1,2,2] = (QC[1])*vrrs[1,1,2] + (WQ[1])*vrrs[1,1,3]
    vrrs[1,3,2] = (QC[2])*vrrs[1,1,2] + (WQ[2])*vrrs[1,1,3]
    vrrs[1,4,2] = (QC[3])*vrrs[1,1,2] + (WQ[3])*vrrs[1,1,3]

    vrrs[2,2,1] = (QC[1])*vrrs[2,1,1] + (WQ[1])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[2,3,1] = (QC[2])*vrrs[2,1,1] + (WQ[2])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[2,4,1] = (QC[3])*vrrs[2,1,1] + (WQ[3])*vrrs[2,1,2] + 0.5/ze*vrrs[1,1,2]

    vrrs[3,2,1] = (QC[1])*vrrs[3,1,1] + (WQ[1])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[3,3,1] = (QC[2])*vrrs[3,1,1] + (WQ[2])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[3,4,1] = (QC[3])*vrrs[3,1,1] + (WQ[3])*vrrs[3,1,2] + 0.5/ze*vrrs[1,1,2]

    vrrs[4,2,1] = (QC[1])*vrrs[4,1,1] + (WQ[1])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[4,3,1] = (QC[2])*vrrs[4,1,1] + (WQ[2])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]
    vrrs[4,4,1] = (QC[3])*vrrs[4,1,1] + (WQ[3])*vrrs[4,1,2] + 0.5/ze*vrrs[1,1,2]

    vrrs[1,5,1] = (QC[1])*vrrs[1,2,1] + (WQ[1])*vrrs[1,2,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,6,1] = (QC[2])*vrrs[1,2,1] + (WQ[2])*vrrs[1,2,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,7,1] = (QC[3])*vrrs[1,2,1] + (WQ[3])*vrrs[1,2,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,8,1] = (QC[2])*vrrs[1,3,1] + (WQ[2])*vrrs[1,3,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,9,1] = (QC[3])*vrrs[1,3,1] + (WQ[3])*vrrs[1,3,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[1,10,1] =(QC[3])*vrrs[1,4,1] + (WQ[3])*vrrs[1,4,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])

    vrrs[2,5,1] = (QC[1])*vrrs[2,2,1] + (WQ[1])*vrrs[2,2,2] +
              1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[2,6,1] = (QC[2])*vrrs[2,2,1] + (WQ[2])*vrrs[2,2,2] +
              1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[2,7,1] = (QC[3])*vrrs[2,2,1] + (WQ[3])*vrrs[2,2,2] +
              1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[2,8,1] = (QC[2])*vrrs[2,3,1] + (WQ[2])*vrrs[2,3,2] +
              1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[2,9,1] = (QC[3])*vrrs[2,3,1] + (WQ[3])*vrrs[2,3,2] +
              1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[2,10,1] =(QC[3])*vrrs[2,4,1] + (WQ[3])*vrrs[2,4,2] +
              1/(2*zeta)*(vrrs[2,1,1]-eta/ze*vrrs[2,1,2]) + 0.5/ze*vrrs[1,1,2]

    vrrs[3,5,1] = (QC[1])*vrrs[3,2,1] + (WQ[1])*vrrs[3,2,2] +
              1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[3,6,1] = (QC[2])*vrrs[3,2,1] + (WQ[2])*vrrs[3,2,2] +
              1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[3,7,1] = (QC[3])*vrrs[3,2,1] + (WQ[3])*vrrs[3,2,2] +
              1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[3,8,1] = (QC[2])*vrrs[3,3,1] + (WQ[2])*vrrs[3,3,2] +
              1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[3,9,1] = (QC[3])*vrrs[3,3,1] + (WQ[3])*vrrs[3,3,2] +
              1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[3,10,1] =(QC[3])*vrrs[3,4,1] + (WQ[3])*vrrs[3,4,2] +
              1/(2*zeta)*(vrrs[3,1,1]-eta/ze*vrrs[3,1,2]) + 0.5/ze*vrrs[1,1,2]

    vrrs[4,5,1] = (QC[1])*vrrs[4,2,1] + (WQ[1])*vrrs[4,2,2] +
              1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[4,6,1] = (QC[2])*vrrs[4,2,1] + (WQ[2])*vrrs[4,2,2] +
              1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[4,7,1] = (QC[3])*vrrs[4,2,1] + (WQ[3])*vrrs[4,2,2] +
              1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[4,8,1] = (QC[2])*vrrs[4,3,1] + (WQ[2])*vrrs[4,3,2] +
              1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[4,9,1] = (QC[3])*vrrs[4,3,1] + (WQ[3])*vrrs[4,3,2] +
              1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[4,10,1] =(QC[3])*vrrs[4,4,1] + (WQ[3])*vrrs[4,4,2] +
              1/(2*zeta)*(vrrs[4,1,1]-eta/ze*vrrs[4,1,2]) + 0.5/ze*vrrs[1,1,2]

    vrrs[5,1,1] = (PA[1])*vrrs[2,1,1] + (WP[1])*vrrs[2,1,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[6,1,1] = (PA[2])*vrrs[2,1,1] + (WP[2])*vrrs[2,1,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[7,1,1] = (PA[3])*vrrs[2,1,1] + (WP[3])*vrrs[2,1,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[8,1,1] = (PA[2])*vrrs[3,1,1] + (WP[2])*vrrs[3,1,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[9,1,1] = (PA[3])*vrrs[3,1,1] + (WP[3])*vrrs[3,1,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])
    vrrs[10,1,1] =(PA[3])*vrrs[4,1,1] + (WP[3])*vrrs[4,1,2] +
              1/(2*zeta)*(vrrs[1,1,1]-eta/ze*vrrs[1,1,2])

    vrrs[5,2,1] = (PA[1])*vrrs[2,2,1] + (WP[1])*vrrs[2,2,2] +
              1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[6,2,1] = (PA[2])*vrrs[2,2,1] + (WP[2])*vrrs[2,2,2] +
              1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[7,2,1] = (PA[3])*vrrs[2,2,1] + (WP[3])*vrrs[2,2,2] +
              1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[8,2,1] = (PA[2])*vrrs[3,2,1] + (WP[2])*vrrs[3,2,2] +
              1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[9,2,1] = (PA[3])*vrrs[3,2,1] + (WP[3])*vrrs[3,2,2] +
              1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[10,2,1] =(PA[3])*vrrs[4,2,1] + (WP[3])*vrrs[4,2,2] +
              1/(2*zeta)*(vrrs[1,2,1]-eta/ze*vrrs[1,2,2]) + 0.5/ze*vrrs[1,1,2]

    vrrs[5,3,1] = (PA[1])*vrrs[2,3,1] + (WP[1])*vrrs[2,3,2] +
              1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[6,3,1] = (PA[2])*vrrs[2,3,1] + (WP[2])*vrrs[2,3,2] +
              1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[7,3,1] = (PA[3])*vrrs[2,3,1] + (WP[3])*vrrs[2,3,2] +
              1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[8,3,1] = (PA[2])*vrrs[3,3,1] + (WP[2])*vrrs[3,3,2] +
              1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[9,3,1] = (PA[3])*vrrs[3,3,1] + (WP[3])*vrrs[3,3,2] +
              1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[10,3,1] =(PA[3])*vrrs[4,3,1] + (WP[3])*vrrs[4,3,2] +
              1/(2*zeta)*(vrrs[1,3,1]-eta/ze*vrrs[1,3,2]) + 0.5/ze*vrrs[1,1,2]

    vrrs[5,4,1] = (PA[1])*vrrs[2,4,1] + (WP[1])*vrrs[2,4,2] +
              1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[6,4,1] = (PA[2])*vrrs[2,4,1] + (WP[2])*vrrs[2,4,2] +
              1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[7,4,1] = (PA[3])*vrrs[2,4,1] + (WP[3])*vrrs[2,4,2] +
              1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[8,4,1] = (PA[2])*vrrs[3,4,1] + (WP[2])*vrrs[3,4,2] +
              1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[9,4,1] = (PA[3])*vrrs[3,4,1] + (WP[3])*vrrs[3,4,2] +
              1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]
    vrrs[10,4,1] =(PA[3])*vrrs[4,4,1] + (WP[3])*vrrs[4,4,2] +
              1/(2*zeta)*(vrrs[1,4,1]-eta/ze*vrrs[1,4,2]) + 0.5/ze*vrrs[1,1,2]

    # [MVector(2,0,0),MVector(1,1,0),MVector(1,0,1),MVector(0,2,0),MVector(0,1,1),MVector(0,0,2)],
    # xx = x-shift from px
    # [xx,xx] = [a,c+1] = x[xx,x] + [xx,s] + 2[x,x]
    # [xy,xx] = x[xy,x] + [xy,s] + [y,x]
    # [xz,xx] = x[xz,x] + [xz,s] + [z,x]
    # [yy,xx] = x[yy,x] + [yy,s]
    # [yz,xx] = x[yz,x] + [yz,s]
    # [zz,xx] = x[zz,x] + [zz,s]
    vrrs[5,5,1] = (QC[1])*vrrs[5,2,1] + (WQ[1])*vrrs[5,2,2] +
         1/(2*zeta)*(vrrs[5,1,1]-eta/ze*vrrs[5,1,2]) + 1/ze*vrrs[2,2,2]
    vrrs[6,5,1] = (QC[1])*vrrs[6,2,1] + (WQ[1])*vrrs[6,2,2] +
         1/(2*zeta)*(vrrs[6,1,1]-eta/ze*vrrs[6,1,2]) + 0.5/ze*vrrs[3,2,2]
    vrrs[7,5,1] = (QC[1])*vrrs[7,2,1] + (WQ[1])*vrrs[7,2,2] +
        1/(2*zeta)*(vrrs[7,1,1]-eta/ze*vrrs[7,1,2]) + 0.5/ze*vrrs[4,2,2]
    vrrs[8,5,1] = (QC[1])*vrrs[8,2,1] + (WQ[1])*vrrs[8,2,2] +
        1/(2*zeta)*(vrrs[8,1,1]-eta/ze*vrrs[8,1,2])
    vrrs[9,5,1] = (QC[1])*vrrs[9,2,1] + (WQ[1])*vrrs[9,2,2] +
        1/(2*zeta)*(vrrs[9,1,1]-eta/ze*vrrs[9,1,2])
    vrrs[10,5,1] = (QC[1])*vrrs[10,2,1] + (WQ[1])*vrrs[10,2,2] +
        1/(2*zeta)*(vrrs[10,1,1]-eta/ze*vrrs[10,1,2])

    # xy = y-shift from px
    # [xx,xy] = y[xx,x]
    # [xy,xy] = y[xy,x] + [x,x]
    # [xz,xy] = y[xz,x]
    # [yy,xy] = y[yy,x] + 2[y,x]
    # [yz,xy] = y[yz,x] + [z,x]
    # [zz,xy] = y[zz,x]

    vrrs[5,6,1] = (QC[2])*vrrs[5,2,1] + (WQ[2])*vrrs[5,2,2]
    vrrs[6,6,1] = (QC[2])*vrrs[6,2,1] + (WQ[2])*vrrs[6,2,2] + 0.5/ze*vrrs[2,2,2]
    vrrs[7,6,1] = (QC[2])*vrrs[7,2,1] + (WQ[2])*vrrs[7,2,2]
    vrrs[8,6,1] = (QC[2])*vrrs[8,2,1] + (WQ[2])*vrrs[8,2,2] + 1/ze*vrrs[3,2,2]
    vrrs[9,6,1] = (QC[2])*vrrs[9,2,1] + (WQ[2])*vrrs[9,2,2] + 0.5/ze*vrrs[4,2,2]
    vrrs[10,6,1] =(QC[2])*vrrs[10,2,1] + (WQ[2])*vrrs[10,2,2]

    # xz = z-shift from px
    # [xx,xz] = z[xx,x] +
    # [xy,xz] = z[xy,x] +
    # [xz,xz] = z[xz,x] + [x,x]
    # [yy,xz] = z[yy,x] +
    # [yz,xz] = z[yz,x] + [y,x]
    # [zz,xz] = z[zz,x] + 2[z,x]

    vrrs[5,7,1] = (QC[3])*vrrs[5,2,1] + (WQ[3])*vrrs[5,2,2]
    vrrs[6,7,1] = (QC[3])*vrrs[6,2,1] + (WQ[3])*vrrs[6,2,2]
    vrrs[7,7,1] = (QC[3])*vrrs[7,2,1] + (WQ[3])*vrrs[7,2,2] + 0.5/ze*vrrs[2,2,2]
    vrrs[8,7,1] = (QC[3])*vrrs[8,2,1] + (WQ[3])*vrrs[8,2,2]
    vrrs[9,7,1] = (QC[3])*vrrs[9,2,1] + (WQ[3])*vrrs[9,2,2] + 0.5/ze*vrrs[3,2,2]
    vrrs[10,7,1] =(QC[3])*vrrs[10,2,1] + (WQ[3])*vrrs[10,2,2] + 1/ze*vrrs[4,2,2]

    # yy = y-shift from py
    # [xx,yy] = y[xx,y] +
    # [xy,yy] = y[xy,y] + [x,y]
    # [xz,yy] = y[xz,y] +
    # [yy,yy] = y[yy,y] + 2[y,y]
    # [yz,yy] = y[yz,y] + [z,y]
    # [zz,yy] = y[zz,y] +

    vrrs[5,8,1] = (QC[2])*vrrs[5,3,1] + (WQ[2])*vrrs[5,3,2]
    vrrs[6,8,1] = (QC[2])*vrrs[6,3,1] + (WQ[2])*vrrs[6,3,2] + 0.5/ze*vrrs[2,3,2]
    vrrs[7,8,1] = (QC[2])*vrrs[7,3,1] + (WQ[2])*vrrs[7,3,2]
    vrrs[8,8,1] = (QC[2])*vrrs[8,3,1] + (WQ[2])*vrrs[8,3,2] + 0.5/ze*vrrs[3,3,2]
    vrrs[9,8,1] = (QC[2])*vrrs[9,3,1] + (WQ[2])*vrrs[9,3,2] + 1/ze*vrrs[4,3,2]
    vrrs[10,8,1] =(QC[2])*vrrs[10,3,1] + (WQ[2])*vrrs[10,3,2]

    # yz = z-shift from py
    # [xx,yz] = z[xx,y] +
    # [xy,yz] = z[xy,y] +
    # [xz,yz] = z[xz,y] +  [x,y]
    # [yy,yz] = z[yy,y] +
    # [yz,yz] = z[yz,y] + [y,y]
    # [zz,yz] = z[zz,y] + 2[z,y]

    vrrs[5,9,1] = (QC[3])*vrrs[5,3,1] + (WQ[3])*vrrs[5,3,2]
    vrrs[6,9,1] = (QC[3])*vrrs[6,3,1] + (WQ[3])*vrrs[6,3,2]
    vrrs[7,9,1] = (QC[3])*vrrs[7,3,1] + (WQ[3])*vrrs[7,3,2] + 0.5/ze*vrrs[2,3,2]
    vrrs[8,9,1] = (QC[3])*vrrs[8,3,1] + (WQ[3])*vrrs[8,3,2]
    vrrs[9,9,1] = (QC[3])*vrrs[9,3,1] + (WQ[3])*vrrs[9,3,2] + 1/ze*vrrs[3,3,2]
    vrrs[10,9,1] =(QC[3])*vrrs[10,3,1] + (WQ[3])*vrrs[10,3,2] + 0.5/ze*vrrs[4,3,2]

    # zz = z-shift from pz
    # [xx,zz] = z[xx,z] +
    # [xy,zz] = z[xy,z] +
    # [xz,zz] = z[xz,z] + [x,z]
    # [yy,zz] = z[yy,z] +
    # [yz,zz] = z[yz,z] + [y,z]
    # [zz,zz] = z[zz,z] + 2[z,z]

    vrrs[5,10,1] = (QC[3])*vrrs[5,4,1] + (WQ[3])*vrrs[5,4,2]
    vrrs[6,10,1] = (QC[3])*vrrs[6,4,1] + (WQ[3])*vrrs[6,4,2]
    vrrs[7,10,1] = (QC[3])*vrrs[7,4,1] + (WQ[3])*vrrs[7,4,2] + 0.5/ze*vrrs[2,4,2]
    vrrs[8,10,1] = (QC[3])*vrrs[8,4,1] + (WQ[3])*vrrs[8,4,2]
    vrrs[9,10,1] = (QC[3])*vrrs[9,4,1] + (WQ[3])*vrrs[9,4,2] + 1/ze*vrrs[3,4,2]
    vrrs[10,10,1] =(QC[3])*vrrs[10,4,1] + (WQ[3])*vrrs[10,4,2] + 0.5/ze*vrrs[4,4,2]

    return vrrs[:,:,1]
end


"""
bijective function {1, ..., k} -> {1, ..., k}
"""
struct Perm{T <: Integer} <: BasicDiscreteFunction
    m :: Vector{T}
    dom :: ShiftedSegment{Int}
    rng :: ShiftedSegment{Int}
end

"""
bijective function {0, .. k-1} -> {0, ..., k-1}
"""
struct OffsetPerm{T <: Integer} <: BasicDiscreteFunction
    m :: Vector{T}
    dom :: Segment{Int}
    rng :: Segment{Int}
end

function Perm(v)
    k = maximum(v)
    dom = Segment(1, k)
    return Perm(v, dom, dom)
end

function OffsetPerm(v)
    k = maximum(v)
    dom = Segment(k)
    return OffsetPerm(v, dom, dom)
end


function spgenspd( n, p )
  nzval = Float64[]
  rowval = Int64[]
  colptr = Array(Int64,n+1)
  k = 1
  for j = 1:n
    colptr[j] = k
    push!(rowval, j)
    push!(nzval, randn() + n)
    k = k+1
    for row in randsubseq(1:j-1,p)
      push!(nzval, randn())
      push!(rowval, row)
      k = k+1
    end
  end
  colptr[n+1] = k

  return SparseMatrixCSC(n,n,colptr,rowval,nzval)
end

function genspd( n )
  A = randn(n,n)
  A = A+A' + n*eye(n)
  return A
end


using Gadfly, Compat

plot((x,y) -> x*exp(-(x-(@compat round(Int, x)))^2-y^2), -8., 8, -2., 2)


# implements the multiple-instance learning model using Neural Networks, as described in
# https://arxiv.org/abs/1609.07257
# Using Neural Network Formalism to Solve Multiple-Instance Problems, Tomas Pevny, Petr Somol
using FileIO, JLD2, Statistics, Mill, Flux
using Flux: throttle, @epochs
using Mill: reflectinmodel
using Base.Iterators: repeated

# load the musk dataset
fMat = load("musk.jld2", "fMat")            # matrix with instances, each column is one sample
bagids = load("musk.jld2", "bagids")        # ties instances to bags
x = BagNode(ArrayNode(fMat), bagids)        # create BagDataset
y = load("musk.jld2", "y")                  # load labels
y = map(i -> maximum(y[i]) + 1, x.bags)     # create labels on bags
y_oh = Flux.onehotbatch(y, 1:2)             # one-hot encoding

# create the model
model = BagModel(
    ArrayModel(Dense(166, 10, Flux.tanh)),                      # model on the level of Flows
    SegmentedMeanMax(10),                                       # aggregation
    ArrayModel(Chain(Dense(20, 10, Flux.tanh), Dense(10, 2))))  # model on the level of bags

# define loss function
loss(x, y_oh) = Flux.logitcrossentropy(model(x).data, y_oh)

# the usual way of training
evalcb = throttle(() -> @show(loss(x, y_oh)), 1)
opt = Flux.ADAM()
@epochs 10 Flux.train!(loss, params(model), repeated((x, y_oh), 1000), opt, cb=evalcb)

# calculate the error on the training set (no testing set right now)
mean(mapslices(argmax, model(x).data, dims=1)' .!= y)


################################################
#
# minmax heap type and constructors
#
################################################

mutable struct BinaryMinMaxHeap{T} <: AbstractMinMaxHeap{T}
    valtree::Vector{T}

    BinaryMinMaxHeap{T}() where {T} = new{T}(Vector{T}())

    function BinaryMinMaxHeap{T}(xs::AbstractVector{T}) where {T}
        valtree = _make_binary_minmax_heap(xs)
        new{T}(valtree)
    end
end

BinaryMinMaxHeap(xs::AbstractVector{T}) where T = BinaryMinMaxHeap{T}(xs)

################################################
#
# core implementation
#
################################################

function _make_binary_minmax_heap(xs)
    valtree = copy(xs)
    for i in length(xs):-1:1
        @inbounds _minmax_heap_trickle_down!(valtree, i)
    end
    return valtree
end

Base.@propagate_inbounds function _minmax_heap_bubble_up!(A::AbstractVector, i::Integer)
    if on_minlevel(i)
        if i > 1 && A[i] > A[hparent(i)]
            # swap to parent and bubble up max
            tmp = A[i]
            A[i] = A[hparent(i)]
            A[hparent(i)] = tmp
            _minmax_heap_bubble_up!(A, hparent(i), Reverse)
        else
            # bubble up min
            _minmax_heap_bubble_up!(A, i, Forward)
        end

    else
        # max level
        if i > 1 && A[i] < A[hparent(i)]
            # swap to parent and bubble up min
            tmp = A[i]
            A[i] = A[hparent(i)]
            A[hparent(i)] = tmp
            _minmax_heap_bubble_up!(A, hparent(i), Forward)
        else
            # bubble up max
            _minmax_heap_bubble_up!(A, i, Reverse)
        end
    end
end

Base.@propagate_inbounds function _minmax_heap_bubble_up!(A::AbstractVector, i::Integer, o::Ordering, x=A[i])
    if hasgrandparent(i)
        gparent = hparent(hparent(i))
        if lt(o, x, A[gparent])
            A[i] = A[gparent]
            A[gparent] = x
            _minmax_heap_bubble_up!(A, gparent, o)
        end
    end
end

Base.@propagate_inbounds function _minmax_heap_trickle_down!(A::AbstractVector, i::Integer)
    if on_minlevel(i)
        _minmax_heap_trickle_down!(A, i, Forward)
    else
        _minmax_heap_trickle_down!(A, i, Reverse)
    end
end

Base.@propagate_inbounds function _minmax_heap_trickle_down!(A::AbstractVector, i::Integer, o::Ordering, x=A[i])

    if haschildren(i, A)
        # get the index of the extremum (min or max) descendant
        extremum = o === Forward ? minimum : maximum
        _, m = extremum((A[j], j) for j in children_and_grandchildren(length(A), i))

        if isgrandchild(m, i)
            if lt(o, A[m], A[i])
                A[i] = A[m]
                A[m] = x
                if lt(o, A[hparent(m)], A[m])
                    t = A[m]
                    A[m] = A[hparent(m)]
                    A[hparent(m)] = t
                end
                _minmax_heap_trickle_down!(A, m, o)
            end
        else
            if lt(o, A[m], A[i])
                A[i] = A[m]
                A[m] = x
            end
        end
    end
end

################################################
#
# utilities
#
################################################

@inline level(i) = floor(Int, log2(i))
@inline lchild(i) = 2*i
@inline rchild(i) = 2*i+1
@inline children(i) = (lchild(i), rchild(i))
@inline hparent(i) = i ÷ 2
@inline on_minlevel(i) = level(i) % 2 == 0
@inline haschildren(i, A) = lchild(i) ≤ length(A)
@inline isgrandchild(j, i) = j > rchild(i)
@inline hasgrandparent(i) = i ≥ 4

"""
    children_and_grandchildren(maxlen, i)

Return the indices of all children and grandchildren of
position `i`.
"""
function children_and_grandchildren(maxlen::T, i::T) where {T <: Integer}
    left, right = children(i)
    _children_and_grandchildren = (left, children(left)..., right, children(right)...)
    return Iterators.filter(<=(maxlen), _children_and_grandchildren)
end

"""
    is_minmax_heap(h::AbstractVector) -> Bool

Return `true` if `A` is a min-max heap. A min-max heap is a
heap where the minimum element is the root and the maximum
element is a child of the root.
"""
function is_minmax_heap(A::AbstractVector)

    for i in 1:length(A)
        if on_minlevel(i)
            # check that A[i] < children A[i]
            #    and grandchildren A[i]
            for j in children_and_grandchildren(length(A), i)
                A[i] ≤ A[j] || return false
            end
        else
            # max layer
            for j in children_and_grandchildren(length(A), i)
                A[i] ≥ A[j] || return false
            end
        end
    end
    return true
end

################################################
#
# interfaces
#
################################################

Base.length(h::BinaryMinMaxHeap) = length(h.valtree)

Base.isempty(h::BinaryMinMaxHeap) = isempty(h.valtree)

"""
    pop!(h::BinaryMinMaxHeap) = popmin!(h)
"""
@inline Base.pop!(h::BinaryMinMaxHeap) = popmin!(h)

function Base.sizehint!(h::BinaryMinMaxHeap, s::Integer)
    sizehint!(h.valtree, s)
    return h
end

"""
    popmin!(h::BinaryMinMaxHeap) -> min

Remove the minimum value from the heap.
"""
function popmin!(h::BinaryMinMaxHeap)
    valtree = h.valtree
    !isempty(valtree) || throw(ArgumentError("heap must be non-empty"))
    @inbounds x = valtree[1]
    y = pop!(valtree)
    if !isempty(valtree)
        @inbounds valtree[1] = y
        @inbounds _minmax_heap_trickle_down!(valtree, 1)
    end
    return x
end

"""
    popmin!(h::BinaryMinMaxHeap, k::Integer) -> vals

Remove up to the `k` smallest values from the heap.
"""
@inline function popmin!(h::BinaryMinMaxHeap, k::Integer)
    return [popmin!(h) for _ in 1:min(length(h), k)]
end

"""
    popmax!(h::BinaryMinMaxHeap) -> max

Remove the maximum value from the heap.
"""
function popmax!(h::BinaryMinMaxHeap)
    valtree = h.valtree
    !isempty(valtree) || throw(ArgumentError("heap must be non-empty"))
    @inbounds x, i = maximum(((valtree[j], j) for j in 1:min(length(valtree), 3)))
    y = pop!(valtree)
    if !isempty(valtree) && i <= length(valtree)
        @inbounds valtree[i] = y
        @inbounds _minmax_heap_trickle_down!(valtree, i)
    end
    return x
end

"""
    popmax!(h::BinaryMinMaxHeap, k::Integer) -> vals

Remove up to the `k` largest values from the heap.
"""
@inline function popmax!(h::BinaryMinMaxHeap, k::Integer)
    return [popmax!(h) for _ in 1:min(length(h), k)]
end

function Base.push!(h::BinaryMinMaxHeap, v)
    valtree = h.valtree
    push!(valtree, v)
    @inbounds _minmax_heap_bubble_up!(valtree, length(valtree))
end

"""
    values(h::BinaryMinMaxHeap)

Returns the elements of the heap in an arbitrary order.
"""
Base.values(h::BinaryMinMaxHeap) = h.valtree

"""
    first(h::BinaryMinMaxHeap)

Get the first (minimum) of the heap.
"""
@inline Base.first(h::BinaryMinMaxHeap) = minimum(h)

@inline function Base.minimum(h::BinaryMinMaxHeap)
    valtree = h.valtree
    !isempty(h) || throw(ArgumentError("heap must be non-empty"))
    return @inbounds h.valtree[1]
end

@inline function Base.maximum(h::BinaryMinMaxHeap)
    valtree = h.valtree
    !isempty(h) || throw(ArgumentError("heap must be non-empty"))
    return @inbounds maximum(@views(valtree[1:min(end, 3)]))
end

Base.empty!(h::BinaryMinMaxHeap) = (empty!(h.valtree); h)

"""
    popall!(h::BinaryMinMaxHeap, ::Ordering = Forward)

Remove and return all the elements of `h` according to
the given ordering. Default is `Forward` (smallest to
largest).
"""
popall!(h::BinaryMinMaxHeap) = popall!(h, Forward)
popall!(h::BinaryMinMaxHeap, ::ForwardOrdering) = popmin!(h, length(h))
popall!(h::BinaryMinMaxHeap, ::ReverseOrdering) = popmax!(h, length(h))


using SynchrotronKernel

"""
    Constants for synchrotron
"""

const c_light    = 2.9979e10
const m_e        = 9.10953e-28
const q_e        = 1.602176487e-20 * c_light
const C_crit     = 3q_e / ( 4π * m_e * c_light ) # Donnert+16, MNRAS 462, 2014–2032 (2016), Eg. 20
                                                        #  -> converted to dimensionless momentum
const γ          = 5.0/3.0
const mJy_factor = 1.e26       # conversion factor from [erg/cm^3/Hz/s] to mJy/cm.

"""
    ν_over_ν_crit(p::Real, B::Real, ν::Real)

Computes the fraction ``x = \frac{ν}{ν_c}`` needed for the synchrotron Kernel.
See Donnert+16, MNRAS 462, 2014–2032 (2016), Eq. 19.
"""
ν_over_ν_crit(p::Real, B::Real, ν::Real, sinθ::Real = 1.0) = ν / (C_crit * B * sinθ * p^2)

"""
    integrate_θ(x_in::Real, θ_steps::Integer=100)

Pitch angle integration in Donnert+16, Eq. 17.
"""
function integrate_θ(x_in::Real, θ_steps::Integer = 64)

    dθ = 0.5π / θ_steps
    K = 0.0
    @inbounds for θ ∈ LinRange(0.0, 0.5π, θ_steps)
        sinθ = sin(θ)
        x = x_in / sinθ
        K += sinθ^2 * synchrotron_kernel(x) * dθ
    end
    return K
end

"""
    integrate_θ(x_in::Real, θ_steps::Integer=100)

Pitch angle integration in Donnert+16, Eq. 17 using Simpson's rule.
"""
function integrate_θ_simpson(x_in::Real, θ_steps::Integer = 50)

    dθ = 0.5π / θ_steps
    #θ = LinRange(0.0, 0.5π, θ_steps)

    F = Vector{Float64}(undef, θ_steps)
    F_mid = Vector{Float64}(undef, θ_steps)

    # sin(0.0) = 0.0
    F[1] = 0.0
    F_mid[1] = sin(0.5dθ)^2 * synchrotron_kernel(x_in / sin(0.5dθ))
    K = 0.0
    @inbounds for i = 2:θ_steps
        # boundary
        sinθ = sin((i - 1) * dθ)
        x = x_in / sinθ
        F[i] = sinθ^2 * synchrotron_kernel(x)

        # mid point
        sinθ = sin((i - 0.5) * dθ)
        x = x_in / sinθ
        F_mid[i] = sinθ^2 * synchrotron_kernel(x)

        # Simpson rule: https://en.wikipedia.org/wiki/Simpson%27s_rule
        K += dθ / 6.0 * (F[i] + F[i-1] + 4F_mid[i])
    end
    return K
end

"""
    synchrotron_emission( f_p::Vector{<:Real},
                                   q::Vector{<:Real},
                                   cut::Real,
                                   B_cgs::Real;
                                   ν0::Real=1.44e9
                                   integrate_pitch_angle::Bool=false,
                                   convert_to_mJy::Bool=false,
                                   reduce_spectrum::Bool = true)

Computes the synchrotron emission (in ``[erg/cm^3/Hz/s]``) for a CR spectrum as described in Donnert+16, MNRAS 462, 2014–2032 (2016), Eq. 17.

# Arguments
- `f_p::Vector{<:Real}`: Spectral Norm of bins in cgs units.
- `q::Vector{<:Real}`:   Slopes of bins.
- `cut::Real`:           Spectral cutoff.
- `B_cgs::Real`:         Magnetic field strength (absolute value) in Gauss.
- `par::CRMomentumDistributionConfig`: Parameters of spectrum

# Keyword Arguments
- `ν0::Real=1.44e9`:                  Observation frequency in ``Hz``.
- `integrate_pitch_angle::Bool=false`: Explicitly integrates over the pitch angle. If `false` assumes ``sin(θ) = 1``.
- `convert_to_mJy::Bool=false`:       Convert the result from ``[erg/cm^3/Hz/s]`` to ``mJy/cm``.
- `reduce_spectrum::Bool=true`:       Return result as spectrum if set to false, otherwise calculate total emission.
"""
function synchrotron_emission( f_p::Vector{<:Real},
                               q::Vector{<:Real},
                               cut::Real,
                               B_cgs::Real,
                               par::CRMomentumDistributionConfig;
                               ν0::Real = 1.44e9,
                               integrate_pitch_angle::Bool = false,
                               convert_to_mJy::Bool = false,
                               reduce_spectrum::Bool = true )

    # prefactor to Eq. 17
    j_ν_prefac = q_e * √(3) / (m_e * c_light^2)

    # include magnetic field into this
    j_ν_prefac *= B_cgs

    # if run without pitch angle integration
    # sinθ = 1.0 -> integral factor π/2
    if !integrate_pitch_angle
        j_ν_prefac *= 0.5π
    end

    # storage array for synchrotron emissivity
    jν = Vector{Float64}(undef, par.Nbins)

    # loop over spectrum
    @inbounds for i = 1:par.Nbins

        # beginning of bin
        p_start = par.pmin * 10.0^((i - 1) * par.bin_width)

        # check if bin is below cutoff
        if p_start > cut
            # set remaining bins to zero
            jν[i:end] = 0
            break
        end

        # x from Eq. 19
        x = ν_over_ν_crit(p_start, B_cgs, ν0)
        # pitch-angle integral
        if integrate_pitch_angle
            K = integrate_θ_simpson(x)
        else
            K = synchrotron_kernel(x)
        end

        # energy density at momentum p * integrated synchrotron kernel
        F_start = 4π * p_start^2 * f_p[i] * K

        # middle of bin
        p_mid = par.pmin * 10.0^((i - 1 / 2) * par.bin_width)
        f_p_mid = f_p[i] * (p_mid / p_start)^(-q[i])

        # x from Eq. 19
        x = ν_over_ν_crit(p_mid, B_cgs, ν0)

        if integrate_pitch_angle
            K = integrate_θ_simpson(x)
        else
            K = synchrotron_kernel(x)
        end

        F_mid = 4π * p_mid^2 * f_p_mid * K

        # end of bin
        p_end = par.pmin * 10.0^(i * par.bin_width)
        if p_end > cut
            p_end = cut
        end

        # construct norm at end of spectrum
        f_p_end = f_p[i] * (p_end / p_start)^(-q[i])

        # x from Eq. 19
        x = ν_over_ν_crit(p_end, B_cgs, ν0)

        if integrate_pitch_angle
            K = integrate_θ_simpson(x)
        else
            K = synchrotron_kernel(x)
        end

        F_end = 4π * p_end^2 * f_p_end * K

        # bin width
        dp = p_end - p_start

        # store total synchrotron emissivity
        # Simpson rule: https://en.wikipedia.org/wiki/Simpson%27s_rule
        jν[i] = dp / 6.0 * (F_start + F_end + 4F_mid)
    end

    if convert_to_mJy
        j_ν_prefac *= mJy_factor
    end

    if reduce_spectrum
        # return total emissivity
        return j_ν_prefac * sum(jν)
    else
        # return emissivity per bin
        return j_ν_prefac .* jν
    end

end


__precompile__()

module NGWP

using LinearAlgebra, SparseArrays, LightGraphs, SimpleWeightedGraphs, Clustering
using JuMP, Clp, Optim, OptimalTransport, MTSG, Statistics, QuadGK, Arpack
import Plots: plot, plot!, scatter, scatter!
import StatsBase: crosscor

filenames = readdir(@__DIR__)

for f in filenames
    if f == "NGWP.jl"
        continue
    elseif occursin(r"^.*\.jl$",f)
        include(joinpath(@__DIR__, f))
    end
end

export eigDAG_Distance, eigHAD_Distance, eigHAD_Affinity
export eigROT_Distance, ROT_Distance, eigEMD_Distance
export eigsROT_Distance
export eigTSD_Distance, K_functional
export natural_eigdist
export SunFlowerGraph, dualgraph
export pc_ngwp, pairclustering
export vm_ngwp, varimax
export lp_ngwp, rising_cutoff, find_pairinds, pair_inds_shadding
export LPHGLET_Synthesis, LPHGLET_Analysis_All, HGLET_dictionary, LPHGLET_dictionary
export unitary_folding_operator, keep_folding!
export ngwp_analysis, ngwp_bestbasis, NGWP_jkl
export natural_eigdist
export nat_spec_filter, ngwf_all_vectors, rngwf_all_vectors, ngwf_vector, frame_approx, rngwf_lx
export scatter_gplot, scatter_gplot!, wiggle, wiggle!
export standardize_eigenvectors!, spike, characteristic, χ, sort_wavelets, transform2D
export getall_expansioncoeffs, approx_error_plot, getall_expansioncoeffs2, approx_error_plot2

end


using LinearAlgebra
using Test

#include("test_solve_eye_minus_a_kron_b.jl")
include("test_solve_eye_plus_at_kron_b.jl")


__precompile__()
module JFVMM

import Base: +, -, *, /, .*, ./
export MeshStructure, BoundaryCondition, CellValue, FaceValue, CellVector,
       arithmeticMean, geometricMean, harmonicMean, upwindMean, linearMean,
       tvdMean, createBC, boundaryConditionTerm, cellBoundary, solvePDE,
       divergenceTerm, gradientTerm, convectionUpwindTerm, createCellVector,
       convectionTerm, convectionTvdTerm, diffusionTerm, createCellVariable,
       createFaceVariable, copyCell, fluxLimiter, createMesh1D,
       createMesh2D, createMesh3D, createMeshRadial2D, createMeshCylindrical2D,
       createMeshCylindrical3D, createMeshCylindrical1D, solveLinearPDE,
       linearSourceTerm, constantSourceTerm, transientTerm,
       solveMUMPSLinearPDE, faceEval, cellEval, permfieldlogrndg, permfieldlogrnde,
       # plot, imshow, xlabel, ylabel, figure, legend, pcolor, contour, colorbar,
       JFVM_test, solveExplicitPDE, reshapeCell,
       cellVolume, reshapeInternalCell, internalCells, domainInt, convectionTvdRHS

# Ignore visualization for now
# visualizeCellVectors
# visualizeCells

include("jfvmm_1types.jl")
include("jfvmm_2meshstructure.jl")
include("jfvmm_3boundarycondition.jl")
include("jfvmm_4domain_variables.jl")
include("jfvmm_5diffusion_terms.jl")
include("transientTerms.jl")
include("domainOperators.jl")
include("convectionTerms.jl")
include("averagingTerms.jl")
include("calculusTerms.jl")
include("sourceTerms.jl")
include("solveVisualizePDE.jl")
include("JFVMtools.jl")
include("jfvm_test.jl")

end # module


module Day25
__revise_mode__ = :eval

using Advent: save_input

const m = 20201227

function loopsize(target)
    x = 1
    for i in Iterators.countfrom()
        x = rem(x * 7, m)
        x == target && return i
    end
end

function solve_a(filename)
    cpub, dpub = parse.(Int, readlines(filename))
    cloops = loopsize(cpub)
    dloops = loopsize(dpub)
    resd = powermod(dpub, cloops, m)
    resc = powermod(cpub, dloops, m)
    @assert resd == resc
    resd
end

solve_b(filename) = nothing

solve(filename = save_input(2020, 25)) = solve_a(filename), solve_b(filename)

end


module TrafficAssignment

include("TAFW/TAFW.jl")
include("TAPAS/TAPAS.jl")

using .TAFW
using .TAPAS
using DataFrames
using Plots

"""
    assigntraffic(network; method=:FW, assignment=:UE, tol=1e-5, maxiters=20, maxruntime=300, log=:off)

Fukushima Frank-Wolfe method for traffic assignment.

# Returns
a named tuple with keys `:metadata`, `:report`, and `:output`
- `metadata::String`  : Text defining the traffic assignment run
- `report::DataFrame` : A log of total network flow, total network cost, and run time for every iteration
- `output::DataFrame` : Flow and cost for every arc from the final iteration

# Arguments
- `network::String`         : Network
- `method::Symbol=:FW`      : One of `:FW`, `:fukushimaFW`, `:conjugateFW`
- `assignment::Symbol=:UE`  : Assignment type; one of `:UE`, `:SO`
- `tol::Float64=1e-5`       : Tolerance level for relative gap
- `maxiters::Int64=20`      : Maximum number of iterations
- `maxruntime::Int64=300`   : Maximum algorithm run time (seconds)
"""
function assigntraffic(network; method=:FW, assignment=:UE, tol=1e-5, maxiters=20, maxruntime=300, log=:off)
    if method == :FW
        G = TAFW.build(network, assignment)
        return FW(G, tol, maxiters, maxruntime, log)
    elseif method == :fukushimaFW
        G = TAFW.build(network, assignment)
        return fukushimaFW(G, tol, maxiters, maxruntime, log)
    elseif method == :conjugateFW
        G = TAFW.build(network, assignment)
        return conjugateFW(G, tol, maxiters, maxruntime, log)
    elseif method == :TAPAS
        G = TAPAS.build(network, assignment)
        return itapas(G, tol, maxiters, maxruntime, log)
    else return (metadata="", report=DataFrame(), solution=DataFrame())
    end
end

"""
    compare(network; methods, assignment=:UE, tol=1e-5, maxiters=20, maxruntime=300, log=:on)

Compare assignment methods.
"""
function compare(network; methods, assignment=:UE, tol=1e-5, maxiters=20, maxruntime=300, log=:on)
    fig = plot()

    for method in methods
        _, report, = assigntraffic(network; method=method, assignment=assignment, tol=tol, maxiters=maxiters, maxruntime=maxruntime, log=log)
        y = report[!,:LOG₁₀RG]
        x = 0:length(y)-1
        plot!(x,y, label=String(method))
    end
    display(fig)
end

"""
    getrg(network, solution::DataFrame)

Returns relative gap for `network` given traffic assignment `solution`.
"""
function getrg(network, solution::DataFrame)
    build, djk, path = TAFW.build, TAFW.djk, TAFW.path  # NOTE: Can chose any assignment method tools to calculate relative gap
    G = build(network, :none)
    N, A, O, K = G.N, G.A, G.O, G.K

    for row in 1:nrow(solution)
        i = solution[row, :FROM]::Int64
        j = solution[row, :TO]::Int64
        x = solution[row, :FLOW]
        c = solution[row, :COST]
        k = K[i,j]
        a = A[k]
        a.x = x
        a.c = c
    end

    num = 0.0
    for (i,o) in enumerate(O)
        r = o.n
        Lᵖ = djk(G, o)
        for (j,s) in enumerate(o.S)
            qᵣₛ = o.Q[j]
            pᵣₛ = path(G, Lᵖ, r, s)
            for a in pᵣₛ num += qᵣₛ * a.c end
        end
    end

    den = 0.0
    for (k,a) in enumerate(A) den += a.x * a.c end

    rg = 1 - num/den
    return rg
end

export assigntraffic

end
# TODO: Save output from traffic assignment solution


function kalman_smoother!(
    smoother_output::SmootherOutput,
    system::LinearUnivariateTimeInvariant,
    filter_output::FilterOutput,
)
    multivariate_system = to_multivariate_time_variant(system)
    return kalman_smoother!(smoother_output, multivariate_system, filter_output)
end

function kalman_smoother!(
    smoother_output::SmootherOutput,
    system::LinearMultivariateTimeVariant{Fl},
    filter_output::FilterOutput,
) where Fl
    # Load dimensions data
    n = size(system.y, 1)
    m = size(system.T[1], 1)

    # Load filter data
    v = filter_output.v
    F = filter_output.F
    a = filter_output.a
    P = filter_output.P

    # Smoothed state and its covariance
    L = Vector{Matrix{Fl}}(undef, n)
    r = Vector{Vector{Fl}}(undef, n)
    N = Vector{Matrix{Fl}}(undef, n)

    # Initialization
    N[end] = zeros(Fl, m, m)
    r[end] = zeros(Fl, m)

    @inbounds for t in n:-1:2
        if any(y_ -> isnan(y_), system.y[t, :])
            r[t - 1] = system.T'[t] * r[t]
            N[t - 1] = system.T'[t] * N[t] * system.T[t]
        else
            Z_transp_invF = system.Z[t]' * inv(F[t])
            K = system.T[t] * P[t] * Z_transp_invF
            L[t] = system.T[t] - K * system.Z[t]
            r[t - 1] = Z_transp_invF * v[t] + L[t]' * r[t]
            N[t - 1] = Z_transp_invF * system.Z[t] + L[t]' * N[t] * L[t]
        end
        smoother_output.alpha[t] = a[t] + P[t] * r[t - 1]
        smoother_output.V[t] = P[t] - P[t] * N[t - 1] * P[t]
    end

    # Last iteration
    if any(y_ -> isnan(y_), system.y[1, :])
        r0 = system.T'[1] * r[1]
        N0 = system.T'[1] * N[1] * system.T[1]
    else
        Z_transp_invF = system.Z[1]' * inv(F[1])
        K = system.T[1] * P[1] * Z_transp_invF
        L[1] = system.T[1] - K * system.Z[1]
        r0 = Z_transp_invF * v[1] + L[1]' * r[1]
        N0 = Z_transp_invF * system.Z[1] + L[1]' * N[1] * L[1]
    end
    smoother_output.alpha[1] = a[1] + P[1] * r0
    smoother_output.V[1] = P[1] - P[1] * N0 * P[1]
    return smoother_output
end


@doc raw"""
    GeneralLinear{n,𝔽} <:
        AbstractGroupManifold{𝔽,MultiplicationOperation,DefaultEmbeddingType}

The general linear group, that is, the group of all invertible matrices in ``𝔽^{n×n}``.

The default metric is the left-``\mathrm{GL}(n)``-right-``\mathrm{O}(n)``-invariant metric
whose inner product is
```math
⟨X_p,Y_p⟩_p = ⟨p^{-1}X_p,p^{-1}Y_p⟩_\mathrm{F} = ⟨X_e, Y_e⟩_\mathrm{F},
```
where ``X_p, Y_p ∈ T_p \mathrm{GL}(n, 𝔽)``,
``X_e = p^{-1}X_p ∈ 𝔤𝔩(n) = T_e \mathrm{GL}(n, 𝔽) = 𝔽^{n×n}`` is the corresponding
vector in the Lie algebra, and ``⟨⋅,⋅⟩_\mathrm{F}`` denotes the Frobenius inner product.

By default, tangent vectors ``X_p`` are represented with their corresponding Lie algebra
vectors ``X_e = p^{-1}X_p``.
"""
struct GeneralLinear{n,𝔽} <:
       AbstractGroupManifold{𝔽,MultiplicationOperation,DefaultGroupDecoratorType} end

GeneralLinear(n, 𝔽::AbstractNumbers=ℝ) = GeneralLinear{n,𝔽}()

function allocation_promotion_function(::GeneralLinear{n,ℂ}, f, ::Tuple) where {n}
    return complex
end

function check_point(G::GeneralLinear, p; kwargs...)
    mpv = check_point(decorated_manifold(G), p; kwargs...)
    mpv === nothing || return mpv
    detp = det(p)
    if iszero(detp)
        return DomainError(
            detp,
            "The matrix $(p) does not lie on $(G), since it is not invertible.",
        )
    end
    return nothing
end
check_point(::GT, ::Identity{GT}; kwargs...) where {GT<:GeneralLinear} = nothing
function check_point(G::GeneralLinear, e::Identity; kwargs...)
    return DomainError(e, "The identity element $(e) does not belong to $(G).")
end

function check_vector(G::GeneralLinear, p, X; kwargs...)
    mpv = check_vector(decorated_manifold(G), p, X; kwargs...)
    mpv === nothing || return mpv
    return nothing
end

decorated_manifold(::GeneralLinear{n,𝔽}) where {n,𝔽} = Euclidean(n, n; field=𝔽)

default_metric_dispatch(::GeneralLinear, ::EuclideanMetric) = Val(true)
default_metric_dispatch(::GeneralLinear, ::LeftInvariantMetric{EuclideanMetric}) = Val(true)

distance(G::GeneralLinear, p, q) = norm(G, p, log(G, p, q))

@doc raw"""
    exp(G::GeneralLinear, p, X)

Compute the exponential map on the [`GeneralLinear`](@ref) group.

The exponential map is
````math
\exp_p \colon X ↦ p \operatorname{Exp}(X^\mathrm{H}) \operatorname{Exp}(X - X^\mathrm{H}),
````

where ``\operatorname{Exp}(⋅)`` denotes the matrix exponential, and ``⋅^\mathrm{H}`` is
the conjugate transpose. [^AndruchowLarotondaRechtVarela2014][^MartinNeff2016]

[^AndruchowLarotondaRechtVarela2014]:
    > Andruchow E., Larotonda G., Recht L., and Varela A.:
    > “The left invariant metric in the general linear group”,
    > Journal of Geometry and Physics 86, pp. 241-257, 2014.
    > doi: [10.1016/j.geomphys.2014.08.009](https://doi.org/10.1016/j.geomphys.2014.08.009),
    > arXiv: [1109.0520v1](https://arxiv.org/abs/1109.0520v1).
[^MartinNeff2016]:
    > Martin, R. J. and Neff, P.:
    > “Minimal geodesics on GL(n) for left-invariant, right-O(n)-invariant Riemannian metrics”,
    > Journal of Geometric Mechanics 8(3), pp. 323-357, 2016.
    > doi: [10.3934/jgm.2016010](https://doi.org/10.3934/jgm.2016010),
    > arXiv: [1409.7849v2](https://arxiv.org/abs/1409.7849v2).
"""
exp(::GeneralLinear, p, X)

function exp!(G::GeneralLinear, q, p, X)
    expX = exp(X)
    if isnormal(X; atol=sqrt(eps(real(eltype(X)))))
        return compose!(G, q, p, expX)
    end
    compose!(G, q, expX', exp(X - X'))
    compose!(G, q, p, q)
    return q
end
function exp!(::GeneralLinear{1}, q, p, X)
    p1 = p isa Identity ? p : p[1]
    q[1] = p1 * exp(X[1])
    return q
end
function exp!(G::GeneralLinear{2}, q, p, X)
    if isnormal(X; atol=sqrt(eps(real(eltype(X)))))
        return compose!(G, q, p, exp(SizedMatrix{2,2}(X)))
    end
    A = SizedMatrix{2,2}(X')
    B = SizedMatrix{2,2}(X) - A
    compose!(G, q, exp(A), exp(B))
    compose!(G, q, p, q)
    return q
end

function get_coordinates(
    ::GeneralLinear{n,ℝ},
    p,
    X,
    ::DefaultOrthonormalBasis{ℝ,TangentSpaceType},
) where {n}
    return vec(X)
end

function get_coordinates!(
    ::GeneralLinear{n,ℝ},
    Xⁱ,
    p,
    X,
    ::DefaultOrthonormalBasis{ℝ,TangentSpaceType},
) where {n}
    return copyto!(Xⁱ, X)
end

function get_vector(
    ::GeneralLinear{n,ℝ},
    p,
    Xⁱ,
    ::DefaultOrthonormalBasis{ℝ,TangentSpaceType},
) where {n}
    return reshape(Xⁱ, n, n)
end

function get_vector!(
    ::GeneralLinear{n,ℝ},
    X,
    p,
    Xⁱ,
    ::DefaultOrthonormalBasis{ℝ,TangentSpaceType},
) where {n}
    return copyto!(X, Xⁱ)
end

function group_exp!(::GeneralLinear{1}, q, X)
    q[1] = exp(X[1])
    return q
end
group_exp!(::GeneralLinear{2}, q, X) = copyto!(q, exp(SizedMatrix{2,2}(X)))

function group_log!(::GeneralLinear{1}, X::AbstractMatrix, p::AbstractMatrix)
    X[1] = log(p[1])
    return X
end

inner(::GeneralLinear, p, X, Y) = dot(X, Y)

invariant_metric_dispatch(::GeneralLinear, ::LeftAction) = Val(true)

inverse_translate_diff(::GeneralLinear, p, q, X, ::LeftAction) = X
inverse_translate_diff(::GeneralLinear, p, q, X, ::RightAction) = p * X / p

function inverse_translate_diff!(G::GeneralLinear, Y, p, q, X, conv::ActionDirection)
    return copyto!(Y, inverse_translate_diff(G, p, q, X, conv))
end

# find sU for s ∈ S⁺ and U ∈ U(n, 𝔽) that minimizes ‖sU - p‖²
function _project_Un_S⁺(p)
    n = LinearAlgebra.checksquare(p)
    F = svd(p)
    s = mean(F.S)
    U = F.U * F.Vt
    return rmul!(U, s)
end

@doc raw"""
    log(G::GeneralLinear, p, q)

Compute the logarithmic map on the [`GeneralLinear(n)`](@ref) group.

The algorithm proceeds in two stages. First, the point ``r = p^{-1} q`` is projected to the
nearest element (under the Frobenius norm) of the direct product subgroup
``\mathrm{O}(n) × S^+``, whose logarithmic map is exactly computed using the matrix
logarithm. This initial tangent vector is then refined using the
[`NLsolveInverseRetraction`](@ref).

For `GeneralLinear(n, ℂ)`, the logarithmic map is instead computed on the realified
supergroup `GeneralLinear(2n)` and the resulting tangent vector is then complexified.

Note that this implementation is experimental.
"""
log(::GeneralLinear, p, q)

function log!(G::GeneralLinear{n,𝔽}, X, p, q) where {n,𝔽}
    pinvq = inverse_translate(G, p, q, LeftAction())
    𝔽 === ℝ && det(pinvq) ≤ 0 && throw(OutOfInjectivityRadiusError())
    e = Identity(G, pinvq)
    if isnormal(pinvq; atol=sqrt(eps(real(eltype(pinvq)))))
        log_safe!(X, pinvq)
    else
        # compute the equivalent logarithm on GL(dim(𝔽) * n, ℝ)
        # this is significantly more stable than computing the complex algorithm
        Gᵣ = GeneralLinear(real_dimension(𝔽) * n, ℝ)
        pinvqᵣ = realify(pinvq, 𝔽)
        Xᵣ = realify(X, 𝔽)
        eᵣ = Identity(Gᵣ, pinvqᵣ)
        log_safe!(Xᵣ, _project_Un_S⁺(pinvqᵣ))
        inverse_retraction = NLsolveInverseRetraction(ExponentialRetraction(), Xᵣ)
        inverse_retract!(Gᵣ, Xᵣ, eᵣ, pinvqᵣ, inverse_retraction)
        unrealify!(X, Xᵣ, 𝔽, n)
    end
    translate_diff!(G, X, p, e, X, LeftAction())
    return X
end
function log!(::GeneralLinear{1}, X, p, q)
    p1 = p isa Identity ? p : p[1]
    X[1] = log(p1 \ q[1])
    return X
end

manifold_dimension(G::GeneralLinear) = manifold_dimension(decorated_manifold(G))

LinearAlgebra.norm(::GeneralLinear, p, X) = norm(X)

project(::GeneralLinear, p) = p
project(::GeneralLinear, p, X) = X

project!(::GeneralLinear, q, p) = copyto!(q, p)
project!(::GeneralLinear, Y, p, X) = copyto!(Y, X)

Base.show(io::IO, ::GeneralLinear{n,𝔽}) where {n,𝔽} = print(io, "GeneralLinear($n, $𝔽)")

translate_diff(::GeneralLinear, p, q, X, ::LeftAction) = X
translate_diff(::GeneralLinear, p, q, X, ::RightAction) = p \ X * p

function translate_diff!(G::GeneralLinear, Y, p, q, X, conv::ActionDirection)
    return copyto!(Y, translate_diff(G, p, q, X, conv))
end

vector_transport_to(::GeneralLinear, p, X, q, ::ParallelTransport) = X

vector_transport_to!(::GeneralLinear, Y, p, X, q, ::ParallelTransport) = copyto!(Y, X)


export  Rectifier,
        square, dsquare, d2square,
        softplus, dsoftplus, d2softplus, invsoftplus,
        explinearunit, dexplinearunit, d2explinearunit, invexplinearunit,
        inverse!, inverse, vinverse,
        grad_x!, grad_x, vgrad_x,
        grad_x_logeval, grad_x_logeval!, vgrad_x_logeval,
        hess_x_logeval, hess_x_logeval!, vhess_x_logeval,
        hess_x!, hess_x, vhess_x,
        evaluate!, vevaluate

"""
$(TYPEDEF)

This structure defines a continuous rectifier g,
i.e. a positive and monotonically increasing function
(e.g. square function, exponential , softplus, explinearunit).

## Fields

$(TYPEDFIELDS)

"""
struct Rectifier
    T::String
end

square(x) = x^2
dsquare(x) = 2.0*x
d2square(x) = 2.0

# Softplus tools
softplus(x) = (log(1.0 + exp(-abs(log(2.0)*x))) + max(log(2.0)*x, 0.0))/log(2.0)
dsoftplus(x) = 1/(1 + exp(-log(2.0)*x))
d2softplus(x) = log(2.0)/(2.0*(1.0 + cosh(log(2.0)*x)))
invsoftplus(x) = min(log(exp(log(2.0)*x) - 1.0)/log(2.0), x)

explinearunit(x) = x < 0.0 ? exp(x) : x + 1.0
dexplinearunit(x) = x < 0.0 ? exp(x) : 1.0
d2explinearunit(x) = x < 0.0 ? exp(x) : 0.0
invexplinearunit(x) = x < 1.0 ? log(x) : x - 1.0

# Type of the rectifier should be in the following list:
# "squared", "exponential", "softplus", "explinearunit"

Rectifier() = Rectifier("softplus")

function (g::Rectifier)(x)
    if g.T=="squared"
        return square(x)
    elseif g.T=="exponential"
        return exp(x)
    elseif g.T=="softplus"
        return softplus(x)
    elseif g.T=="explinearunit"
        return explinearunit(x)
    end
end

function evaluate!(result, g::Rectifier, x)
    @assert size(result,1) == size(x,1) "Dimension of result and x don't match"
    if g.T=="squared"
        vmap!(square, result, x)
        return result
    elseif g.T=="exponential"
        vmap!(exp, result, x)
        return result
    elseif g.T=="softplus"
        vmap!(softplus, result, x)
        return result
    elseif g.T=="explinearunit"
        vmap!(explinearunit, result, x)
        return result
    end
end

vevaluate(g::Rectifier, x) = evaluate!(zero(x), g, x)

function inverse(g::Rectifier, x)
    @assert x>=0 "Input to rectifier is negative"
    if g.T=="squared"
        error("squared rectifier is not invertible")
    elseif g.T=="exponential"
        return log(x)
    elseif g.T=="softplus"
        return invsoftplus(x)
    elseif g.T=="explinearunit"
        return invexplinearunit(x)
    end
end

function inverse!(result, g::Rectifier, x)
    @assert all(x .> 0) "Input to rectifier is negative"
    @assert size(result,1) == size(x,1) "Dimension of result and x don't match"
    if g.T=="squared"
        error("squared rectifier is not invertible")
    elseif g.T=="exponential"
        vmap!(log, result, x)
        return result
    elseif g.T=="softplus"
        vmap!(invsoftplus, result, x)
        return result
    elseif g.T=="explinearunit"
        vmap!(invexplinearunit, result, x)
        return result
    end
end

vinverse(g::Rectifier, x)  = inverse!(zero(x), g, x)

function grad_x(g::Rectifier, x)
    if g.T=="squared"
        return dsquare(x)
    elseif g.T=="exponential"
        return exp(x)
    elseif g.T=="softplus"
        return dsoftplus(x)
    elseif g.T=="explinearunit"
        return dexplinearunit(x)
    end
end

function grad_x!(result, g::Rectifier, x)
    @assert size(result,1) == size(x,1) "Dimension of result and x don't match"
    if g.T=="squared"
        vmap!(dsquare, result, x)
        return result
    elseif g.T=="exponential"
        vmap!(exp, result, x)
        return result
    elseif g.T=="softplus"
        vmap!(dsoftplus, result, x)
        return result
    elseif g.T=="explinearunit"
        vmap!(dexplinearunit, result, x)
        return result
    end
end

vgrad_x(g::Rectifier, x) = grad_x!(zero(x), g, x)

""" Compute g′(x)/g(x) i.e d/dx log(g(x))"""
function grad_x_logeval(g::Rectifier, x::T) where {T <: Real}
    if g.T=="squared"
        return dsquare(x)/square(x)
    elseif g.T=="exponential"
        return 1.0
    elseif g.T=="softplus"
        return dsoftplus(x)/softplus(x)
    elseif g.T=="explinearunit"
        return dexplinearunit(x)/explinearunit(x)
    end
end

function grad_x_logeval!(result, g::Rectifier, x)
    @assert size(result,1) == size(x,1) "Dimension of result and x don't match"
    if g.T=="squared"
        vmap!(xi->dsquare(xi)/square(xi), result, x)
        return result
    elseif g.T=="exponential"
        vmap!(1.0, result, x)
        return result
    elseif g.T=="softplus"
        vmap!(xi->dsoftplus(xi)/softplus(xi), result, x)
        return result
    elseif g.T=="explinearunit"
        vmap!(xi->dexplinearunit(xi)/explinearunit(xi), result, x)
        return result
    end
end

vgrad_x_logeval(g::Rectifier, x) = grad_x_logeval!(zero(x), g, x)

# Compute (g″(x)g(x)-g′(x)^2)/g(x) i.e d^2/dx^2 log(g(x))
function hess_x_logeval(g::Rectifier, x::T) where {T <: Real}
    if g.T=="squared"
        return (d2square(x)*square(x) - dsquare(x)^2)/square(x)^2
    elseif g.T=="exponential"
        return 0.0
    elseif g.T=="softplus"
        return (d2softplus(x)*softplus(x) - dsoftplus(x)^2)/softplus(x)^2
    elseif g.T=="explinearunit"
        return (d2explinearunit(x)*explinearunit(x) - dexplinearunit(x)^2)/explinearunit(x)^2
    end
end

function hess_x_logeval!(result, g::Rectifier, x)
    @assert size(result,1) == size(x,1) "Dimension of result and x don't match"
    if g.T=="squared"
        vmap!(xi->(d2square(xi)*square(xi) - dsquare(xi)^2)/square(xi)^2, result, x)
        return result
    elseif g.T=="exponential"
        vmap!(0.0, result, x)
        return result
    elseif g.T=="softplus"
        vmap!(xi->(d2softplus(xi)*softplus(xi) - dsoftplus(xi)^2)/softplus(xi)^2, result, x)
        return result
    elseif g.T=="explinearunit"
        vmap!(xi->(d2explinearunit(xi)*explinearunit(xi) - dexplinearunit(xi)^2)/explinearunit(xi)^2, result, x)
        return result
    end
end

vhess_x_logeval(g::Rectifier, x) = hess_x_logeval!(zero(x), g, x)

function hess_x(g::Rectifier, x::T) where {T <: Real}
    if g.T=="squared"
        return d2square(x)
    elseif g.T=="exponential"
        return exp(x)
    elseif g.T=="softplus"
        return d2softplus(x)
    elseif g.T=="explinearunit"
        return d2explinearunit(x)
    end
end

function hess_x!(result, g::Rectifier, x)
    @assert size(result,1) == size(x,1) "Dimension of result and x don't match"
    if g.T=="squared"
        vmap!(d2square, result, x)
        return result
    elseif g.T=="exponential"
        vmap!(exp, result, x)
        return result
    elseif g.T=="softplus"
        vmap!(d2softplus, result, x)
        return result
    elseif g.T=="explinearunit"
        vmap!(d2explinearunit, result, x)
        return result
    end
end

vhess_x(g::Rectifier, x) = hess_x!(zero(x), g, x)


######
##FEM Poisson dx Convergence Tests
######
using FiniteElementDiffEq, DiffEqDevTools,DiffEqProblemLibrary

cs = cs_fempoisson_wave

sim = test_convergence(cs,FEMDiffEqPoisson())

#Plot Result
#plot(sim,xguide="Delta x")

#Returns true if convergence is like dx^2 in L2
sim.𝒪est[:L2]-2 <.1


using VectorAutoregressions, Base.Test

# comparison against http://www-personal.umich.edu/~lkilian/figure9_1_chol.zip
y = readdlm("cholvar_test_data.csv",',')

V = VAR(y, 4, true)

@test V.p == 4
@test V.Σ ≈ [
        312.524637669663 0.773643992117187 0.919341486304653
        0.773643992117187 0.0515304724687740 0.0148802276681216
        0.919341486304653 0.0148802276681216 0.557021891337960
        ]

@test V.β[:,1:4] ≈ [
        -0.680676727884458 -0.00637960178073601 0.936519611007488 1.75104737240770
        -0.0152242907833718 0.00177981748889471 0.599086054531876 0.0281955980964510
        0.436329751995733 -0.00236721376013309 -0.218737236527547 0.332361646200618
        ]

@test V.ϵ[:,1:4] ≈ [
        70.3934221013155 -0.443580106581005 4.21164200500382 1.22299717886091
        0.189207494365181 0.287694213161629 0.692414550347607 0.405418261036420
        -1.39379479232130 0.277340573620367 -0.573151724739961 -0.395501452903569
        ]

mIRFa = IRFs_a(V,4,true)

@test [mIRFa.IRF[1,1:4]'; mIRFa.IRF[4,1:4]'; mIRFa.IRF[7,1:4]'] ≈ [
        17.6783663744607 0.0192642533976305 -2.85156060358692 0.594909295963393
        0.0437621879606954 0.0591478592307352 0.113331057827868 0.109932348126692
        0.0520037579735191 -0.0341368375885218 -0.111284843056732 -0.0491881671489620
        ]

mIRFb = IRFs_b(V,4,10,true)

@test [mIRFb.IRF[1,1:4]'; mIRFb.IRF[4,1:4]'; mIRFb.IRF[7,1:4]'] ≈ [
        17.6783663744607 0.0192642533976305 -2.85156060358692 0.594909295963393
        0.0437621879606954 0.0591478592307352 0.113331057827868 0.109932348126692
        0.0520037579735191 -0.0341368375885218 -0.111284843056732 -0.0491881671489620
        ]