# Reference journal article:
# Nikkhoo M. and Walter T.R., 2015. Triangular dislocation: An analytical,
# artefact-free solution.
# Submitted to Geophysical Journal International
#
# Copyright (c) 2014 Mehdi Nikkhoo
#
# Permission is hereby granted, free of charge, to any person obtaining a
# copy of this software and associated documentation files
# (the "Software"), to deal in the Software without restriction, including
# without limitation the rights to use, copy, modify, merge, publish,
# distribute, sublicense, and/or sell copies of the Software, and to permit
# persons to whom the Software is furnished to do so, subject to the
# following conditions:
#
# The above copyright notice and this permission notice shall be included
# in all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN
# NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
# DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
# OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
# USE OR OTHER DEALINGS IN THE SOFTWARE.
#
# I appreciate any comments or bug reports.
#
# Mehdi Nikkhoo
# created: 2013.1.24
# Last modified: 2014.7.30
#
# VolcanoTectonics Research Group
# Section 2.1, Physics of Earthquakes and Volcanoes
# Department 2, Physics of the Earth
# Helmholtz Centre Potsdam
# German Research Centre for Geosciences (GFZ)
#
# email:
# mehdi.nikkhoo@gfz-potsdam.de
# mehdi.nikkhoo@gmail.com
# -------------------------------------------------------
# Translated by Pengcheng Shi (shipengcheng1230@gmail.com) 08/2018

export disp_tri3_fs, disp_tri3_hs, strain_tri3_fs, strain_tri3_hs, stress_tri3_fs, stress_tri3_hs

const _ey = [0, 1, 0]
const _ez = [0, 0, 1]

"""
    disp_tri3_hs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, nu::T) where {T, V}

Compute displacement risen from triangular dislocation in elastic *halfspace*.
    Please see [original version (in supporting information)](https://academic.oup.com/gji/article/201/2/1119/572006#86405752)
    for details, especially the **coordinate system** used here.

## Arguments
- `X`, `Y`, `Z`: observational coordinates
- `P1`, `P2`, `P3`: three triangular vertices coordinates respectively
- `Ss`, `Ds`, `Ts`: triangular dislocation vector, Strike-slip, Dip-slip, Tensile-slip respectively
- `nu`: poisson ratio

## Output
By order: ``u_x``, ``u_y``, ``u_z``
"""
function disp_tri3_hs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, nu::T) where {T, V}
    @assert (Z ≤ zero(T) && P1[3] ≤ zero(T) && P2[3] ≤ zero(T) && P3[3] ≤ zero(T))  "Half-space solution: Z coordinates must be negative!"
    A = transform_matrix(P1, P2, P3)
    ueMS, unMS, uvMS = _disp_tri3_fs(X, Y, Z, P1, P2, P3, Ss, Ds, Ts, nu, A')
    ueFSC, unFSC, uvFSC = _td_disp_harmonic_func(X, Y, Z, P1, P2, P3, Ss, Ds, Ts, nu, A)
    P1[3] *= -one(T)
    P2[3] *= -one(T)
    P3[3] *= -one(T)
    allatsurface = P1[3] == zero(T) && P2[3] == zero(T) && P3[3] == zero(T)
    if !allatsurface
        A *= -one(T)
        A[3,1] *= -one(T)
        A[3,2] *= -one(T)
        A[3,3] *= -one(T)
    end
    ueIS, unIS, uvIS = _disp_tri3_fs(X, Y, Z, P1, P2, P3, Ss, Ds, Ts, nu, A')
    uvIS = ifelse(allatsurface, -uvIS, uvIS)
    ue = ueMS + ueIS + ueFSC
    un = unMS + unIS + unFSC
    uv = uvMS + uvIS + uvFSC
    allatsurface && begin ue *= -one(T); un *= -one(T); uv *= -one(T) end
    P1[3] *= -one(T)
    P2[3] *= -one(T)
    P3[3] *= -one(T)
    return ue, un, uv
end

"""
    disp_tri3_fs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, nu::T) where {T, V}

Compute displacement risen from triangular dislocation in elastic *fullspace*.
    Please see [original version (in supporting information)](https://academic.oup.com/gji/article/201/2/1119/572006#86405752)
    for details, especially the **coordinate system** used here.

## Arguments
The same as [`disp_tri3_hs`](@ref)
"""
function disp_tri3_fs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, nu::T) where {T, V}
    A = transform_matrix(P1, P2, P3)
    _disp_tri3_fs(X, Y, Z, P1, P2, P3, Ss, Ds, Ts, nu, A')
end

@inline function transform_matrix(P1::V, P2::V, P3::V) where V<:AbstractVector{T} where T
    Vnorm = cross(P2 - P1, P3 - P1)
    normalize!(Vnorm)
    Vstrike = cross(_ez, Vnorm)

    if norm(Vstrike) == zero(T)
        Vstrike = _ey * Vnorm[3]
        if P1[3] > zero(T)
            Vstrike *= -one(T)
        end
    end
    normalize!(Vstrike)
    Vdip = Vnorm × Vstrike
    hcat(Vnorm, Vstrike, Vdip)
end

@inline function _disp_tri3_fs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, nu::T, At::M) where {T, V, M}
    bx, by, bz = Ts, Ss, Ds
    p1, p2, p3 = [zeros(T, 3) for _ in 1: 3]

    x, y, z = coord_trans(X - P2[1], Y - P2[2], Z - P2[3], At)
    p1[1], p1[2], p1[3] = coord_trans(P1[1] - P2[1], P1[2] - P2[2], P1[3] - P2[3], At)
    p3[1], p3[2], p3[3] = coord_trans(P3[1] - P2[1], P3[2] - P2[2], P3[3] - P2[3], At)

    e12 = normalize(p2 - p1)
    e13 = normalize(p3 - p1)
    e23 = normalize(p3 - p2)

    A = acos(dot(e12, e13))
    B = acos(dot(-e12, e23))
    C = acos(dot(e23, e13))

    Trimode = trimodefinder(y, z, x, p1, p2, p3)

    if Trimode == 1
        u1Tp, v1Tp, w1Tp = TDSetupD(x, y, z, A, bx, by, bz, nu, p1, -e13)
        u2Tp, v2Tp, w2Tp = TDSetupD(x, y, z, B, bx, by, bz, nu, p2, e12)
        u3Tp, v3Tp, w3Tp = TDSetupD(x, y, z, C, bx, by, bz, nu, p3, e23)
        u = u1Tp + u2Tp + u3Tp
        v = v1Tp + v2Tp + v3Tp
        w = w1Tp + w2Tp + w3Tp
    end

    if Trimode == -1
        u1Tn, v1Tn, w1Tn = TDSetupD(x, y, z, A, bx, by, bz, nu, p1, e13)
        u2Tn, v2Tn, w2Tn = TDSetupD(x, y, z, B, bx, by, bz, nu, p2, -e12)
        u3Tn, v3Tn, w3Tn = TDSetupD(x, y, z, C, bx, by, bz, nu, p3, -e23)
        u = u1Tn + u2Tn + u3Tn
        v = v1Tn + v2Tn + v3Tn
        w = w1Tn + w2Tn + w3Tn
    end

    if Trimode == 0
        u = NaN
        v = NaN
        w = NaN
    end

    a1, a2, a3 = -x, p1[2] - y, p1[3] - z
    b1, b2, b3 = -x, -y, -z
    c1, c2, c3 = -x, p3[2] - y, p3[3] - z
    na = hypot(a1, a2, a3)
    nb = hypot(b1, b2, b3)
    nc = hypot(c1, c2, c3)

    Fiy = a1 * (b2 * c3 - b3 * c2)- a2 * (b1 * c3 - b3 * c1) + a3 * (b1 * c2 - b2 * c1)
    Fix = na * nb * nc + (a1 * b1 + a2 * b2 + a3 * b3) * nc + (a1 * c1 + a2 * c2 + a3 * c3) * nb + (b1 * c1 + b2 * c2 + b3 * c3) * na
    Fi = -2 * atan(Fiy, Fix) / 4 / π
    u += bx * Fi
    v += by * Fi
    w += bz * Fi
    ue, un, uv = coord_trans(u, v, w, At')
end

@inline function _td_disp_harmonic_func(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, nu::T, A::M) where {T, V, M}
    bx, by, bz = Ts, Ss, Ds
    bX, bY, bZ = coord_trans(bx, by, bz, A)
    u1, v1, w1 = AngSetupFSC(X, Y, Z, bX, bY, bZ, P1, P2, nu)
    u2, v2, w2 = AngSetupFSC(X, Y, Z, bX, bY, bZ, P2, P3, nu)
    u3, v3, w3 = AngSetupFSC(X, Y, Z, bX, bY, bZ, P3, P1, nu)
    return u1 + u2 + u3, v1 + v2 + v3, w1 + w2 + w3
end

@inline function AngSetupFSC(X::T, Y::T, Z::T, bX::T, bY::T, bZ::T, PA::V, PB::V, nu::T) where {T, V}
    sv1, sv2, sv3 = PB[1] - PA[1], PB[2] - PA[2], PB[3] - PA[3]
    svr = hypot(sv1, sv2, sv3)
    beta = acos(-sv3 / svr)
    if beta ≈ zero(T) || beta ≈ π
        return zero(T), zero(T), zero(T)
    else
        ey1 = [sv1, sv2, zero(T)]
        normalize!(ey1)
        ey3 = -_ez
        ey2 = cross(ey3, ey1)
        A = hcat(ey1, ey2, ey3)
        y1A, y2A, y3A = coord_trans(X - PA[1], Y - PA[2], Z - PA[3], A)
        y1AB, y2AB, y3AB = coord_trans(sv1, sv2, sv3, A)
        y1B = y1A - y1AB
        y2B = y2A - y2AB
        y3B = y3A - y3AB
        b1, b2, b3 = coord_trans(bX, bY, bZ, A)
        angle = ifelse(beta * y1A ≥ zero(T), -π + beta, beta)
        v1A, v2A, v3A = AngDisDispFSC(y1A, y2A, y3A, angle, b1, b2, b3, nu, -PA[3])
        v1B, v2B, v3B = AngDisDispFSC(y1B, y2B, y3B, angle, b1, b2, b3, nu, -PB[3])
        v1 = v1B - v1A
        v2 = v2B - v2A
        v3 = v3B - v3A
        ue, un, uv = coord_trans(v1, v2, v3, A')
        return ue, un, uv
    end
end

@inline function AngDisDispFSC(y1::T, y2::T, y3::T, beta::T, b1::T, b2::T, b3::T, nu::T, a::T) where T
    sinB, cosB = sincos(beta)
    cotB = cot(beta)
    y3b = y3 + 2a
    z1b = y1 * cosB + y3b * sinB
    z3b = -y1 * sinB + y3b * cosB
    rb = hypot(y1, y2, y3b)
    Fib = 2 * atan(-y2 / (-(rb + y3b) * cot(beta/2) + y1))

    v1cb1 = (b1 / 4 / pi / (1 - nu) * (-2 * (1 - nu) * (1 - 2 * nu) * Fib * cotB ^ 2 + (1 - 2 * nu) * y2 /
        (rb + y3b) * ((1 - 2 * nu - a / rb) * cotB - y1 / (rb + y3b) * (nu + a / rb)) + (1 - 2 * nu) *
        y2 * cosB * cotB / (rb + z3b) * (cosB + a / rb) + a * y2 * (y3b - a) * cotB / rb ^ 3 + y2 *
        (y3b - a) / (rb * (rb + y3b)) * (-(1 - 2 * nu) * cotB + y1 / (rb + y3b) * (2 * nu + a / rb) +
        a * y1 / rb ^ 2) +y2 * (y3b - a) / (rb * (rb + z3b)) * (cosB / (rb + z3b) * ((rb *
        cosB + y3b) * ((1 - 2 * nu) * cosB - a / rb) * cotB + 2 * (1 - nu) * (rb * sinB - y1) * cosB) -
        a * y3b * cosB * cotB / rb ^ 2)))

    v2cb1 = ((b1 / 4 / pi / (1 - nu) * ((1 - 2 * nu) * ((2 * (1 - nu) * cotB ^ 2 - nu) * log(rb + y3b) - (2 *
        (1 - nu) * cotB ^ 2 + 1 - 2 * nu)* cosB * log(rb + z3b)) -(1 - 2 * nu) / (rb + y3b) * (y1 *
        cotB * (1 - 2 * nu - a / rb) + nu * y3b - a + y2 ^ 2 / (rb + y3b) * (nu + a / rb)) -(1 - 2 *
        nu) * z1b * cotB / (rb + z3b) * (cosB + a / rb) - a * y1 * (y3b - a) * cotB / rb ^ 3 +
        (y3b - a) / (rb + y3b) * (-2 * nu + 1 / rb * ((1 - 2 * nu) * y1 * cotB - a) + y2 ^ 2 / (rb *
        (rb + y3b)) * (2 * nu + a / rb) + a * y2 ^ 2 / rb ^ 3) +(y3b - a) / (rb + z3b) * (cosB ^ 2 -
        1 / rb * ((1 - 2 * nu) * z1b * cotB + a * cosB) + a * y3b * z1b * cotB / rb ^ 3 - 1 / (rb *
        (rb + z3b)) * (y2 ^ 2 * cosB ^ 2 - a * z1b * cotB / rb * (rb * cosB + y3b))))))

    v3cb1 = (b1 / 4 / pi / (1 - nu) * (2 * (1 - nu) * (((1 - 2 * nu) * Fib * cotB) + (y2 / (rb + y3b) * (2 *
        nu + a / rb)) -(y2 * cosB / (rb + z3b) * (cosB + a / rb))) +y2 * (y3b - a) / rb * (2 *
        nu / (rb + y3b) + a / rb ^ 2) +y2 * (y3b - a) * cosB / (rb * (rb + z3b)) * (1 - 2 * nu -
        (rb * cosB + y3b) / (rb + z3b) * (cosB + a / rb) - a * y3b / rb ^ 2)))

    v1cb2 = (b2 / 4 / pi / (1 - nu) * ((1 - 2 * nu) * ((2 * (1 - nu) * cotB ^ 2 + nu) * log(rb + y3b) - (2 *
        (1 - nu) * cotB ^ 2 + 1)* cosB * log(rb + z3b)) +(1 - 2 * nu) / (rb + y3b) * (-(1 - 2 * nu) *
        y1 * cotB + nu * y3b - a + a * y1 * cotB / rb + y1 ^ 2 / (rb + y3b) * (nu + a / rb)) -(1 - 2 *
        nu)* cotB / (rb + z3b) * (z1b * cosB - a * (rb * sinB - y1) / (rb * cosB)) - a * y1 *
        (y3b - a) * cotB / rb ^ 3 + (y3b - a) / (rb + y3b) * (2 * nu + 1 / rb * ((1 - 2 * nu) * y1 *
        cotB + a) -y1 ^ 2 / (rb * (rb + y3b)) * (2 * nu + a / rb) - a * y1 ^ 2 / rb ^ 3) +(y3b - a) *
        cotB / (rb + z3b) * (-cosB * sinB + a * y1 * y3b / (rb ^ 3 * cosB) + (rb * sinB - y1) /
        rb * (2 * (1 - nu) * cosB - (rb * cosB + y3b) / (rb + z3b) * (1 + a / (rb * cosB))))))

    v2cb2 = (b2 / 4 / pi / (1 - nu) * (2 * (1 - nu) * (1 - 2 * nu) * Fib * cotB ^ 2 + (1 - 2 * nu) * y2 /
        (rb + y3b) * (-(1 - 2 * nu - a / rb) * cotB + y1 / (rb + y3b) * (nu + a / rb)) - (1 - 2 * nu) *
        y2 * cotB / (rb + z3b) * (1 + a / (rb * cosB)) - a * y2 * (y3b - a) * cotB / rb ^ 3 + y2 *
        (y3b - a) / (rb * (rb + y3b)) * ((1 - 2 * nu) * cotB - 2 * nu * y1 / (rb + y3b) - a * y1 / rb *
        (1 / rb + 1 / (rb + y3b))) +y2 * (y3b - a) * cotB / (rb * (rb + z3b)) * (-2 * (1 - nu) *
        cosB + (rb * cosB + y3b) / (rb + z3b) * (1 + a / (rb * cosB)) + a * y3b / (rb ^ 2 * cosB))))

    v3cb2 = (b2 / 4 / pi / (1 - nu) * (-2 * (1 - nu) * (1 - 2 * nu) * cotB * (log(rb + y3b) - cosB *
        log(rb + z3b)) -2 * (1 - nu) * y1 / (rb + y3b) * (2 * nu + a / rb) + 2 * (1 - nu) * z1b / (rb +
        z3b) * (cosB + a / rb) + (y3b - a) / rb * ((1 - 2 * nu) * cotB - 2 * nu * y1 / (rb + y3b) - a *
        y1 / rb ^ 2) -(y3b - a) / (rb + z3b) * (cosB * sinB + (rb * cosB + y3b) * cotB / rb *
        (2 * (1 - nu) * cosB - (rb * cosB + y3b) / (rb + z3b)) + a / rb * (sinB - y3b * z1b /
        rb ^ 2 - z1b * (rb * cosB + y3b) / (rb * (rb + z3b))))))

    v1cb3 = (b3 / 4 / pi / (1 - nu) * ((1 - 2 * nu) * (y2 / (rb + y3b) * (1 + a / rb) - y2 * cosB / (rb +
        z3b) * (cosB + a / rb)) -y2 * (y3b - a) / rb * (a / rb ^ 2 + 1 / (rb + y3b)) + y2 *
        (y3b - a) * cosB / (rb * (rb + z3b)) * ((rb * cosB + y3b) / (rb + z3b) * (cosB + a /
        rb) +a * y3b / rb ^ 2)))

    v2cb3 = (b3 / 4 / pi / (1 - nu) * ((1 - 2 * nu) * (-sinB * log(rb + z3b) - y1 / (rb + y3b) * (1 + a /
        rb) +z1b / (rb + z3b) * (cosB + a / rb)) +y1 * (y3b - a) / rb * (a / rb ^ 2 + 1 / (rb +
        y3b)) -(y3b - a) / (rb + z3b) * (sinB * (cosB - a / rb) + z1b / rb * (1 + a * y3b /
        rb ^ 2) -1 / (rb * (rb + z3b)) * (y2 ^ 2 * cosB * sinB - a * z1b / rb * (rb * cosB + y3b)))))

    v3cb3 = (b3 / 4 / pi / (1 - nu) * (2 * (1 - nu) * Fib + 2 * (1 - nu) * (y2 * sinB / (rb + z3b) * (cosB +
        a / rb)) +y2 * (y3b - a) * sinB / (rb * (rb + z3b)) * (1 + (rb * cosB + y3b) / (rb +
        z3b) * (cosB + a / rb) + a * y3b / rb ^ 2)))

    return v1cb1 + v1cb2 + v1cb3, v2cb1 + v2cb2 + v2cb3, v3cb1 + v3cb2 + v3cb3
end

@inline function trimodefinder(x::T, y::T, z::T, p1::V, p2::V, p3::V) where {T, V}
    a = ((p2[3] - p3[3]) * (x - p3[2]) + (p3[2] - p2[2]) * (y - p3[3])) /
        ((p2[3] - p3[3]) * (p1[2] - p3[2]) + (p3[2] - p2[2]) * (p1[3] - p3[3]))
    b = ((p3[3] - p1[3]) * (x - p3[2]) + (p1[2] - p3[2]) * (y - p3[3])) /
        ((p2[3] - p3[3]) * (p1[2] - p3[2]) + (p3[2] - p2[2]) * (p1[3] - p3[3]))
    c = 1 - a - b
    trimode = 1
    if ((a ≤ 0) && (b > c) && (c > a)) || ((b ≤ 0) && (c > a) && (a > b)) || ((c ≤ 0) && (a > b) && (b > c))
        trimode = -1
    end
    if ((a == 0) && (b ≥ 0) && (c ≥ 0)) || ((a ≥ 0) && (b == 0) && (c ≥ 0)) || ((a ≥ 0) && (b ≥ 0) && (c == 0))
        trimode = 0
    end
    if (trimode == 0) && (z ≠ 0)
        trimode = 1
    end
    return trimode
end

@inline function TDSetupD(x::T, y::T, z::T, alpha::T, bx::T, by::T, bz::T, nu::T, TriVertex::V, SideVec::V) where {T, V}
    y1 = SideVec[3] * (y - TriVertex[2]) - SideVec[2] * (z - TriVertex[3])
    z1 = SideVec[2] * (y - TriVertex[2]) + SideVec[3] * (z - TriVertex[3])
    by1 = SideVec[3] * by - SideVec[2] * bz
    bz1 = SideVec[2] * by + SideVec[3] * bz
    u, v0, w0 = AngDisDisp(x, y1, z1, -π + alpha, bx, by1, bz1, nu)
    v = SideVec[3] * v0 + SideVec[2] * w0
    w = -SideVec[2] * v0 + SideVec[3] * w0
    return u, v, w
end

@inline function _TDstrain_HarFunc(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, λ::T, μ::T, At::M) where {T, V, M}
    bx, by, bz = Ts, Ss, Ds
    bX, bY, bZ = coord_trans(bx, by, bz, At)
    exx1, eyy1, ezz1, exy1, exz1, eyz1 = AngSetupFSC_S(X, Y, Z, bX, bY, bZ, P1, P2, λ, μ)
    exx2, eyy2, ezz2, exy2, exz2, eyz2 = AngSetupFSC_S(X, Y, Z, bX, bY, bZ, P2, P3, λ, μ)
    exx3, eyy3, ezz3, exy3, exz3, eyz3 = AngSetupFSC_S(X, Y, Z, bX, bY, bZ, P3, P1, λ, μ)
    return exx1 + exx2 + exx3, eyy1 + eyy2 + eyy3, ezz1 + ezz2 + ezz3, exy1 + exy2 + exy3, exz1 + exz2 + exz3, eyz1 + eyz2 + eyz3
end

@inline function AngSetupFSC_S(X::T, Y::T, Z::T, bX::T, bY::T, bZ::T, PA::V, PB::V, λ::T, μ::T) where {T, V}
    nu = λ / (μ + λ) / 2
    sv1, sv2, sv3 = PB[1] - PA[1], PB[2] - PA[2], PB[3] - PA[3]
    svr = hypot(sv1, sv2, sv3)
    beta = acos(-sv3 / svr)
    if beta ≈ zero(T) || beta ≈ π
        return zero(T), zero(T), zero(T), zero(T), zero(T), zero(T)
    else
        ey1 = [sv1, sv2, zero(T)]
        normalize!(ey1)
        ey3 = -_ez
        ey2 = cross(ey3, ey1)
        A = hcat(ey1, ey2, ey3)
        y1A, y2A, y3A = coord_trans(X - PA[1], Y - PA[2], Z - PA[3], A)
        y1AB, y2AB, y3AB = coord_trans(sv1, sv2, sv3, A)
        y1B = y1A - y1AB
        y2B = y2A - y2AB
        y3B = y3A - y3AB
        b1, b2, b3 = coord_trans(bX, bY, bZ, A)
        if beta * y1A ≥ zero(T)
            v11A, v22A, v33A, v12A, v13A, v23A = AngDisStrainFSC(-y1A, -y2A, y3A, π - beta, -b1, -b2, b3, nu, -PA[3])
            v13A *= -one(T)
            v23A *= -one(T)
            v11B, v22B, v33B, v12B, v13B, v23B = AngDisStrainFSC(-y1B, -y2B, y3B, π - beta, -b1, -b2, b3, nu, -PB[3])
            v13B *= -one(T)
            v23B *= -one(T)
        else
            v11A, v22A, v33A, v12A, v13A, v23A = AngDisStrainFSC(y1A, y2A, y3A, beta, b1, b2, b3, nu, -PA[3])
            v11B, v22B, v33B, v12B, v13B, v23B = AngDisStrainFSC(y1B, y2B, y3B, beta, b1, b2, b3, nu, -PB[3])
        end
        v11 = v11B - v11A
        v22 = v22B - v22A
        v33 = v33B - v33A
        v12 = v12B - v12A
        v13 = v13B - v13A
        v23 = v23B - v23A
        Exx, Eyy, Ezz, Exy, Exz, Eyz = TensTrans(v11, v22, v33, v12, v13, v23, A')
        return Exx, Eyy, Ezz, Exy, Exz, Eyz
    end
end

@inline function AngDisStrainFSC(y1::T, y2::T, y3::T, beta::T, b1::T, b2::T, b3::T, nu::T, a::T) where T
    sinB, cosB = sincos(beta)
    cotB = cot(beta)
    y3b = y3 + 2a
    z1b = y1 * cosB + y3b * sinB
    z3b = -y1 * sinB + y3b * cosB
    rb2 = y1^2 + y2^2 + y3b^2
    rb = sqrt(rb2)
    W1 = rb * cosB + y3b
    W2 = cosB + a / rb
    W3 = cosB + y3b / rb
    W4 = nu + a / rb
    W5 = 2nu + a / rb
    W6 = rb + y3b
    W7 = rb + z3b
    W8 = y3 + a
    W9 = 1 + a / rb / cosB
    N1 = 1-2*nu

    rFib_ry2 = z1b / rb / (rb + z3b) - y1 / rb / (rb + y3b) # y2 = x in ADCS
    rFib_ry1 = y2 / rb / (rb + y3b) - cosB * y2 / rb / (rb + z3b) # y1 =y in ADCS
    rFib_ry3 = -sinB * y2 / rb / (rb + z3b) # y3 = z in ADCS

    v11 = (b1 * (1 / 4 * ((-2 + 2 * nu) * N1 * rFib_ry1 * cotB ^ 2 - N1 * y2 / W6 ^ 2 * ((1 - W5) * cotB -
        y1 / W6 * W4) /rb *y1+N1 *y2 /W6 * (a / rb ^ 3 * y1 * cotB - 1 / W6 * W4 + y1 ^ 2 /
        W6 ^ 2 * W4 / rb + y1 ^ 2 / W6 * a / rb ^ 3) -N1 * y2 * cosB * cotB / W7 ^ 2 * W2 * (y1 /
        rb - sinB) -N1 * y2 * cosB * cotB / W7 * a / rb ^ 3 * y1 - 3 * a * y2 * W8 * cotB / rb ^ 5. *
        y1 - y2 * W8 / rb ^ 3 / W6 * (-N1 * cotB + y1 / W6 * W5 + a * y1 / rb2) * y1 - y2 * W8 /
        rb2 / W6 ^ 2 * (-N1 * cotB + y1 / W6 * W5 + a * y1 / rb2) * y1 + y2 * W8 / rb / W6 *
        (1 / W6 * W5 - y1 ^ 2 / W6 ^ 2 * W5 / rb - y1 ^ 2 / W6 * a / rb ^ 3 + a / rb2 - 2 * a * y1 ^
        2 / rb2 ^ 2) -y2 * W8 / rb ^ 3 / W7 * (cosB / W7 * (W1 * (N1 * cosB - a / rb) * cotB +
        (2 - 2 * nu) * (rb * sinB - y1) * cosB) -a * y3b * cosB * cotB / rb2) * y1 - y2 * W8 / rb /
        W7 ^ 2 * (cosB / W7 * (W1 * (N1 * cosB - a / rb) * cotB + (2 - 2 * nu) * (rb * sinB - y1) *
        cosB) -a * y3b * cosB * cotB / rb2) * (y1 / rb - sinB) + y2 * W8 / rb / W7 * (-cosB /
        W7 ^ 2 * (W1 * (N1 * cosB - a / rb) * cotB + (2 - 2 * nu) * (rb * sinB - y1) * cosB) * (y1 /
        rb - sinB) +cosB / W7 * (1 / rb * cosB * y1 * (N1 * cosB - a / rb) * cotB + W1 * a / rb ^
        3 * y1 * cotB + (2 - 2 * nu) * (1 / rb * sinB * y1 - 1) * cosB) +2 * a * y3b * cosB * cotB /
        rb2 ^ 2 * y1)) /π/(1 - nu)) +
        b2 * (1 / 4 * (N1 * (((2 - 2 * nu) * cotB ^ 2 + nu) / rb * y1 / W6 - ((2 - 2 * nu) * cotB ^ 2 + 1) *
        cosB * (y1 / rb - sinB) / W7) -N1 / W6 ^ 2 * (-N1 * y1 * cotB + nu * y3b - a + a * y1 *
        cotB / rb + y1 ^ 2 / W6 * W4) /rb *y1+N1 /W6 * (-N1 * cotB + a * cotB / rb - a *
        y1 ^ 2 * cotB / rb ^ 3 + 2 * y1 / W6 * W4 - y1 ^ 3 / W6 ^ 2 * W4 / rb - y1 ^ 3 / W6 * a /
        rb ^ 3) +N1 * cotB / W7 ^ 2 * (z1b * cosB - a * (rb * sinB - y1) / rb / cosB) * (y1 /
        rb - sinB) -N1 * cotB / W7 * (cosB ^ 2 - a * (1 / rb * sinB * y1 - 1) / rb / cosB + a *
        (rb * sinB - y1) / rb ^ 3 / cosB * y1) -a * W8 * cotB / rb ^ 3 + 3 * a * y1 ^ 2 * W8 *
        cotB / rb ^ 5 - W8 / W6 ^ 2 * (2 * nu + 1 / rb * (N1 * y1 * cotB + a) - y1 ^ 2 / rb / W6 *
        W5 - a * y1 ^ 2 / rb ^ 3) /rb *y1+W8 /W6 * (-1 / rb ^ 3 * (N1 * y1 * cotB + a) * y1 +
        1 / rb * N1 * cotB - 2 * y1 / rb / W6 * W5 + y1 ^ 3 / rb ^ 3 / W6 * W5 + y1 ^ 3 / rb2 /
        W6 ^ 2 * W5 + y1 ^ 3 / rb2 ^ 2 / W6 * a - 2 * a / rb ^ 3 * y1 + 3 * a * y1 ^ 3 / rb ^ 5) -W8 *
        cotB / W7 ^ 2 * (-cosB * sinB + a * y1 * y3b / rb ^ 3 / cosB + (rb * sinB - y1) / rb *
        ((2 - 2 * nu) * cosB - W1 / W7 * W9)) * (y1 / rb - sinB) + W8 * cotB / W7 * (a * y3b /
        rb ^ 3 / cosB - 3 * a * y1 ^ 2 * y3b / rb ^ 5. / cosB + (1 / rb * sinB * y1 - 1) / rb *
        ((2 - 2 * nu) * cosB - W1 / W7 * W9) - (rb * sinB - y1) / rb ^ 3 * ((2 - 2 * nu) * cosB - W1 /
        W7 * W9) * y1 + (rb * sinB - y1) / rb * (-1 / rb * cosB * y1 / W7 * W9 + W1 / W7 ^ 2 *
        W9 * (y1 / rb - sinB) + W1 / W7 * a / rb ^ 3 / cosB * y1))) /π/(1 - nu)) +
        b3 * (1 / 4 * (N1 * (-y2 / W6 ^ 2 * (1 + a / rb) / rb * y1 - y2 / W6 * a / rb ^ 3 * y1 + y2 *
        cosB / W7 ^ 2 * W2 * (y1 / rb - sinB) + y2 * cosB / W7 * a / rb ^ 3 * y1) +y2 * W8 /
        rb ^ 3 * (a / rb2 + 1 / W6) * y1 - y2 * W8 / rb * (-2 * a / rb2 ^ 2 * y1 - 1 / W6 ^ 2 /
        rb * y1) -y2 * W8 * cosB / rb ^ 3 / W7 * (W1 / W7 * W2 + a * y3b / rb2) * y1 - y2 * W8 *
        cosB / rb / W7 ^ 2 * (W1 / W7 * W2 + a * y3b / rb2) * (y1 / rb - sinB) + y2 * W8 *
        cosB / rb / W7 * (1 / rb * cosB * y1 / W7 * W2 - W1 / W7 ^ 2 * W2 * (y1 / rb - sinB) -
        W1 / W7 * a / rb ^ 3 * y1 - 2 * a * y3b / rb2 ^ 2 * y1)) /π/(1 - nu)))

    v22 = (b1 * (1 / 4 * (N1 * (((2 - 2 * nu) * cotB ^ 2 - nu) / rb * y2 / W6 - ((2 - 2 * nu) * cotB ^ 2 + 1 -
        2 * nu)* cosB / rb * y2 / W7) +N1 / W6 ^ 2 * (y1 * cotB * (1 - W5) + nu * y3b - a + y2 ^
        2 / W6 * W4) /rb *y2-N1 /W6 * (a * y1 * cotB / rb ^ 3 * y2 + 2 * y2 / W6 * W4 - y2 ^
        3 / W6 ^ 2 * W4 / rb - y2 ^ 3 / W6 * a / rb ^ 3) +N1 * z1b * cotB / W7 ^ 2 * W2 / rb *
        y2 + N1 * z1b * cotB / W7 * a / rb ^ 3 * y2 + 3 * a * y2 * W8 * cotB / rb ^ 5. * y1 - W8 /
        W6 ^ 2 * (-2 * nu + 1 / rb * (N1 * y1 * cotB - a) + y2 ^ 2 / rb / W6 * W5 + a * y2 ^ 2 /
        rb ^ 3) /rb *y2+W8 /W6 * (-1 / rb ^ 3 * (N1 * y1 * cotB - a) * y2 + 2 * y2 / rb /
        W6 * W5 - y2 ^ 3 / rb ^ 3 / W6 * W5 - y2 ^ 3 / rb2 / W6 ^ 2 * W5 - y2 ^ 3 / rb2 ^ 2 / W6 *
        a + 2 * a / rb ^ 3 * y2 - 3 * a * y2 ^ 3 / rb ^ 5) -W8 / W7 ^ 2 * (cosB ^ 2 - 1 / rb * (N1 *
        z1b * cotB + a * cosB) +a * y3b * z1b * cotB / rb ^ 3 - 1 / rb / W7 * (y2 ^ 2 * cosB ^ 2 -
        a * z1b * cotB / rb * W1)) /rb *y2+W8 /W7 * (1 / rb ^ 3 * (N1 * z1b * cotB + a *
        cosB) * y2 - 3 * a * y3b * z1b * cotB / rb ^ 5. * y2 + 1 / rb ^ 3 / W7 * (y2 ^ 2 * cosB ^ 2 -
        a * z1b * cotB / rb * W1) * y2 + 1 / rb2 / W7 ^ 2 * (y2 ^ 2 * cosB ^ 2 - a * z1b * cotB /
        rb * W1) * y2 - 1 / rb / W7 * (2 * y2 * cosB ^ 2 + a * z1b * cotB / rb ^ 3 * W1 * y2 - a *
        z1b * cotB / rb2 * cosB * y2))) /π/(1 - nu)) +
        b2 * (1 / 4 * ((2 - 2 * nu) * N1 * rFib_ry2 * cotB ^ 2 + N1 / W6 * ((W5 - 1) * cotB + y1 / W6 *
        W4) -N1 * y2 ^ 2 / W6 ^ 2 * ((W5 - 1) * cotB + y1 / W6 * W4) / rb + N1 * y2 / W6 * (-a /
        rb ^ 3 * y2 * cotB - y1 / W6 ^ 2 * W4 / rb * y2 - y2 / W6 * a / rb ^ 3 * y1) -N1 * cotB /
        W7 * W9 + N1 * y2 ^ 2 * cotB / W7 ^ 2 * W9 / rb + N1 * y2 ^ 2 * cotB / W7 * a / rb ^ 3 /
        cosB - a * W8 * cotB / rb ^ 3 + 3 * a * y2 ^ 2 * W8 * cotB / rb ^ 5 + W8 / rb / W6 * (N1 *
        cotB - 2 * nu * y1 / W6 - a * y1 / rb * (1 / rb + 1 / W6)) -y2 ^ 2 * W8 / rb ^ 3 / W6 *
        (N1 * cotB - 2 * nu * y1 / W6 - a * y1 / rb * (1 / rb + 1 / W6)) - y2 ^ 2 * W8 / rb2 / W6 ^
        2 * (N1 * cotB - 2 * nu * y1 / W6 - a * y1 / rb * (1 / rb + 1 / W6)) + y2 * W8 / rb / W6 *
        (2 * nu * y1 / W6 ^ 2 / rb * y2 + a * y1 / rb ^ 3 * (1 / rb + 1 / W6) * y2 - a * y1 / rb *
        (-1 / rb ^ 3 * y2 - 1 / W6 ^ 2 / rb * y2)) +W8 * cotB / rb / W7 * ((-2 + 2 * nu) * cosB +
        W1 / W7 * W9 + a * y3b / rb2 / cosB) -y2 ^ 2 * W8 * cotB / rb ^ 3 / W7 * ((-2 + 2 * nu) *
        cosB + W1 / W7 * W9 + a * y3b / rb2 / cosB) -y2 ^ 2 * W8 * cotB / rb2 / W7 ^ 2 * ((-2 +
        2 * nu)* cosB + W1 / W7 * W9 + a * y3b / rb2 / cosB) +y2 * W8 * cotB / rb / W7 * (1 /
        rb * cosB * y2 / W7 * W9 - W1 / W7 ^ 2 * W9 / rb * y2 - W1 / W7 * a / rb ^ 3 / cosB * y2 -
        2 * a * y3b / rb2 ^ 2 / cosB * y2)) /π/(1 - nu)) +
        b3 * (1 / 4 * (N1 * (-sinB / rb * y2 / W7 + y2 / W6 ^ 2 * (1 + a / rb) / rb * y1 + y2 / W6 *
        a / rb ^ 3 * y1 - z1b / W7 ^ 2 * W2 / rb * y2 - z1b / W7 * a / rb ^ 3 * y2) -y2 * W8 /
        rb ^ 3 * (a / rb2 + 1 / W6) * y1 + y1 * W8 / rb * (-2 * a / rb2 ^ 2 * y2 - 1 / W6 ^ 2 /
        rb * y2) +W8 / W7 ^ 2 * (sinB * (cosB - a / rb) + z1b / rb * (1 + a * y3b / rb2) - 1 /
        rb / W7 * (y2 ^ 2 * cosB * sinB - a * z1b / rb * W1)) /rb *y2-W8 /W7 * (sinB * a /
        rb ^ 3 * y2 - z1b / rb ^ 3 * (1 + a * y3b / rb2) * y2 - 2 * z1b / rb ^ 5 * a * y3b * y2 +
        1 / rb ^ 3 / W7 * (y2 ^ 2 * cosB * sinB - a * z1b / rb * W1) * y2 + 1 / rb2 / W7 ^ 2 *
        (y2 ^ 2 * cosB * sinB - a * z1b / rb * W1) * y2 - 1 / rb / W7 * (2 * y2 * cosB * sinB + a *
        z1b / rb ^ 3 * W1 * y2 - a * z1b / rb2 * cosB * y2))) /π/(1 - nu)))

    v33 = (b1 * (1 / 4 * ((2 - 2 * nu) * (N1 * rFib_ry3 * cotB - y2 / W6 ^ 2 * W5 * (y3b / rb + 1) -
        1 / 2 * y2 / W6 * a / rb ^ 3 * 2 * y3b + y2 * cosB / W7 ^ 2 * W2 * W3 + 1 / 2 * y2 * cosB / W7 *
        a / rb ^ 3 * 2 * y3b) +y2 / rb * (2 * nu / W6 + a / rb2) - 1 / 2 * y2 * W8 / rb ^ 3 * (2 *
        nu / W6 + a / rb2)* 2 * y3b + y2 * W8 / rb * (-2 * nu / W6 ^ 2 * (y3b / rb + 1) - a /
        rb2 ^ 2 * 2 * y3b) +y2 * cosB / rb / W7 * (1 - 2 * nu - W1 / W7 * W2 - a * y3b / rb2) -
        1 / 2 * y2 * W8 * cosB / rb ^ 3 / W7 * (1 - 2 * nu - W1 / W7 * W2 - a * y3b / rb2) * 2 *
        y3b - y2 * W8 * cosB / rb / W7 ^ 2 * (1 - 2 * nu - W1 / W7 * W2 - a * y3b / rb2) * W3 + y2 *
        W8 * cosB / rb / W7 * (-(cosB * y3b / rb + 1) / W7 * W2 + W1 / W7 ^ 2 * W2 * W3 + 1 / 2 *
        W1 / W7 * a / rb ^ 3 * 2 * y3b - a / rb2 + a * y3b / rb2 ^ 2 * 2 * y3b)) /π/(1 - nu)) +
        b2 * (1 / 4 * ((-2 + 2 * nu) * N1 * cotB * ((y3b / rb + 1) / W6 - cosB * W3 / W7) + (2 - 2 * nu) *
        y1 / W6 ^ 2 * W5 * (y3b / rb + 1) + 1 / 2 * (2 - 2 * nu) * y1 / W6 * a / rb ^ 3 * 2 * y3b + (2 -
        2 * nu)* sinB / W7 * W2 - (2 - 2 * nu) * z1b / W7 ^ 2 * W2 * W3 - 1 / 2 * (2 - 2 * nu) * z1b /
        W7 * a / rb ^ 3 * 2 * y3b + 1 / rb * (N1 * cotB - 2 * nu * y1 / W6 - a * y1 / rb2) - 1 / 2 *
        W8 / rb ^ 3 * (N1 * cotB - 2 * nu * y1 / W6 - a * y1 / rb2) * 2 * y3b + W8 / rb * (2 * nu *
        y1 / W6 ^ 2 * (y3b / rb + 1) + a * y1 / rb2 ^ 2 * 2 * y3b) -1 / W7 * (cosB * sinB + W1 *
        cotB / rb * ((2 - 2 * nu) * cosB - W1 / W7) + a / rb * (sinB - y3b * z1b / rb2 - z1b *
        W1 / rb / W7)) +W8 / W7 ^ 2 * (cosB * sinB + W1 * cotB / rb * ((2 - 2 * nu) * cosB - W1 /
        W7) +a / rb * (sinB - y3b * z1b / rb2 - z1b * W1 / rb / W7)) * W3 - W8 / W7 * ((cosB *
        y3b / rb + 1)* cotB / rb * ((2 - 2 * nu) * cosB - W1 / W7) - 1 / 2 * W1 * cotB / rb ^ 3 *
        ((2 - 2 * nu) * cosB - W1 / W7) * 2 * y3b + W1 * cotB / rb * (-(cosB * y3b / rb + 1) / W7 +
        W1 / W7 ^ 2 * W3) -1 / 2 * a / rb ^ 3 * (sinB - y3b * z1b / rb2 - z1b * W1 / rb / W7) *
        2 * y3b + a / rb * (-z1b / rb2 - y3b * sinB / rb2 + y3b * z1b / rb2 ^ 2 * 2 * y3b -
        sinB * W1 / rb / W7 - z1b * (cosB * y3b / rb + 1) / rb / W7 + 1 / 2 * z1b * W1 / rb ^ 3 /
        W7 * 2 * y3b + z1b * W1 / rb / W7 ^ 2 * W3))) /π/(1 - nu)) +
        b3 * (1 / 4 * ((2 - 2 * nu) * rFib_ry3 - (2 - 2 * nu) * y2 * sinB / W7 ^ 2 * W2 * W3 - 1 / 2 *
        (2 - 2 * nu) * y2 * sinB / W7 * a / rb ^ 3 * 2 * y3b + y2 * sinB / rb / W7 * (1 + W1 / W7 *
        W2 + a * y3b / rb2) -1 / 2 * y2 * W8 * sinB / rb ^ 3 / W7 * (1 + W1 / W7 * W2 + a * y3b /
        rb2)* 2 * y3b - y2 * W8 * sinB / rb / W7 ^ 2 * (1 + W1 / W7 * W2 + a * y3b / rb2) * W3 +
        y2 * W8 * sinB / rb / W7 * ((cosB * y3b / rb + 1) / W7 * W2 - W1 / W7 ^ 2 * W2 * W3 -
        1 / 2 * W1 / W7 * a / rb ^ 3 * 2 * y3b + a / rb2 - a * y3b / rb2 ^ 2 * 2 * y3b)) /π/(1 - nu)))

    v12 = (b1 / 2 * (1 / 4 * ((-2 + 2 * nu) * N1 * rFib_ry2 * cotB ^ 2 + N1 / W6 * ((1 - W5) * cotB - y1 /
        W6 * W4) -N1 * y2 ^ 2 / W6 ^ 2 * ((1 - W5) * cotB - y1 / W6 * W4) / rb + N1 * y2 / W6 *
        (a / rb ^ 3 * y2 * cotB + y1 / W6 ^ 2 * W4 / rb * y2 + y2 / W6 * a / rb ^ 3 * y1) + N1 *
        cosB * cotB / W7 * W2 - N1 * y2 ^ 2 * cosB * cotB / W7 ^ 2 * W2 / rb - N1 * y2 ^ 2 * cosB *
        cotB / W7 * a / rb ^ 3 + a * W8 * cotB / rb ^ 3 - 3 * a * y2 ^ 2 * W8 * cotB / rb ^ 5 + W8 /
        rb / W6 * (-N1 * cotB + y1 / W6 * W5 + a * y1 / rb2) - y2 ^ 2 * W8 / rb ^ 3 / W6 * (-N1 *
        cotB + y1 / W6 * W5 + a * y1 / rb2) -y2 ^ 2 * W8 / rb2 / W6 ^ 2 * (-N1 * cotB + y1 /
        W6 * W5 + a * y1 / rb2) +y2 * W8 / rb / W6 * (-y1 / W6 ^ 2 * W5 / rb * y2 - y2 / W6 *
        a / rb ^ 3 * y1 - 2 * a * y1 / rb2 ^ 2 * y2) +W8 / rb / W7 * (cosB / W7 * (W1 * (N1 *
        cosB - a / rb)* cotB + (2 - 2 * nu) * (rb * sinB - y1) * cosB) -a * y3b * cosB * cotB /
        rb2) -y2 ^ 2 * W8 / rb ^ 3 / W7 * (cosB / W7 * (W1 * (N1 * cosB - a / rb) * cotB + (2 -
        2 * nu) * (rb * sinB - y1) * cosB) -a * y3b * cosB * cotB / rb2) -y2 ^ 2 * W8 / rb2 /
        W7 ^ 2 * (cosB / W7 * (W1 * (N1 * cosB - a / rb) * cotB + (2 - 2 * nu) * (rb * sinB - y1) *
        cosB) -a * y3b * cosB * cotB / rb2) +y2 * W8 / rb / W7 * (-cosB / W7 ^ 2 * (W1 *
        (N1 * cosB - a / rb) * cotB + (2 - 2 * nu) * (rb * sinB - y1) * cosB) /rb *y2+cosB /
        W7 * (1 / rb * cosB * y2 * (N1 * cosB - a / rb) * cotB + W1 * a / rb ^ 3 * y2 * cotB + (2 - 2 *
        nu) /rb*sinB *y2*cosB)+2*a *y3b*cosB*cotB /rb2 ^ 2 * y2)) /π/(1 - nu)) +
        b2 / 2 * (1 / 4 * (N1 * (((2 - 2 * nu) * cotB ^ 2 + nu) / rb * y2 / W6 - ((2 - 2 * nu) * cotB ^ 2 + 1) *
        cosB / rb * y2 / W7) -N1 / W6 ^ 2 * (-N1 * y1 * cotB + nu * y3b - a + a * y1 * cotB / rb +
        y1 ^ 2 / W6 * W4) /rb *y2+N1 /W6 * (-a * y1 * cotB / rb ^ 3 * y2 - y1 ^ 2 / W6 ^
        2 * W4 / rb * y2 - y1 ^ 2 / W6 * a / rb ^ 3 * y2) +N1 * cotB / W7 ^ 2 * (z1b * cosB - a *
        (rb * sinB - y1) / rb / cosB) /rb *y2-N1*cotB /W7 * (-a / rb2 * sinB * y2 /
        cosB + a * (rb * sinB - y1) / rb ^ 3 / cosB * y2) +3 * a * y2 * W8 * cotB / rb ^ 5. * y1 -
        W8 / W6 ^ 2 * (2 * nu + 1 / rb * (N1 * y1 * cotB + a) - y1 ^ 2 / rb / W6 * W5 - a * y1 ^ 2 /
        rb ^ 3) /rb *y2+W8 /W6 * (-1 / rb ^ 3 * (N1 * y1 * cotB + a) * y2 + y1 ^ 2 / rb ^
        3 / W6 * W5 * y2 + y1 ^ 2 / rb2 / W6 ^ 2 * W5 * y2 + y1 ^ 2 / rb2 ^ 2 / W6 * a * y2 + 3 *
        a * y1 ^ 2 / rb ^ 5. * y2) -W8 * cotB / W7 ^ 2 * (-cosB * sinB + a * y1 * y3b / rb ^ 3 /
        cosB + (rb * sinB - y1) / rb * ((2 - 2 * nu) * cosB - W1 / W7 * W9)) /rb *y2+W8*cotB /
        W7 * (-3 * a * y1 * y3b / rb ^ 5. / cosB * y2 + 1 / rb2 * sinB * y2 * ((2 - 2 * nu) * cosB -
        W1 / W7 * W9) -(rb * sinB - y1) / rb ^ 3 * ((2 - 2 * nu) * cosB - W1 / W7 * W9) * y2 + (rb *
        sinB - y1) /rb *(-1 /rb * cosB * y2 / W7 * W9 + W1 / W7 ^ 2 * W9 / rb * y2 + W1 / W7 *
        a / rb ^ 3 / cosB * y2))) /π/(1 - nu)) +
        b3 / 2 * (1 / 4 * (N1 * (1 / W6 * (1 + a / rb) - y2 ^ 2 / W6 ^ 2 * (1 + a / rb) / rb - y2 ^ 2 /
        W6 * a / rb ^ 3 - cosB / W7 * W2 + y2 ^ 2 * cosB / W7 ^ 2 * W2 / rb + y2 ^ 2 * cosB / W7 *
        a / rb ^ 3) -W8 / rb * (a / rb2 + 1 / W6) + y2 ^ 2 * W8 / rb ^ 3 * (a / rb2 + 1 / W6) -
        y2 * W8 / rb * (-2 * a / rb2 ^ 2 * y2 - 1 / W6 ^ 2 / rb * y2) + W8 * cosB / rb / W7 *
        (W1 / W7 * W2 + a * y3b / rb2) - y2 ^ 2 * W8 * cosB / rb ^ 3 / W7 * (W1 / W7 * W2 + a *
        y3b / rb2) -y2 ^ 2 * W8 * cosB / rb2 / W7 ^ 2 * (W1 / W7 * W2 + a * y3b / rb2) + y2 *
        W8 * cosB / rb / W7 * (1 / rb * cosB * y2 / W7 * W2 - W1 / W7 ^ 2 * W2 / rb * y2 - W1 /
        W7 * a / rb ^ 3 * y2 - 2 * a * y3b / rb2 ^ 2 * y2)) /π/(1 - nu)) +
        b1 / 2 * (1 / 4 * (N1 * (((2 - 2 * nu) * cotB ^ 2 - nu) / rb * y1 / W6 - ((2 - 2 * nu) * cotB ^ 2 + 1 -
        2 * nu)* cosB * (y1 / rb - sinB) / W7) +N1 / W6 ^ 2 * (y1 * cotB * (1 - W5) + nu * y3b -
        a + y2 ^ 2 / W6 * W4) /rb *y1-N1 /W6 * ((1 - W5) * cotB + a * y1 ^ 2 * cotB / rb ^ 3 -
        y2 ^ 2 / W6 ^ 2 * W4 / rb * y1 - y2 ^ 2 / W6 * a / rb ^ 3 * y1) -N1 * cosB * cotB / W7 *
        W2 + N1 * z1b * cotB / W7 ^ 2 * W2 * (y1 / rb - sinB) + N1 * z1b * cotB / W7 * a / rb ^
        3 * y1 - a * W8 * cotB / rb ^ 3 + 3 * a * y1 ^ 2 * W8 * cotB / rb ^ 5 - W8 / W6 ^ 2 * (-2 *
        nu + 1 / rb * (N1 * y1 * cotB - a) + y2 ^ 2 / rb / W6 * W5 + a * y2 ^ 2 / rb ^ 3) /rb *
        y1 + W8 / W6 * (-1 / rb ^ 3 * (N1 * y1 * cotB - a) * y1 + 1 / rb * N1 * cotB - y2 ^ 2 /
        rb ^ 3 / W6 * W5 * y1 - y2 ^ 2 / rb2 / W6 ^ 2 * W5 * y1 - y2 ^ 2 / rb2 ^ 2 / W6 * a * y1 -
        3 * a * y2 ^ 2 / rb ^ 5. * y1) -W8 / W7 ^ 2 * (cosB ^ 2 - 1 / rb * (N1 * z1b * cotB + a *
        cosB) +a * y3b * z1b * cotB / rb ^ 3 - 1 / rb / W7 * (y2 ^ 2 * cosB ^ 2 - a * z1b * cotB /
        rb * W1)) * (y1 / rb - sinB) + W8 / W7 * (1 / rb ^ 3 * (N1 * z1b * cotB + a * cosB) *
        y1 - 1 / rb * N1 * cosB * cotB + a * y3b * cosB * cotB / rb ^ 3 - 3 * a * y3b * z1b * cotB /
        rb ^ 5. * y1 + 1 / rb ^ 3 / W7 * (y2 ^ 2 * cosB ^ 2 - a * z1b * cotB / rb * W1) * y1 + 1 /
        rb / W7 ^ 2 * (y2 ^ 2 * cosB ^ 2 - a * z1b * cotB / rb * W1) * (y1 / rb - sinB) - 1 / rb /
        W7 * (-a * cosB * cotB / rb * W1 + a * z1b * cotB / rb ^ 3 * W1 * y1 - a * z1b * cotB /
        rb2 * cosB * y1))) /π/(1 - nu)) +
        b2 / 2 * (1 / 4 * ((2 - 2 * nu) * N1 * rFib_ry1 * cotB ^ 2 - N1 * y2 / W6 ^ 2 * ((W5 - 1) * cotB +
        y1 / W6 * W4) /rb *y1+N1 *y2 /W6 * (-a / rb ^ 3 * y1 * cotB + 1 / W6 * W4 - y1 ^
        2 / W6 ^ 2 * W4 / rb - y1 ^ 2 / W6 * a / rb ^ 3) +N1 * y2 * cotB / W7 ^ 2 * W9 * (y1 /
        rb - sinB) +N1 * y2 * cotB / W7 * a / rb ^ 3 / cosB * y1 + 3 * a * y2 * W8 * cotB / rb ^
        5. * y1 - y2 * W8 / rb ^ 3 / W6 * (N1 * cotB - 2 * nu * y1 / W6 - a * y1 / rb * (1 / rb + 1 /
        W6)) * y1 - y2 * W8 / rb2 / W6 ^ 2 * (N1 * cotB - 2 * nu * y1 / W6 - a * y1 / rb * (1 /
        rb + 1 / W6)) * y1 + y2 * W8 / rb / W6 * (-2 * nu / W6 + 2 * nu * y1 ^ 2 / W6 ^ 2 / rb - a /
        rb * (1 / rb + 1 / W6) + a * y1 ^ 2 / rb ^ 3 * (1 / rb + 1 / W6) - a * y1 / rb * (-1 /
        rb ^ 3 * y1 - 1 / W6 ^ 2 / rb * y1)) -y2 * W8 * cotB / rb ^ 3 / W7 * ((-2 + 2 * nu) *
        cosB + W1 / W7 * W9 + a * y3b / rb2 / cosB) * y1 - y2 * W8 * cotB / rb / W7 ^ 2 * ((-2 +
        2 * nu)* cosB + W1 / W7 * W9 + a * y3b / rb2 / cosB) * (y1 / rb - sinB) + y2 * W8 *
        cotB / rb / W7 * (1 / rb * cosB * y1 / W7 * W9 - W1 / W7 ^ 2 * W9 * (y1 / rb - sinB) -
        W1 / W7 * a / rb ^ 3 / cosB * y1 - 2 * a * y3b / rb2 ^ 2 / cosB * y1)) /π/(1 - nu)) +
        b3 / 2 * (1 / 4 * (N1 * (-sinB * (y1 / rb - sinB) / W7 - 1 / W6 * (1 + a / rb) + y1 ^ 2 / W6 ^
        2 * (1 + a / rb) / rb + y1 ^ 2 / W6 * a / rb ^ 3 + cosB / W7 * W2 - z1b / W7 ^ 2 * W2 *
        (y1 / rb - sinB) - z1b / W7 * a / rb ^ 3 * y1) +W8 / rb * (a / rb2 + 1 / W6) - y1 ^ 2 *
        W8 / rb ^ 3 * (a / rb2 + 1 / W6) + y1 * W8 / rb * (-2 * a / rb2 ^ 2 * y1 - 1 / W6 ^ 2 /
        rb * y1) +W8 / W7 ^ 2 * (sinB * (cosB - a / rb) + z1b / rb * (1 + a * y3b / rb2) - 1 /
        rb / W7 * (y2 ^ 2 * cosB * sinB - a * z1b / rb * W1)) * (y1 / rb - sinB) - W8 / W7 *
        (sinB * a / rb ^ 3 * y1 + cosB / rb * (1 + a * y3b / rb2) - z1b / rb ^ 3 * (1 + a * y3b /
        rb2) * y1 - 2 * z1b / rb ^ 5 * a * y3b * y1 + 1 / rb ^ 3 / W7 * (y2 ^ 2 * cosB * sinB - a *
        z1b / rb * W1) * y1 + 1 / rb / W7 ^ 2 * (y2 ^ 2 * cosB * sinB - a * z1b / rb * W1) *
        (y1 / rb - sinB) - 1 / rb / W7 * (-a * cosB / rb * W1 + a * z1b / rb ^ 3 * W1 * y1 - a *
        z1b / rb2 * cosB * y1))) /π/(1 - nu)))

    v13 = (b1 / 2 * (1 / 4 * ((-2 + 2 * nu) * N1 * rFib_ry3 * cotB ^ 2 - N1 * y2 / W6 ^ 2 * ((1 - W5) *
        cotB - y1 / W6 * W4) * (y3b / rb + 1) + N1 * y2 / W6 * (1 / 2 * a / rb ^ 3 * 2 * y3b * cotB +
        y1 / W6 ^ 2 * W4 * (y3b / rb + 1) + 1 / 2 * y1 / W6 * a / rb ^ 3 * 2 * y3b) -N1 * y2 * cosB *
        cotB / W7 ^ 2 * W2 * W3 - 1 / 2 * N1 * y2 * cosB * cotB / W7 * a / rb ^ 3 * 2 * y3b + a /
        rb ^ 3 * y2 * cotB - 3 / 2 * a * y2 * W8 * cotB / rb ^ 5 * 2 * y3b + y2 / rb / W6 * (-N1 *
        cotB + y1 / W6 * W5 + a * y1 / rb2) -1 / 2 * y2 * W8 / rb ^ 3 / W6 * (-N1 * cotB + y1 /
        W6 * W5 + a * y1 / rb2)* 2 * y3b - y2 * W8 / rb / W6 ^ 2 * (-N1 * cotB + y1 / W6 * W5 +
        a * y1 / rb2) * (y3b / rb + 1) + y2 * W8 / rb / W6 * (-y1 / W6 ^ 2 * W5 * (y3b / rb +
        1) -1 / 2 * y1 / W6 * a / rb ^ 3 * 2 * y3b - a * y1 / rb2 ^ 2 * 2 * y3b) +y2 / rb / W7 *
        (cosB / W7 * (W1 * (N1 * cosB - a / rb) * cotB + (2 - 2 * nu) * (rb * sinB - y1) * cosB) -
        a * y3b * cosB * cotB / rb2) -1 / 2 * y2 * W8 / rb ^ 3 / W7 * (cosB / W7 * (W1 * (N1 *
        cosB - a / rb)* cotB + (2 - 2 * nu) * (rb * sinB - y1) * cosB) -a * y3b * cosB * cotB /
        rb2)* 2 * y3b - y2 * W8 / rb / W7 ^ 2 * (cosB / W7 * (W1 * (N1 * cosB - a / rb) * cotB +
        (2 - 2 * nu) * (rb * sinB - y1) * cosB) -a * y3b * cosB * cotB / rb2) * W3 + y2 * W8 / rb /
        W7 * (-cosB / W7 ^ 2 * (W1 * (N1 * cosB - a / rb) * cotB + (2 - 2 * nu) * (rb * sinB - y1) *
        cosB) * W3 + cosB / W7 * ((cosB * y3b / rb + 1) * (N1 * cosB - a / rb) * cotB + 1 / 2 * W1 *
        a / rb ^ 3 * 2 * y3b * cotB + 1 / 2 * (2 - 2 * nu) / rb * sinB * 2 * y3b * cosB) -a * cosB *
        cotB / rb2 + a * y3b * cosB * cotB / rb2 ^ 2 * 2 * y3b)) /π/(1 - nu)) +
        b2 / 2 * (1 / 4 * (N1 * (((2 - 2 * nu) * cotB ^ 2 + nu) * (y3b / rb + 1) / W6 - ((2 - 2 * nu) * cotB ^
        2 + 1)* cosB * W3 / W7) -N1 / W6 ^ 2 * (-N1 * y1 * cotB + nu * y3b - a + a * y1 * cotB /
        rb + y1 ^ 2 / W6 * W4) * (y3b / rb + 1) + N1 / W6 * (nu - 1 / 2 * a * y1 * cotB / rb ^ 3 * 2 *
        y3b - y1 ^ 2 / W6 ^ 2 * W4 * (y3b / rb + 1) - 1 / 2 * y1 ^ 2 / W6 * a / rb ^ 3 * 2 * y3b) +
        N1 * cotB / W7 ^ 2 * (z1b * cosB - a * (rb * sinB - y1) / rb / cosB) * W3 - N1 * cotB /
        W7 * (cosB * sinB - 1 / 2 * a / rb2 * sinB * 2 * y3b / cosB + 1 / 2 * a * (rb * sinB - y1) /
        rb ^ 3 / cosB * 2 * y3b) -a / rb ^ 3 * y1 * cotB + 3 / 2 * a * y1 * W8 * cotB / rb ^ 5 * 2 *
        y3b + 1 / W6 * (2 * nu + 1 / rb * (N1 * y1 * cotB + a) - y1 ^ 2 / rb / W6 * W5 - a * y1 ^ 2 /
        rb ^ 3) -W8 / W6 ^ 2 * (2 * nu + 1 / rb * (N1 * y1 * cotB + a) - y1 ^ 2 / rb / W6 * W5 - a *
        y1 ^ 2 / rb ^ 3) * (y3b / rb + 1) + W8 / W6 * (-1 / 2 / rb ^ 3 * (N1 * y1 * cotB + a) * 2 *
        y3b + 1 / 2 * y1 ^ 2 / rb ^ 3 / W6 * W5 * 2 * y3b + y1 ^ 2 / rb / W6 ^ 2 * W5 * (y3b / rb +
        1) +1 / 2 * y1 ^ 2 / rb2 ^ 2 / W6 * a * 2 * y3b + 3 / 2 * a * y1 ^ 2 / rb ^ 5 * 2 * y3b) +
        cotB / W7 * (-cosB * sinB + a * y1 * y3b / rb ^ 3 / cosB + (rb * sinB - y1) / rb * ((2 -
        2 * nu)* cosB - W1 / W7 * W9)) -W8 * cotB / W7 ^ 2 * (-cosB * sinB + a * y1 * y3b / rb ^
        3 / cosB + (rb * sinB - y1) / rb * ((2 - 2 * nu) * cosB - W1 / W7 * W9)) * W3 + W8 * cotB /
        W7 * (a / rb ^ 3 / cosB * y1 - 3 / 2 * a * y1 * y3b / rb ^ 5. / cosB * 2 * y3b + 1 / 2 /
        rb2 * sinB * 2 * y3b * ((2 - 2 * nu) * cosB - W1 / W7 * W9) - 1 / 2 * (rb * sinB - y1) / rb ^
        3 * ((2 - 2 * nu) * cosB - W1 / W7 * W9) * 2 * y3b + (rb * sinB - y1) / rb * (-(cosB * y3b /
        rb + 1) /W7 *W9+W1 /W7 ^ 2 * W9 * W3 + 1 / 2 * W1 / W7 * a / rb ^ 3 / cosB * 2 *
        y3b))) /π/(1 - nu)) +
        b3 / 2 * (1 / 4 * (N1 * (-y2 / W6 ^ 2 * (1 + a / rb) * (y3b / rb + 1) - 1 / 2 * y2 / W6 * a /
        rb ^ 3 * 2 * y3b + y2 * cosB / W7 ^ 2 * W2 * W3 + 1 / 2 * y2 * cosB / W7 * a / rb ^ 3 * 2 *
        y3b) -y2 / rb * (a / rb2 + 1 / W6) + 1 / 2 * y2 * W8 / rb ^ 3 * (a / rb2 + 1 / W6) * 2 *
        y3b - y2 * W8 / rb * (-a / rb2 ^ 2 * 2 * y3b - 1 / W6 ^ 2 * (y3b / rb + 1)) + y2 * cosB /
        rb / W7 * (W1 / W7 * W2 + a * y3b / rb2) - 1 / 2 * y2 * W8 * cosB / rb ^ 3 / W7 * (W1 /
        W7 * W2 + a * y3b / rb2)* 2 * y3b - y2 * W8 * cosB / rb / W7 ^ 2 * (W1 / W7 * W2 + a *
        y3b / rb2) * W3 + y2 * W8 * cosB / rb / W7 * ((cosB * y3b / rb + 1) / W7 * W2 - W1 /
        W7 ^ 2 * W2 * W3 - 1 / 2 * W1 / W7 * a / rb ^ 3 * 2 * y3b + a / rb2 - a * y3b / rb2 ^ 2 * 2 *
        y3b)) /π/(1 - nu)) +
        b1 / 2 * (1 / 4 * ((2 - 2 * nu) * (N1 * rFib_ry1 * cotB - y1 / W6 ^ 2 * W5 / rb * y2 - y2 / W6 *
        a / rb ^ 3 * y1 + y2 * cosB / W7 ^ 2 * W2 * (y1 / rb - sinB) + y2 * cosB / W7 * a / rb ^
        3 * y1) -y2 * W8 / rb ^ 3 * (2 * nu / W6 + a / rb2) * y1 + y2 * W8 / rb * (-2 * nu / W6 ^
        2 / rb * y1 - 2 * a / rb2 ^ 2 * y1) -y2 * W8 * cosB / rb ^ 3 / W7 * (1 - 2 * nu - W1 / W7 *
        W2 - a * y3b / rb2) * y1 - y2 * W8 * cosB / rb / W7 ^ 2 * (1 - 2 * nu - W1 / W7 * W2 - a *
        y3b / rb2) * (y1 / rb - sinB) + y2 * W8 * cosB / rb / W7 * (-1 / rb * cosB * y1 / W7 *
        W2 + W1 / W7 ^ 2 * W2 * (y1 / rb - sinB) + W1 / W7 * a / rb ^ 3 * y1 + 2 * a * y3b / rb2 ^
        2 * y1)) /π/(1 - nu)) +
        b2 / 2 * (1 / 4 * ((-2 + 2 * nu) * N1 * cotB * (1 / rb * y1 / W6 - cosB * (y1 / rb - sinB) / W7) -
        (2 - 2 * nu) / W6 * W5 + (2 - 2 * nu) * y1 ^ 2 / W6 ^ 2 * W5 / rb + (2 - 2 * nu) * y1 ^ 2 / W6 *
        a / rb ^ 3 + (2 - 2 * nu) * cosB / W7 * W2 - (2 - 2 * nu) * z1b / W7 ^ 2 * W2 * (y1 / rb -
        sinB) -(2 - 2 * nu) * z1b / W7 * a / rb ^ 3 * y1 - W8 / rb ^ 3 * (N1 * cotB - 2 * nu * y1 /
        W6 - a * y1 / rb2) * y1 + W8 / rb * (-2 * nu / W6 + 2 * nu * y1 ^ 2 / W6 ^ 2 / rb - a / rb2 +
        2 * a * y1 ^ 2 / rb2 ^ 2) +W8 / W7 ^ 2 * (cosB * sinB + W1 * cotB / rb * ((2 - 2 * nu) *
        cosB - W1 / W7) +a / rb * (sinB - y3b * z1b / rb2 - z1b * W1 / rb / W7)) * (y1 / rb -
        sinB) -W8 / W7 * (1 / rb2 * cosB * y1 * cotB * ((2 - 2 * nu) * cosB - W1 / W7) - W1 *
        cotB / rb ^ 3 * ((2 - 2 * nu) * cosB - W1 / W7) * y1 + W1 * cotB / rb * (-1 / rb * cosB *
        y1 / W7 + W1 / W7 ^ 2 * (y1 / rb - sinB)) -a / rb ^ 3 * (sinB - y3b * z1b / rb2 -
        z1b * W1 / rb / W7) * y1 + a / rb * (-y3b * cosB / rb2 + 2 * y3b * z1b / rb2 ^ 2 * y1 -
        cosB * W1 / rb / W7 - z1b / rb2 * cosB * y1 / W7 + z1b * W1 / rb ^ 3 / W7 * y1 + z1b *
        W1 / rb / W7 ^ 2 * (y1 / rb - sinB)))) /π/(1 - nu)) +
        b3 / 2 * (1 / 4 * ((2 - 2 * nu) * rFib_ry1 - (2 - 2 * nu) * y2 * sinB / W7 ^ 2 * W2 * (y1 / rb -
        sinB) -(2 - 2 * nu) * y2 * sinB / W7 * a / rb ^ 3 * y1 - y2 * W8 * sinB / rb ^ 3 / W7 * (1 +
        W1 / W7 * W2 + a * y3b / rb2) * y1 - y2 * W8 * sinB / rb / W7 ^ 2 * (1 + W1 / W7 * W2 +
        a * y3b / rb2) * (y1 / rb - sinB) + y2 * W8 * sinB / rb / W7 * (1 / rb * cosB * y1 /
        W7 * W2 - W1 / W7 ^ 2 * W2 * (y1 / rb - sinB) - W1 / W7 * a / rb ^ 3 * y1 - 2 * a * y3b /
        rb2 ^ 2 * y1)) /π/(1 - nu)))

    v23 = (b1 / 2 * (1 / 4 * (N1 * (((2 - 2 * nu) * cotB ^ 2 - nu) * (y3b / rb + 1) / W6 - ((2 - 2 * nu) *
        cotB ^ 2 + 1 - 2 * nu)* cosB * W3 / W7) +N1 / W6 ^ 2 * (y1 * cotB * (1 - W5) + nu * y3b - a +
        y2 ^ 2 / W6 * W4) * (y3b / rb + 1) - N1 / W6 * (1 / 2 * a * y1 * cotB / rb ^ 3 * 2 * y3b +
        nu - y2 ^ 2 / W6 ^ 2 * W4 * (y3b / rb + 1) - 1 / 2 * y2 ^ 2 / W6 * a / rb ^ 3 * 2 * y3b) -N1 *
        sinB * cotB / W7 * W2 + N1 * z1b * cotB / W7 ^ 2 * W2 * W3 + 1 / 2 * N1 * z1b * cotB / W7 *
        a / rb ^ 3 * 2 * y3b - a / rb ^ 3 * y1 * cotB + 3 / 2 * a * y1 * W8 * cotB / rb ^ 5 * 2 * y3b +
        1 / W6 * (-2 * nu + 1 / rb * (N1 * y1 * cotB - a) + y2 ^ 2 / rb / W6 * W5 + a * y2 ^ 2 /
        rb ^ 3) -W8 / W6 ^ 2 * (-2 * nu + 1 / rb * (N1 * y1 * cotB - a) + y2 ^ 2 / rb / W6 * W5 +
        a * y2 ^ 2 / rb ^ 3) * (y3b / rb + 1) + W8 / W6 * (-1 / 2 / rb ^ 3 * (N1 * y1 * cotB - a) *
        2 * y3b - 1 / 2 * y2 ^ 2 / rb ^ 3 / W6 * W5 * 2 * y3b - y2 ^ 2 / rb / W6 ^ 2 * W5 * (y3b /
        rb + 1) -1 / 2 * y2 ^ 2 / rb2 ^ 2 / W6 * a * 2 * y3b - 3 / 2 * a * y2 ^ 2 / rb ^ 5 * 2 * y3b) +
        1 / W7 * (cosB ^ 2 - 1 / rb * (N1 * z1b * cotB + a * cosB) + a * y3b * z1b * cotB / rb ^
        3 - 1 / rb / W7 * (y2 ^ 2 * cosB ^ 2 - a * z1b * cotB / rb * W1)) -W8 / W7 ^ 2 * (cosB ^ 2 -
        1 / rb * (N1 * z1b * cotB + a * cosB) + a * y3b * z1b * cotB / rb ^ 3 - 1 / rb / W7 *
        (y2 ^ 2 * cosB ^ 2 - a * z1b * cotB / rb * W1)) * W3 + W8 / W7 * (1 / 2 / rb ^ 3 * (N1 *
        z1b * cotB + a * cosB)* 2 * y3b - 1 / rb * N1 * sinB * cotB + a * z1b * cotB / rb ^ 3 + a *
        y3b * sinB * cotB / rb ^ 3 - 3 / 2 * a * y3b * z1b * cotB / rb ^ 5 * 2 * y3b + 1 / 2 / rb ^
        3 / W7 * (y2 ^ 2 * cosB ^ 2 - a * z1b * cotB / rb * W1) * 2 * y3b + 1 / rb / W7 ^ 2 * (y2 ^
        2 * cosB ^ 2 - a * z1b * cotB / rb * W1) * W3 - 1 / rb / W7 * (-a * sinB * cotB / rb * W1 +
        1 / 2 * a * z1b * cotB / rb ^ 3 * W1 * 2 * y3b - a * z1b * cotB / rb * (cosB * y3b / rb +
        1)))) /π/(1 - nu)) +
        b2 / 2 * (1 / 4 * ((2 - 2 * nu) * N1 * rFib_ry3 * cotB ^ 2 - N1 * y2 / W6 ^ 2 * ((W5 - 1) * cotB +
        y1 / W6 * W4) * (y3b / rb + 1) + N1 * y2 / W6 * (-1 / 2 * a / rb ^ 3 * 2 * y3b * cotB - y1 /
        W6 ^ 2 * W4 * (y3b / rb + 1) - 1 / 2 * y1 / W6 * a / rb ^ 3 * 2 * y3b) +N1 * y2 * cotB /
        W7 ^ 2 * W9 * W3 + 1 / 2 * N1 * y2 * cotB / W7 * a / rb ^ 3 / cosB * 2 * y3b - a / rb ^ 3 *
        y2 * cotB + 3 / 2 * a * y2 * W8 * cotB / rb ^ 5 * 2 * y3b + y2 / rb / W6 * (N1 * cotB - 2 *
        nu * y1 / W6 - a * y1 / rb * (1 / rb + 1 / W6)) -1 / 2 * y2 * W8 / rb ^ 3 / W6 * (N1 *
        cotB - 2 * nu * y1 / W6 - a * y1 / rb * (1 / rb + 1 / W6))* 2 * y3b - y2 * W8 / rb / W6 ^
        2 * (N1 * cotB - 2 * nu * y1 / W6 - a * y1 / rb * (1 / rb + 1 / W6)) * (y3b / rb + 1) + y2 *
        W8 / rb / W6 * (2 * nu * y1 / W6 ^ 2 * (y3b / rb + 1) + 1 / 2 * a * y1 / rb ^ 3 * (1 / rb +
        1 / W6)* 2 * y3b - a * y1 / rb * (-1 / 2 / rb ^ 3 * 2 * y3b - 1 / W6 ^ 2 * (y3b / rb +
        1))) +y2 * cotB / rb / W7 * ((-2 + 2 * nu) * cosB + W1 / W7 * W9 + a * y3b / rb2 / cosB) -
        1 / 2 * y2 * W8 * cotB / rb ^ 3 / W7 * ((-2 + 2 * nu) * cosB + W1 / W7 * W9 + a * y3b /
        rb2 / cosB)* 2 * y3b - y2 * W8 * cotB / rb / W7 ^ 2 * ((-2 + 2 * nu) * cosB + W1 / W7 *
        W9 + a * y3b / rb2 / cosB) * W3 + y2 * W8 * cotB / rb / W7 * ((cosB * y3b / rb + 1) /
        W7 * W9 - W1 / W7 ^ 2 * W9 * W3 - 1 / 2 * W1 / W7 * a / rb ^ 3 / cosB * 2 * y3b + a / rb2 /
        cosB - a * y3b / rb2 ^ 2 / cosB * 2 * y3b)) /π/(1 - nu)) +
        b3 / 2 * (1 / 4 * (N1 * (-sinB * W3 / W7 + y1 / W6 ^ 2 * (1 + a / rb) * (y3b / rb + 1) +
        1 / 2 * y1 / W6 * a / rb ^ 3 * 2 * y3b + sinB / W7 * W2 - z1b / W7 ^ 2 * W2 * W3 - 1 / 2 *
        z1b / W7 * a / rb ^ 3 * 2 * y3b) +y1 / rb * (a / rb2 + 1 / W6) - 1 / 2 * y1 * W8 / rb ^
        3 * (a / rb2 + 1 / W6) * 2 * y3b + y1 * W8 / rb * (-a / rb2 ^ 2 * 2 * y3b - 1 / W6 ^ 2 *
        (y3b / rb + 1)) -1 / W7 * (sinB * (cosB - a / rb) + z1b / rb * (1 + a * y3b / rb2) - 1 /
        rb / W7 * (y2 ^ 2 * cosB * sinB - a * z1b / rb * W1)) +W8 / W7 ^ 2 * (sinB * (cosB -
        a / rb) +z1b / rb * (1 + a * y3b / rb2) - 1 / rb / W7 * (y2 ^ 2 * cosB * sinB - a * z1b /
        rb * W1)) * W3 - W8 / W7 * (1 / 2 * sinB * a / rb ^ 3 * 2 * y3b + sinB / rb * (1 + a * y3b /
        rb2) -1 / 2 * z1b / rb ^ 3 * (1 + a * y3b / rb2) * 2 * y3b + z1b / rb * (a / rb2 - a *
        y3b / rb2 ^ 2 * 2 * y3b) +1 / 2 / rb ^ 3 / W7 * (y2 ^ 2 * cosB * sinB - a * z1b / rb *
        W1)* 2 * y3b + 1 / rb / W7 ^ 2 * (y2 ^ 2 * cosB * sinB - a * z1b / rb * W1) * W3 - 1 /
        rb / W7 * (-a * sinB / rb * W1 + 1 / 2 * a * z1b / rb ^ 3 * W1 * 2 * y3b - a * z1b / rb *
        (cosB * y3b / rb + 1)))) /π/(1 - nu)) +
        b1 / 2 * (1 / 4 * ((2 - 2 * nu) * (N1 * rFib_ry2 * cotB + 1 / W6 * W5 - y2 ^ 2 / W6 ^ 2 * W5 /
        rb - y2 ^ 2 / W6 * a / rb ^ 3 - cosB / W7 * W2 + y2 ^ 2 * cosB / W7 ^ 2 * W2 / rb + y2 ^ 2 *
        cosB / W7 * a / rb ^ 3) +W8 / rb * (2 * nu / W6 + a / rb2) - y2 ^ 2 * W8 / rb ^ 3 * (2 *
        nu / W6 + a / rb2) +y2 * W8 / rb * (-2 * nu / W6 ^ 2 / rb * y2 - 2 * a / rb2 ^ 2 * y2) +
        W8 * cosB / rb / W7 * (1 - 2 * nu - W1 / W7 * W2 - a * y3b / rb2) - y2 ^ 2 * W8 * cosB /
        rb ^ 3 / W7 * (1 - 2 * nu - W1 / W7 * W2 - a * y3b / rb2) - y2 ^ 2 * W8 * cosB / rb2 / W7 ^
        2 * (1 - 2 * nu - W1 / W7 * W2 - a * y3b / rb2) + y2 * W8 * cosB / rb / W7 * (-1 / rb *
        cosB * y2 / W7 * W2 + W1 / W7 ^ 2 * W2 / rb * y2 + W1 / W7 * a / rb ^ 3 * y2 + 2 * a *
        y3b / rb2 ^ 2 * y2)) /π/(1 - nu)) +
        b2 / 2 * (1 / 4 * ((-2 + 2 * nu) * N1 * cotB * (1 / rb * y2 / W6 - cosB / rb * y2 / W7) + (2 -
        2 * nu) * y1 / W6 ^ 2 * W5 / rb * y2 + (2 - 2 * nu) * y1 / W6 * a / rb ^ 3 * y2 - (2 - 2 *
        nu) * z1b / W7 ^ 2 * W2 / rb * y2 - (2 - 2 * nu) * z1b / W7 * a / rb ^ 3 * y2 - W8 / rb ^
        3 * (N1 * cotB - 2 * nu * y1 / W6 - a * y1 / rb2) * y2 + W8 / rb * (2 * nu * y1 / W6 ^ 2 /
        rb * y2 + 2 * a * y1 / rb2 ^ 2 * y2) +W8 / W7 ^ 2 * (cosB * sinB + W1 * cotB / rb * ((2 -
        2 * nu)* cosB - W1 / W7) +a / rb * (sinB - y3b * z1b / rb2 - z1b * W1 / rb / W7)) /
        rb * y2 - W8 / W7 * (1 / rb2 * cosB * y2 * cotB * ((2 - 2 * nu) * cosB - W1 / W7) - W1 *
        cotB / rb ^ 3 * ((2 - 2 * nu) * cosB - W1 / W7) * y2 + W1 * cotB / rb * (-cosB / rb *
        y2 / W7 + W1 / W7 ^ 2 / rb * y2) -a / rb ^ 3 * (sinB - y3b * z1b / rb2 - z1b * W1 /
        rb / W7) * y2 + a / rb * (2 * y3b * z1b / rb2 ^ 2 * y2 - z1b / rb2 * cosB * y2 / W7 +
        z1b * W1 / rb ^ 3 / W7 * y2 + z1b * W1 / rb2 / W7 ^ 2 * y2))) /π/(1 - nu)) +
        b3 / 2 * (1 / 4 * ((2 - 2 * nu) * rFib_ry2 + (2 - 2 * nu) * sinB / W7 * W2 - (2 - 2 * nu) * y2 ^ 2 *
        sinB / W7 ^ 2 * W2 / rb - (2 - 2 * nu) * y2 ^ 2 * sinB / W7 * a / rb ^ 3 + W8 * sinB / rb /
        W7 * (1 + W1 / W7 * W2 + a * y3b / rb2) - y2 ^ 2 * W8 * sinB / rb ^ 3 / W7 * (1 + W1 /
        W7 * W2 + a * y3b / rb2) -y2 ^ 2 * W8 * sinB / rb2 / W7 ^ 2 * (1 + W1 / W7 * W2 + a *
        y3b / rb2) +y2 * W8 * sinB / rb / W7 * (1 / rb * cosB * y2 / W7 * W2 - W1 / W7 ^ 2 *
        W2 / rb * y2 - W1 / W7 * a / rb ^ 3 * y2 - 2 * a * y3b / rb2 ^ 2 * y2)) /π/(1 - nu)))

    return v11, v22, v33, v12, v13, v23
end

function strain_tri3_fs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, λ::T, μ::T) where {T, V}
    A = transform_matrix(P1, P2, P3)
    _strain_tri3_fs(X, Y, Z, P1, P2, P3, Ss, Ds, Ts, λ, μ, A')
end

"""
    stress_tri3_fs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, λ::T, μ::T) where {T, V}

Compute stress risen from triangular dislocation in elastic *fullspace*.
    Please see [original version (in supporting information)](https://academic.oup.com/gji/article/201/2/1119/572006#86405752)
    for details, especially the **coordinate system** used here.

## Arguments
The same as [`stress_tri3_hs`](@ref)
"""
function stress_tri3_fs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, λ::T, μ::T) where {T, V}
    exx, eyy, ezz, exy, exz, eyz = strain_tri3_fs(X, Y, Z, P1, P2, P3, Ss, Ds, Ts, λ, μ)
    return _strain2stress(exx, eyy, ezz, exy, exz, eyz, λ, μ)
end

@inline function _strain2stress(exx::T, eyy::T, ezz::T, exy::T, exz::T, eyz::T, λ::T, μ::T) where T
    ekk = exx + eyy + ezz
    σxx = λ * ekk + 2μ * exx
    σyy = λ * ekk + 2μ * eyy
    σzz = λ * ekk + 2μ * ezz
    σxy = 2μ * exy
    σxz = 2μ * exz
    σyz = 2μ * eyz
    return σxx, σyy, σzz, σxy, σxz, σyz
end

@inline function _strain_tri3_fs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, λ::T, μ::T, At::M) where {T, V, M}
    nu = λ / (μ + λ) / 2
    bx, by, bz = Ts, Ss, Ds
    p1, p2, p3 = [zeros(T, 3) for _ in 1: 3]
    x, y, z = coord_trans(X - P2[1], Y - P2[2], Z - P2[3], At)
    p1[1], p1[2], p1[3] = coord_trans(P1[1] - P2[1], P1[2] - P2[2], P1[3] - P2[3], At)
    p3[1], p3[2], p3[3] = coord_trans(P3[1] - P2[1], P3[2] - P2[2], P3[3] - P2[3], At)

    e12 = normalize(p2 - p1)
    e13 = normalize(p3 - p1)
    e23 = normalize(p3 - p2)

    A = acos(dot(e12, e13))
    B = acos(dot(-e12, e23))
    C = acos(dot(e23, e13))

    Trimode = trimodefinder(y, z, x, p1, p2, p3)

    if Trimode == 1
        Exx1Tp, Eyy1Tp, Ezz1Tp, Exy1Tp, Exz1Tp, Eyz1Tp = TDSetupS(x, y, z, A, bx, by, bz, nu, p1, -e13)
        Exx2Tp, Eyy2Tp, Ezz2Tp, Exy2Tp, Exz2Tp, Eyz2Tp = TDSetupS(x, y, z, B, bx, by, bz, nu, p2, e12)
        Exx3Tp, Eyy3Tp, Ezz3Tp, Exy3Tp, Exz3Tp, Eyz3Tp = TDSetupS(x, y, z, C, bx, by, bz, nu, p3, e23)
        exx = Exx1Tp + Exx2Tp + Exx3Tp
        eyy = Eyy1Tp + Eyy2Tp + Eyy3Tp
        ezz = Ezz1Tp + Ezz2Tp + Ezz3Tp
        exy = Exy1Tp + Exy2Tp + Exy3Tp
        exz = Exz1Tp + Exz2Tp + Exz3Tp
        eyz = Eyz1Tp + Eyz2Tp + Eyz3Tp
    end

    if Trimode == -1
        Exx1Tn, Eyy1Tn, Ezz1Tn, Exy1Tn, Exz1Tn, Eyz1Tn = TDSetupS(x, y, z, A, bx, by, bz, nu, p1, e13)
        Exx2Tn, Eyy2Tn, Ezz2Tn, Exy2Tn, Exz2Tn, Eyz2Tn = TDSetupS(x, y, z, B, bx, by, bz, nu, p2, -e12)
        Exx3Tn, Eyy3Tn, Ezz3Tn, Exy3Tn, Exz3Tn, Eyz3Tn = TDSetupS(x, y, z, C, bx, by, bz, nu, p3, -e23)
        exx = Exx1Tn + Exx2Tn + Exx3Tn
        eyy = Eyy1Tn + Eyy2Tn + Eyy3Tn
        ezz = Ezz1Tn + Ezz2Tn + Ezz3Tn
        exy = Exy1Tn + Exy2Tn + Exy3Tn
        exz = Exz1Tn + Exz2Tn + Exz3Tn
        eyz = Eyz1Tn + Eyz2Tn + Eyz3Tn
    end

    if Trimode == 0
        exx = NaN
        eyy = NaN
        ezz = NaN
        exy = NaN
        exz = NaN
        eyz = NaN
    end

    Exx, Eyy, Ezz, Exy, Exz, Eyz = TensTrans(exx, eyy, ezz, exy, exz, eyz, At')
    return Exx, Eyy, Ezz, Exy, Exz, Eyz
end

@inline function TDSetupS(x::T, y::T, z::T, alpha::T, bx::T, by::T, bz::T, nu::T, TriVertex::V, SideVec::V) where {T, V}
    y1 = SideVec[3] * (y - TriVertex[2]) - SideVec[2] * (z - TriVertex[3])
    z1 = SideVec[2] * (y - TriVertex[2]) + SideVec[3] * (z - TriVertex[3])
    by1 = SideVec[3] * by - SideVec[2] * bz
    bz1 = SideVec[2] * by + SideVec[3] * bz

    exx, eyy, ezz, exy, exz, eyz = AngDisStrain(x, y1, z1, -π + alpha, bx, by1, bz1, nu)
    B = zeros(T, 3, 3)
    B[1,1] = one(T)
    B[2,2] = SideVec[3]
    B[2,3] = SideVec[2]
    B[3,2] = -SideVec[2]
    B[3,3] = SideVec[3]
    exx, eyy, ezz, exy, exz, eyz = TensTrans(exx, eyy, ezz, exy, exz, eyz, B)
    return exx, eyy, ezz, exy, exz, eyz
end

function strain_tri3_hs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, λ::T, μ::T) where {T, V}
    A = transform_matrix(P1, P2, P3)
    strms11, strms22, strms33, strms12, strms13, strms23 = _strain_tri3_fs(X, Y, Z, P1, P2, P3, Ss, Ds, Ts, λ, μ, A')
    strfsc11, strfsc22, strfsc33, strfsc12, strfsc13, strfsc23 = _TDstrain_HarFunc(X, Y, Z, P1, P2, P3, Ss, Ds, Ts, λ, μ, A)
    P1[3] *= -one(T)
    P2[3] *= -one(T)
    P3[3] *= -one(T)
    allatsurface = P1[3] == zero(T) && P2[3] == zero(T) && P3[3] == zero(T)
    if !allatsurface
        A *= -one(T)
        A[3,1] *= -one(T)
        A[3,2] *= -one(T)
        A[3,3] *= -one(T)
    end
    stris11, stris22, stris33, stris12, stris13, stris23 = _strain_tri3_fs(X, Y, Z, P1, P2, P3, Ss, Ds, Ts, λ, μ, A')
    if allatsurface
        stris13 *= -one(T)
        stris23 *= -one(T)
    end
    P1[3] *= -one(T)
    P2[3] *= -one(T)
    P3[3] *= -one(T)
    e11 = strms11 + strfsc11 + stris11
    e22 = strms22 + strfsc22 + stris22
    e33 = strms33 + strfsc33 + stris33
    e12 = strms12 + strfsc12 + stris12
    e13 = strms13 + strfsc13 + stris13
    e23 = strms23 + strfsc23 + stris23
    return e11, e22, e33, e12, e13, e23
end

"""
    stress_tri3_hs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, λ::T, μ::T) where {T, V}

Compute stress risen from triangular dislocation in elastic *halfspace*.
    Please see [original version (in supporting information)](https://academic.oup.com/gji/article/201/2/1119/572006#86405752)
    for details, especially the **coordinate system** used here.

## Arguments
- `X`, `Y`, `Z`: observational coordinates
- `P1`, `P2`, `P3`: three triangular vertices coordinates respectively
- `Ss`, `Ds`, `Ts`: triangular dislocation vector, Strike-slip, Dip-slip, Tensile-slip respectively
- `λ`: Lamé's first parameter
- `μ`: shear modulus

## Output
By order: ``σ_{xx}``, ``σ_{yy}``, ``σ_{zz}``,
    ``σ_{xy}``, ``σ_{xz}``, ``σ_{yz}``.
"""
function stress_tri3_hs(X::T, Y::T, Z::T, P1::V, P2::V, P3::V, Ss::T, Ds::T, Ts::T, λ::T, μ::T) where {T, V}
    exx, eyy, ezz, exy, exz, eyz = strain_tri3_hs(X, Y, Z, P1, P2, P3, Ss, Ds, Ts, λ, μ)
    return _strain2stress(exx, eyy, ezz, exy, exz, eyz, λ, μ)
end


using TailRiskScenGen
using Distributions

# Inputs - change as necessary
dim = 20
gens = 30
samples = 100000
srand(1)

# Randomly generate distribution and cone
Z = Normal()
μ = rand(Z, dim)
A = rand(Z, (dim,dim))
Σ = A'A
dist = MvNormal(μ, Σ)
K = FiniteCone(rand(Z, (dim, gens)))

function test(dist::MvNormal, K::FiniteCone, samples::Int64)
    dim = K.dim
    for i in 1:samples
        project(K, rand(dim))
    end
end

# Small test to compile necessary functions
test(dist, K, 5)

# Real test
@time test(dist, K, samples)


using Bijectors, Optim, Plots, Random, StatsFuns, Stheno, WarpedRegressors

using WarpedRegressors: InvBoxCox, affine

# Specify a Warped-GP.
f = GP(kernel(EQ(); l=0.5, s=3.0), GPC())
σ² = 0.01
ϕ = InvBoxCox(0.5)
g = warp(f, ϕ)

# Generate some training data.
rng = MersenneTwister(123456)
Ntr = 25
Nte = 200
x = 3 * randn(rng, Ntr + Nte)
xtr = x[1:Ntr]
xte = x[Ntr+1:end]
g_x = g(x, σ²)
y = rand(rng, g_x)
ytr = y[1:Ntr]
yte = y[Ntr+1:end]

# Specify parameter handler.
unpack(θ) = (
    σ² = log1pexp(θ[1]) + 1e-6,
    l  = log1pexp(θ[2]) + 1e-6,
    s  = log1pexp(θ[3]) + 1e-6,
    λ  = log1pexp(θ[4]) + 1e-6,
)

# Infer σ², l, and s.
function nlml(θ::AbstractVector{<:Real})
    θ = unpack(θ)
    f = GP(kernel(EQ(); l=θ.l, s=θ.s), GPC())
    ϕ = InvBoxCox(θ.λ)
    g = warp(f, ϕ)
    return -logpdf(g(xtr, θ.σ²), ytr)
end
θ0 = randn(4)
results = Optim.optimize(nlml, θ0, NelderMead())
θ = unpack(results.minimizer)

# Construct posterior Warped GP
f_learned = GP(kernel(EQ(); l=θ.l, s=θ.s), GPC())
g_learned = warp(f_learned, ϕ)
g_posterior = posterior(g_learned(xtr, θ.σ²), ytr)

# Visualise the posterior.
let
    xpr = range(-10.0, 10.0; length=400)
    Ypr = hcat([rand(rng, g_posterior(xpr, θ.σ²)) for _ in 1:5000]...)
    median_pr = map(ys->quantile(ys, 0.5), eachrow(Ypr));
    q95_pr = map(ys->quantile(ys, 0.95), eachrow(Ypr));
    q05_pr = map(ys->quantile(ys, 0.05), eachrow(Ypr));

    plotly()
    plt = plot()
    plot!(plt, xpr, Ypr[:, 1:10]; linecolor=:blue, linewidth=0.5, linealpha=0.2, label="")
    plot!(plt, xpr, median_pr; linecolor=:blue, linewidth=1.0, linealpha=1.0, label="")
    plot!(plt, xpr, [median_pr, median_pr];
        linewidth=0.0,
        fillrange=[q05_pr, q95_pr],
        fillalpha=0.3,
        fillcolor=:blue,
        label="",
    )
    scatter!(plt, xtr, ytr; label="", markersize=2, markercolor=:blue)
    scatter!(plt, xte, yte; label="", markersize=2, markercolor=:black)
    display(plt)
end


using ADCME
using PyPlot

x0 = rand(100)
x0 = @. x0*0.4 + 0.3
x1 = collect(LinRange(0, 1, 100))
y0 = sin.(2π*x0)
w = Variable(fc_init([1, 20, 20, 20, 1]))
y = squeeze(fc(x0, [20, 20, 20, 1], w))
loss = sum((y - y0)^2)

sess = Session(); init(sess)
BFGS!(sess, loss)
y1 = run(sess, y)
plot(x0, y0, ".", label="Data")
x_dnn = run(sess,  squeeze(fc(x1, [20, 20, 20, 1], w)))
plot(x1, x_dnn,  "--", label="DNN Estimation")
legend()
w1 = run(sess, w)

##############################

μ = Variable(w1)
ρ = Variable(zeros(length(μ)))
σ = log(1+exp(ρ))

function likelihood(z)
    w = μ + σ * z
    y = squeeze(fc(x0, [20, 20, 20, 1], w))
    sum((y - y0)^2) - sum((w-μ)^2/(2σ^2)) + sum((w-w1)^2)
end

function inference(x)
    z = tf.random_normal((length(σ),), dtype=tf.float64)
    w = μ + σ * z
    y = squeeze(fc(x, [20, 20, 20, 1], w))|>squeeze
end

W = tf.random_normal((10, length(w)), dtype=tf.float64)
L = constant(0.0)
for i = 1:10
    global L += likelihood(W[i])
end

y2 = inference(x1)

opt = AdamOptimizer(0.01).minimize(L)
init(sess)
# run(sess, L)
losses = []
for i = 1:2000
    _, l = run(sess, [opt, L])
    push!(losses, l)
    @info i, l
end

Y = zeros(100, 1000)
for i = 1:1000
    Y[:,i] = run(sess, y2)
end

for i = 1:1000
    plot(x1, Y[:,i], "--", color="gray", alpha=0.5)
end
plot(x1, x_dnn, label="DNN Estimation")
plot(x0, y1, ".", label="Data")
legend()

##############################
# Naive Uncertainty Quantification
function inference_naive(x)
    z = tf.random_normal((length(w1),), dtype=tf.float64)
    w = w1 + log(2)*z
    y = squeeze(fc(x, [20, 20, 20, 1], w))|>squeeze
end
y3 = inference(x1)

Y = zeros(100, 1000)
for i = 1:1000
    Y[:,i] = run(sess, y3)
end

for i = 1:1000
    plot(x1, Y[:,i], "--", color="gray", alpha=0.5)
end
plot(x1, x_dnn, label="DNN Estimation")
plot(x0, y1, ".", label="Data")
legend()


"""
International Standard Atmospheric (ISA) model

The International Standard Atmosphere (ISA) is a static atmospheric model of how
the pressure, temperature, density, and viscosity of the Earth's atmosphere
change over a wide range of altitudes or elevations. It has been established to
provide a common reference for temperature and pressure and consists of tables
of values at various altitudes, plus some formulas by which those values were
derived.

It is also known as the ICAO Standard Atmosphere, ISA is a standard against
which to compare the actual atmosphere at any point and time. The real
atmosphere differs from ISA in many ways. Sea level pressure varies from day to
day, and there are wide extremes of temperature at all levels.

*Only metric units are used.*

Source: EUROCONTROL BADA 4 User Manual Chapter 2.2 Atmosphere Model

Source: www.skybrary.aero/index.php/International_Standard_Atmosphere_(ISA)

Source: en.wikipedia.org/wiki/International_Standard_Atmosphere
"""
module ISAtmosphere

export T_K, T₀_K, p_Pa, p₀_Pa, ρ_kg_m³, ρ₀_kg_m³, a_m_s, a₀_m_s, g₀_m_s²,
conditions, AtmosConditions, Vcas2Vtas, Vtas2Vcas, M2Vtas, Vtas2M, Hp_trans_m,
M2Vcas, Vcas2M, Hp_trop_m, κ, R_M²_Ks², βT∇_K_m

"EUROCONTROL BADA 4 User Manual Section 2.2.1/2.2.2"

"Standard atmospheric temperature [K] at Mean Sea Level (MSL)"
const T₀_K = 288.15
"Standard atmospheric pressure [Pa] at Mean Sea Level (MSL)"
const p₀_Pa = 101325.0
"Standard atmospheric density [kg/m³] at Mean Sea Level (MSL)"
const ρ₀_kg_m³ = 1.225
"Speed of sound [m/s] at Mean Sea Level (MSL)"
const a₀_m_s = 340.294
"Adiabatic index of air []"
const κ = 1.4
"constant used for Vtas<->Vcas conversion"
const μ = (κ - 1.0)/κ
"Real gas constant for air [M²/(Ks²)]"
const R_M²_Ks² = 287.05287
"Graviation acceleration [m/s²] at Mean Sea Level (MSL)"
const g₀_m_s² = 9.80665
"ISA temperature gradient [K/m] with altitude below the tropopause"
const βT∇_K_m = -0.0065
"Geopotential pressure altitude [m] of Tropopause"
const Hp_trop_m = 11000.0

"""
    AtmosConditions(Hp_m, T_K, ΔT_K, p_Pa, ρ_kg_m³, a_m_s)

Immutable STRUCT to keep a set of atmospheric conditions together. This struct
can be used to also store an arbitrary set of atmospheric conditions.
The function `conditions` can be used to create the struct.
"""
struct AtmosConditions
    Hp_m::Float64
    T_K::Float64
    ΔT_K::Float64
    p_Pa::Float64
    ρ_kg_m³::Float64
    a_m_s::Float64
end

"""
    T_K(Hp_m [, ΔT_K= 0.0])

Returns the atmospheric temperature [K] at pressure altitude `Hp_m` [m] and
with temperature offset `ΔT_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-13/16
"""
function T_K(Hp_m::Float64, ΔT_K::Float64 = 0.0)
    if Hp_m ≤ Hp_trop_m
        return T₀_K + ΔT_K + βT∇_K_m * Hp_m
    else
        return T₀_K + ΔT_K + βT∇_K_m * Hp_trop_m
    end
end

T_K(Hp_m::Real, ΔT_K::Real = zero(Real)) = T_K(convert(Float64, Hp_m),
convert(Float64, ΔT_K))

"""
    p_Pa(Hp_m[, ΔT_K = 0.0])

Return the air pressure [Pa] at pressure altitude `Hp_m` [m] and with
temperature offset `ΔT_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-18/20
"""
function p_Pa(Hp_m::Float64, ΔT_K::Float64 = 0.0)
    if Hp_m ≤ Hp_trop_m
        return p₀_Pa*((T_K(Hp_m, ΔT_K) - ΔT_K) / T₀_K) ^ (-g₀_m_s²/(βT∇_K_m*R_M²_Ks²))
    else
        p_trop = p₀_Pa*((T_K(Hp_trop_m, ΔT_K) - ΔT_K) / T₀_K) ^ (-g₀_m_s²/(βT∇_K_m*R_M²_Ks²))
        return p_trop * exp(-g₀_m_s² / (R_M²_Ks²*T_K(Hp_trop_m)) * (Hp_m - Hp_trop_m))
    end
end

p_Pa(Hp_m::Real, ΔT_K::Real = zero(Real)) = p_Pa(convert(Float64, Hp_m), convert(Float64, ΔT_K))

"""
    ρ_kg_m³(p_Pa, T_K)

Return the air density [kg/m³] at pressure level `p_Pa` [Pa] and temperature
`T_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-21
"""
function ρ_kg_m³(p_Pa::Float64, T_K::Float64)
    return p_Pa / (R_M²_Ks² * T_K)
end

ρ_kg_m³(p_Pa::Real, T_K::Real) = ρ_kg_m³(convert(Float64, p_Pa), convert(Float64, T_K))

"""
    a_m_s(T_K)

Return the speed of sound [m/s] at the temperature `T_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-22
"""
function a_m_s(T_K::Float64)
    return √(κ*R_M²_Ks²*T_K)
end

a_m_s(T_K::Real) = a_m_s(convert(Float64, T_K))

"""
    Vcas2Vtas(Vcas_m_s, p_Pa, T_K)

Return the true airspeed `Vtas_m_s` [m/s] as a function of the calibrated
airspeed `Vcas_m_s` [m/s] at pressure level `p_Pa` [Pa] and with temperature
`T_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-23
"""
function Vcas2Vtas(Vcas_m_s::Float64, p_Pa::Float64, T_K::Float64)
    ρᵥ = ρ_kg_m³(p_Pa, T_K)
    return (2.0*p_Pa/(μ*ρᵥ)*((1.0+p₀_Pa/p_Pa*((1.0+μ*ρ₀_kg_m³/(2.0*p₀_Pa)*Vcas_m_s^2)
            ^(1.0/μ)-1.0))^μ-1.0))^0.5
end

Vcas2Vtas(Vcas_m_s::Real, p_Pa::Real, T_K::Real) = Vcas2Vtas(convert(Float64, Vcas_m_s),
convert(Float64, p_Pa), convert(Float64, T_K))

"""
    Vcas2Vtas(Vcas_m_s, AtmosConditions)
"""
Vcas2Vtas(Vcas_m_s::Real, cond::AtmosConditions) = Vcas2Vtas(Vcas_m_s,
cond.p_Pa, cond.T_K)

"""
    Vtas2Vcas(Vtas_m_s, p_Pa, T_K)

Return calibrated airspeed `Vcas_m_s` [m/s] as a function of the true airspeed
`Vtas_m_s` [m/s] at pressure level `p_Pa` [Pa] and with temperature `T_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-24
"""
function Vtas2Vcas(Vtas_m_s::Float64, p_Pa::Float64, T_K::Float64)
    ρᵥ = ρ_kg_m³(p_Pa, T_K)
    return (2.0*p₀_Pa/(μ*ρ₀_kg_m³)*((1.0+p_Pa/p₀_Pa*((1.0+μ*ρᵥ/(2.0*p_Pa)*Vtas_m_s^2)
            ^(1.0/μ)-1.0))^μ-1.0))^0.5
end

Vtas2Vcas(Vtas_m_s::Real, p_Pa::Real, T_K::Real) = Vtas2Vcas(convert(Float64, Vtas_m_s),
convert(Float64, p_Pa), convert(Float64, T_K))

"""
    Vtas2Vcas(Vtas_m_s, AtmosConditions)
"""
Vtas2Vcas(Vtas_m_s::Real, conditions::AtmosConditions) = Vtas2Vcas(Vtas_m_s,
conditions.p_Pa, conditions.T_K)

"""
    M2Vtas(M, T_K)

Return true airspeed `Vtas_m_s` [m/s] as a function of the Mach number `M`
and temperature `T_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-26
"""
function M2Vtas(M::Float64, T_K::Float64)
    return M*a_m_s(T_K)
end

M2Vtas(M::Real, T_K::Real) = M2Vtas(convert(Float64, M), convert(Float64, T_K))

M2Vtas(M::Float64, conditions::AtmosConditions) = M * conditions.a_m_s

"""
    M2Vtas(M, AtmosConditions)
"""
M2Vtas(M::Real, conditions::AtmosConditions) = convert(Float64, M) *
conditions.a_m_s

"""
    Vtas2M(Vtas_m_s, T_K)

Return the Mach number `M` as a function of the true airspeed `Vtas_m_s` [m/s]
and temperature `T_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-26 (reversed)
"""
function Vtas2M(Vtas_m_s::Float64, T_K::Float64)
    return Vtas_m_s/a_m_s(T_K)
end

Vtas2M(Vtas_m_s::Real, T_K::Real) = Vtas2M(convert(Float64, Vtas_m_s),
convert(Float64, T_K))

"""
    Vtas2M(Vtas_m_s, AtmosConditions)
"""
Vtas2M(Vtas_m_s::Real, conditions::AtmosConditions) = convert(Float64, Vtas_m_s) /
conditions.a_m_s

"""
    M2Vcas(M, p_Pa, T_K)

Return the calibrated airspeed `Vcas_m_s` [m/s] as a function of the Mach number
`M` at pressure level `p_Pa` [Pa] and with temperature `T_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-26
"""
function M2Vcas(M::Float64, p_Pa::Float64, T_K::Float64)
    return Vtas2Vcas(M2Vtas(M, T_K), p_Pa, T_K)
end

M2Vcas(M::Real, p_Pa::Real, T_K::Real) = M2Vcas(convert(Float64, M),
convert(Float64, p_Pa), convert(Float64, T_K))

"""
    M2Vcas(M, AtmosConditions)
"""
M2Vcas(M::Real, conditions::AtmosConditions) = M2Vcas(M, conditions.p_Pa,
conditions.T_K)

"""
    Vcas2M(Vcas_m_s, T_K)

Return the Mach number `M` as a function of the calibrated airspeed `Vcas_m_s`
[m/s] at pressure level `p_Pa` [Pa] and with temperature `T_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-26 (reversed)
"""
function Vcas2M(Vcas_m_s::Float64, p_Pa::Float64, T_K::Float64)
    return Vtas2M(Vcas2Vtas(Vcas_m_s, p_Pa, T_K), T_K)
end

Vcas2M(Vcas_m_s::Real, p_Pa::Real, T_K::Real) = Vcas2M(convert(Float64, Vcas_m_s),
convert(Float64, p_Pa), convert(Float64, T_K))

"""
    Vcas2M(Vcas_m_s, AtmosConditions)
"""
Vcas2M(Vcas_m_s::Real, conditions::AtmosConditions) = Vcas2M(Vcas_m_s,
conditions.p_Pa, conditions.T_K)

"""
    Hp_trans_m(Vcas_m_s, M)

Return the transition altitude `Hp_trans_m` (also called crossover altitude) [m]
between a given calibrated airspeed `Vcas_m_s` [m/s] and a Mach number `M`.

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-27/28/29
"""
function Hp_trans_m(Vcas_m_s::Float64, M::Float64)
    δ_trans = ((1.0+(κ-1.0)/2.0*(Vcas_m_s/a₀_m_s)^2)
                ^(1.0/μ)-1.0)/((1.0+(κ-1.0)/2.0*M^2)^(1.0/μ)-1.0)
    θ_trans = δ_trans^(-βT∇_K_m * R_M²_Ks² / g₀_m_s²)
    return T₀_K/βT∇_K_m*(θ_trans - 1.0)
end

Hp_trans_m(Vcas_m_s::Real, M::Real) = Hp_trans_m(convert(Float64, Vcas_m_s),
convert(Float64, M))

"""
    θ(T_K)

Return temperature ratio `θ` for temperature `T_K` [K].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-30
"""
θ(T_K::Float64) = T_K / T₀_K
θ(T_K::Real) = θ(convert(Float64, T_K))

"""
    δ(p_Pa)

Return pressure ratio `δ` for air pressure `p_Pa` [Pa].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-31
"""
δ(p_Pa::Float64) = p_Pa / p₀_Pa
δ(p_Pa::Real) = δ(convert(Float64, p_Pa))

"""
    σ(ρ_kg_m³)
Return the density ratio `σ` for the air density `ρ_kg_m³` [kg/m³].

Source: EUROCONTROL BADA 4 User Manual eq. 2.2-32
"""
σ(ρ_kg_m³::Float64) = ρ_kg_m³ / ρ₀_kg_m³
σ(ρ_kg_m³::Real) = σ(convert(Float64, ρ_kg_m³))

"""
    conditions(Hp_m[, ΔT_K = 0.0])

Create `AtmosConditions` struct with the atmospheric conditions `T_K` [K],
`p_Pa` [Pa], and `ρ_kg_m³` [kg/m³], and the speed of sound `a_m_s` [m/s] at a
given altitude `Hp_m` [m] and `ΔT_K` [K] temperature offset.
"""
function conditions(Hp_m::Float64, ΔT_K::Float64 = 0.0)
    T = T_K(Hp_m, ΔT_K)
    p = p_Pa(Hp_m, ΔT_K)
    ρ = ρ_kg_m³(p, T)
    a = a_m_s(T)
    return AtmosConditions(Hp_m, T, ΔT_K, p, ρ, a)
end

end # module


""
function objective_min_fuel_cost(pm::AbstractPowerModel)
    model = PowerModels.check_cost_models(pm)
    if model == 1
        return objective_min_pwl_fuel_cost(pm)
    elseif model == 2
        return objective_min_polynomial_fuel_cost(pm)
    else
        error("Only cost models of types 1 and 2 are supported at this time, given cost model type of $(model)")
    end

end

""
function objective_min_polynomial_fuel_cost(pm::AbstractPowerModel)
    order = PowerModels.calc_max_cost_index(pm.data)-1

    if order == 1
        return _objective_min_polynomial_fuel_cost_linear(pm)
    elseif order == 2
        return _objective_min_polynomial_fuel_cost_quadratic(pm)
    else
        error("cost model order of $(order) is not supported")
    end
end

function _objective_min_polynomial_fuel_cost_linear(pm::AbstractPowerModel)
    from_idx = Dict()
    for (n, nw_ref) in nws(pm)
        from_idx[n] = Dict(arc[1] => arc for arc in nw_ref[:arcs_from_dc])
    end

    return @objective(pm.model, Min,
        sum(
            sum(   gen["cost"][1]*sum( var(pm, n, c, :pg, i) for c in conductor_ids(pm, n))+
                   gen["cost"][2] for (i,gen) in nw_ref[:gen])
        for (n, nw_ref) in nws(pm))
    )
end
""
function _objective_min_polynomial_fuel_cost_quadratic(pm::AbstractPowerModel)
    from_idx = Dict()
    for (n, nw_ref) in nws(pm)
        from_idx[n] = Dict(arc[1] => arc for arc in nw_ref[:arcs_from_dc])
    end

    return @objective(pm.model, Min,
        sum(
            sum(   gen["cost"][1]*sum( var(pm, n, c, :pg, i) for c in conductor_ids(pm, n))^2 +
                   gen["cost"][2]*sum( var(pm, n, c, :pg, i) for c in conductor_ids(pm, n))+
                   gen["cost"][3] for (i,gen) in nw_ref[:gen])
        for (n, nw_ref) in nws(pm))
    )
end
""
function objective_min_polynomial_fuel_cost(pm::AbstractConicModel)
    PowerModels.check_polynomial_cost_models(pm)
    pg_sqr = Dict()
    for (n, nw_ref) in PowerModels.nws(pm)
        for cnd in PowerModels.conductor_ids(pm, n)
            pg_sqr = PowerModels.var(pm, n, cnd)[:pg_sqr] = @variable(pm.model,
                [i in PowerModels.ids(pm, n, :gen)], base_name="$(n)_$(cnd)_pg_sqr",
                lower_bound = PowerModels.ref(pm, n, :gen, i, "pmin", cnd)^2,
                upper_bound = PowerModels.ref(pm, n, :gen, i, "pmax", cnd)^2
            )
            for (i, gen) in nw_ref[:gen]
                # @constraint(pm.model, norm([2*var(pm, n, cnd, :pg, i), pg_sqr[i]-1]) <= pg_sqr[i]+1)
                @constraint(pm.model, [pg_sqr[i], var(pm, n, cnd, :pg, i)/sqrt(2), var(pm, n, cnd, :pg, i)/sqrt(2)] in JuMP.SecondOrderCone())
            end

        end
    end
    print("pipi")
    return @objective(pm.model, Min,
        sum(
            sum( gen["cost"][1]*sum(PowerModels.var(pm, n, cnd, :pg_sqr, i) for cnd in PowerModels.conductor_ids(pm, n)) +
                 gen["cost"][2]*sum(PowerModels.var(pm, n, cnd, :pg, i) for cnd in PowerModels.conductor_ids(pm, n)) +
                 gen["cost"][3] for (i,gen) in nw_ref[:gen])
        for (n, nw_ref) in PowerModels.nws(pm))
    )
end
""
function objective_min_pwl_fuel_cost(pm::AbstractPowerModel)

    for (n, nw_ref) in PowerModels.nws(pm)
        pg_cost = PowerModels.var(pm, n)[:pg_cost] = @variable(pm.model,
            [i in PowerModels.ids(pm, n, :gen)], base_name="$(n)_pg_cost"
        )

        # pwl cost
        gen_lines = PowerModels.get_lines(nw_ref[:gen])
        for (i, gen) in nw_ref[:gen]
            for line in gen_lines[i]
                @constraint(pm.model, pg_cost[i] >= line["slope"]*sum(PowerModels.var(pm, n, cnd, :pg, i) for cnd in PowerModels.conductor_ids(pm, n)) + line["intercept"])
            end
        end

    end

    return @objective(pm.model, Min,
        sum(
            sum( PowerModels.var(pm, n,:pg_cost, i) for (i,gen) in nw_ref[:gen])
        for (n, nw_ref) in PowerModels.nws(pm))
        )
end


#
# Truncate to Support Sampler
#
# ensures that the sampled value is within defined support
# only DistributionSamplers are supported
#
# We use the Truncated functionality of the Distributions package, but this currently does not
# handle all underlying distributions.
# We leave it to the Distributions to raise an error when it is not compatible: the advantage is that
# additional distributions will be automatically handled should the Distributions package handle them
# in future versions.
#

type TruncateToSupportSampler <: ModifyingSampler
	subsampler::DistributionSampler
	function TruncateToSupportSampler(subsampler::DistributionSampler)
		new(subsampler)
	end
end

function sample(s::TruncateToSupportSampler, support, cc::ChoiceContext)
	# TODO: check whether it is better to store truncated distribution, and only
	# recreate when support changes.  If so, will need to trap setparams call so
	# as to clear any stored truncated distribution
	if support[1] == support[2]
		# can give an error when support[1] == support[2]
		x = support[1]
	else
		truncateddistribution = Truncated(s.subsampler.distribution, support[1], support[2])
		x = rand(truncateddistribution)
	end
	x, Dict{Symbol, Any}(:sub=>Dict{Symbol, Any}(:rnd=>x))
end


module Reinforcement

	using MLJ: categorical, mcc
	using Flux

	"""
		generate_score_fitness(x::Vector, y::Vector)

	Generates a fitness function that increases the fitness score depending on whether the
	model finds the right answer.

	Example: y = 0, predicted_y = 0, model_output = model(example), then
				score += model_output[predicted_y+1]
			#takes the value of the respective output node

			 y = 0, predicted_y = 1, model_output = model(example), then
			 	score = score
			#does nothing
	"""
	function generate_score_fitness(x::Vector, y::Vector)
		function fitness(chain::Chain)
			score = 0.0
			for (xi, yi) in zip(x, y)
				out = chain(xi)
				predicted = argmax(out) - 1
				if predicted == yi
					if yi == 1
						score += out[2]
					else
						score += out[1]
					end
				end
			end
			score
		end
	end

	"""
		generate_mcc_fitness(x::Vector, y::Vector)

	Generates a fitness function that increases the fitness score depending on the
	Matthews Correlation Coefficient (MCC).
	"""
	function generate_mcc_fitness(x::Vector, y::Vector)
		predicted = categorical(zeros(Int, length(y)))
		caty = categorical(y)
		function fitness(chain::Chain)
			for (xi, i) in zip(x, eachindex(y))
				out = chain(xi)
				predicted[i] = argmax(out) - 1
			end
			mcc(caty, predicted)
		end
	end

	export generate_score_fitness, generate_mcc_fitness

end


export interest, rate

"""
    interest(amount, rate)

Calculate interest from an ;amout; and interest rate of 'rate'.
"""
function interest(amount, rate)
    return amount * (1 + rate)
end

"""
    rate(amount, interest)

Calculate interest rate based on an  'amount' and 'interest'.
"""

function rate(amount, interest)
    return interest / amount
end

"""
    sum(x::Number, y::Number) -> x + y
Calculate the sum of the two given parameters
"""
function sum(x::Number, y::Number)
    return x + y
end


using NeuralPDE
using Quadrature, Cubature, Cuba
using Flux, ModelingToolkit, GalacticOptim, Optim, DiffEqFlux
using Plots
using DelimitedFiles
using QuasiMonteCarlo
import ModelingToolkit: Interval, infimum, supremum

function hamilton_jacobi(strategy, minimizer, maxIters)

    ##  DECLARATIONS
    @parameters  t x1 x2 x3 x4
    @variables   u(..)

    Dt = Differential(t)

    Dx1 = Differential(x1)
    Dx2 = Differential(x2)
    Dx3 = Differential(x3)
    Dx4 = Differential(x4)

    Dxx1 = Differential(x1)^2
    Dxx2 = Differential(x2)^2
    Dxx3 = Differential(x3)^2
    Dxx4 = Differential(x4)^2

    # Discretization
    tmax         = 1.0
    x1width      = 1.0
    x2width      = 1.0
    x3width      = 1.0
    x4width      = 1.0

    tMeshNum     = 10
    x1MeshNum    = 10
    x2MeshNum    = 10
    x3MeshNum    = 10
    x4MeshNum    = 10

    dt   = tmax/tMeshNum
    dx1  = x1width/x1MeshNum
    dx2  = x2width/x2MeshNum
    dx3  = x3width/x3MeshNum
    dx4  = x4width/x4MeshNum

    domains = [t ∈ Interval(0.0,tmax),
               x1 ∈ Interval(0.0,x1width),
               x2 ∈ Interval(0.0,x2width),
               x3 ∈ Interval(0.0,x3width),
               x4 ∈ Interval(0.0,x4width)]

    ts  = 0.0 : dt : tmax
    x1s = 0.0 : dx1 : x1width
    x2s = 0.0 : dx2 : x2width
    x3s = 0.0 : dx3 : x3width
    x4s = 0.0 : dx4 : x4width

    λ = 1.0f0

    # Operators
    Δu = Dxx1(u(t,x1,x2,x3,x4)) + Dxx2(u(t,x1,x2,x3,x4)) + Dxx3(u(t,x1,x2,x3,x4)) + Dxx4(u(t,x1,x2,x3,x4)) # Laplacian
    ∇u = [Dx1(u(t,x1,x2,x3,x4)), Dx2(u(t,x1,x2,x3,x4)),Dx3(u(t,x1,x2,x3,x4)),Dx4(u(t,x1,x2,x3,x4))]

    # Equation
    eq = Dt(u(t,x1,x2,x3,x4)) + Δu - λ*sum(∇u.^2) ~ 0  #HAMILTON-JACOBI-BELLMAN EQUATION

    terminalCondition =  log((1 + x1*x1 + x2*x2 + x3*x3 + x4*x4)/2) # see PNAS paper

    bcs = [u(tmax,x1,x2,x3,x4) ~ terminalCondition]  #PNAS paper again

    ## NEURAL NETWORK
    n = 10   #neuron number

    chain = FastChain(FastDense(5,n,Flux.σ),FastDense(n,n,Flux.σ),FastDense(n,1))   #Neural network from Flux library

    indvars = [t,x1,x2,x3,x4]   #phisically independent variables
    depvars = [u(t,x1,x2,x3,x4)]       #dependent (target) variable

    dim = length(domains)

    losses = []
    error  = []
    times = []

    dx_err = 0.2

    error_strategy = GridTraining(dx_err)

    initθ = Float64.(DiffEqFlux.initial_params(chain))
    eltypeθ = eltype(initθ)
    parameterless_type_θ = DiffEqBase.parameterless_type(initθ)

    phi = NeuralPDE.get_phi(chain,parameterless_type_θ)
    derivative = NeuralPDE.get_numeric_derivative()

    _pde_loss_function = NeuralPDE.build_loss_function(eq,indvars,depvars,
                                             phi,derivative,nothing,chain,initθ,error_strategy)

    bc_indvars = NeuralPDE.get_variables(bcs,indvars,depvars)
    _bc_loss_functions = [NeuralPDE.build_loss_function(bc,indvars,depvars,
                                              phi,derivative,nothing,chain,initθ,error_strategy,
                                              bc_indvars = bc_indvar) for (bc,bc_indvar) in zip(bcs,bc_indvars)]

    train_sets = NeuralPDE.generate_training_sets(domains,dx_err,[eq],bcs,eltypeθ,indvars,depvars)
    train_domain_set, train_bound_set = train_sets

    pde_loss_functions = [NeuralPDE.get_loss_function(_pde_loss_function,
                                          train_domain_set[1],eltypeθ,
                                          parameterless_type_θ,error_strategy)]

    bc_loss_functions = [NeuralPDE.get_loss_function(bc,
                                          train_set,eltypeθ,
                                          parameterless_type_θ,error_strategy) for (train_set,bc) in zip(train_bound_set, _bc_loss_functions)]
    loss_functions =  [pde_loss_functions;bc_loss_functions]
    function loss_function_(θ,p)
        return sum(map(l->l(θ) ,loss_functions))
    end

    cb_ = function (p,l)
        deltaT_s = time_ns() #Start a clock when the callback begins, this will evaluate questo misurerà anche il calcolo degli uniform error

        ctime = time_ns() - startTime - timeCounter #This variable is the time to use for the time benchmark plot
        append!(times, ctime/10^9) #Conversion nanosec to seconds
        append!(losses, l)
        loss_ = loss_function_(p,nothing)
        append!(error, loss_)

        timeCounter = timeCounter + time_ns() - deltaT_s #timeCounter sums all delays due to the callback functions of the previous iterations

        return false
    end

    @named pde_system = PDESystem(eq, bcs, domains, indvars, depvars)

    discretization = NeuralPDE.PhysicsInformedNN(chain, strategy; init_params =initθ)
    prob = NeuralPDE.discretize(pde_system,discretization)

    timeCounter = 0.0
    startTime = time_ns() #Fix initial time (t=0) before starting the training
    res = GalacticOptim.solve(prob, minimizer, cb = cb_, maxiters=maxIters)

    phi = discretization.phi

    params = res.minimizer

    # Model prediction
    domain = [ts,x1s,x2s,x3s,x4s]

    u_predict = [reshape([first(phi([t,x1,x2,x3,x4],res.minimizer)) for x1 in x1s for x2 in x2s for x3 in x3s for x4 in x4s], (length(x1s),length(x2s), length(x3s),length(x4s))) for t in ts]  #matrix of model's prediction

    return [error, params, domain, times, losses]
end

maxIters = [(1,1,1,1000,1000,1000,1000),(1,1,1,300,300,300,300)] #iters for ADAM/LBFGS
# maxIters = [(1,1,1,1,1,2,2),(1,1,1,3,3,3,3)] #iters for ADAM/LBFGS

strategies = [NeuralPDE.QuadratureTraining(quadrature_alg = CubaCuhre(), reltol = 1, abstol = 1e-4, maxiters = 100),
              NeuralPDE.QuadratureTraining(quadrature_alg = HCubatureJL(), reltol = 1, abstol = 1e-4, maxiters = 100, batch = 0),
              NeuralPDE.QuadratureTraining(quadrature_alg = CubatureJLh(), reltol = 1, abstol = 1e-4, maxiters = 100),
              NeuralPDE.QuadratureTraining(quadrature_alg = CubatureJLp(), reltol = 1, abstol = 1e-4, maxiters = 100),
              NeuralPDE.GridTraining(0.2),
              NeuralPDE.StochasticTraining(400 ; bcs_points= 50),
              NeuralPDE.QuasiRandomTraining(400 ; bcs_points= 50)]

strategies_short_name = ["CubaCuhre",
                        "HCubatureJL",
                        "CubatureJLh",
                        "CubatureJLp",
                        "GridTraining",
                        "StochasticTraining",
                        "QuasiRandomTraining"]

minimizers = [ADAM(0.005),
              #BFGS()]
              LBFGS()]

minimizers_short_name = ["ADAM",
                         "LBFGS"]
                        #"BFGS"]

# Run models
error_res =  Dict()
domains = Dict()
params_res = Dict()  #to use same params for the next run
times = Dict()
losses_res = Dict()

print("Starting run")
## Convergence

for min =1:length(minimizers) # minimizer
      for strat=1:length(strategies) # strategy
            # println(string(strategies_short_name[strat], "  ", minimizers_short_name[min]))
            res = hamilton_jacobi(strategies[strat], minimizers[min], maxIters[min][strat])
            push!(error_res, string(strat,min)     => res[1])
            push!(params_res, string(strat,min) => res[2])
            push!(domains, string(strat,min)        => res[3])
            push!(times, string(strat,min)        => res[4])
            push!(losses_res, string(strat,min)   => res[5])
      end
end

#Plotting the first strategy with the first minimizer out from the loop to initialize the canvas
current_label = string(strategies_short_name[1], " + " , minimizers_short_name[1])
error = Plots.plot(times["11"], error_res["11"], yaxis=:log10, label = current_label)#, xlims = (0,10))#legend = true)#, size=(1200,700))
plot!(error, times["21"], error_res["21"], yaxis=:log10, label = string(strategies_short_name[2], " + " , minimizers_short_name[1]))
plot!(error, times["31"], error_res["31"], yaxis=:log10, label = string(strategies_short_name[3], " + " , minimizers_short_name[1]))
plot!(error, times["41"], error_res["41"], yaxis=:log10, label = string(strategies_short_name[4], " + " , minimizers_short_name[1]))
plot!(error, times["51"], error_res["51"], yaxis=:log10, label = string(strategies_short_name[5], " + " , minimizers_short_name[1]))
plot!(error, times["61"], error_res["61"], yaxis=:log10, label = string(strategies_short_name[6], " + " , minimizers_short_name[1]))
plot!(error, times["71"], error_res["71"], yaxis=:log10, label = string(strategies_short_name[7], " + " , minimizers_short_name[1]))

plot!(error, times["12"], error_res["12"], yaxis=:log10, label = string(strategies_short_name[1], " + " , minimizers_short_name[2]))
plot!(error, times["22"], error_res["22"], yaxis=:log10, label = string(strategies_short_name[2], " + " , minimizers_short_name[2]))
plot!(error, times["32"], error_res["32"], yaxis=:log10, label = string(strategies_short_name[3], " + " , minimizers_short_name[2]))
plot!(error, times["42"], error_res["42"], yaxis=:log10, label = string(strategies_short_name[4], " + " , minimizers_short_name[2]))
plot!(error, times["52"], error_res["52"], yaxis=:log10, label = string(strategies_short_name[5], " + " , minimizers_short_name[2]))
plot!(error, times["62"], error_res["62"], yaxis=:log10, label = string(strategies_short_name[6], " + " , minimizers_short_name[2]))
plot!(error, times["72"], error_res["72"], yaxis=:log10, title = string("Hamilton Jacobi convergence ADAM/LBFGS"), ylabel = "log(error)",xlabel = "t", label = string(strategies_short_name[7], " + " , minimizers_short_name[2]))

using SciMLBenchmarks
SciMLBenchmarks.bench_footer(WEAVE_ARGS[:folder],WEAVE_ARGS[:file])


# Process Design and Synthesis

# 25
function process_synthesis(x)
        ## process synthesis problem
        x1 = x[1]; x2 = round5(x[2]);
        ## objective function
        f = zeros(2);
        f[1] = x2 + 2*x1;
        f[2] = -x1 .^ 2 -x2;
        ## constraints
        g = zeros(2);
        h = zeros(1);
        g[1] = f[2] +1.25;
        g[2] = x1 + x2 -1.6;

        return f, g, h
end

# 26
function process_sythesis_and_design(x)

        ## Process sythesis and Design Problems
        x1 = x[1]; x2 = x[2]; x3 = round5(x[3]);
        ## objective function
        f = zeros(2);
        f[1] = -x3 + x2 + 2*x1;
        f[2] = -x1 .^ 2 - x2 + x1 .* x3;
        ## constraints
        h = zeros(1);
        g = zeros(1);
        h[1] = -2 .* exp(-x2) + x1;
        g[1] = x2 - x1 + x3;
        return f, g, h
end

# 27
function process_flow_sheeting_problem(x)

        ## Process flow sheeting Problem
        x1 = x[1]; x2 = x[2]; x3 = x[3];
        ## objective function
        f = zeros(2);
        f[1] = -0.7 .* x3 + 0.8 + 5 .* (0.5 - x1) .^ 2;
        f[2] = x1 - x3;
        ## constraints
        g = zeros(3);
        h = zeros(1);
        g[1] = -(exp(x1 - 0.2) + x2);
        g[2] = x2 + 1.1 .* x3 - 1;
        g[3] = x1 - x3 - 0.2;
        return f, g, h
end

# 28
function two_reactor_problem(x)
        ## Two reactor Problem
        x1 = x[1];
        x2 = x[2];
        v1 = x[3];
        v2 = x[4];
        y1 = round5(x[5]);
        y2 = round5(x[6]);
        x_ = x[7];

        z1 = 0.9*(1-exp(-0.5 .* v1)) .* x1;
        z2 = 0.8*(1-exp(-0.4*v2)) .* x2;
        ## objective function
        f = zeros(2);
        f[1] = 7.5 .* y1 + 5.5 .* y2 + 7 .* v1 + 6 .* v2 + 5 .* x_;
        f[2] = x1 + x2;
        ## constraints
        g = zeros(4);
        h = zeros(4);
        h[1] = y1 + y2 - 1;
        h[2] = z1 + z2 - 10;
        h[3] = x1 + x2 -x_;
        h[4] = z1 .* y1 + z2 .* y2 - 10;
        g[1] = v1 - 10*y1 -1e-6;
        g[2] = v2 - 10*y2;
        g[3] = x1 - 20*y1;
        g[4] = x2 - 20*y2;
        return f, g, h
end

# 29
function process_synthesis_problem(x)
        ## process synthesis problem
        x1 = x[1]; x2 = x[2]; x3 = x[3]; x4 = round5(x[4]);
        x5 = round5(x[5]); x6 = round5(x[6]); x7 = round5(x[7]);
        ## objective function
        f = zeros(2);
        f[1] = (1-x4) .^ 2 + (1-x5) .^ 2 + (1-x6) .^ 2 - log(abs(1+x7)+1e-6);
        f[2] = (1-x1) .^ 2 + (2-x2) .^ 2 + (3-x3) .^ 2;
        ## constraints
        g = zeros(9);
        h = zeros(1);
        g[1] = x1 + x2 + x3 + x4 + x5 + x6 -5;
        g[2] = x6 .^ 3 + x1 .^ 2 + x2 .^ 2 + x3 .^ 2 - 5.5;
        g[3] = x1 + x4 - 1.2;
        g[4] = x2 + x5 - 1.8;
        g[5] = x3 + x6 - 2.5;
        g[6] = x1 + x7 - 1.2;
        g[7] = x5 .^ 2 + x2 .^ 2 - 1.64;
        g[8] = x6 .^ 2 + x3 .^ 2 - 4.25;
        g[9] = x5 .^ 2 + x3 .^ 2 - 4.64;
        return f, g, h
end


using LinearAlgebra # HIDE
using Random:seed!  # HIDE
seed!(0)            # HIDE
                    # HIDE
x = randn(5)
y = randn(5)

for i in 1:5
    println(rpad("*"^i, 10, '-'), round(dot(x, y), digits=1))
end


export getHelmholtzOperator, getHelmholtzABL

"""
	H = getHelmholtzOperator(m,gamma,omega,Mesh)

	build Helmholtz operator
		H = Laplace - omega^2 * MassMatrix(m,gamma)

	Input:
		m                  - model
		gamma              - attenuation
		omega::Number      - frequency
		Mesh::RegularMesh  - mesh

	Output
		H::SparseMatrixCSC
"""
function getHelmholtzOperator(m,gamma,omega::Number,Mesh::RegularMesh)

Lap   = getNodalLaplacianMatrix(Mesh)
M     = getMassMatrix(vec(m),vec(gamma),Mesh)
# Get the Helmholtz operator (note the sign)
H = Lap .- omega^2 * M
return H
end

function getHelmholtzABL(Msh::RegularMesh,NeumannAtFirstDim::Bool,ABLpad::Array{Int64},ABLamp::Float64)
  h = Msh.h;
  n = Msh.n;
  pad = ABLpad;
  ntup = tuple(n...);

  if Msh.dim==2
	gamma = zeros(ntup);
	b_bwd1 = ((pad[1]:-1:1).^2)./pad[1]^2;
	b_bwd2 = ((pad[2]:-1:1).^2)./pad[2]^2;

	b_fwd1 = ((1:pad[1]).^2)./pad[1]^2;
	b_fwd2 = ((1:pad[2]).^2)./pad[2]^2;
	I1 = (n[1] - pad[1] + 1):n[1];
	I2 = (n[2] - pad[2] + 1):n[2];

	if NeumannAtFirstDim==false
		gamma[:,1:pad[2]] += ones(n[1],1)*b_bwd2';
		gamma[1:pad[1],1:pad[2]] -= b_bwd1*b_bwd2';
		gamma[I1,1:pad[2]] -= b_fwd1*b_bwd2';
	end

	gamma[:,I2] +=  ones(n[1],1)*b_fwd2';
	gamma[1:pad[1],:] += b_bwd1*ones(1,n[2]);
	gamma[I1,:] += b_fwd1*ones(1,n[2]);
	gamma[1:pad[1],I2] -= b_bwd1*b_fwd2';
	gamma[I1,I2] -= b_fwd1*b_fwd2';
	gamma *= ABLamp;
  else
	x1 = linspace(-1,1,n[1]);
	x2 = linspace(-1,1,n[2]);
	x3 = linspace( 0,1,n[3]);
	X1,X2,X3 = ndgrid(x1,x2,x3);
	padx1 = ABLpad[1];
	padx2 = ABLpad[2];
	padx3 = ABLpad[3];
	gammaL = (X1 - x1[padx1]).^2;
	gammaL[padx1+1:end,:,:] = 0.0
	gammaR = (X1 - x1[end-padx1+1]).^2
	gammaR[1:end-padx1,:,:] = 0.0

	gammat = gammaL + gammaR;
	gammat = gammat/maximum(gammat);
	gamma = copy(gammat);
	gammat[:] = 0.0;

	gammaL = (X2 - x2[padx2]).^2;
	gammaL[:,padx2+1:end,:] = 0.0
	gammaR = (X2 - x2[end-padx2+1]).^2
	gammaR[:,1:end-padx2,:] = 0.0

	gammat = gammaL + gammaR
	gammat = gammat/maximum(gammat);
	gamma += gammat;

	gammat = (X3 - x3[end-padx3+1]).^2
	gammat[:,:,1:end-padx3] = 0.0
	gammat = gammat/maximum(gammat);
	gamma += gammat;
	gamma *= ABLamp;
	gamma[gamma.>=ABLamp] = ABLamp;
  end
  return gamma;
end