# order 2
h = [1//2, 1, 1, 1, 1, 1, 1]
e = [1//1, 0, 0, 0, 0, 0, 0]
d1 = [-3//2, 2, -1//2, 0, 0, 0, 0]
d2 = [1//1, -2, 1, 0, 0, 0, 0]
d3 = [-1//1, 3, -3, 1, 0, 0, 0]
H = Diagonal(h)

q1 = [0 1//2 0 0 0 0 0]
q2 = [-1//2 0 1//2 0 0 0 0]
q3 = [0 -1//2 0 1//2 0 0 0]
q4 = [0 0 -1//2 0 1//2 0 0]
q5 = [0 0 0 -1//2 0 1//2 0]
q6 = [0 0 0 0 -1//2 0 1//1]
q7 = [0 0 0 0 0 -1//2 0]
Q = vcat(q1, q2, q3, q4, q5, q6, q7)

m1 = [1//1 -1 0 0 0 0 0]
m2 = [m1[2] 2 -1 0 0 0 0]
m3 = [0 -1 2 -1 0 0 0]
m4 = [0 0 -1 2 -1 0 0]
m5 = [0 0 0 -1 2 -1 0]
m6 = [0 0 0 0 -1 2 -1]
m7 = [0 0 0 0 0 -1 2 ]
M = vcat(m1, m2, m3, m4, m5, m6, m7)

r1 = [0 -13//16 7//8 -1//16 0 0 0]
r2 = [-r1[2] 0 -23//16 5//8 0 0 0]
r3 = [-r1[3] -r2[3] 0 -17//16 1//2 0 0]
r4 = [0 -1//2 2//2 0 -2//2 1//2 0]
r5 = [0 0 -1//2 2//2 0 -2//2 1//2]
r6 = [0 0 0 -1//2 2//2 0 -2//2]
r7 = [0 0 0 0 -1//2 2//2 0]
R = vcat(r1, r2, r3, r4, r5, r6, r7)

n1 = [13//10 -12//5 9//10 1//5 0 0 0]
n2 = [n1[2] 26//5 -16//5 2//5 0 0 0]
n3 = [n1[3] n2[3] 47//10 -17//5 1 0 0]
n4 = [n1[4] n2[4] n3[4] -29//5 -4 1 0]
n5 = [0 0 1//1 -4 6 -4 1]
n6 = [0 0 0 1//1 -4 6 -4]
n7 = [0 0 0 0 1//1 -4 6]
N = vcat(n1, n2, n3, n4, n5, n6, n7)

D1 = H \ (Q - 1//2*e*e'); display(D1)
D2 = H \ (-M - e*d1'); display(D2)
D3 = H \ (R + 1//2*d1*d1' - e*d2'); display(D3)
D4 = H \ (N - e*d3' + d1*d2'); display(D4)

x=1//1:length(h)
ks = 0:5
for k in ks
    u=x.^k
    println("k = ", k)
    println(D1*u - k*x.^(k-1))
end
for k in ks
    u=x.^k
    println("k = ", k)
    println(D2*u - k*(k-1)*x.^(k-2))
end
for k in ks
    u=x.^k
    println("k = ", k)
    println(D3*u - k*(k-1)*(k-2)*x.^(k-3))
end
for k in ks
    u=x.^k
    println("k = ", k)
    println(D4*u - k*(k-1)*(k-2)*(k-3)*x.^(k-4))
end

# order 4
h = [35809//100800,
    13297//11200,
    5701//5600,
    45109//50400,
    35191//33600,
    33503//33600,
    1, 1, 1, 1]
e = [1//1, 0, 0, 0, 0, 0, 0, 0, 0, 0]
d1 = [-11//6, 18//6, -9//6, 2//6, 0, 0, 0, 0, 0, 0]
d2 = [2//1, -5, 4, -1, 0, 0, 0, 0, 0, 0]
d3 = [-1//1, 3, -3, 1, 0, 0, 0, 0, 0, 0]
H = Diagonal(h)

q1 = [0 526249//907200 -10819//777600 -50767//907200 -631//28800 91//7776 0 0 0 0]
q2 = [-q1[2] 0 1421209//2721600 16657//201600 -8467//453600 -33059//5443200 0 0 0 0]
q3 = [-q1[3] -q2[3] 0 631187//1360800 400139//5443200 -8789//302400 0 0 0 0]
q4 = [-q1[4] -q2[4] -q3[4] 0 496403//907200 -308533//5443200 0 0 0 0]
q5 = [-q1[5] -q2[5] -q3[5] -q4[5] 0 1805647//2721600 -1//12 0 0 0]
q6 = [-q1[6] -q2[6] -q3[6] -q4[6] -q5[6] 0 8//12 -1//12 0 0]
q7 = [0 0 0 0 1//12 -8//12 0 8//12 -1//12 0]
q8 = [0 0 0 0 0 1//12 -8//12 0 8//12 -1//12]
q9 = [0 0 0 0 0 0 1//12 -8//12 0 8//12]
q10 = [0 0 0 0 0 0 0 1//12 -8//12 0]
Q = vcat(q1, q2, q3, q4, q5, q6, q7, q8, q9, q10)

m1 = [2386127//2177280 -515449//453600 -10781//777600 61567//1360800 6817//403200 -1069//136080 0 0 0 0]
m2 = [m1[2] 4756039//2177280 -1270009//1360800 -3751//28800 3067//680400 119459//10886400 0 0 0 0]
m3 = [m1[3] m2[3] 111623//60480 -555587//680400 -551339//5443200 8789//453600 0 0 0 0]
m4 = [m1[4] m2[4] m3[4] 1025327//544320 -464003//453600 222133//5443200 0 0 0 0]
m5 = [m1[5] m2[5] m3[5] m4[5] 5074159//2177280 -1784047//1360800 1//12 0 0 0]
m6 = [m1[6] m2[6] m3[6] m4[6] m5[6] 1812749//725760 -16//12 1//12 0 0]
m7 = [0 0 0 0 1//12 -16//12 30//12 -16//12 1//12 0]
m8 = [0 0 0 0 0 1//12 -16//12 30//12 -16//12 1//12]
m9 = [0 0 0 0 0 0 1//12 -16//12 30//12 -16//12]
m10 = [0 0 0 0 0 0 0 1//12 -16//12 30//12]
M = vcat(m1, m2, m3, m4, m5, m6, m7, m8, m9, m10)

r1 = [0 -88471//67200 58139//33600 -1167//2800 -89//11200 7//640 0 0 0 0]
r2 = [-r1[2] 0 -43723//16800 46783//33600 -191//3200 -1567//33600 0 0 0 0]
r3 = [-r1[3] -r2[3] 0 -4049//2400 29083//33600 -71//1400 0 0 0 0]
r4 = [-r1[4] -r2[4] -r3[4] 0 -8591//5600 10613//11200 -1//8 0 0 0]
r5 = [-r1[5] -r2[5] -r3[5] -r4[5] 0 -108271//67200 8//8 -1//8 0 0]
r6 = [-r1[6] -r2[6] -r3[6] -r4[6] -r5[6] 0 -13//8 8//8 -1//8 0]
r7 = [0 0 0 1//8 -8//8 13//8 0 -13//8 8//8 -1//8]
r8 = [0 0 0 0 1//8 -8//8 13//8 0 -13//8 8//8]
r9 = [0 0 0 0 0 1//8 -8//8 13//8 0 -13//8]
r10 = [0 0 0 0 0 0 1//8 -8//8 13//8 0]
R = vcat(r1, r2, r3, r4, r5, r6, r7, r8, r9, r10)

n1 = [4596181//1814400 -10307743//1814400 160961//43200 -535019//907200 109057//1814400 -29273//604800 0 0 0 0]
n2 = [n1[2] 8368543//604800 -9558943//907200 2177057//907200 -11351//86400 204257//1814400 0 0 0 0]
n3 = [n1[3] n2[3] 4938581//453600 -786473//151200 1141057//907200 -120619//907200 0 0 0 0]
n4 = [n1[4] n2[4] n3[4] 3146581//453600 -4614143//907200 24587//14400 -1//6 0 0 0]
n5 = [n1[5] n2[5] n3[5] n4[5] 185709//22400 -11293343//1814400 12//6 -1//6 0 0]
n6 = [n1[6] n2[6] n3[6] n4[6] n5[6] 16787381//1814400 -39//6 12//6 -1//6 0]
n7 = [0 0 0 -1//6 12//6 -39//6 56//6 -39//6 12//6 -1//6]
n8 = [0 0 0 0 -1//6 12//6 -39//6 56//6 -39//6 12//6]
n9 = [0 0 0 0 0 -1//6 12//6 -39//6 56//6 -39//6]
n10 = [0 0 0 0 0 0 -1//6 12//6 -39//6 56//6]
N = vcat(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10)

D1 = H \ (Q - 1//2*e*e'); display(D1)
D2 = H \ (-M - e*d1'); display(D2)
D3 = H \ (R + 1//2*d1*d1' - e*d2'); display(D3)
D4 = H \ (N - e*d3' + d1*d2'); display(D4)

x=1//1:length(h)
ks = 0:7
for k in ks
    u=x.^k
    println("k = ", k)
    println(Float64.(D1*u - k*x.^(k-1)))
end
for k in ks
    u=x.^k
    println("k = ", k)
    println(Float64.(D2*u - k*(k-1)*x.^(k-2)))
end
for k in ks
    u=x.^k
    println("k = ", k)
    println(Float64.(D3*u - k*(k-1)*(k-2)*x.^(k-3)))
end
for k in ks
    u=x.^k
    println("k = ", k)
    println(Float64.(D4*u - k*(k-1)*(k-2)*(k-3)*x.^(k-4)))
end

# order 6
h = [318365//1016064,
    145979//103680,
    139177//241920,
    964969//725760,
    593477//725760,
    52009//48384,
    141893//145152,
    1019713//1016064,
    1, 1, 1, 1]
e = [1//1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
d1 = [-25//12, 48//12, -36//12, 16//12, -3//12, 0, 0, 0, 0, 0, 0, 0]
d2 = [35//12, -104//12, 114//12, -56//12, 11//12, 0, 0, 0, 0, 0, 0, 0]
d3 = [-5//2, 18//2, -24//2, 14//2, -3//2, 0, 0, 0, 0, 0, 0, 0]
H = Diagonal(h)

q1 = reshape(
    [0, 1547358409//2421619200, -422423//11211200, -1002751721//8717829120,
    -15605263//484323840, 1023865//24216192, 291943739//21794572800, -24659//2534400,
    0, 0, 0, 0], 1, length(h))
q2 = reshape(
    [-q1[2], 0, 23031829//62899200, 10784027//34594560,
    2859215//31135104, -45982103//345945600, -26681//1182720, 538846039//21794572800,
    0, 0, 0, 0], 1, length(h))
q3 = reshape(
    [-q1[3], -q2[3], 0, 28368209//69189120,
    -9693137//69189120, 1289363//17740800, -39181//5491200, -168647//24216192,
    0, 0, 0, 0], 1, length(h))
q4 = reshape(
    [-q1[4], -q2[4], -q3[4], 0,
    5833151//10644480, 4353179//69189120, 2462459//155675520, -215471//10762752,
    0, 0, 0, 0], 1, length(h))
q5 = reshape(
    [-q1[5], -q2[5], -q3[5], -q4[5], 0, 7521509//13837824, -1013231//11531520, 103152839//8717829120,
    0, 0, 0, 0], 1, length(h))
q6 = reshape(
    [-q1[6], -q2[6], -q3[6], -q4[6], -q5[6], 0, 67795697//98841600, -17263733//151351200,
    1//60, 0, 0, 0], 1, length(h))
q7 = reshape(
    [-q1[7], -q2[7], -q3[7], -q4[7], -q5[7], -q6[7], 0, 1769933569//2421619200,
    -9//60, 1//60, 0, 0], 1, length(h))
q8 = reshape(
    [-q1[8], -q2[8], -q3[8], -q4[8], -q5[8], -q6[8], -q7[8], 0,
    45//60, -9//60, 1//60, 0], 1, length(h))
q9 = [0 0 0 0 0 -1//60 9//60 -45//60 0 45//60 -9//60 1//60]
q10 = [0 0 0 0 0 0 -1//60 9//60 -45//60 0 45//60 -9//60]
q11 = [0 0 0 0 0 0 0 -1//60 9//60 -45//60 0 45//60]
q12 = [0 0 0 0 0 0 0 0 -1//60 9//60 -45//60 0]
Q = vcat(q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11, q12)

m1 = reshape(
    [4347276223//3736212480, -1534657609//1210809600, 68879//3057600, 1092927401//13076743680,
    18145423//968647680, -1143817//60540480, -355447739//65383718400, 56081//16473600,
    0, 0, 0, 0], 1, length(h))
m2 = reshape(
    [m1[2], 42416226217//18681062400, -228654119//345945600, -12245627//34594560,
    -2995295//46702656, 52836503//691891200, 119351//12812800, -634102039//65383718400,
    0, 0, 0, 0], 1, length(h))
m3 = reshape(
    [m1[3], m2[3], 5399287//4193280, -24739409//34594560,
    7878737//69189120, -1917829//31449600, 39727//3660800, 10259//4656960,
    0, 0, 0, 0], 1, length(h))
m4 = reshape(
    [m1[4], m2[4], m3[4], 7780367599//3736212480,
    -70085363//69189120, -500209//6289920, -311543//17962560, 278191//21525504,
    0, 0, 0, 0], 1, length(h))
m5 = reshape(
    [m1[5], m2[5], m3[5], m4[5],
    7116321131//3736212480, -545081//532224, 811631//11531520, -84101639//13076743680,
    0, 0, 0, 0], 1, length(h))
m6 = reshape(
    [m1[6], m2[6], m3[6], m4[6],
    m5[6], 324760747//138378240, -65995697//49420800, 1469203//13759200,
    -2//180, 0, 0, 0], 1, length(h))
m7 = reshape(
    [m1[7], m2[7], m3[7], m4[7],  m5[7], m6[7], 48284442317//18681062400, -1762877569//1210809600,
    27//180, -2//180, 0, 0], 1, length(h))
m8 = reshape(
    [m1[8], m2[8], m3[8], m4[8],  m5[8], m6[8], m7[8], 10117212851//3736212480,
    -270//180, 27//180, -2//180, 0], 1, length(h))
m9 = [0 0 0 0 0 -2//180 27//180 -270//180 490//180 -270//180 27//180 -2//180]
m10 = [0 0 0 0 0 0 -2//180 27//180 -270//180 490//180 -270//180 27//180]
m11 = [0 0 0 0 0 0 0 -2//180 27//180 -270//180 490//180 -270//180]
m12 = [0 0 0 0 0 0 0 0 -2//180 27//180 -270//180 490//180]
M = vcat(m1, m2, m3, m4, m5, m6, m7, m8, m9, m10, m11, m12)

r1 = reshape(
    [0, -10882810591//5811886080, 398713069//132088320, -1746657571//1162377216,
    56050639//145297152, -11473393//1162377216, -38062741//1452971520, 30473//4392960,
    0, 0, 0, 0], 1, length(h))
r2 = reshape(
    [-r1[2], 0, -3720544343//830269440, 767707019//207567360,
    -1047978301//830269440, 1240729//14826240, 6807397//55351296, -50022767//1452971520,
    0, 0, 0, 0], 1, length(h))
r3 = reshape(
    [-r1[3], -r2[3], 0, -2870078009//830269440,
    74962049//29652480, -12944857//30750720, -17846623//103783680, 68707591//1162377216,
    0, 0, 0, 0], 1, length(h))
r4 = reshape(
    [-r1[4], -r2[4], -r3[4], 0,
    -727867087//276756480, 327603877//207567360, -175223717//830269440, 1353613//726485760,
    0, 0, 0, 0], 1, length(h))
r5 = reshape(
    [-r1[5], -r2[5], -r3[5], -r4[5],
    0, -1804641793//830269440, 311038417//207567360, -1932566239//5811886080,
    7//240, 0, 0, 0], 1, length(h))
r6 = reshape(
    [-r1[6], -r2[6], -r3[6], -r4[6], -r5[6], 0, -1760949511//830269440, 2105883973//1452971520,
    -72//240, 7//240, 0, 0], 1, length(h))
r7 = reshape(
    [-r1[7], -r2[7], -r3[7], -r4[7], -r5[7], -r6[7], 0, -1081094773//528353280,
    338//240, -72//240, 7//240, 0], 1, length(h))
r8 = reshape(
    [-r1[8], -r2[8], -r3[8], -r4[8], -r5[8], -r6[8], -r7[8], 0,
    -488//240, 338//240, -72//240, 7//240], 1, length(h))
r9 = [0 0 0 0 -7//240 72//240 -338//240 488//240 0 -488//240 338//240 -72//240]
r10 = [0 0 0 0 0 -7//240 72//240 -338//240 488//240 0 -488//240 338//240]
r11 = [0 0 0 0 0 0 -7//240 72//240 -338//240 488//240 0 -488//240]
r12 = [0 0 0 0 0 0 0 -7//240 72//240 -338//240 488//240 0]
R = vcat(r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12)

n1 = reshape(
    [40833734273//10761070320, -162181998421//16397821440, 4696168417//521748864, -245714671483//68870850048,
    2185939219//2869618752, -15248255797//114784750080, 345156907//12298366080, 6388381//1093188096,
    0, 0, 0, 0], 1, length(h))
n2 = reshape(
    [n1[2], 147281127041//5380535160, -3072614435609//114784750080, 320122985851//28696187520,
    -768046031383//344354250240, 7861605187//14348093760, -803762437//4251287040, 167394281//86088562560,
    0, 0, 0, 0], 1, length(h))
n3 = reshape(
    [n1[3], n2[3], 139712483333//4782697920, -1634124842747//114784750080,
    90855193447//28696187520, -26412188989//38261583360, 668741173//1793511720, -132673781//2342545920,
    0, 0, 0, 0], 1, length(h))
n4 = reshape(
    [n1[4], n2[4], n3[4], 437353997177//43044281280,
    -172873969321//38261583360, 34759553483//28696187520, -98928859751//344354250240, 295000207//3587023440,
    0, 0, 0, 0], 1, length(h))
n5 = reshape(
    [n1[5], n2[5], n3[5], n4[5],
    126711914423//21522140640, -520477408939//114784750080, 49581230003//28696187520, -99640101991//344354250240,
    7//240, 0, 0, 0], 1, length(h))
n6 = reshape(
    [n1[6], n2[6], n3[6], n4[6],
    n5[6], 19422074929//2391348960, -772894368601//114784750080, 10579712849//4099455360,
    -96//240, 7//240, 0, 0], 1, length(h))
n7 = reshape(
    [n1[7], n2[7], n3[7], n4[7],
    n5[7], n6[7], 456715296239//43044281280, -915425403107//114784750080,
    676//240, -96//240, 7//240, 0], 1, length(h))
n8 = reshape(
    [n1[8], n2[8], n3[8], n4[8], n5[8], n6[8], n7[8], 488029542379//43044281280,
    -1952//240, 676//240, -96//240, 7//240], 1, length(h))
n9 = [0 0 0 0 7//240 -96//240 676//240 -1952//240 2730//240 -1952//240 676//240 -96//240]
n10 = [0 0 0 0 0 7//240 -96//240 676//240 -1952//240 2730//240 -1952//240 676//240]
n11 = [0 0 0 0 0 0 7//240 -96//240 676//240 -1952//240 2730//240 -1952//240]
n12 = [0 0 0 0 0 0 0 7//240 -96//240 676//240 -1952//240 2730//240]
N = vcat(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10, n11, n12)

D1 = H \ (Q - 1//2*e*e'); display(D1)
D2 = H \ (-M - e*d1'); display(D2)
D3 = H \ (R + 1//2*d1*d1' - e*d2'); display(D3)
D4 = H \ (N - e*d3' + d1*d2'); display(D4)


#=
-------------------------------------------------------------------------------------------------------------
This file contains the student template for Project 6: 2D Wave Equation using the unified CG/DG method with AGGP storage
introduced in F.X. Giraldo's Introduction to Element-based Galerkin Methods using
Tensor-Product Bases: Analysis, Algorithms, and Applications.

The solution is obtained using Algorithm 17.7 whereby the RHS vector is constructed at each time stage.
The volume integral contribution is constructed using Algorithm 16.11 except that the element differentiation matrix is computed and stored.
The flux integral contribution is constructed using Algorithm 16.12 (all in weak form).

It uses the LSRK45 method as the time-integrator and Rusanov fluxes for DG.

Written by F.X. Giraldo on July 10, 2021.
Department of Applied Mathematics
Naval Postgraduate School
Monterey, CA 93943

The interpolation points used are the following:
ipoints=1: Lobatto
ipoints=2: Legendre
ipoints=3: Chebyshev
ipoints=4: Equi-spaced

The integration points used are the following:
qpoints=1: Lobatto
qpoints=2: Legendre
-------------------------------------------------------------------------------------------------------------
=#

using Plots
using LinearAlgebra

include("QuadraturePoints.jl")
include("element_matrices.jl")
include("global_matrices.jl")
include("create_grid.jl")
include("create_CGDG_storage.jl")
include("create_side.jl")
include("create_face.jl")
include("compute_normals.jl")
include("create_face_periodicity.jl")
include("compute_metrics.jl")
include("global_matrices.jl")
include("exact_solution.jl")
include("compute_norm.jl")
include("ti_LSRK.jl")

#Some Constants
DFloat = Float64
machine_zero=eps(DFloat)

#Main Function
function main()

    #------------------------Only Change stuff here-------------------#
    N=8 #polynomial order
    Nex=4 #number of elements along x (ξ)
    Ney=4 #number of elements along y (η)
    space_method="DG" #CG or DG
    time_final=DFloat(0.25) #in revolutions
    plot_grid=false
    plot_solution=true
    plot_movie=false
    warp_grid=false
    #------------------------Only Change stuff here-------------------#

    Q=N #changing this requires modifying CREATE_RHS_FLUX which assumes Q=N
    Np=N+1
    Nq=Q+1
    Npts=Np^2
    Nqs=Nq^2
    ipoints=1 #lobatto
    qpoints=1 #lobatto
    Courant_max=DFloat(0.5)
    icase=1 #CCW moving Gaussian
    Ne=Nex*Ney
    Nx=Nex*N+1
    Ny=Ney*N+1
    Npoin_CG=Nx*Ny
    Npoin_DG=Npts*Ne
    Npoin=Npoin_CG
    Nboun=2*Nex + 2*Ney
    Nface=2*Ne + Nex + Ney
    if icase == 1
        c=2*π;
    end
    rotate_grid=false #not working with periodicity
    time_final=time_final*c
    @show(N,Q,Ne,Npoin_CG,Npoin_DG,Nboun,Nface,Nx,Ny)

    #Select Interpolation Points
    if ipoints == 1
        (ξ,ω) = QuadraturePoints.lobatto_gauss(Np,DFloat) #Lobatto
    elseif ipoints == 2
        (ξ,ω) = QuadraturePoints.legendre_gauss(Np,DFloat) #Legendre
    elseif ipoints == 3
        (ξ,ω) = QuadraturePoints.chebyshev_gauss(Np,DFloat) #Chebyshev
    elseif ipoints == 4
        (ξ,ω) = QuadraturePoints.equispaced_points(Np,DFloat) #Equi-spaced
    end

    #Select Quadrature Points
    if qpoints == 1
        (ξq,ωq) = QuadraturePoints.lobatto_gauss(Nq,DFloat) #Lobatto
    elseif qpoints == 2
        (ξq,ωq) = QuadraturePoints.legendre_gauss(Nq,DFloat) #Legendre
    end

    #Construct Lagrange Polynomials
    (ψ,dψ) = QuadraturePoints.lagrange_basis(Np,Nq,ξ,ξq,DFloat)

    #Construct Grid and (i,e)->I map
    (coord_CG,intma_CG,bsido_CG,periodicity_CG) = create_grid(Np,Npoin_CG,Ne,Nboun,Nex,Ney,ξ,plot_grid,warp_grid,rotate_grid,DFloat)

    #Store the CGDG-Storage Grid
    (coord,intma,periodicity,DG_to_CG,Npoin) = create_CGDG_Storage(space_method,Npoin_CG,Npoin_DG,coord_CG,intma_CG,periodicity_CG,Ne,Np,DFloat)
    @show(space_method,Npoin)

    #Construct Metric Terms
    (ξ_x,ξ_y,η_x,η_y,jac) = compute_metrics(coord_CG,intma_CG,ψ,dψ,Ne,Np,Nq,DFloat)

    #Construct Face arrays
    (iside,~) = create_side(intma_CG,bsido_CG,Npoin_CG,Ne,Nboun,Nface,Np,DFloat)
    (face,mapL,mapR) = create_face(iside,intma_CG,Nface,Np)
    (normals,jac_face) = compute_normals(face,intma_CG,coord_CG,Nface,Np,Nq,ψ,dψ)
    (face,iside) = create_face_periodicity!(face,iside,coord_CG,Nface,Nboun)

    #Construct Element Matrices
    (Me,De_x,De_y) = element_matrices(ψ,dψ,ξ_x,ξ_y,η_x,η_y,jac,ωq,Ne,Np,Nq,DFloat)

    #Construct Global Matrices
    M = global_matrices(Me,intma,periodicity,Ne,Np,Npoin,DFloat)

    #Compute Exact Solution
    time=DFloat(0.0)
    (qe,ue) = exact_solution(coord,Npoin,time,icase,DFloat)
    q0=copy(qe)

    #Recompute DT, NTIME
    dx_min=coord[1,2]-coord[1,1]
    umax=maximum(ue)
    dt=Courant_max*dx_min/umax
    courant=umax*dt/dx_min
    ntime = ceil(Int64, time_final / dt)
    println("Courant = ",courant," dt = ",dt," ntime = ",ntime)

    #Time Integration
    #-----------------------------Students Add their Functions inside ti_LSRK--------------------------#
    #-----------------------------Students Add their Functions inside ti_LSRK--------------------------#
    (q0,time) = ti_LSRK!(q0,ue,coord,M,De_x,De_y,intma,periodicity,face,mapL,mapR,normals,jac_face,ωq,time,ntime,dt,space_method,plot_movie,Nx,Ny,coord_CG,periodicity_CG,DG_to_CG,DFloat)
    #-----------------------------Students Add their Functions inside ti_LSRK--------------------------#
    #-----------------------------Students Add their Functions inside ti_LSRK--------------------------#

    #Print Time and Extrema
    println(" itime = ",ntime," time = ",time," qmax = ",maximum(q0)," qmin = ",minimum(q0))

    #Compute L2 Norm
    (qe,ue) = exact_solution(coord,Npoin,time,icase,DFloat)
    norm2 = compute_norm(q0,qe,Npoin,DFloat)
    println("norm.L2 = ",norm2)

    #Plot Solution
    if (plot_solution)
        #Compute gridpoint solution
        q_sol=zeros(DFloat,Npoin_CG)
        qe_sol=zeros(DFloat,Npoin_CG)
        lhowm=zeros(Int64,Npoin_CG)
        for i=1:Npoin
            ip_CG=periodicity_CG[DG_to_CG[i]]
            q_sol[ip_CG] += q0[i]
            qe_sol[ip_CG] += qe[i]
            lhowm[ip_CG] += 1
        end
        for i=1:Npoin_CG
            q_sol[i]=q_sol[i]/lhowm[i]
            qe_sol[i]=qe_sol[i]/lhowm[i];
        end

        x=reshape(coord_CG[1,:],(Nx,Ny))
        y=reshape(coord_CG[2,:],(Nx,Ny))
        xx=x[:,1]
        yy=y[1,:]

        #Numerical Solution
        qq=reshape(q_sol,(Nx,Ny))
        p1 = contourf(xx,yy,qq')
        title!(p1,"$space_method solution")
        xlabel!(p1,"x")
        ylabel!(p1,"y")

        #Exact Solution
        qq=reshape(qe_sol,(Nx,Ny))
        p2 = contourf(xx,yy,qq')
        title!(p2,"Exact solution")
        xlabel!(p2,"x")
        ylabel!(p2,"y")
        figure_1=plot(p1,p2)
        display(figure_1)
    end

    println("Done") #output

end

#----------------------------------#
# Run the main function
#----------------------------------#
main()


function phi_galerk_modelA(phi, V, t, dt, lambda, r, g, k, xstep, T, D)
    noise = sqrt(T * dt / xstep) * sampler(size(phi))
    d = length(size(phi))
    R = CartesianIndices(phi)
    out = similar(phi)
    @. out = (phi - dt * (g * phi^2 + r + lambda * V[1]) * phi + noise)
    out = fft(out)
    for I in R
        out[I] = 1.0 / (1.0 + dt * sum([k[I[i]]^2 for i = 1:d])) * out[I]
    end
    out = ifft(out)
    @. phi = real(out)
end

#t =0.0
#r = 0.1
#g = 0.0
#D = 1.0
#T = 1.0
#lambda = 0.0
#@btime phi_galerk_modelA(phi, V, t, dt, lambda, r, g, k, xstep, T, D)


using LinearAlgebra
using LinearAlgebra: I

using ForwardDiff
using StaticArrays

import IntervalArithmetic: where_bisect

export Bisection, Newton, Krawczyk, GradientContractor, TrivialContractor

struct Newton end
struct Krawczyk end
struct Bisection end

function _contract(f, derivative, region::T, mid_point, ::Newton) where {T}
    m = convert(T, mid(region, mid_point))
    return m - (f(m) / derivative(region))
end

function _contract(f, jacobian, region::R, mid_point, ::Newton) where {T, R <: AbstractVector{T}}
    m = T.(mid.(region, mid_point))
    J = jacobian(region)
    fm = f(m)
    try
        return convert(R, m .- (J \ fm))
    catch
        return region .± Inf
    end
end

function _contract(f, derivative, region::T, mid_point, ::Krawczyk) where T
    m = convert(T, mid(region, mid_point))
    Y = 1 / derivative(m)
    return m - Y * f(m) + (1 - Y * derivative(region)) * (region - m)
end

function _contract(f, jacobian, region::T, mid_point, ::Krawczyk) where T <: AbstractVector
    m = mid.(region, mid_point)
    mm = convert(T, m)
    J = jacobian(region)
    fmm = f(mm)
    try
        Y = mid.(inv(jacobian(mm)))
        return m - Y * fmm + (I - Y * J) * (region - m)
    catch
        return region .± Inf
    end
end

"""
    GradientContractor(f, method, jacobian)
    GradientContractor(f, method, region)

Contractor for `f` based on `method` (`Newton()` or `Krawczyk()`), either using
the explicitely supplied function `jacobian(region)` or a jacobian method for
`region` using automatic differentiation of `f` from `ForwardDiff.jl`.

    (contractor::GradientContractor)(region, mid_point = IntervalArithmetic.where_bisect)

Calling instances of `GradientContractor` returns a contraction of `region`.
"""
struct GradientContractor{T}
    f
    method::T
    jacobian
end

function GradientContractor(f, method, ::R) where R <: AbstractVector
    function grad(region)
        jac = similar(region, Size(length(region), length(region)))
        ForwardDiff.jacobian!(jac, f, region)
        return jac
    end
    return GradientContractor(f, method, grad)
end

function GradientContractor(f, method, ::R) where R <: Number
    derivative(region) = ForwardDiff.derivative(f, region)
    return GradientContractor(f, method, derivative)
end

function (contractor::GradientContractor{T})(region, mid_point = where_bisect) where T
    contraction = _contract(
        contractor.f,
        contractor.jacobian,
        region,
        mid_point,
        contractor.method
    )
    if _isempty(contraction)
        throw(DomainError(region))
    end
    return contraction
end

"""
    TrivialContractor()

Trivial contractor used for benchmarking which solely relies on bisections.

    (contractor::TrivialContractor)(region) = region

Calling instances of `TrivialContractor` returns `region` itself.
"""
struct TrivialContractor end

(contractor::TrivialContractor)(region) = region


using EconPDEs

# PDE with 1 state variable
include("./AssetPricing/CampbellCochrane.jl")
@time pdesolve(m, stategrid, yend)
#   0.011112 seconds (286.45 k allocations: 12.268 MiB)

# PDE with 2 state variables
include("./AssetPricing/BansalYaron.jl")
@time pdesolve(m, stategrid, yend)
# 0.081095 seconds (485.84 k allocations: 78.910 MiB, 11.41% gc time)

# System of 4 PDEs with 1 state variable
include("./AssetPricing/GarleanuPanageas.jl")
@time pdesolve(m, stategrid, yend)
# 0.047346 seconds (167.95 k allocations: 65.049 MiB, 12.90% gc time)

# System of 3 PDEs with 2 state variables
include("./AssetPricing/DiTella.jl")
@time pdesolve(m, stategrid, yend)
# 0.912719 seconds (2.55 M allocations: 1.303 GiB, 9.01% gc time)


using StrainRecon
using Base.Test

@testset "findM" begin
    n = 3
    m = 5
    nsamp = 5
    w = randomw(n,nsamp)
    Mtrue = randomM(m,n)
    R = 1/(1e-8)*StrainRecon.sdiag(ones(m))
    for i = 1:nsamp
        d = Mtrue*w[:,i]
        param = getStrainReconParam(d,n,R=R)
        M_nsol,pm = findM(param, w[:,i], nsol = 2^n, probM = true)
        for j = 1:2^n
            M = M_nsol[:,:,j]*1
            #@test M*w[:,i]
        end

        @test all(diff(pm,2) .<= 0) # check that subsequent rows of M in nth + 1 matrix are not better than previous matrix

        @test norm(sum(pm,2) .- 1) < 1e-8 # sum of p(M) across matrices at the same row = 1

    end
end


incircle(x) = transpose(x) * x < 1

computeπ(tol) = foldl(right,
       Map(incircle)
    |> Mean()
    |> Drop(10)
    |> Map(x -> x*4)
    |> TakeWhile(x -> x.err > tol)
    , Quasi(2)
    )

computeπ(0.001)


using LinearAlgebra
using Flux
import Flux.gpu, Flux.cpu

# Output layer
mutable struct output{T}
    C2::Union{Matrix{T},CuMatrix{T}}
    D21::Union{Matrix{T},CuMatrix{T}}
    D22::Union{Matrix{T},CuMatrix{T}}
    by::Union{Vector{T},CuVector{T}}
end

function output{T}(nu::Int, nx::Int, nv::Int, ny::Int) where T
    C2 = randn(T, ny, nx) / convert(T, sqrt(ny + nx))
    D21 = randn(T, ny, nv) / convert(T, sqrt(ny + nv))
    D22 = zeros(T, ny, nu)
    by = randn(T, ny) / convert(T, sqrt(ny))
    return output{T}(C2, D21, D22, by)
end

(layer::output)(x, w, u) = layer.C2 * x + layer.D21 * w + layer.D22 * u .+ layer.by
Flux.trainable(layer::output) = (layer.C2, layer.D21, layer.by)

function Flux.gpu(M::output{T}) where T
    if T != Float32
        println("Moving type: ", T, " to gpu may not be supported. Try Float32!")
    end
    return output{T}(gpu(M.C2), gpu(M.D21), gpu(M.D22), gpu(M.by))
end

function Flux.cpu(M::output{T}) where T
    return output{T}(cpu(M.C2), cpu(M.D21), cpu(M.D22), cpu(M.by))
end


function fit_mode_quantile(::Type{LogitNormal}, mode::Real, qp::QuantilePoint)
    matchModeUpper(mode, qp.q, Val(40); perc = qp.p)
end

function matchModeUpper(
    mode::T, upper::T
    , ::Val{nTry}
    ; perc::Real = 0.99
) where {nTry, T<:Real}
    mode == 0.5 && return matchMedianUpper(LogitNormal, 0.5, upper; perc=perc)
    # for given mu we can compute sigma by mode and upper quantile
    # hence univariate search for mu
    # we now that mu is in (\code{logit(mode)},0) for \code{mode < 0.5} and in
    # \code{(0,logit(mode))} for \code{mode > 0.5} for unimodal distribution
    # there might be a maximum in the middle and optimized misses the low part
    # hence, first get near the global minimum by a evaluating the cost at a
    # grid that is spaced narrower at the edge
    #
    logitMode = logit(mode)
    logitUpper = logit(upper)
    upperMu = abs(logitMode) - eps()
    muTry = SVector{nTry}(
        sign(logitMode) .* log.(range(1,stop=exp(upperMu),length=nTry)))
    oF(mu) = ofLogitNormalModeUpper(mu, mode, logitMode, logitUpper, perc)
    ofMuTry = oF.(muTry)
    iMin = argmin(ofMuTry)
    # on postive side muTry are increasing, on negative side muTry decreasing
    # neet to have the lower value at the beginning of the interval
    interval = (logitMode >= 0) ?
        (muTry[max(1,iMin-1)], muTry[min(nTry,iMin+1)]) :
        (muTry[max(1,iMin+1)], muTry[min(nTry,max(1,iMin-1))])
    resOpt = optimize(oF, interval...)
    Optim.converged(resOpt) || error("could not find minimum")
    μ = Optim.minimizer(resOpt)
    σ = sqrt((logitMode - μ)/(2*mode - 1))
    LogitNormal{T}(μ,σ)
end

function matchModeUpper(
    mode::S, upper::T, ::Val{nTry}; kwargs...
) where {nTry, T<:Real, S<:Real}
    p = promote(mode, upper)
    matchModeUpper(p[1], p[2], Val(nTry); kwargs...)
end

"objective function used by `matchModeUpper(LogitNormal,...)`"
function ofLogitNormalModeUpper(mu, mode, logitMode, logitUpper, perc)
  # given mu and mode, we can calculate sigma,
  # predict the percentile of logitUpper
  # and return the squared difference as cost to be minimized
  sigma2 = (logitMode - mu)/(2.0*mode - 1.0)
  sigma2 > 0.0 || return prevfloat(Inf)
  predp = normcdf(mu, sqrt(sigma2), logitUpper)
  diff = predp - perc
  diff*diff
end

function fit_mode_flat(::Type{LogitNormal}, mode::T, ::Val{nTry} = Val(40)) where {T<:Real,nTry}
    mode == 0.5 && return(LogitNormal{T}(0.0, sqrt(2)))
    is_right = mode > 0.5
    mode_r = is_right ? mode : 1.0 - mode
    res_opt = optimize(x -> of_mode_flat(x, mode_r, logit(mode_r)), 0.0, 0.5)
    Optim.converged(res_opt) || error("could not find minimum")
    xt = res_opt.minimizer
    σ2 = (1/xt + 1/(1-xt))/2
    μr = logit(mode_r) - σ2*(2.0*mode_r - 1.0)
    μ = is_right ? μr : -μr
    LogitNormal{T}(μ,sqrt(σ2))
end

function of_mode_flat(x, m, logitm = logit(m))
  # lhs = 1/x + 1/(1-x)
  # rhs = (logitm - logit(x))/(m-x)
  # lhs = (m-x)/x + (m-x)/(1-x)
  # rhs = (logitm - logit(x))
  lhs = m/x + (m-1)/(1-x)
  rhs = logitm - logit(x)
  d = lhs - rhs
  d*d
end


"""
    BornMayer{T<:AbstractFloat} <: EmpiricalPotential{NamedTuple{(:A, :ρ)},NamedTuple{(:rcutoff,)}}

The Born-Mayer-Huggins potential is a two-body intermolecular potential with five typical parameters, primarily describing the interaction of neutral atoms. Formally, the interaction between two atoms at a distance, ``r``, is given by

```math
\\begin{aligned}
V_{LJ}(r; \\epsilon, \\sigma, rcutoff, species) =
    \\begin{cases}
    0 & r > rcutoff \\\\
    A e^{\\frac{\\sigma-r}{\\rho}} + \\left( \\frac{C}{r} \\right)^8 - \\left( \\frac{D}{r} \\right) & r < rcutoff.
    \\end{cases}
\\end{aligned}
```

Users must supply five parameters, ``A`` (units of energy), ``\\sigma`` (units of distance), ``\\rho`` (units of distance), ``C`` ``\\&`` ``D`` (units of energy ``\\times`` units of distance), and radial cutoff (units of distance).
"""
struct BornMayer{T<:AbstractFloat} <: EmpiricalPotential{NamedTuple{(:A, :ρ, :σ, :C, :D)},NamedTuple{(:rcutoff,)}}
    A::T
    ρ::T
    σ::T
    C::T
    D::T
    rcutoff::T
    species::Tuple
    function BornMayer(A, ρ, σ, C, D, rcutoff, species)
        A, ρ, σ, C, D, rcutoff = promote(austrip(A), austrip(ρ), austrip(σ), austrip(C), austrip(D), austrip(rcutoff))
        new{typeof(rcutoff)}(A, ρ, σ, C, D, rcutoff, Tuple(species))
    end
    function BornMayer{T}(; A::T, ρ::T, σ::T, C::T, D::T, rcutoff::T, species::Tuple) where {T}
        new{T}(A, ρ, σ, C, D, rcutoff, species)
    end
end

get_rcutoff(bm::BornMayer) = bm.rcutoff
get_species(bm::BornMayer) = bm.species

potential_energy(R::AbstractFloat, bm::BornMayer) = bm.A * exp((bm.σ - R) / bm.ρ) + (bm.C / R)^6 - (bm.D/R)^8
force(R::AbstractFloat, r::SVector{3}, bm::BornMayer) = (bm.A * exp((bm.σ-R) / bm.ρ)/ (bm.ρ*R) + ((bm.C/R)^6 - (bm.D/R)^8)/R^2)r


function RMP2{T}(refWfn::Fermi.HartreeFock.RHF,alg::Fermi.MollerPlesset.Conventional) where T <: AbstractFloat
    print_header()
    dmp2 = 0.0
    nocc = refWfn.ndocc
    @output "*  executing restricted MP2\n"
    @output "   nocc: {:>3}\n" nocc
    @output "   nvir: {:>3}\n" refWfn.nvir
    rocc = 1:1:nocc
    rvir = nocc+1:1:nocc+refWfn.nvir
    epsa = refWfn.eps
    @output "*  performing AO->MO integral transformation ... "
    t = @elapsed moeri = refWfn.ints["OOVV"]
    @output "done in {:>5.2f}s\n" t
    @output "*  Computing MP2 energy ... "
    t = @elapsed for b in rvir
        for a in rvir
            for j in rocc
                for i in rocc
                    aa = a - nocc
                    bb = b - nocc
                    dmp2 +=
                        (
                            moeri[i, j, aa, bb] *
                            (2 * moeri[i, j, aa, bb] - moeri[i, j, bb, aa])
                        ) / (epsa[i] + epsa[j] - epsa[a] - epsa[b])
                end
            end
        end
    end
    @output "done in {:>5.2f}s\n" t
    @output "   @RMP2 {:>20.17f} Eh\n" dmp2
    RMP2(dmp2,Fermi.MemTensor{T}(zeros(0,0,0,0)))
end


module q4

import Evolve

steps = 32000                # number of simulation steps
samplesteps = 50            # steps between measurements
L = 2                       # width of the box
xmin = -1                   # leftmost point in the box
N = 2000                    # number of subintervals
ϵ = L/N                     # width of each subinterval
k = 200                     # wavenumbers
σ = 0.05                    # wavepacket width
h = 1000                      # energy is 123.368 for w = 0.2
b = 1
ss = [0.001, 0.01, 0.1]                    # wall thickness
w = 0.2                     # pocket width
x0 = -0.5                   # wavepacket starting position

# Functions for square well states and 1st excited state
Φground = x -> √(2/w)*cos(π*x/w)
Φexcited = x -> √(2/w)*sin(2*π*x/w)
Φs = [:Φground, :Φexcited]

# Run the trials
@sync @parallel for Φ0 in Φs
    for s in ss
        print("Initializing for Φ0 = $(string(Φ0)), s = $s... ")
        V(xs) = map(x -> Evolve.pocket(L, h, b, s, w, xmin, x), xs)      # pocket potential
        AV = Evolve.A(V)            # evolution matrix constructor for square well

        A = AV(L, N, xmin)
        Φ = eval(Φ0)
        ϕ0 = zeros(Complex, N+1)
        xpocket = [(0.5L-0.5w):ϵ:(0.5L+0.5w)]
        ϕ0[int((0.5L-0.5w)N/L):int((0.5L+0.5w)N/L)] = Φ(xpocket)
        x = (L/N .* [0:N] + xmin)   # x positions

        # Evolve the system and record positions every so many steps
        print("evolving... ")
        test = x -> (abs(x[2]) > 1e-2 || abs(x[end-1]) > 1e-2)
        ϕ = Evolve.evolve(A, ϕ0, steps, test)
        phis = [ϕ0 ϕ[:,samplesteps:samplesteps:end]]

        # calculate transmission and reflection probabilities
        print("calculating transmission probabilities...")
        Ts = Float64[]
        Rs = Float64[]
        for i in 1:size(phis)[2]
            phi = phis[:,i]
            T = mean(abs(phi[1:int((0.5L-0.5w-s)N/L),end]).^2)
            T += mean(abs(phi[int((0.5L+0.5w+s)N/L):end,end]).^2)
            R = mean(abs(phi[int((0.5L-0.5w)N/L):int((0.5L+0.5w)N/L),end]).^2)
            I = T + R
            T /= I
            R /= I
            push!(Ts, T)
            push!(Rs, R)
            ### print("T = $T, R = $R...")
        end
        writedlm("trans_pocket_$(string(Φ0))\_$s.dat", [Ts Rs])

        println("done after $(size(ϕ)[2]) steps.")
        writedlm("phis_pocket_im_$(string(Φ0))\_$s.dat", imag(phis))
        writedlm("phis_pocket_re_$(string(Φ0))\_$s.dat", real(phis))
    end
end

### writedlm("decay.dat", [ss Ts Rs])
end


function loglikelihood(dists, p::Path, consts::Vector{Float64})
    like = 0.0
    xx = reshape(p.x, (length(p.x), 1))
    a=xx[1] * p.θ[5]
    b=xx[2] * p.θ[6]
    for i in eachindex(dists)
        like += pdf(dists[i], [a,b])[1] * consts[i]
    end
    log(like)
end

function getCost!(sys::System, pathEnsemble::Ensemble, i::Int64)
    dists = components(sys.data.dd[sys.times[i]])
    n = ncomponents(sys.data.dd[sys.times[i]])
    consts = (2*pi).^(length.(dists)/2).*sqrt.(det.(diagm.(var.(dists))))./n
    for k in eachindex(pathEnsemble)
        costll!(dists, pathEnsemble[k], consts)
    end
end

function costll!(dists, p::Path, consts::Vector{Float64})
    p.dm -= loglikelihood(dists, p, consts)
end


# Test gausslaguerre.jl
import FastGaussQuadrature.getUQ
import FastGaussQuadrature.asyAiry
import FastGaussQuadrature.asyAirygen
import FastGaussQuadrature.asyBessel
import FastGaussQuadrature.asyBesselgen
import FastGaussQuadrature.asyBulk
import FastGaussQuadrature.asyBulkgen
import FastGaussQuadrature.polyAsyRH
import FastGaussQuadrature.polyAsyRHgen

# Test integration
tol = 3.e-13
tolGen = 2.e-9
# Possibly set srand(...) to get the same results for integration of a random polynomial.
ns = [42; 251; 5000];
for wei = 1:3
    if wei == 1; alpha = 0.0;qm = 1.0; m = 1;
    elseif wei == 2; alpha = pi+0.1; qm = 1.0; m = 1;
    elseif wei == 3; alpha = 0.67; qm = 0.34; m = 4;
    end

    for ni = 1:length(ns)
        n = ns[ni]
	# Make sure the integrand does not overflow at x[n]
        d = min(2*n-1, ceil(Int64, log(realmax(Float64)-4*n)/n ) )
        ra = rand(d+1)
        function fct(x)
            if length(x) == 1
                return sum(ra.*(x.^(0:d)))
            end
            result = NaN*x
            for i = 1:length(x)
                result[i] = sum(ra.*(x[i].^(0:d)))
            end
            return result
        end
        fcti(x) = fct(x).*exp(-qm*x.^m).*x^(alpha)
        # Test integration of a random polynomial which should be integrated exactly:
        exa = quadgk(fcti, 0, Inf, reltol = tol)[1]
        for methi = 1:ifelse(ni == 1, 1, 5)
            if (ni == 1)
                # Golub-Welsch is the accurate algorithm for low n but does not allow m or qm != 1.
                if wei == 3; continue; end
                println("    α = $alpha, n = $ni, Golub-Welsch")
                x, w = gausslaguerre( n, alpha )
            elseif methi == 1
                println("    α = $alpha, n = $ni, default")
                x, w = gausslaguerre( n, alpha, "default", qm, m)
            elseif methi == 2
                if wei == 3; continue; end # General weight
                println("    α = $alpha, n = $ni, RH")
                x, w = gausslaguerre( n, alpha, "RH", qm, m)
            elseif methi == 3
                if wei == 3; continue; end # General weight
                println("    α = $alpha, n = $ni, RHW")
                x, w = gausslaguerre( n, alpha, "RHW", qm, m)
            elseif methi == 4
                println("    α = $alpha, n = $ni, gen")
                x, w = gausslaguerre( n, alpha, "gen", qm, m)
            elseif methi == 5
                println("    α = $alpha, n = $ni, genW")
                x, w = gausslaguerre( n, alpha, "genW", qm, m)
            end
            # Multiply the tolerance by sqrt(n) because the error on each node/meaningful weight should be about 1e-14
            @test abs(dot(x,w) - gamma((alpha+2)/m)*qm^(-(alpha+2)/m)/m )/gamma((alpha+2)/m)/qm^(-(alpha+2)/m)*m <= sqrt(n)*ifelse(wei == 3, tolGen, tol)
            @test abs(dot(x.^2,w) - gamma((alpha+3)/m)*qm^(-(alpha+3)/m)/m )/gamma((alpha+3)/m)/qm^(-(alpha+3)/m)*m <= sqrt(n)*ifelse(wei == 3, tolGen, tol)
            if (n == 42) && (wei == 1)
                @test abs(x[37] - 98.388267163326702) < tol
                @test abs(w[7] - 0.055372813167092) < tol
            elseif (n == 251) && (wei == 1)
                @test abs(x[37] - 13.309000189442097) < tol
                @test abs(w[3] - 0.050091759039996) < tol
            end
            @test abs(dot(fct(x), w) - exa)/abs(exa) < sqrt(n)*ifelse(wei == 3, tolGen, tol)
	    end
    end
end

# Test whether higher-order terms correspond between "RH(W)", "gen(W)" and exact results computed in high precision.
n = NaN
tolHot = eps(Float64)*10
np = 200
tolEx = np*4e-15
T = ceil(Int64, 34/log(np) )
@test T == 7
for alpha = [0.0; 4.15]
    println("    α = $alpha")
    UQ = getUQ(alpha, 1.0, 1, T+1)

    z = 0.95 # > 3.7/4
    mnxi = 2*np*( sqrt(z)*sqrt(1 - z) - acos(sqrt(z) ) )
    fn = (mnxi*3im/2)^(2/3)
    aRHa = asyAiry(np, 4*np*z, alpha, T)
    @test abs(asyAirygen(np, z, alpha, T, 1.0, 1, UQ, fn, true) -aRHa)./abs(aRHa) < tolHot
    # Not using Q has an explicit substraction of ceil(3*T/2)-order poles, so loosen the tolerance.
    @test abs(asyAirygen(np, z, alpha, T, 1.0, 1, UQ, fn, false, mnxi*1im/np) -aRHa)./abs(aRHa) < 300*tolHot

    z = 0.01 # < sqrt(np)/4/np
    mnxi = 2*np*( sqrt(z)*sqrt(1 - z) - acos(sqrt(z) ) )
    aRHe = asyBessel(np, 4*np*z, alpha, T)
    @test abs(asyBesselgen(np, z, alpha, T, 1.0, 1, UQ, mnxi + pi*np, true) -aRHe)./abs(aRHe) < 20*tolHot
    @test abs(asyBesselgen(np, z, alpha, T, 1.0, 1, UQ, mnxi + pi*np, false) -aRHe)./abs(aRHe) < 20*tolHot

    z = 0.35
    mnxi = 2*np*( sqrt(z)*sqrt(1 - z) - acos(sqrt(z) ) )
    aRHu = asyBulk(np, 4*np*z, alpha, T)
    @test abs(asyBulkgen(np, z, alpha, T, 1.0, 1, UQ, mnxi) -aRHu)./abs(aRHu) < tolHot

    a = alpha
    factorp = (1/3840*a^10 - 5/2304*a^9 + 11/2304*a^8 + 7/1920*a^7 - 229/11520*a^6 + 107/34560*a^5 + 2653/103680*a^4 - 989/155520*a^3 -         3481/311040*a^2 + 139/103680*a + 9871/6531840)/np^5
    factorp = factorp + (1/384*a^8 - 1/96*a^7 + 1/576*a^6 + 43/1440*a^5 - 5/384*a^4 - 23/864*a^3 + 163/25920*a^2 + 31/6480*a - 139/155520)/np^4
    factorp = factorp + (1/48*a^6 - 1/48*a^5 - 1/24*a^4 + 5/144*a^3 + 1/36*a^2 - 1/144*a - 31/6480)/np^3
    factorp = factorp + (1/8*a^4 + 1/12*a^3 - 1/24*a^2 + 1/72)/np^2
    factorp = factorp + (1/2*a^2 + 1/2*a + 1/6)/np^1 +1
    # Add an asymptotic approximation of gamma(alpha+n+1)/gamma(n+1)
    factorp = (4*np)^(-alpha/2-1/2)*sqrt(1/2/pi)/sqrt(factorp)*sqrt(np^alpha*(1+alpha*(alpha+1)/2/np + alpha*(3*alpha^3+2*alpha^2-3*alpha-2)/24/np^2 + alpha^2*(alpha+1)^2*(alpha^2-3*alpha+2)/48/np^3 + alpha*(15*alpha^7-60*alpha^6-10*alpha^5+192*alpha^4 -25*alpha^3-180*alpha^2+20*alpha+48)/5760/np^4 + alpha^2*(alpha+1)^2*(3*alpha^6-31*alpha^5+109*alpha^4-125*alpha^3-88*alpha^2+276*alpha-144)/11520/np^5) )
    if alpha == 0.0
        @test norm([+6.975493821675553e-05  ,  -8.294589753829749e-05/1im]-vec(UQ[1,:,6,1,2]))/norm(UQ[1,:,6,1,2]) < tolHot
        @test norm([-6.975493821675553e-05  ,  +1.401453805748335e-05/1im]-vec(UQ[1,:,6,1,1]))/norm(UQ[1,:,6,1,1]) < tolHot*10
        # We choose to test the constants most prone to errors.
        @test norm([-0.01196102063075393    ,  +0.1904949571852236/1im]-vec(UQ[1,:,6,3,4]))/norm(UQ[1,:,6,1,4]) < tolHot*1e3
        @test norm([+1.309628097160176e-05  ,  -0.001326131973043531/1im]-vec(UQ[1,:,6,3,3]))/norm(UQ[1,:,6,1,3]) < tolHot*10

        # Compare with results obtained from a high precision calculation of the Laguerre polynomials with standard normalisation.
        @test abs(-4.9327289759964085492326419516911524182405e163/exp(0.95*4*np/2)/factorp -aRHa)/abs(aRHa) < tolEx
        @test abs(-1.6224971613979232593218524827791333158448e59/exp(0.35*4*np/2)/factorp -aRHu)/abs(aRHu) < tolEx
        @test abs(-3.9144031220161646274486036731469804751328/exp(0.01*4*np/2)/factorp -aRHe)/abs(aRHe) < tolEx
    else
        @test abs(5/49152*alpha^8 - 35/49152*alpha^7 + 67/49152*alpha^6 + 11/36864*alpha^5 - 1529/589824*alpha^4 + 1891/2359296*alpha^3 + 26827/26542080*alpha^2 - 109/524288*alpha - 190867/1698693120  -UQ[1,1,4,1,2])/abs(UQ[1,1,4,1,2]) < 6*tolHot
        @test abs(-1/24576*alpha^8 - 17/24576*alpha^7 - 107/147456*alpha^6 + 1751/983040*alpha^5 + 1535/1179648*alpha^4 - 853/884736*alpha^3 - 7583/26542080*alpha^2 + 14131/141557760*alpha - 12829/1274019840 -1im*4^alpha*UQ[1,2,4,2,1])/abs(4^alpha*UQ[1,2,4,2,1]) < tolHot
        @test abs(-23/259187003712000*alpha^21 + 353/76796149248000*alpha^20 - 333791/4561691265331200*alpha^19 + 87631/600222534912000*alpha^18 + 24928777/4001483566080000*alpha^17 - 205079/4803701760000*alpha^16 - 121008773/882680198400000*alpha^15 + 221720011/117690693120000*alpha^14 - 11808705347/16991593819200000*alpha^13 - 119431297679/3485455142400000*alpha^12 + 100119618541/1742727571200000*alpha^11 + 3526062948193/12357522777600000*alpha^10 - 83939498901133/123575227776000000*alpha^9 - 48402313811119/41191742592000000*alpha^8 + 4645283107358809/1050389436096000000*alpha^7 - 91277362389443/40399593696000000*alpha^6 - 49328830551721603/7622617782780000000*alpha^5 + 23985609069512791/1960101715572000000*alpha^4 + 2660362470248081/3587114250720000000*alpha^3 - 779839198817230403/60980942262240000000*alpha^2 + 361055235303455299/3347022171893400000000*alpha + 91136762727842675269/36817243890827400000000 - 1im*4^alpha*UQ[1,2,4,5,3])/abs(4^alpha*UQ[1,2,4,5,3]) < 3e-11 # First term = 8.9e-14 times alpha^21=9.5e12, and some integers in the formula cannot be represented in Int64, so lower accuracy can be expected
        @test abs(1/362880*alpha^13 - 1/453600*alpha^12 - 71/435456*alpha^11 + 1361/7257600*alpha^10 + 625/217728*alpha^9 - 21569/7257600*alpha^8 - 1337/64800*alpha^7 + 291101/21772800*alpha^6 + 9329/170100*alpha^5 + 3224237/59875200*alpha^4 - 272927/3628800*alpha^3 - 6823639/20528640*alpha^2 + 138793/2721600*alpha + 40018577/179625600 - UQ[1,1,3,4,4])/abs(UQ[1,1,3,4,4]) < 40*tolHot

        @test abs(-3.7536142288312445691754139973055317263278e162/exp(0.95*4*np/2)/factorp -aRHa)/abs(aRHa) < tolEx
        @test abs(1.3002334410655323780033551365563985116651e59/exp(0.35*4*np/2)/factorp -aRHu)/abs(aRHu) < tolEx
        @test abs(-2395.51952258921326919097391744358588135/exp(0.01*4*np/2)/factorp -aRHe)/abs(aRHe) < tolEx
    end

    for n = ceil(Int64, linspace(111, 5000, 5) )
        for z = linspace(1e-4, 0.99, 10)
            aRH = polyAsyRH(n, 4*n*z, alpha, T)
            @test abs(polyAsyRHgen(n, 4*n*z, alpha, T, 1.0, 1, UQ) -aRH)/abs(aRH) < 2*tolEx
        end
    end
end


## This code was suggested by Doug Bates, August 20, 2018
function compute_maxlam(xty::AbstractVector{<:Real},
                         penalty_factor::AbstractVector{<:Real})
    # compute largest value for lambda.
    # we need to divide by penalty_factor (making sure
    # not to divide by zero) to account for
    # differential penalization
    lambda_max = maximum(x / (iszero(pf) ? one(pf) : pf) for (x, pf) in zip(xty, penalty_factor))
end

function make_steps_A(xtx::AbstractMatrix{T}) where T <: BLAS.BlasFloat
    pp, p  = size(xtx)

    # compute largest eigenvalue
    d, v   = eigs(xtx, nev = 1, which = :LR)
    d      = reinterpret(Float64, d)[1]

    # make a vector of stepsizes based on d
    steps  = repeat([d], outer = p)

    # Calculate A from oem algorithm
    A      = Diagonal(steps) - xtx
    steps, A, d
end

function compute_xtx_y(x::AbstractMatrix{<:Real},
                       y::AbstractVector{<:Real},
                       intercept::Bool = true,
                       standardize::Bool = true)

    n, p = size(x)

    intscale = 1.0

    if standardize
        colsquare = sqrt.(sum(x .^ 2, dims = 1) / (n - 1))
        colsums   = sum(x, dims = 1) / n
    else
        colsquare = ones(p)
        colsums   = ones(p)
    end

    colsquare_inv = 1 ./ colsquare

    if intercept & standardize
        xtx = zeros(p + 1, p + 1)
        xty = zeros(p + 1)
        intscale = 1 / n

        colsums .*= colsquare_inv

        xtx[2:(p+1),2:(p+1)] = Diagonal(colsquare_inv[1,:]) * Symmetric(x'x) * Diagonal(colsquare_inv[1,:]) / n
        xtx[1, 2:(p+1)] = colsums * intscale
        xtx[2:(p+1), 1] = xtx[1, 2:(p+1)]'
        xtx[1, 1] = 1.0 * intscale
        xty[2:(p+1)] = ((x' * y) .* colsquare_inv') / n'
        xty[1] = mean(y) * intscale

    elseif intercept
        xtx = zeros(p + 1, p + 1)
        xty = zeros(p + 1)

        xtx[2:(p+1), 2:(p+1)] = Symmetric(x'x) / n
        xtx[1, 2:(p+1)] = colsums
        xtx[2:(p+1), 1] = xtx[1, 2:(p+1)]'
        xtx[1, 1] = 1.0

        xty[2:(p+1)] = x' * y / n'
        xty[1] = mean(y)
    elseif standardize
        xtx = Diagonal(colsquare_inv[1,:]) * Symmetric(x'x) * Diagonal(colsquare_inv[1,:]) / n
        xty = (((x' * y) .* colsquare_inv') / n')[:,1]
    else
        xtx = Symmetric(x'x) / n
        xty = x' * y / n'
    end
    Symmetric(xtx), xty, colsquare_inv, intscale
end

function create_steps(x::StridedMatrix{T},
                      group_col::StridedVector{Int}) where T <: BLAS.BlasFloat
    grps   = unique(group_col)
    n_grps = length(grps)
    n, p   = size(x)

    steps  = ones(p)
    p_cur = 0

    xtx_list = Any[]
    grp_idx_list = Any[]
    grp_bool_list = Any[]
    steps_list = Any[]
    eigens = zeros(n_grps)
    for j in 1:n_grps
        cur_grp = group_col .== grps[j]
        grp_len = sum(cur_grp)
        xc      = x[:,cur_grp]
        xtx_cur = Symmetric(xc'xc) / n

        d, v   = eigs(xtx_cur, nev = 1)

        push!(steps_list, repeat(d, outer = grp_len))
        push!(xtx_list, xtx_cur)
        push!(grp_bool_list, cur_grp)
        push!(grp_idx_list, findall(cur_grp))
        eigens[j] = d[1]
    end
    eigens, steps_list, xtx_list, grp_idx_list, grp_bool_list, grps, n_grps
end


# This file is a part of Julia. License is MIT: https://julialang.org/license

module TestHessenberg

using Test, LinearAlgebra, Random

# for tuple tests below
≅(x,y) = all(p -> p[1] ≈ p[2], zip(x,y))

let n = 10
    Random.seed!(1234321)

    Areal  = randn(n,n)/2
    Aimg   = randn(n,n)/2
    b_ = randn(n)
    B_ = randn(n,3)

    # UpperHessenberg methods not covered by the tests below
    @testset "UpperHessenberg" begin
        A = Areal
        H = UpperHessenberg(A)
        AH = triu(A,-1)
        @test UpperHessenberg(H) === H
        @test parent(H) === A
        @test Matrix(H) == Array(H) == H == AH
        @test real(H) == real(AH)
        @test real(UpperHessenberg{ComplexF64}(A)) == H
        @test real(UpperHessenberg{ComplexF64}(H)) == H
        sim = similar(H, ComplexF64)
        @test sim isa UpperHessenberg{ComplexF64}
        @test size(sim) == size(H)
        for x in (2,2+3im)
            @test x*H == H*x == x*AH
            for op in (+,-)
                @test op(H,x*I) == op(AH,x*I) == op(op(x*I,H))
                @test op(H,x*I)*x == op(AH,x*I)*x == x*op(H,x*I)
            end
        end
        @test [H[i,j] for i=1:size(H,1), j=1:size(H,2)] == triu(A,-1)
        H1 = LinearAlgebra.fillstored!(copy(H), 1)
        @test H1 == triu(fill(1, n,n), -1)
        @test tril(H1.data,-2) == tril(H.data,-2)
        A2, H2 = copy(A), copy(H)
        A2[1:4,3]=H2[1:4,3]=1:4
        H2[5,3]=0
        @test H2 == triu(A2,-1)
        @test_throws ArgumentError H[5,3]=1
        Hc = UpperHessenberg(Areal + im .* Aimg)
        AHc = triu(Areal + im .* Aimg,-1)
        @test real(Hc) == real(AHc)
        @test imag(Hc) == imag(AHc)
        @test Array(copy(adjoint(Hc))) == adjoint(Array(Hc))
        @test Array(copy(transpose(Hc))) == transpose(Array(Hc))
        @test rmul!(copy(Hc), 2.0) == lmul!(2.0, copy(Hc))
        H = UpperHessenberg(Areal)
        @test Array(Hc + H) == Array(Hc) + Array(H)
        @test Array(Hc - H) == Array(Hc) - Array(H)
    end

    @testset for eltya in (Float32, Float64, ComplexF32, ComplexF64, Int), herm in (false, true)
        A_ = eltya == Int ?
                rand(1:7, n, n) :
                convert(Matrix{eltya}, eltya <: Complex ?
                    complex.(Areal, Aimg) :
                    Areal)
        A = herm ? Hermitian(A_ + A_') : A_

        H = hessenberg(A)
        @test Hessenberg(H) === H
        eltyh = eltype(H)
        @test size(H.Q, 1) == size(A, 1)
        @test size(H.Q, 2) == size(A, 2)
        @test size(H.Q) == size(A)
        @test size(H) == size(A)
        @test_throws ErrorException H.Z
        @test convert(Array, H) ≈ A
        @test (H.Q * H.H) * H.Q' ≈ A ≈ (Matrix(H.Q) * Matrix(H.H)) * Matrix(H.Q)'
        @test (H.Q' *A) * H.Q ≈ H.H
        #getindex for HessenbergQ
        @test H.Q[1,1] ≈ Array(H.Q)[1,1]

        # REPL show
        hessstring = sprint((t, s) -> show(t, "text/plain", s), H)
        qstring = sprint((t, s) -> show(t, "text/plain", s), H.Q)
        hstring = sprint((t, s) -> show(t, "text/plain", s), H.H)
        @test hessstring == "$(summary(H))\nQ factor:\n$qstring\nH factor:\n$hstring"

        #iterate
        q,h = H
        @test q == H.Q
        @test h == H.H

        @test convert(Array, 2 * H) ≈ 2 * A ≈ convert(Array, H * 2)
        @test convert(Array, H + 2I) ≈ A + 2I ≈ convert(Array, 2I + H)
        @test convert(Array, H + (2+4im)I) ≈ A + (2+4im)I ≈ convert(Array, (2+4im)I + H)
        @test convert(Array, H - 2I) ≈ A - 2I ≈ -convert(Array, 2I - H)
        @test convert(Array, -H) == -convert(Array, H)
        @test convert(Array, 2*(H + (2+4im)I)) ≈ 2A + (4+8im)I

        b = convert(Vector{eltype(H)}, b_)
        B = convert(Matrix{eltype(H)}, B_)
        @test H \ b ≈ A \ b ≈ H \ complex(b)
        @test H \ B ≈ A \ B ≈ H \ complex(B)
        @test (H - I) \ B ≈ (A - I) \ B
        @test (H - (3+4im)I) \ B ≈ (A - (3+4im)I) \ B
        @test b' / H ≈ b' / A ≈ complex.(b') / H
        @test B' / H ≈ B' / A ≈ complex(B') / H
        @test B' / (H - I) ≈ B' / (A - I)
        @test B' / (H - (3+4im)I) ≈ B' / (A - (3+4im)I)
        @test (H - (3+4im)I)' \ B ≈ (A - (3+4im)I)' \ B
        @test B' / (H - (3+4im)I)' ≈ B' / (A - (3+4im)I)'

        for shift in (0,1,3+4im)
            @test det(H + shift*I) ≈ det(A + shift*I)
            @test logabsdet(H + shift*I) ≅ logabsdet(A + shift*I)
        end

        HM = Matrix(h)
        @test dot(b, h, b) ≈ dot(h'b, b) ≈ dot(b, HM, b) ≈ dot(HM'b, b)
        c = b .+ 1
        @test dot(b, h, c) ≈ dot(h'b, c) ≈ dot(b, HM, c) ≈ dot(HM'b, c)
    end
end

# check logdet on a matrix that has a positive determinant
let A = [0.5 0.1 0.9 0.4; 0.9 0.7 0.5 0.4; 0.3 0.4 0.9 0.0; 0.4 0.0 0.0 0.5]
    @test logdet(hessenberg(A)) ≈ logdet(A) ≈ -3.5065578973199822
end

@testset "Base.propertynames" begin
    F =  hessenberg([4. 9. 7.; 4. 4. 1.; 4. 3. 2.])
    @test Base.propertynames(F) == (:Q, :H, :μ)
    @test Base.propertynames(F, true) == (:Q, :H, :μ, :τ, :factors, :uplo)
end

end # module TestHessenberg


# ---
# title: 220. Contains Duplicate III
# id: problem220
# author: Tian Jun
# date: 2020-10-31
# difficulty: Medium
# categories: Sort, Ordered Map
# link: <https://leetcode.com/problems/contains-duplicate-iii/description/>
# hidden: true
# ---
#
# Given an array of integers, find out whether there are two distinct indices
# _i_ and _j_ in the array such that the **absolute** difference between
# **nums[i]** and **nums[j]** is at most _t_ and the **absolute** difference
# between _i_ and _j_ is at most _k_.
#
#
#
# **Example 1:**
#
#
#
#     Input: nums = [1,2,3,1], k = 3, t = 0
#     Output: true
#
#
# **Example 2:**
#
#
#
#     Input: nums = [1,0,1,1], k = 1, t = 2
#     Output: true
#
#
# **Example 3:**
#
#
#
#     Input: nums = [1,5,9,1,5,9], k = 2, t = 3
#     Output: false
#
#
#
#
# **Constraints:**
#
#   * `0 <= nums.length <= 2 * 104`
#   * `-231 <= nums[i] <= 231 - 1`
#   * `0 <= k <= 104`
#   * `0 <= t <= 231 - 1`
#
#
## @lc code=start
using LeetCode

## add your code here:
## @lc code=end


## quadrature.jl : nystrom integration methods

"""
Abstract type `QuadratureRule`

The following quadrature rules are implemented:

- `Midpoint::QuadratureRule`: the midpoint rule
- `Trapezoidal::QuadratureRule`: the trapezoidal rule
- `Simpson::QuadratureRule`: Simpson's rule
- `GaussLegendre::QuadratureRule`: Gauss-Legendre quadrature rule
- `EOLE::QuadratureRule`: expansion-optimal linear estimation

See also: [`Midpoint`](@ref), [`Trapezoidal`](@ref), [`Simpson`](@ref), [`GaussLegendre`](@ref), [`EOLE`](@ref)
"""
abstract type QuadratureRule end

"""
    GaussLegendre()

Gauss-Legendre quadrature method.

See also: [`Midpoint`](@ref), [`Trapezoidal`](@ref), [`Simpson`](@ref), [`EOLE`](@ref)
"""
struct GaussLegendre <: QuadratureRule end

"""
    EOLE()

Expansion-optimal linear estimation.

See also: [`Midpoint`](@ref), [`Trapezoidal`](@ref), [`Simpson`](@ref), [`GaussLegendre`](@ref)
"""
struct EOLE <: QuadratureRule end

"""
    Simpson()

Simpson's method.

See also: [`Midpoint`](@ref), [`Trapezoidal`](@ref), [`GaussLegendre`](@ref), [`EOLE`](@ref)
"""
struct Simpson <: QuadratureRule end

"""
    Midpoint()

The midpoint rule.

See also: [`Trapezoidal`](@ref), [`Simpson`](@ref), [`GaussLegendre`](@ref), [`EOLE`](@ref)
"""
struct Midpoint <: QuadratureRule end

"""
    Trapezoidal()

The trapezoidal rule.

See also: [`Midpoint`](@ref), [`Simpson`](@ref), [`GaussLegendre`](@ref), [`EOLE`](@ref)
"""
struct Trapezoidal <: QuadratureRule end

# get nodes and weights of Gauss-Legendre quadrature on [a,b]
function get_nodes_and_weights(n::Integer, a, b, q::GaussLegendre)
    nodes, weights = gausslegendre(n)
    weights = (b-a)/2*weights
    nodes = (b-a)/2*nodes .+ (a+b)/2
    return nodes, weights
end

# get nodes and weights of structured grid on [a,b]
function get_nodes_and_weights(n::Integer, a, b, q::EOLE)
    nodes = n == 1 ? [(a+b)/2] : range(a; stop = b,length = n)
    weights = fill((b-a) / n, n)
    return nodes, weights
end

# get nodes and weights of Simpson's rule on [a,b]
function get_nodes_and_weights(n::Integer, a, b, q::Simpson)
    iseven(n) || begin
        @warn "to use Simpson's rule, n must be even (received $(n)). I will continue with n = $(n+1)"
        n += 1
    end
    Δx = (b-a)/n
    nodes = a:Δx:b
    weights = repeat(2:2:4, outer=Int(n/2))
    weights[1] = 1
    push!(weights,1)
    weights *= Δx/3
    return nodes, weights
end

# get nodes and weights of Midpoint rule on [a,b]
function get_nodes_and_weights(n::Integer, a, b, q::Midpoint)
    Δx = (b-a)/n
    nodes = (a+Δx/2):Δx:(b-Δx/2)
    weights = fill(Δx, size(nodes))
    return nodes, weights
end

# get nodes and weights of Trapezoidal rule on [a,b]
function get_nodes_and_weights(n::Integer, a, b, q::Trapezoidal)
    Δx = (b-a)/n
    nodes = a:Δx:b
    weights = fill(Δx, length(nodes))
    weights[1] /= 2
    weights[end] /= 2
    return nodes, weights
end

## EigenSolvers
"""
Abstract type `AbstractEigenSolver`

The following eigensolvers are implemented:
-`EigenSolver<:AbstractEigenSolver`: eigenvalue decomposition using **eigen**
-`EigsSolver<:AbstractEigenSolver`: eigenvalue decomposition using **eigs**

See also: [`EigenSolver`](@ref), [`EigsSolver`](@ref)
"""
abstract type AbstractEigenSolver end

"""
    EigenSolver()

Eigenvalue decomposition using **eigen**.

See also: [`EigsSolver`](@ref)
"""
struct EigenSolver <: AbstractEigenSolver end

compute(A, n, ::EigenSolver) = eigen(A, sortby=λ->-abs(real(λ)))

"""
    EigsSolver()

Eigenvalue decomposition using **eigs**.

See also: [`EigenSolver`](@ref)
"""
struct EigsSolver <: AbstractEigenSolver end

compute(A, n, ::EigsSolver) = eigs(A, nev=n, ritzvec=true, which=:LM, v0=randn(size(A,1)))


using ITensors
using Test
using Suppressor

include("util.jl")

@testset "SVD Algorithms" begin

  @testset "Matrix With Zero Sing Val" begin
    M = [1.0 2.0 5.0 4.0;
         1.0 1.0 1.0 1.0;
         0.0 0.5 0.5 1.0;
         0.0 1.0 1.0 2.0]
    U,S,V = NDTensors.svd_recursive(M)
    @test norm(U*LinearAlgebra.Diagonal(S)*V'-M) < 1E-13
  end

  @testset "Real Matrix" begin
    M = rand(10,20)
    U,S,V = NDTensors.svd_recursive(M)
    @test norm(U*LinearAlgebra.Diagonal(S)*V'-M) < 1E-12

    M = rand(20,10)
    U,S,V = NDTensors.svd_recursive(M)
    @test norm(U*LinearAlgebra.Diagonal(S)*V'-M) < 1E-12
  end

  @testset "Cplx Matrix" begin
    M = rand(ComplexF64,10,15)
    U,S,V = NDTensors.svd_recursive(M)
    @test norm(U*LinearAlgebra.Diagonal(S)*V'-M) < 1E-13

    M = rand(ComplexF64,15,10)
    U,S,V = NDTensors.svd_recursive(M)
    @test norm(U*LinearAlgebra.Diagonal(S)*V'-M) < 1E-13
  end

  @testset "Regression Test 1" begin
    # Implementation of the SVD was giving low
    # accuracy for this case
    M = rand(2,2,2,2)

    M[:, :, 1, 1] =
    [7.713134067177845   -0.16367628720441685;
     -1.5253996568409225   1.3577749944302373]

    M[:, :, 2, 1] =
     [0.0                -2.1219889218225276;
      -8.320068013774126   0.43565608213298096]

    M[:, :, 1, 2] =
     [0.0  -8.662721825820825;
      0.0  -0.46817091771736885]

    M[:, :, 2, 2] =
     [0.0   0.0;
      0.0  -8.159570989998151]

    t1 = Index(2,"t1")
    t2 = Index(2,"t2")
    u1 = Index(2,"u1")
    u2 = Index(2,"u2")

    T = itensor(M,t1,t2,u1,u2)

    U,S,V = svd(T,(u1,t1))
    @test norm(U*S*V-T)/norm(T) < 1E-10
  end

  @testset "Ill-conditioned matrix" begin
    d = 5000
    i = Index(d, "i")
    T = itensor(make_illconditioned_matrix(dim(i)), i', i)

    @suppress begin
      F = svd(T, i'; alg = "divide_and_conquer")
    end
    # Depending on the LAPACK implementation,
    # this sometimes works so don't test it
    #@test isnothing(F)

    F = svd(T, i'; alg = "qr_iteration")
    @test !isnothing(F)
    @test F.U * F.S * F.V ≈ T
  end

end

nothing


using Thermo
using Unitful
using Unitful: K, °C, hPa, Pa
using Base.Test

T=300 # K
p=800 # hPa

# water vapor saturation methods
Lv()
Lv(T-273.15)
Lv(T*u"K")
Lv((300:303)u"K")
Lv([300 301]u"K")

@show es(T-273.15,p)
@show es(T*u"K",p*u"hPa")
# weird mixed units/unitless
@show es(273.15u"K",800)
@show es(0.0,800u"hPa")
@show es(T*u"K",p)
@show es((T-273.15)u"°C",p*u"hPa")
@show es((T-273.15),p*u"hPa")

@show qs(T-273.15,p)
@show qs(T*u"K",p*u"hPa")
@show qs(°C.(T*K),p*u"hPa")

# potential temperature methods
# nondimensional methods
@show theta(T,p)
@show theta(T,p,1000)
# weird cases with mixed unitless and units should be errors
@show theta(T*u"K",p)
@show theta((T-273.15)*u"°C",p)
#
@show theta(T,p*u"hPa")
@show theta(T,p*100*u"Pa")
@show theta(T*u"K",p*u"hPa")
@show theta(T*u"K",p*u"hPa")
@show theta(T*u"K",p*100*u"Pa")
@show theta((T-273.15)*u"°C",p*u"hPa")
@show theta((T-273.15)*u"°C",p*u"hPa")
@show theta((T-273.15)*u"°C",p*100*u"Pa")
# unit conversions are easy with Unitful; dispatch is hard

# test broadcasting arrays
T=collect(273:3:303)
@show theta(T,p)
p=collect(100:100:1000)
nd=min(length(p),length(T))
T,p = T[1:nd],p[1:nd]
@show theta(T,p)
@show theta(T,p.')


module mmodal1
using FinEtools
using FinEtoolsDeforLinear
using FinEtoolsFlexStructures.CrossSectionModule: CrossSectionRectangle
using FinEtoolsFlexStructures.MeshFrameMemberModule: frame_member, merge_members
using FinEtoolsFlexStructures.FEMMCorotBeamModule
using FinEtoolsFlexStructures.FEMMCorotBeamModule: FEMMCorotBeam
stiffness = FEMMCorotBeamModule.stiffness
mass = FEMMCorotBeamModule.mass
using FinEtoolsFlexStructures.RotUtilModule: initial_Rfield, update_rotation_field!
using LinearAlgebra: dot
using Arpack
using LinearAlgebra
using SparseArrays
using Test
function test()
        # Parameters:
        E=71240.0;#MPa
        nu=0.31;# Poisson ratio
        rho=5e-9;
        b=3.0; h=30.0; L=240.0; # cross-sectional dimensions and length of each leg in millimeters
        # Choose the mass formulation:
        mass_type=2;
        scale = 0.4

        # Reference frequencies
        reffs = [11.2732, 30.5269]
        neigvs = 2;

        # Cross-sectional properties
        cs = CrossSectionRectangle(s -> b, s -> h, s -> [0.0, 1.0, 0.0])

        # Select the number of elements per leg.
        xyz =
        n = 4;
        members = []
        push!(members, frame_member([0 0 L; L 0 L], n, cs))
        push!(members, frame_member([L 0 L; L 0 0], n, cs))
        fens, fes = merge_members(members; tolerance = L / 10000);

        # Material properties
        material = MatDeforElastIso(DeforModelRed3D, rho, E, nu, 0.0)

        # Construct the requisite fields, geometry and displacement
        # Initialize configuration variables
        geom0 = NodalField(fens.xyz)
        u0 = NodalField(zeros(size(fens.xyz,1), 3))
        Rfield0 = initial_Rfield(fens)
        dchi = NodalField(zeros(size(fens.xyz,1), 6))

        # Apply EBC's
        l1 = selectnode(fens; box = [0 0 0 0 L L], tolerance = L/10000)
        for i in [1,2,3,4,5,6]
            setebc!(dchi, l1, true, i)
        end
        applyebc!(dchi)
        numberdofs!(dchi);

        # Assemble the global discrete system
        femm = FEMMCorotBeam(IntegDomain(fes, GaussRule(1, 2)), material)
        K = stiffness(femm, geom0, u0, Rfield0, dchi);
        M = mass(femm, geom0, u0, Rfield0, dchi);

        # Solve the eigenvalue problem
        d,v,nev,nconv = eigs(K, M; nev=2*neigvs, which=:SM)
        fs = real(sqrt.(complex(d)))/(2*pi)
        # println("Natural frequencies: $fs [Hz]")
        # println("Reference: $reffs [Hz]")

        @test norm(reffs - fs[1:length(reffs)]) ./ norm(reffs) <= 3.0e-5

        # Visualize vibration modes
        # mode = 2
        # tbox = plot_space_box([[-L/2 -L/2 0]; [L/2 L/2 1.1*L]])
        # tenv0 = plot_solid(fens, fes; x = geom0.values, u = 0.0.*dchi.values[:, 1:3], R = Rfield0.values, facecolor = "rgb(125, 155, 125)", opacity = 0.3);
        # plots = cat(tbox, tenv0; dims = 1)
        # pl = render(plots; title = "Mode $(mode)")
        # Rfield1 = deepcopy(Rfield0)
        # for xscale in scale.*sin.(collect(0:1:72).*(2*pi/21))
        #     scattersysvec!(dchi, xscale.*v[:, mode])
        #     Rfield1 = deepcopy(Rfield0)
        #     update_rotation_field!(Rfield1, dchi)
        #     tenv1 = plot_solid(fens, fes; x = geom0.values, u = dchi.values[:, 1:3], R = Rfield1.values, facecolor = "rgb(25, 255, 25)");
        #     plots = cat(tbox, tenv0, tenv1; dims = 1)
        #     react!(pl, plots, pl.plot.layout)
        #     sleep(0.115)
        # end

        return true
    end # argyr_l_frame_modal_anim

end
using .mmodal1
mmodal1.test()

module mmassform11
using FinEtools
using FinEtoolsDeforLinear
using FinEtoolsFlexStructures.CrossSectionModule: CrossSectionRectangle
using FinEtoolsFlexStructures.MeshFrameMemberModule: frame_member, merge_members
using FinEtoolsFlexStructures.FEMMCorotBeamModule
using FinEtoolsFlexStructures.FEMMCorotBeamModule: FEMMCorotBeam
stiffness = FEMMCorotBeamModule.stiffness
mass = FEMMCorotBeamModule.mass
geostiffness = FEMMCorotBeamModule.geostiffness
using FinEtoolsFlexStructures.RotUtilModule: initial_Rfield, update_rotation_field!
using LinearAlgebra: dot
using Arpack
using LinearAlgebra
using SparseArrays
using FinEtoolsFlexStructures.FESetCorotBeamModule: FESetL2CorotBeam, local_frame_and_def!, local_mass_original!, local_stiffness!, natural_forces!, local_geometric_stiffness!, local_forces!
using FinEtoolsFlexStructures.FESetCorotBeamModule: local_mass_CONSISTENT_WITH_ROTATION_INERTIA!, MASS_TYPE_CONSISTENT_WITH_ROTATION_INERTIA

using Test

function test()
    # Parameters:
    E = 1000000.0;
    nu = 0.3;
    rho = 2700
    L1 = L0 = L =   30.0; # Length of the beam
    b =  0.5; # width
    h =  4.0; # height
    ecoords0 = fill(0.0, 2, 3);
    ecoords1 = fill(0.0, 2, 3)
    edisp1 = fill(0.0, 2, 3);
    evel1 = fill(0.0, 2, 6);
    evel1f = fill(0.0, 2, 6)
    dofnums = zeros(FInt, 1, 12);
    F0 = fill(0.0, 3, 3);
    Ft = fill(0.0, 3, 3);
    FtI = fill(0.0, 3, 3);
    FtJ = fill(0.0, 3, 3)
    Te = fill(0.0, 12, 12)
    tempelmat1 = fill(0.0, 12, 12);
    tempelmat2 = fill(0.0, 12, 12);
    tempelmat3 = fill(0.0, 12, 12)
    elmat = fill(0.0, 12, 12);
    elmatTe = fill(0.0, 12, 12);
    elmato = fill(0.0, 12, 12)
    elvec = fill(0.0, 12);
    elvecf = fill(0.0, 12)
    aN = fill(0.0, 6, 12)
    dN = fill(0.0, 6)
    DN = fill(0.0, 6, 6)
    PN = fill(0.0, 6)
    LF = fill(0.0, 12)
    R1I = fill(0.0, 3, 3);
    R1J = fill(0.0, 3, 3);
    OS = fill(0.0, 3, 3)

    # Reference frequencies
    reffs = [48.5475, 124.1839]

    # Cross-sectional properties
    cs = CrossSectionRectangle(s -> b, s -> h, s -> [-1.0, 0.0, 0.0])

    # Select the number of elements per leg.
    n = 32;
    members = []
    push!(members, frame_member([0 0 0; 0 0 L], n, cs))
    fens, fes = merge_members(members; tolerance = L / 10000);

    A, I1, I2, I3, J, x1x2_vector = fes.A, fes.I1, fes.I2, fes.I3, fes.J, fes.x1x2_vector
    G = E / 2 / (1 + nu)
    i = 1

    A, I1, I2, I3, rho, L = 1.3, 35.1, 32.3, 16.5, 10000.0, 3.333
    MM = fill(0.0, 12, 12)
    local_mass_CONSISTENT_WITH_ROTATION_INERTIA!(MM, A, I1, I2, I3, rho, L)
    MMp = fill(0.0, 12, 12)
    local_mass_original!(MMp, A, I1, I2, I3, rho, L, MASS_TYPE_CONSISTENT_WITH_ROTATION_INERTIA)
    @test norm(MM - MMp) / norm(MM) <= 1.0e-6
    true
end
end
using .mmassform11
mmassform11.test()

module mmassform10
using FinEtools
using FinEtoolsDeforLinear
using FinEtoolsFlexStructures.CrossSectionModule: CrossSectionRectangle
using FinEtoolsFlexStructures.MeshFrameMemberModule: frame_member, merge_members
using FinEtoolsFlexStructures.FEMMCorotBeamModule
using FinEtoolsFlexStructures.FEMMCorotBeamModule: FEMMCorotBeam
stiffness = FEMMCorotBeamModule.stiffness
mass = FEMMCorotBeamModule.mass
geostiffness = FEMMCorotBeamModule.geostiffness
using FinEtoolsFlexStructures.RotUtilModule: initial_Rfield, update_rotation_field!
using LinearAlgebra: dot
using Arpack
using LinearAlgebra
using SparseArrays
using FinEtoolsFlexStructures.FESetCorotBeamModule: FESetL2CorotBeam, local_frame_and_def!, local_mass_original!, local_stiffness!, natural_forces!, local_geometric_stiffness!, local_forces!
using FinEtoolsFlexStructures.FESetCorotBeamModule: local_mass_CONSISTENT_NO_ROTATION_INERTIA!, MASS_TYPE_CONSISTENT_NO_ROTATION_INERTIA

using Test
function test()
    # Parameters:
    E = 1000000.0;
    nu = 0.3;
    rho = 2700
    L1 = L0 = L =   30.0; # Length of the beam
    b =  0.5; # width
    h =  4.0; # height
    ecoords0 = fill(0.0, 2, 3);
    ecoords1 = fill(0.0, 2, 3)
    edisp1 = fill(0.0, 2, 3);
    evel1 = fill(0.0, 2, 6);
    evel1f = fill(0.0, 2, 6)
    dofnums = zeros(FInt, 1, 12);
    F0 = fill(0.0, 3, 3);
    Ft = fill(0.0, 3, 3);
    FtI = fill(0.0, 3, 3);
    FtJ = fill(0.0, 3, 3)
    Te = fill(0.0, 12, 12)
    tempelmat1 = fill(0.0, 12, 12);
    tempelmat2 = fill(0.0, 12, 12);
    tempelmat3 = fill(0.0, 12, 12)
    elmat = fill(0.0, 12, 12);
    elmatTe = fill(0.0, 12, 12);
    elmato = fill(0.0, 12, 12)
    elvec = fill(0.0, 12);
    elvecf = fill(0.0, 12)
    aN = fill(0.0, 6, 12)
    dN = fill(0.0, 6)
    DN = fill(0.0, 6, 6)
    PN = fill(0.0, 6)
    LF = fill(0.0, 12)
    R1I = fill(0.0, 3, 3);
    R1J = fill(0.0, 3, 3);
    OS = fill(0.0, 3, 3)

    # Reference frequencies
    reffs = [48.5475, 124.1839]

    # Cross-sectional properties
    cs = CrossSectionRectangle(s -> b, s -> h, s -> [-1.0, 0.0, 0.0])

    # Select the number of elements per leg.
    n = 32;
    members = []
    push!(members, frame_member([0 0 0; 0 0 L], n, cs))
    fens, fes = merge_members(members; tolerance = L / 10000);

    A, I1, I2, I3, J, x1x2_vector = fes.A, fes.I1, fes.I2, fes.I3, fes.J, fes.x1x2_vector
    G = E / 2 / (1 + nu)
    i = 1

    A, I1, I2, I3, rho, L = 1.3, 35.1, 32.3, 16.5, 10000.0, 3.333
    MM = fill(0.0, 12, 12)
    local_mass_CONSISTENT_NO_ROTATION_INERTIA!(MM, A, I1, I2, I3, rho, L)
    MMp = fill(0.0, 12, 12)
    local_mass_original!(MMp, A, I1, I2, I3, rho, L, MASS_TYPE_CONSISTENT_NO_ROTATION_INERTIA)
    @test norm(MM - MMp) / norm(MM) <= 1.0e-6
    true
end
end
using .mmassform10
mmassform10.test()

module mmassform12
using FinEtools
using FinEtoolsDeforLinear
using FinEtoolsFlexStructures.CrossSectionModule: CrossSectionRectangle
using FinEtoolsFlexStructures.MeshFrameMemberModule: frame_member, merge_members
using FinEtoolsFlexStructures.FEMMCorotBeamModule
using FinEtoolsFlexStructures.FEMMCorotBeamModule: FEMMCorotBeam
stiffness = FEMMCorotBeamModule.stiffness
mass = FEMMCorotBeamModule.mass
geostiffness = FEMMCorotBeamModule.geostiffness
using FinEtoolsFlexStructures.RotUtilModule: initial_Rfield, update_rotation_field!
using LinearAlgebra: dot
using Arpack
using LinearAlgebra
using SparseArrays
using FinEtoolsFlexStructures.FESetCorotBeamModule: FESetL2CorotBeam, local_frame_and_def!, local_mass_original!, local_stiffness!, natural_forces!, local_geometric_stiffness!, local_forces!
using FinEtoolsFlexStructures.FESetCorotBeamModule: local_mass_LUMPED_DIAGONAL_NO_ROTATION_INERTIA!, MASS_TYPE_LUMPED_DIAGONAL_NO_ROTATION_INERTIA

using Test
function test()
    # Parameters:
    E = 1000000.0;
    nu = 0.3;
    rho = 2700
    L1 = L0 = L =   30.0; # Length of the beam
    b =  0.5; # width
    h =  4.0; # height
    ecoords0 = fill(0.0, 2, 3);
    ecoords1 = fill(0.0, 2, 3)
    edisp1 = fill(0.0, 2, 3);
    evel1 = fill(0.0, 2, 6);
    evel1f = fill(0.0, 2, 6)
    dofnums = zeros(FInt, 1, 12);
    F0 = fill(0.0, 3, 3);
    Ft = fill(0.0, 3, 3);
    FtI = fill(0.0, 3, 3);
    FtJ = fill(0.0, 3, 3)
    Te = fill(0.0, 12, 12)
    tempelmat1 = fill(0.0, 12, 12);
    tempelmat2 = fill(0.0, 12, 12);
    tempelmat3 = fill(0.0, 12, 12)
    elmat = fill(0.0, 12, 12);
    elmatTe = fill(0.0, 12, 12);
    elmato = fill(0.0, 12, 12)
    elvec = fill(0.0, 12);
    elvecf = fill(0.0, 12)
    aN = fill(0.0, 6, 12)
    dN = fill(0.0, 6)
    DN = fill(0.0, 6, 6)
    PN = fill(0.0, 6)
    LF = fill(0.0, 12)
    R1I = fill(0.0, 3, 3);
    R1J = fill(0.0, 3, 3);
    OS = fill(0.0, 3, 3)

    # Reference frequencies
    reffs = [48.5475, 124.1839]

    # Cross-sectional properties
    cs = CrossSectionRectangle(s -> b, s -> h, s -> [-1.0, 0.0, 0.0])

    # Select the number of elements per leg.
    n = 32;
    members = []
    push!(members, frame_member([0 0 0; 0 0 L], n, cs))
    fens, fes = merge_members(members; tolerance = L / 10000);

    A, I1, I2, I3, J, x1x2_vector = fes.A, fes.I1, fes.I2, fes.I3, fes.J, fes.x1x2_vector
    G = E / 2 / (1 + nu)
    i = 1

    A, I1, I2, I3, rho, L = 1.3, 35.1, 32.3, 16.5, 10000.0, 3.333
    MM = fill(0.0, 12, 12)
    local_mass_LUMPED_DIAGONAL_NO_ROTATION_INERTIA!(MM, A, I1, I2, I3, rho, L)
    MMp = fill(0.0, 12, 12)
    local_mass_original!(MMp, A, I1, I2, I3, rho, L, MASS_TYPE_LUMPED_DIAGONAL_NO_ROTATION_INERTIA)
    @test norm(MM - MMp) / norm(MM) <= 1.0e-6
    true
end
end
using .mmassform12
mmassform12.test()

module mmassform13
using FinEtools
using FinEtoolsDeforLinear
using FinEtoolsFlexStructures.CrossSectionModule: CrossSectionRectangle
using FinEtoolsFlexStructures.MeshFrameMemberModule: frame_member, merge_members
using FinEtoolsFlexStructures.FEMMCorotBeamModule
using FinEtoolsFlexStructures.FEMMCorotBeamModule: FEMMCorotBeam
stiffness = FEMMCorotBeamModule.stiffness
mass = FEMMCorotBeamModule.mass
geostiffness = FEMMCorotBeamModule.geostiffness
using FinEtoolsFlexStructures.RotUtilModule: initial_Rfield, update_rotation_field!
using LinearAlgebra: dot
using Arpack
using LinearAlgebra
using SparseArrays
using FinEtoolsFlexStructures.FESetCorotBeamModule: FESetL2CorotBeam, local_frame_and_def!, local_mass_original!, local_stiffness!, natural_forces!, local_geometric_stiffness!, local_forces!
using FinEtoolsFlexStructures.FESetCorotBeamModule: local_mass_LUMPED_DIAGONAL_WITH_ROTATION_INERTIA!, MASS_TYPE_LUMPED_DIAGONAL_WITH_ROTATION_INERTIA

using Test
function test()
    # Parameters:
    E = 1000000.0;
    nu = 0.3;
    rho = 2700
    L1 = L0 = L =   30.0; # Length of the beam
    b =  0.5; # width
    h =  4.0; # height
    ecoords0 = fill(0.0, 2, 3);
    ecoords1 = fill(0.0, 2, 3)
    edisp1 = fill(0.0, 2, 3);
    evel1 = fill(0.0, 2, 6);
    evel1f = fill(0.0, 2, 6)
    dofnums = zeros(FInt, 1, 12);
    F0 = fill(0.0, 3, 3);
    Ft = fill(0.0, 3, 3);
    FtI = fill(0.0, 3, 3);
    FtJ = fill(0.0, 3, 3)
    Te = fill(0.0, 12, 12)
    tempelmat1 = fill(0.0, 12, 12);
    tempelmat2 = fill(0.0, 12, 12);
    tempelmat3 = fill(0.0, 12, 12)
    elmat = fill(0.0, 12, 12);
    elmatTe = fill(0.0, 12, 12);
    elmato = fill(0.0, 12, 12)
    elvec = fill(0.0, 12);
    elvecf = fill(0.0, 12)
    aN = fill(0.0, 6, 12)
    dN = fill(0.0, 6)
    DN = fill(0.0, 6, 6)
    PN = fill(0.0, 6)
    LF = fill(0.0, 12)
    R1I = fill(0.0, 3, 3);
    R1J = fill(0.0, 3, 3);
    OS = fill(0.0, 3, 3)

    # Reference frequencies
    reffs = [48.5475, 124.1839]

    # Cross-sectional properties
    cs = CrossSectionRectangle(s -> b, s -> h, s -> [-1.0, 0.0, 0.0])

    # Select the number of elements per leg.
    n = 32;
    members = []
    push!(members, frame_member([0 0 0; 0 0 L], n, cs))
    fens, fes = merge_members(members; tolerance = L / 10000);

    A, I1, I2, I3, J, x1x2_vector = fes.A, fes.I1, fes.I2, fes.I3, fes.J, fes.x1x2_vector
    G = E / 2 / (1 + nu)
    i = 1

    A, I1, I2, I3, rho, L = 1.3, 35.1, 32.3, 16.5, 10000.0, 3.333
    MM = fill(0.0, 12, 12)
    local_mass_LUMPED_DIAGONAL_WITH_ROTATION_INERTIA!(MM, A, I1, I2, I3, rho, L)
    MMp = fill(0.0, 12, 12)
    local_mass_original!(MMp, A, I1, I2, I3, rho, L, MASS_TYPE_LUMPED_DIAGONAL_WITH_ROTATION_INERTIA)
    @test norm(MM - MMp) / norm(MM) <= 1.0e-6
    true
end
end
using .mmassform13
mmassform13.test()


function _lattice_configuration!(x, y, z, dist, dist_half, rc, N)
    # Define the first positions
    x[1] = -rc + dist_half
    y[1] = -rc + dist_half
    z[1] = -rc + dist_half
    # Create a complete lattice
    @inbounds for i = 2:N
        x[i] = x[i - 1] + dist
        y[i] = y[i - 1]
        z[i] = z[i - 1]

        if x[i] > rc
            x[i] = x[1]
            y[i] = y[i - 1] + dist

            if y[i] > rc
                x[i] = x[1]
                y[i] = y[1]
                z[i] = z[i - 1] + dist
            end
        end
    end
end

@doc raw"""
    initialize!(sys::SimulationSystem)

Creates a meshgrid-like initial configuration for the given `sys` simulation
system. It uses the internal parameters to make use of the density, cut-off
radius and total number of particles. It modifies `sys` positions by using
views of the array.

# Arguments
- `sys::SimulationSystem`: The system to initialize. It modifies the system's
positions.
"""
function initialize!(sys::SimulationSystem)
    # Get the array views from the system positions
    x = view(sys.system.positions, :, 1)
    y = view(sys.system.positions, :, 2)
    z = view(sys.system.positions, :, 3)
    # Define the inter-particle distance
    distance = oftype(sys.ρ, ∛(1.0 / sys.ρ))
    # And then half of that
    distance_half = oftype(sys.ρ, distance * 0.5)
    _lattice_configuration!(x, y, z, distance, distance_half, sys.rc, sys.params.N)
end


"""
    random_mps(ElT::Type{<:Number},sites::Vector{<: Index},χ::Int64,σ::Float64)

Make a random MPS with bond dimension `χ` using the Hilbert spacee `sites`.
Each bulk tensor has one site index, and two link indices. The components of each
tensor, with type `ElT`, are randomly drawn from a uniform distribution centered
around zero, with width `σ`.
"""
function random_mps(ElT::Type{<:Number}, sites::Vector{<:Index}, χ::Int64, σ::Float64)
  d = dim(sites[1]) # Dimension of the local Hilbert space
  N = length(sites)
  links = [Index(χ; tags="Link, l=$l") for l in 1:(N - 1)]
  M = ITensor[]
  if N == 1
    rand_mat = σ * (ones(d) - 2 * rand(d))
    if ElT <: Complex
      rand_mat += im * σ * (ones(d) - 2 * rand(d))
    end
    push!(M, ITensor(rand_mat, sites[1]))
    return MPS(M)
  end
  # Site 1
  rand_mat = σ * (ones(d, χ) - 2 * rand(d, χ))
  if ElT <: Complex
    rand_mat += im * σ * (ones(d, χ) - 2 * rand(d, χ))
  end
  push!(M, ITensor(rand_mat, sites[1], links[1]))
  for j in 2:(N - 1)
    rand_mat = σ * (ones(χ, d, χ) - 2 * rand(χ, d, χ))
    if ElT <: Complex
      rand_mat += im * σ * (ones(χ, d, χ) - 2 * rand(χ, d, χ))
    end
    push!(M, ITensor(rand_mat, links[j - 1], sites[j], links[j]))
  end
  # Site N
  rand_mat = σ * (ones(χ, d) - 2 * rand(χ, d))
  if ElT <: Complex
    rand_mat += im * σ * (ones(χ, d) - 2 * rand(χ, d))
  end
  push!(M, ITensor(rand_mat, links[N - 1], sites[N]))
  return MPS(M)
end

"""
    random_mpo(ElT::Type{<:Number},sites::Vector{<: Index},χ::Int64,σ::Float64;
               processtags::Bool=false)

Make a random MPO with bond dimension `χ` using the Hilbert spacee `sites`.
Each bulk tensor has two site indices, and two link indices. The components of each
tensor, with type `ElT`, are randomly drawn from a uniform distribution centered
around zero, with width `σ`.
If `processtags=true`, add the tag `input` to the bra, and the tag `output`
to the ket.
"""
function random_mpo(
  ElT::Type{<:Number}, sites::Vector{<:Index}, χ::Int64, σ::Float64; processtags::Bool=false
)
  d = dim(sites[1]) # Dimension of the local Hilbert space
  #@show sites
  N = length(sites)
  links = [Index(χ; tags="Link, l=$l") for l in 1:(N - 1)]

  M = ITensor[]
  if N == 1
    rand_mat = σ * (ones(d, d) - 2 * rand(d, d))
    if ElT <: Complex
      rand_mat += im * σ * (ones(d, d) - 2 * rand(d, d))
    end
    push!(M, ITensor(rand_mat, sites[1]', sites[1]))
    return MPO(M)
  end

  # Site 1
  rand_mat = σ * (ones(d, χ, d) - 2 * rand(d, χ, d))
  if ElT <: Complex
    rand_mat += im * σ * (ones(d, χ, d) - 2 * rand(d, χ, d))
  end
  push!(M, ITensor(rand_mat, sites[1]', links[1], sites[1]))
  for j in 2:(N - 1)
    rand_mat = σ * (ones(d, χ, d, χ) - 2 * rand(d, χ, d, χ))
    if ElT <: Complex
      rand_mat += im * σ * (ones(d, χ, d, χ) - 2 * rand(d, χ, d, χ))
    end
    push!(M, ITensor(rand_mat, sites[j]', links[j - 1], sites[j], links[j]))
  end
  # Site N
  rand_mat = σ * (ones(d, χ, d) - 2 * rand(d, χ, d))
  if ElT <: Complex
    rand_mat += im * σ * (ones(d, χ, d) - 2 * rand(d, χ, d))
  end
  push!(M, ITensor(rand_mat, sites[N]', links[N - 1], sites[N]))

  U = MPO(M)

  proc_tagcheck = (any(x -> hastags(x, "Input"), sites))
  if proc_tagcheck
    removetags!(U, "Input")
  end
  return U
end

"""
    random_lpdo(ElT::Type{<:Number},sites::Vector{<: Index},χ::Int64,ξ::Int64,σ::Float64)

Make a random LPDO with bond dimension `χ`, kraus dimension `ξ` ,and using
the Hilbert spacee `sites`. Each bulk tensor has one site index, one kraus index,
and two link indices. The components of each tensor, with type `ElT`, are
randomly drawn from a uniform distribution centered around zero, with width `σ`.
"""
function random_lpdo(
  ElT::Type{<:Number},
  sites::Vector{<:Index},
  χ::Int64,
  ξ::Int64,
  σ::Float64;
  purifier_tags=default_purifier_tags,
)
  d = dim(sites[1]) # Dimension of the local Hilbert space
  N = length(sites)
  links = [Index(χ; tags="Link, l=$l") for l in 1:(N - 1)]
  kraus = [Index(ξ; tags=addtags(purifier_tags, "k=$s")) for s in 1:N]

  M = ITensor[]
  if N == 1
    # Site 1
    rand_mat = σ * (ones(d, ξ) - 2 * rand(rng, d, ξ))
    if ElT <: Complex
      rand_mat += im * σ * (ones(d, ξ) - 2 * rand(rng, d, ξ))
    end
    push!(M, ITensor(rand_mat, sites[1], kraus[1]))
    return LPDO(MPO(M))
  end

  # Site 1
  rand_mat = σ * (ones(d, χ, ξ) - 2 * rand(d, χ, ξ))
  if ElT <: Complex
    rand_mat += im * σ * (ones(d, χ, ξ) - 2 * rand(d, χ, ξ))
  end
  push!(M, ITensor(rand_mat, sites[1], links[1], kraus[1]))
  # Site 2..N-1
  for j in 2:(N - 1)
    rand_mat = σ * (ones(d, χ, ξ, χ) - 2 * rand(d, χ, ξ, χ))
    if ElT <: Complex
      rand_mat += im * σ * (ones(d, χ, ξ, χ) - 2 * rand(d, χ, ξ, χ))
    end
    push!(M, ITensor(rand_mat, sites[j], links[j - 1], kraus[j], links[j]))
  end
  # Site N
  rand_mat = σ * (ones(d, χ, ξ) - 2 * rand(d, χ, ξ))
  if ElT <: Complex
    rand_mat += im * σ * (ones(d, χ, ξ) - 2 * rand(d, χ, ξ))
  end
  push!(M, ITensor(rand_mat, sites[N], links[N - 1], kraus[N]))

  return LPDO(MPO(M), purifier_tags)
end

"""
    random_choi(ElT::Type{<:Number},sites::Vector{<: Index},χ::Int64,ξ::Int64,σ::Float64;
                purifier_tags = default_purifier_tags)

Make a random Choi matrix with bond dimension `χ`, kraus dimension `ξ` ,and using
the Hilbert spacee `sites`. Each bulk tensor has two site indices (corresponding
to input and output indices, one kraus index,and two link indices. The components
of each tensor, with type `ElT`, are randomly drawn from a uniform distribution
centered around zero, with width `σ`.
"""
function random_choi(
  ElT::Type{<:Number},
  sites::Vector{<:Index},
  χ::Int64,
  ξ::Int64,
  σ::Float64;
  purifier_tags=default_purifier_tags,
)
  d = dim(sites[1]) # Dimension of the local Hilbert space
  N = length(sites)
  links = [Index(χ; tags="Link, l=$l") for l in 1:(N - 1)]
  kraus = [Index(ξ; tags=addtags(purifier_tags, "k=$s")) for s in 1:N]

  M = ITensor[]
  if N == 1
    # Site 1
    rand_mat = σ * (ones(d, d, ξ) - 2 * rand(d, d, ξ))
    if ElT <: Complex
      rand_mat += im * σ * (ones(d, d, ξ) - 2 * rand(d, d, ξ))
    end
    push!(M, ITensor(rand_mat, sites[1], sites[1]', kraus[1]))
    return LPDO(choitags(MPO(M)), purifier_tags)
  end

  # Site 1
  rand_mat = σ * (ones(d, d, χ, ξ) - 2 * rand(d, d, χ, ξ))
  if ElT <: Complex
    rand_mat += im * σ * (ones(d, d, χ, ξ) - 2 * rand(d, d, χ, ξ))
  end
  push!(M, ITensor(rand_mat, sites[1], sites[1]', links[1], kraus[1]))
  # Site 2..N-1
  for j in 2:(N - 1)
    rand_mat = σ * (ones(d, d, χ, ξ, χ) - 2 * rand(d, d, χ, ξ, χ))
    if ElT <: Complex
      rand_mat += im * σ * (ones(d, d, χ, ξ, χ) - 2 * rand(d, d, χ, ξ, χ))
    end
    push!(M, ITensor(rand_mat, sites[j], sites[j]', links[j - 1], kraus[j], links[j]))
  end
  # Site N
  rand_mat = σ * (ones(d, d, χ, ξ) - 2 * rand(d, d, χ, ξ))
  if ElT <: Complex
    rand_mat += im * σ * (ones(d, d, χ, ξ) - 2 * rand(d, d, χ, ξ))
  end
  push!(M, ITensor(rand_mat, sites[N], sites[N]', links[N - 1], kraus[N]))

  Λ = MPO(M)
  has_inputtags = (any(x -> hastags(x, "Input"), sites))
  has_outputtags = (any(x -> hastags(x, "Output"), sites))
  if !has_inputtags && !has_outputtags
    addtags!(Λ, "Input"; plev=0, tags="Qubit")
    addtags!(Λ, "Output"; plev=1, tags="Qubit")
  elseif !has_outputtags
    for j in 1:N
      replacetags!(Λ, "Input", "Output"; plev=1)
    end
  end
  noprime!(Λ)
  return LPDO(Λ, purifier_tags)
end

"""
    randomstate(N::Int64; kwargs...)

    randomstate(ElT::Type{<: Number}, N::Int64; kwargs...)

Generates a random quantum state of N qubits.

Optionally, specify an element type, such as `randomstate(Float64, 10)` for a random real state (by default it is complex).

# Arguments
  - `N`: number of qubits
  - `mixed`: if false (default), generate a random MPS; if true, generates a random LPDO
  - `alg`: algorithm used for initialization: `"rand"` initializes random tensor elements;
    `"circuit"` initializes with a random quantum circuit (MPS only).
  - `σ`: size of the 0-centered uniform distribution in `alg="rand"`.
  - `χ`: bond dimension of the MPS/LPDO
  - 'ξ`: kraus dimension (LPDO)
  - `normalize`: if true, return normalized state
"""
function randomstate(ElT::Type{<:Number}, N::Int64; kwargs...)
  sites = siteinds("Qubit", N)
  return randomstate(ElT, sites; kwargs...)
end

randomstate(N::Int64; kwargs...) = randomstate(ComplexF64, N; kwargs...)

function randomstate(ElT::Type{<:Number}, sites::Vector{<:Index}; kwargs...)
  mixed::Bool = get(kwargs, :mixed, false)
  lpdo::Bool = get(kwargs, :lpdo, true)
  if !mixed
    return randomstate(ElT, MPS, sites; kwargs...)
  else
    if lpdo
      return randomstate(ElT, LPDO, sites; kwargs...)
    else
      return randomstate(ElT, MPO, sites; kwargs...)
    end
  end
end

randomstate(sites::Vector{<:Index}; kwargs...) = randomstate(ComplexF64, sites; kwargs...)

function randomstate(ElT::Type{<:Number}, T::Type, sites::Vector{<:Index}; kwargs...)
  χ::Int64 = get(kwargs, :χ, 1)
  ξ::Int64 = get(kwargs, :ξ, 1)
  σ::Float64 = get(kwargs, :σ, 0.1)
  purifier_tags = get(kwargs, :purifier_tags, default_purifier_tags)
  alg::String = get(kwargs, :alg, "rand")
  normalize::Bool = get(kwargs, :normalize, false)

  if ξ > 1
    M = random_lpdo(ElT, sites, χ, ξ, σ; purifier_tags=purifier_tags)
  else
    if T == MPS
      # Build MPS by random parameter initialization
      alg == "rand" && (M = random_mps(ElT, sites, χ, σ))
      alg == "circuit" && (M = randomMPS(ElT, sites, χ))
    elseif T == MPO
      error("initialization of random MPO density matrix not yet implemented.")
    elseif T == LPDO
      M = random_lpdo(ElT, sites, χ, ξ, σ; purifier_tags=purifier_tags)
    else
      error("ansatz type not recognized")
    end
  end
  normalize && normalize!(M)
  return M
end

"""
    randomstate(M::Union{MPS,MPO,LPDO}; kwargs...)

Generate a random state with same Hilbert space (i.e. site indices)
of a reference state `M`.
"""
function randomstate(ElT::Type{<:Number}, M::Union{MPS,MPO,LPDO}; kwargs...)
  hM = hilbertspace(M)
  return randomstate(ElT, hM; kwargs...)
end

randomstate(M::Union{MPS,MPO,LPDO}; kwargs...) = randomstate(ComplexF64, M; kwargs...)

"""
    randomprocess(N::Int64; kwargs...)

    randomprocess(ElT::Type{<: Number}, N::Int64; kwargs...)

Generates a random quantum procecss of N qubits.

Optionally choose the element type with calls like `randomprocess(Float64, 10)` (by default it is complex).

# Arguments
  - `N`: number of qubits
  - `mixed`: if false (default), generates a random MPO; if true, generates a random LPDO.
  - `alg`: initialization criteria, set to `"randompars"` (see `randomstate`).
  - `σ`: size of the 0-centered uniform distribution in `alg="rand"`.
  - `χ`: bond dimension of the MPO/LPDO.
  - 'ξ`: kraus dimension (LPDO).
"""
function randomprocess(ElT::Type{<:Number}, N::Int64; kwargs...)
  sites = siteinds("Qubit", N)
  return randomprocess(ElT, sites; kwargs...)
end

randomprocess(N::Int64; kwargs...) = randomprocess(ComplexF64, N; kwargs...)

function randomprocess(ElT::Type{<:Number}, sites::Vector{<:Index}; kwargs...)
  mixed::Bool = get(kwargs, :mixed, false)
  if mixed
    return randomprocess(ElT, LPDO, sites; kwargs...)
  end
  return randomprocess(ElT, MPO, sites; kwargs...)
end

function randomprocess(sites::Vector{<:Index}; kwargs...)
  return randomprocess(ComplexF64, sites; kwargs...)
end

function randomprocess(ElT::Type{<:Number}, T::Type, sites::Vector{<:Index}; kwargs...)
  χ::Int64 = get(kwargs, :χ, 1)
  ξ::Int64 = get(kwargs, :ξ, 1)
  σ::Float64 = get(kwargs, :σ, 0.1)
  purifier_tags = get(kwargs, :purifier_tags, default_purifier_tags)
  alg::String = get(kwargs, :alg, "rand")
  normalize::Bool = get(kwargs, :normalize, false)
  processtags = !(any(x -> hastags(x, "Input"), sites))

  if ξ > 1
    M = random_choi(ElT, sites, χ, ξ, σ; purifier_tags=purifier_tags)
  else
    if T == MPO
      if alg == "rand"
        M = random_mpo(ElT, sites, χ, σ; processtags=processtags)
      else
        error("randomMPO with circuit initialization not implemented yet")
      end
    elseif T == LPDO
      M = random_choi(ElT, sites, χ, ξ, σ; purifier_tags=purifier_tags)
    else
      error("ansatz type not recognized")
    end
  end
  if normalize
    if M isa MPO
      Φ = normalize!(unitary_mpo_to_choi_mps(M))
      Φ = Φ * √2^length(M)
      M = choi_mps_to_unitary_mpo(Φ)
    else
      normalize!(M; localnorm = 2)
    end
  end
  return M
end

function randomprocess(T::Type, sites::Vector{<:Index}; kwargs...)
  return randomprocess(ComplexF64, T, sites; kwargs...)
end

"""
    randomprocess(M::Union{MPS,MPO}; kwargs...)

Generate a random process with same Hilbert space (i.e. input
and output indices)of a reference process `M`.
"""
function randomprocess(ElT::Type{<:Number}, M::Union{MPS,MPO}; kwargs...)
  mixed = get(kwargs, :mixed, false)
  N = length(M)
  s = Index[]
  for j in 1:N
    push!(s, firstind(M[j]; tags="Site", plev=0))
  end
  proc = randomprocess(ElT, s; mixed=mixed, kwargs...)
  return proc
end

randomprocess(M::Union{MPS,MPO}; kwargs...) = randomprocess(ComplexF64, M; kwargs...)

randomprocess(ElT::Type{<:Number}, L::LPDO; kwargs...) = randomprocess(ElT, L.X; kwargs...)

randomprocess(L::LPDO; kwargs...) = randomprocess(ComplexF64, L; kwargs...)


push!(LOAD_PATH, "../LF_common/")
using m_LF3d
include("../LF_common/sparse_LF3d.jl")
include("build_nabla2_x.jl")
include("build_nabla2_y.jl")
include("build_nabla2_z.jl")

const ⊗ = kron

function test_main( NN::Array{Int64} )

  println("NN = ", NN)

  Nx = NN[1]
  Ny = NN[2]
  Nz = NN[3]

  AA = [0.0, 0.0, 0.0]
  BB = [16.0, 16.0, 16.0]

  # Initialize LF
  LF = init_LF3d_c( NN, AA, BB, verbose=true )

  print("Building nabla2_x")
  @time Lx_v1 = build_nabla2_x(LF)

  print("Building nabla2_y")
  @time Ly_v1 = build_nabla2_y(LF)

  print("Building nabla2_z")
  @time Lz_v1 = build_nabla2_z(LF)

  #print("Forming Laplacian:")
  #@time ∇2 = Lx_v1 + Ly_v1

  #print("\n")

  #print("Using Kronecker product directly: nabla2_x")
  #@time Lx_v2 = LF.LFx.D2jl ⊗ speye(Ny) ⊗ speye(Nz)

  #print("Using Kronecker product directly: nabla2_y")
  #@time Ly_v2 = speye(Nx) ⊗ LF.LFy.D2jl ⊗ speye(Nz)

  #@printf("Using Kronecker product directly: nabla2_z")
  #@time Lz_v2 = speye(Nx) ⊗ speye(Ny) ⊗ LF.LFz.D2jl

  #print("Forming Laplacian:")
  #@time ∇2 = Lx_v2 + Ly_v2

end

@code_native test_main([3,3,3])

test_main( [80, 80, 80])


function bandwidth(A::SparseMatrixCSC{Tv, Ti}) where {Tv, Ti}
    m, n = size(A)
    bot = zeros(Ti, n + 1)
    bot[n + 1] = m + 1
    for j = n:-1:1
        if A.colptr[j] < A.colptr[j + 1]
            bot[j] = min(bot[j + 1], A.rowval[A.colptr[j]])
        else
            bot[j] = bot[j + 1]
        end
    end
    top = zeros(Ti, n + 1)
    top[1] = 0
    for j = 1:n
        if A.colptr[j] < A.colptr[j + 1]
            top[j + 1] = max(top[j], A.rowval[A.colptr[j + 1] - 1])
        else
            top[j + 1] = top[j]
        end
    end

    return maximum(top[2:n + 1] .- bot[1:n])
end

@testset "CuthillMcKee" begin
    A = matrices["HB/can_292"]
    (σ, τ) = permute_plaid(A, CuthillMcKeePermuter())
    @test σ.prm === τ.prm
    (σ′, τ′) = permute_plaid(A, CuthillMcKeePermuter(reverse=true))
    @test σ.prm == reverse(σ′.prm)
    @test τ.prm == reverse(τ′.prm)
    A = SparseMatrixCSC(A)
    (σ, τ) = permute_plaid(A, CuthillMcKeePermuter())
    @test σ.prm === τ.prm
    (σ′, τ′) = permute_plaid(A, CuthillMcKeePermuter(reverse=true))
    @test σ.prm == reverse(σ′.prm)
    @test τ.prm == reverse(τ′.prm)
    @test bandwidth(A[perm(σ), perm(τ)]) < bandwidth(A)
    @test bandwidth(permutedims(A[perm(σ), perm(τ)])) < bandwidth(permutedims(A))
    A = matrices["HB/west0132"]
    (σ, τ) = permute_plaid(A, CuthillMcKeePermuter())
    (σ′, τ′) = permute_plaid(A, CuthillMcKeePermuter(reverse=true))
    @test σ.prm == reverse(σ′.prm)
    @test τ.prm == reverse(τ′.prm)
    @test bandwidth(A[perm(σ), perm(τ)]) < bandwidth(A)
    @test bandwidth(permutedims(A[perm(σ), perm(τ)])) < bandwidth(permutedims(A))
    A = matrices["LPnetlib/lp_etamacro"]
    (σ, τ) = permute_plaid(A, CuthillMcKeePermuter())
    (σ′, τ′) = permute_plaid(A, CuthillMcKeePermuter(reverse=true))
    @test σ.prm == reverse(σ′.prm)
    @test τ.prm == reverse(τ′.prm)
    @test bandwidth(A[perm(σ), perm(τ)]) < bandwidth(A)
    @test bandwidth(permutedims(A[perm(σ), perm(τ)])) < bandwidth(permutedims(A))
end


include("../required.jl")

transect

all_ids = vcat(unique(transect.newPredID), unique(transect.newPreyID))

##Create Adjacency matrix
#create n x n matrix of zeros
M = zeros(Int16, (length(unique(transect.newPredID))+length(unique(transect.newPreyID)))+1, (length(unique(transect.newPredID))+length(unique(transect.newPreyID)))+1)
#assign names for cols
for y in 2:length(all_ids)+1
    M[y,1] = all_ids[y-1]
end

#assign names for rows
for x in 2:length(all_ids)+1
    M[1,x] = all_ids[x-1]
end
M

# Fill adjacency matrix with ones where there are interactions
for i in 1:length(all_ids)
    M[transect[i, 1]+1, transect[i, 2]+1] = 1
end
M

# # Test to make sure the adjacency matrix is correctly filled
# transectTest = [[1,6],[2,7], [3,8], [4,9],[5, 10]]
# MTest = zeros(Int16, 6, 6)
# for y in 2:6
#     MTest[y,1] = transectTest[y-1][1]
# end
#
# for x in 2:6
#     MTest[1,x] = transectTest[x-1][2]
# end
#
# for i in 1:5
#     MTest[i + 1, transectTest[i][2] - 5 + 1] = 1
# end


include("testsystem1.jl")
srand(12)
Y, X = sample(20, 100, M)

function testmean(X, μ)
    @test norm(mean(X) - μ) < 1.96*std(X)/sqrt(length(X))
end
# Test the test: [try testmean(randn(1000), 0.0).value catch false end for i in 1:1000]

testmean(X[1, :, 1], 1.0)

function tostatic(M::LinearHomogSystem)
    d2, d = Kalman.dims(M)
    LinearHomogSystem(SVector{d}(M.x0),
    SMatrix{d,d}(M.P0),
    SMatrix{d,d}(M.Phi),
    SVector{d}(M.b),
    SMatrix{d,d}(M.Q),
    SVector{d2}(M.y),
    SMatrix{d2,d}(M.H),
    SMatrix{d2,d2}(M.R))
end
M2 = tostatic(M)

include("testsystem.jl")
Y, X = sample(20, 100, M)

# Reproducibility
@test sample(MersenneTwister(1), 5, 5, M) == sample(MersenneTwister(1), 5, 5, M)


const SpinOneSite = Union{TagType"S=1", TagType"SpinOne"}

function siteinds(::SpinOneSite,
                  N::Int; kwargs...)
  conserve_qns = get(kwargs,:conserve_qns,false)
  conserve_sz = get(kwargs,:conserve_sz,conserve_qns)
  if conserve_sz
    up = QN("Sz",+2) => 1
    z0 = QN("Sz", 0) => 1
    dn = QN("Sz",-2) => 1
    return [Index(up,z0,dn;tags="Site,S=1,n=$n") for n=1:N]
  end
  return [Index(3,"Site,S=1,n=$n") for n=1:N]
end

function state(::SpinOneSite,
               st::AbstractString)
  if st == "Up" || st == "↑"
    return 1
  elseif st == "Z0" || st == "0"
    return 2
  elseif st == "Dn" || st == "↓"
    return 3
  end
  throw(ArgumentError("State string \"$st\" not recognized for SpinOne site"))
  return 0
end

function op(::SpinOneSite,
            s::Index,
            opname::AbstractString)::ITensor
  Up = s(1)
  UpP = s'(1)
  Z0 = s(2)
  Z0P = s'(2)
  Dn = s(3)
  DnP = s'(3)

  Op = emptyITensor(s',dag(s))

  if opname == "S⁺" || opname == "Splus" || opname == "S+"
    Op[Z0P, Dn] = √2
    Op[UpP, Z0] = √2
  elseif opname == "S⁻" || opname == "Sminus" || opname == "S-"
    Op[Z0P, Up] = √2
    Op[DnP, Z0] = √2
  elseif opname == "Sˣ" || opname == "Sx"
    Op[Z0P, Up] = 1.0/√2
    Op[UpP, Z0] = 1.0/√2
    Op[DnP, Z0] = 1.0/√2
    Op[Z0P, Dn] = 1.0/√2
  elseif opname == "iSʸ" || opname == "iSy"
    Op[Z0P, Up] = -1.0/√2
    Op[UpP, Z0] = +1.0/√2
    Op[DnP, Z0] = -1.0/√2
    Op[Z0P, Dn] = +1.0/√2
  elseif opname == "Sʸ" || opname == "Sy"
    Op = complex(Op)
    Op[Z0P, Up] = +1.0/√2im
    Op[UpP, Z0] = -1.0/√2im
    Op[DnP, Z0] = +1.0/√2im
    Op[Z0P, Dn] = -1.0/√2im
  elseif opname == "Sᶻ" || opname == "Sz"
    Op[UpP, Up] = 1.0
    Op[DnP, Dn] = -1.0
  elseif opname == "Sᶻ²" || opname == "Sz2"
    Op[UpP, Up] = 1.0
    Op[DnP, Dn] = 1.0
  elseif opname == "Sˣ²" || opname == "Sx2"
    Op[UpP, Up] = 0.5
    Op[DnP, Up] = 0.5
    Op[Z0P, Z0] = 1.0
    Op[UpP, Dn] = 0.5
    Op[DnP, Dn] = 0.5
  elseif opname == "Sʸ²" || opname == "Sy2"
    Op[UpP, Up] = 0.5
    Op[DnP, Up] = -0.5
    Op[Z0P, Z0] = 1.0
    Op[UpP, Dn] = -0.5
    Op[DnP, Dn] = 0.5
  elseif opname == "projUp"
    Op[UpP, Up] = 1.
  elseif opname == "projZ0"
    Op[Z0P, Z0] = 1.
  elseif opname == "projDn"
    Op[DnP, Dn] = 1.
  elseif opname == "XUp"
    xup = emptyITensor(ComplexF64,s)
    xup[Up] = 0.5
    xup[Z0] = im*√2
    xup[Dn] = 0.5
    return xup
  elseif opname == "XZ0"
    xZ0 = emptyITensor(ComplexF64,s)
    xZ0[Up] = im*√2
    xZ0[Dn] = -im*√2
    return xZ0
  elseif opname == "XDn"
    xdn = emptyITensor(ComplexF64,s)
    xdn[Up] = 0.5
    xdn[Z0] = -im*√2
    xdn[Dn] = 0.5
    return xdn
  else
    throw(ArgumentError("Operator name '$opname' not recognized for SpinOneSite"))
  end
  return Op
end