module TestRR

using Compat
using Compat.Test
using IntervalArithmetic
using Clp
using EAGODomainReduction

type t_solver
    LP_solver
end

type test_opt
    numVar
    numConstr
    f
    g
    solver::t_solver
    gL
    gU
    gL_loc
    gU_loc
end

f(x) = x[1]^2+x[2]^2
g(x) = [x[1]+x[2];
        x[1]-x[2]]

in_opt = t_solver(ClpSolver())
opt = test_opt(2,2,f,g,in_opt,[2 -1],[3 0],[1,2],[1,2])
X = [Interval(0.0,2.0) for i=1:2]
STD_Linear_RR!(X,[opt],7.0)

@test 0.5-1E-4 <= X[1].lo <= 0.5+1E-4
@test 1.25-1E-4 <= X[1].hi <= 1.25+1E-4
@test 1.0-1E-4 <= X[2].lo <= 1.0+1E-4
@test 1.75-1E-4 <= X[2].hi <= 1.75+1E-4

end


third32  = Float32(1)/Float32(3)
thirdR32 = Robust32(1)/Robust32(3)
two32  = Float32(2)
twoR32 = Robust32(2)
fourthirds = Float32(4)/Float32(3)

@testset "add/sub" begin
  @test two32 + two32 == twoR32 + twoR32
  @test two32 - third32 == twoR32 - thirdR32

  # @test fourthirds - (two32 - 2*third32) > 2.75 * (fourthirds - (twoR32 - 2*thirdR32))
end

@testset "mul/div" begin
  @test abs(2/9 -  (third32 * 2*third32)) > 9 *( abs(2/9 - (thirdR32 * 2*thirdR32)))
  @test two32 - third32 == twoR32 - thirdR32

  #@test fourthirds - (two32 - 2*third32) > 2.75 * (fourthirds - (twoR32 - 2*thirdR32))
end

@testset "root/power" begin
  @test abs(two32 - sqrt(sqrt(two32))^4) > abs(twoR32 - sqrt(sqrt(twoR32))^4)
  @test abs(two32 - sqrt(cbrt(two32))^6) > abs(twoR32 - sqrt(cbrt(twoR32))^6)
end


# SECTION 3: other visualization in BENCHMARK & fertility experiments
# ----------------------------------------------
# 3.1 benchmark: correlations (during plotting years)
   println(
      "Cor between Aging Population Ratio & gap/pool expenditures: ",
      Statistics.cor( dict_Demog_base["AgingPopuRatio"][idx_plot], (DatPkg_base.Dt[:LI] ./ DatPkg_base.Dt[:AggPoolExp])[idx_plot] )
   )
   println(
      "Cor between Aging Population Ratio & gap/pool incomes: ",
      Statistics.cor( dict_Demog_base["AgingPopuRatio"][idx_plot], (DatPkg_base.Dt[:LI] ./ DatPkg_base.Dt[:AggPoolIn])[idx_plot] )
   )
   println(
      "Cor between Aging Population Ratio & gap/GDP: ",
      Statistics.cor( dict_Demog_base["AgingPopuRatio"][idx_plot], (DatPkg_base.Dt[:LI] ./ DatPkg_base.Dt[:Y])[idx_plot] )
   )
   println(
      "Cor between Aging Population Ratio & gap/tax revenues: ",
      Statistics.cor( dict_Demog_base["AgingPopuRatio"][idx_plot], (DatPkg_base.Dt[:LI] ./ ( DatPkg_base.Dt[:TRw] .+ DatPkg_base.Dt[:TRc] ) )[idx_plot] )
   )

#


module Floquet
using ..Hop, LinearAlgebra, QuadGK

export FloquetHamiltonian, get_floquet_occupation, get_illuminated_hamiltonian, get_illuminated_band

struct FloquetHamiltonian
    Hs::Vector{Matrix{ComplexF64}}
    Hf::Matrix{ComplexF64}
    Ω::Float64
    nstates::Int64
    nfstates::Int64
    harmonics_cutoff::Int64
end

function Base.show(io::IO, f::FloquetHamiltonian)
    print(io, "FloquetHamiltonian: $(f.nstates) states with $(2*f.harmonics_cutoff+1) harmonics.")
end

"""
FloquetHamiltonian(Hs::Vector{Matrix{T}}, Ω::Real, harmonics_cutoff::Integer) where T<:Number
```
Floquet Hamiltonian.

Generally, Floquet theory deals with time periodic
Hamiltonians ``H(t+Ω)=H(t)``.

# Constructor Arguments

 - `Hs` is a list of Hamiltonian looking like ``[H_0, H_1, H_2, ...]``.
   ``H_n`` is defined as: ``H_n=\\frac{1}{T}\\int_0^T e^{inΩt} H(t) dt``.
 - `Ω` is the frequency (provided in eV) of the Floquet system.
 - `harmonics_cutoff` determines how many harmonics are used in Floquet Hamiltonian

# Fields
 - `Hs::Vector{Matrix{ComplexF64}}`, see above explanations.
 - `Hf::Matrix{ComplexF64}` is Floquet Hamiltonian.
   With basis functions defined as ``u_n=e^{-inΩt}``, ``H^F_{mn}=H_{m-n}-mΩδ_{mn}``.
   The basis of returned Floquet Hamiltonian is
   ``(e^{iNΩt}, e^{i(N-1)Ωt}, ..., e^{-iNΩt})``
   where internal states are omitted.
 - `Ω::Float64`, see above explanations.
 - `nstates::Int64` is number of internal states.
 - `nfstates::Int64` is number of Floquet states.
 - `harmonics_cutoff::Int64`, see above explanations.
"""
function FloquetHamiltonian(Hs::Vector{Matrix{T}}, Ω::Real, harmonics_cutoff::Integer) where T<:Number
    nstates = size(Hs[1], 1)
    nfstates = nstates*(2*harmonics_cutoff+1)
    nhams = length(Hs)
    Hf = zeros(ComplexF64, (nfstates, nfstates))
    for j = 0:(nhams-1)
        for i = 1:(2harmonics_cutoff+1-j)
            if j == 0
                Hf[nstates*(i-1)+1:nstates*i, nstates*(i-1)+1:nstates*i] = Hs[1]-(i-harmonics_cutoff-1)*Ω*I
            else
                Hf[nstates*(i-1)+1:nstates*i, nstates*(i-1+j)+1:nstates*(i+j)] = Hs[j+1]'
                Hf[nstates*(i-1+j)+1:nstates*(i+j), nstates*(i-1)+1:nstates*i] = Hs[j+1]
            end
        end
    end
    @assert ishermitian(Hf)
    return FloquetHamiltonian(Hs, Hf, Ω, nstates, nfstates, harmonics_cutoff)
end

function get_floquet_GR(f::FloquetHamiltonian, ω::Real; Γ::Real=1.0)
    return inv((ω+im*Γ/2)*I-f.Hf)
end

function get_floquet_Σless(f::FloquetHamiltonian, ω::Real; Γ::Real=1.0, μ::Real=1.0)
    Σ = zeros(ComplexF64, size(f.Hf))
    for i=1:(2*f.harmonics_cutoff+1)
        Σ[(i-1)*f.nstates+1:i*f.nstates, (i-1)*f.nstates+1:i*f.nstates] =
            im*Γ*fermi(ω+(i-f.harmonics_cutoff-1)*f.Ω, μ)*Matrix(1.0I, f.nstates, f.nstates)
    end
    return Σ
end

function get_floquet_Gless(f::FloquetHamiltonian, ω::Real; Γ::Real=1.0, μ::Real=1.0)
    GR = get_floquet_GR(f, ω, Γ=Γ)
    Σless = get_floquet_Σless(f, ω, Γ=Γ, μ=μ)
    return GR*Σless*(GR')
end

"""
```julia
get_floquet_occupation(f::FloquetHamiltonian; Γ::Real=1.0, μ::Real=0.0,
    atol::Real=1.0e-3) --> Vector{Float64}
```

Calculate occupation number of `f` assuming coupling to a heat bath with coupling
constant `Γ`. Chemical potential of the heat bath is `μ`. `atol` is the integration
absolute tolerance.
"""
function get_floquet_occupation(f::FloquetHamiltonian; Γ::Real=1.0, μ::Real=0.0,
    atol::Real=1.0e-3)
    _, egvecs = eigen(f.Hf)
    occs = zeros(f.nstates)
    for i=(f.harmonics_cutoff*f.nstates+1):(f.harmonics_cutoff+1)*f.nstates
        occ = -im*quadgk(
        ω->egvecs[:, i]'*get_floquet_Gless(f, ω, Γ=Γ, μ=μ)*egvecs[:, i],
        -Inf, Inf, atol=atol)[1]/(2π)
        @assert imag(occ) < 1.0e-6
        occs[i-f.harmonics_cutoff*f.nstates] = real(occ)
    end
    return occs
end

"""
```julia
get_illuminated_hamiltonian(t::TightBindingModel, k::Vector{<:Real};
    A::Vector{<:Number}, Ω::Real, harmonics_cutoff::Integer=2)
    --> FloquetHamiltonian
```

Calculate Floquet Hamiltonian of `t` at `k` point under the illumination of light with frequency Ω.
Peierls substitution is performed to the lowest order. Only electric field is taken
into account. The parameter `A` looks like ``[A_x, A_y, A_z]`` denoting vector potential
``A(t)=[A_x, A_y, A_z]e^{-iΩt}+c.c.``. Floquet Hamiltonian is truncated
up to `harmonics_cutoff` harmonics.
"""
function  get_illuminated_hamiltonian(t::TightBindingModel, k::Vector{<:Real};
    A::Vector{<:Number}, Ω::Real, harmonics_cutoff::Integer=2)
    @assert length(A) == 3 "Length of A is not correct."

    H0 = gethamiltonian(t, k)

    # Unfortunately we cannot calculate Hamiltonian by gethamiltonian since
    # H1 is not necessarily Hermitian.
    cpositions = t.lat*t.positions # Cartesian positions
    H1 = zeros(ComplexF64, size(H0))
    for (R, hopping) = t.hoppings
        cR = t.lat*R
        Peierlshopping = zeros(ComplexF64, size(H0))
        for n = 1:t.norbits
            for m = 1:t.norbits
                Peierlshopping[m, n] = im*hopping[m, n]*((cpositions[:, n]-cpositions[:, m]+cR)⋅A)
            end
        end
        H1 += exp(2π*im*(k⋅R))*Peierlshopping
    end

    return FloquetHamiltonian([H0, H1], Ω, harmonics_cutoff)
end

"""
```julia
function get_illuminated_band(t::TightBindingModel; A::Vector{<:Number}, Ω::Real,
    kp::KPath, harmonics_cutoff::Integer=2) --> Matrix{Float64}
```

Calculate bands of `t` on `kp` under the illumination of light with frequency Ω.
Peierls substitution is performed to the lowest order. Only electric field is taken
into account. The parameter `A` looks like ``[A_x, A_y, A_z]`` denoting vector potential
``A(t)=[A_x, A_y, A_z]e^{-iΩt}+c.c.``. Floquet Hamiltonian is truncated
up to `harmonics_cutoff` harmonics.

This function returns Floquet band energies at each k stored in column.
"""
function get_illuminated_band(t::TightBindingModel; A::Vector{<:Number}, Ω::Real,
    kp::KPath, harmonics_cutoff::Integer=2)
    bands = zeros(t.norbits*(2harmonics_cutoff+1), kp.nkpts)
    for ik=1:kp.nkpts
        bands[:, ik] = eigvals(
            get_illuminated_hamiltonian(t, kp.kpts[:, ik], A=A, Ω=Ω, harmonics_cutoff=harmonics_cutoff).Hf
        )
    end
    return bands
end

function get_illuminated_occupation(t::TightBindingModel, kp::KPath;
    Γ::Real, μ::Real, A::Vector{<:Number}, Ω::Real, harmonics_cutoff::Integer, atol::Real=1.0e-3)
    occs = zeros((t.norbits, kp.nkpts))
    for ik=1:kp.nkpts
        f = get_illuminated_hamiltonian(t, kp.kpts[:, ik], A=A, Ω=Ω, harmonics_cutoff=harmonics_cutoff)
        occs[:, ik] = get_floquet_occupation(f; μ=μ, Γ=Γ, atol=atol)
    end
    return occs
end

fermi(E, μ; T=0) = E<μ ? 1.0 : 0.0

end


# NewSU2Irrep
# Test of a bare-bones sector implementation, which is set to be `DegenerateNonAbelian`
# (even though it is not)
module NewSectors

export NewSU2Irrep

using HalfIntegers, WignerSymbols, TensorLabXD

struct NewSU2Irrep <: TensorLabXD.Sector
    j::HalfInt
    function NewSU2Irrep(j)
        j >= zero(j) || error("Not a valid SU₂ irrep")
        new(j)
    end
end
Base.convert(::Type{NewSU2Irrep}, j::Real) = NewSU2Irrep(j)

const _su2one = NewSU2Irrep(zero(HalfInt))
Base.one(::Type{NewSU2Irrep}) = _su2one
Base.conj(s::NewSU2Irrep) = s
TensorLabXD.:⊗(s1::NewSU2Irrep, s2::NewSU2Irrep) =
    TensorLabXD.SectorSet{NewSU2Irrep}(abs(s1.j-s2.j):(s1.j+s2.j))

Base.IteratorSize(::Type{TensorLabXD.SectorValues{NewSU2Irrep}}) = Base.IsInfinite()
Base.iterate(::TensorLabXD.SectorValues{NewSU2Irrep}, i = 0) = (NewSU2Irrep(half(i)), i+1)
# Base.getindex(::SectorValues{NewSU2Irrep}, i::Int) =
#     1 <= i ? NewSU2Irrep(half(i-1)) : throw(BoundsError(values(NewSU2Irrep), i))
# findindex(::SectorValues{NewSU2Irrep}, s::NewSU2Irrep) = twice(s.j)+1

# TensorLabXD.dim(s::NewSU2Irrep) = twice(s.j)+1
#
TensorLabXD.FusionStyle(::Type{NewSU2Irrep}) = GenericFusion()
TensorLabXD.BraidingStyle(::Type{NewSU2Irrep}) = Bosonic()
Base.isreal(::Type{NewSU2Irrep}) = true

TensorLabXD.Nsymbol(sa::NewSU2Irrep, sb::NewSU2Irrep, sc::NewSU2Irrep) =
    convert(Int, WignerSymbols.δ(sa.j, sb.j, sc.j))
function TensorLabXD.Fsymbol(s1::NewSU2Irrep, s2::NewSU2Irrep, s3::NewSU2Irrep,
                            s4::NewSU2Irrep, s5::NewSU2Irrep, s6::NewSU2Irrep)
    n1 = Nsymbol(s1, s2, s5)
    n2 = Nsymbol(s5, s3, s4)
    n3 = Nsymbol(s2, s3, s6)
    n4 = Nsymbol(s1, s6, s4)
    f = all(==(_su2one), (s1, s2, s3, s4, s5, s6)) ? 1.0 :
                sqrt(dim(s5) * dim(s6)) * WignerSymbols.racahW(Float64, s1.j, s2.j,
                                                                s4.j, s3.j, s5.j, s6.j)
    return fill(f , (n1, n2, n3, n4))
end
function TensorLabXD.Rsymbol(sa::NewSU2Irrep, sb::NewSU2Irrep, sc::NewSU2Irrep)
    Nsymbol(sa, sb, sc) > 0 || return fill(0., (0,0))
    return fill(iseven(convert(Int, sa.j+sb.j-sc.j)) ? 1.0 : -1.0, (1, 1))
end
TensorLabXD.dim(s::NewSU2Irrep) = twice(s.j) + 1

function TensorLabXD.fusiontensor(a::NewSU2Irrep, b::NewSU2Irrep, c::NewSU2Irrep)
    C = Array{Float64}(undef, dim(a), dim(b), dim(c), 1)
    ja, jb, jc = a.j, b.j, c.j

    for kc = 1:dim(c), kb = 1:dim(b), ka = 1:dim(a)
        C[ka,kb,kc,1] = WignerSymbols.clebschgordan(ja, ja+1-ka, jb, jb+1-kb, jc, jc+1-kc)
    end
    return C
end

Base.hash(s::NewSU2Irrep, h::UInt) = hash(s.j, h)
Base.isless(s1::NewSU2Irrep, s2::NewSU2Irrep) = isless(s1.j, s2.j)

end


using ReachabilityBenchmarks, MathematicalSystems, Reachability, Plots

include(@relpath "fiveDimSys_model.jl")
include(@relpath "fiveDimSys_specifications.jl")

S = fiveDimSys_model()
X0, options = fiveDimSys_specification()
options = Options(options)

# initial value problem
problem = InitialValueProblem(S, X0)

# partition with 1D blocks
partition = [[i] for i in 1:5]

# reachability algorithms
algorithm_reach1 = BFFPSV18(:δ => 0.005, :vars => [1, 2],
                            :partition => partition)
algorithm_reach2 = GLGM06(:δ => 0.005, :max_order => 5)

# compute flowpipe
options[:mode] = "reach"
solution1 = solve(problem, options; op=algorithm_reach1)
solution2 = solve(problem, options; op=algorithm_reach2)

# project flowpipe to time and x₁
solution1.options[:plot_vars] = [1, 2]
solution_proj1 = project(solution1)
solution2.options[:plot_vars] = [1, 2]
solution_proj2 = project(solution2)

# plot projection
plot(solution_proj1)
savefig("fiveDimSys1")
plot(solution_proj2)
savefig("fiveDimSys2")


#== # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# Description
#
#   Functions related with the model IAU-76/FK5.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# References
#
#   [1] Vallado, D. A (2013). Fundamentals of Astrodynamics and Applications.
#       Microcosm Press, Hawthorn, CA, USA.
#
#   [2] Gontier, A. M., Capitaine, N (1991). High-Accuracy Equation of Equinoxes
#       and VLBI Astrometric Modelling. Radio Interferometry: Theory, Techniques
#       and Applications, IAU Coll. 131, ASP Conference Series, Vol. 19.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # ==#

export rITRFtoPEF_fk5, rPEFtoITRF_fk5
export rPEFtoTOD_fk5,  rTODtoPEF_fk5
export rTODtoMOD_fk5,  rMODtoTOD_fk5
export rMODtoGCRF_fk5, rGCRFtoMOD_fk5

export rITRFtoGCRF_fk5, rGCRFtoITRF_fk5
export rPEFtoMOD_fk5,   rMODtoPEF_fk5

################################################################################
#                            IAU-76/FK5 Reductions
################################################################################
#
# The conversion between the Geocentric Celestial Reference Frame (GCRF) to the
# International Terrestrial Reference Frame (ITRF) is done by means of:
#
#                    GCRF <=> MOD <=> TOD <=> PEF <=> ITRF
#
# in which:
#   - MOD: Mean of Date frame.
#   - TOD: True of Date frame.
#   - PEF: Pseudo-Earth fixed frame.
#
# Every rotation will be coded as a function using the IAU-76/FK5 theory.
# Additionally, composed rotations will also available. In general, the API is:
#
#   function r<Origin Frame>to<Destination Frame>_fk5
#
# The arguments vary depending on the origin and destination frame and should be
# verified using the function documentation.
#
################################################################################

################################################################################
#                               Single Rotations
################################################################################

#                                 ITRF <=> PEF
# ==============================================================================

"""
    function rITRFtoPEF_fk5([T,] x_p::Number, y_p::Number)

Compute the rotation that aligns the International Terrestrial Reference Frame
(ITRF) with the Pseudo-Earth Fixed (PEF) frame considering the polar motion
represented by the angles `x_p` [rad] and `y_p` [rad] that are obtained from
IERS EOP Data (see `get_iers_eop`).

`x_p` is the polar motion displacement about X-axis, which is the IERS Reference
Meridian direction (positive south along the 0˚ longitude meridian). `y_p` is
the polar motion displacement about Y-axis (90˚W or 270˚E meridian).

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the ITRF frame with the PEF frame. The rotation
representation is selected by the optional parameter `T`.

# Remarks

The ITRF is defined based on the International Reference Pole (IRP), which is
the location of the terrestrial pole agreed by international committees [1]. The
Pseudo-Earth Fixed, on the other hand, is defined based on the Earth axis of
rotation, or the Celestial Intermediate Pole (CIP). Hence, PEF XY-plane contains
the True Equator. Furthermore, since the recovered latitude and longitude are
sensitive to the CIP, then it should be computed considering the PEF frame.

"""
rITRFtoPEF_fk5(x_p::Number, y_p::Number) = rITRFtoPEF_fk5(DCM, x_p, y_p)

rITRFtoPEF_fk5(T::Type, x_p::Number, y_p::Number) =
    # Notice that `x_p` and `y_p` are displacements in X and Y directions and
    # **not** rotation angles. Hence, a displacement in X is a rotation in Y and
    # a displacement in Y is a rotation in X.
    smallangle_to_rot(T, +y_p, +x_p, 0)

"""
    function rPEFtoITRF_fk5([T,] x_p::Number, y_p::Number)

Compute the rotation that aligns the Pseudo-Earth Fixed (PEF) with the
International Terrestrial Reference Frame (ITRF) considering the polar motion
represented by the angles `x_p` [rad] and `y_p` [rad] that are obtained from
IERS EOP Data (see `get_iers_eop`).

`x_p` is the polar motion displacement about X-axis, which is the IERS Reference
Meridian direction (positive south along the 0˚ longitude meridian). `y_p` is
the polar motion displacement about Y-axis (90˚W or 270˚E meridian).

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the PEF frame with the ITRF. The rotation
representation is selected by the optional parameter `T`.

# Remarks

The ITRF is defined based on the International Reference Pole (IRP), which is
the location of the terrestrial pole agreed by international committees [1]. The
Pseudo-Earth Fixed, on the other hand, is defined based on the Earth axis of
rotation, or the Celestial Intermediate Pole (CIP). Hence, PEF XY-plane contains
the True Equator. Furthermore, since the recovered latitude and longitude are
sensitive to the CIP, then it should be computed considering the PEF frame.

"""
rPEFtoITRF_fk5(x_p::Number, y_p::Number) = rPEFtoITRF_fk5(DCM, x_p, y_p)

rPEFtoITRF_fk5(T::Type, x_p::Number, y_p::Number) =
    # Notice that `x_p` and `y_p` are displacements in X and Y directions and
    # **not** rotation angles. Hence, a displacement in X is a rotation in Y and
    # a displacement in Y is a rotation in X.
    smallangle_to_rot(T, -y_p, -x_p, 0)

#                                 PEF <=> TOD
# ==============================================================================

"""
    function rPEFtoTOD_fk5([T,] JD_UT1::Number, JD_TT::Number [, δΔψ_1980::Number])

Compute the rotation that aligns the Pseudo-Earth Fixed (PEF) frame with the
True of Date (TOD) frame at the Julian Day `JD_UT1` [UT1] and `JD_TT`
[Terrestrial Time]. This algorithm uses the IAU-76/FK5 theory. Notice that one
can provide correction for the nutation in longitude (`δΔψ_1980`) \\[rad] that
is usually obtained from IERS EOP Data (see `get_iers_eop`).

The Julian Day in UT1 is used to compute the Greenwich Mean Sidereal Time (GMST)
(see `JDtoGMST`), whereas the Julian Day in Terrestrial Time is used to compute
the nutation in the longitude. Notice that the Julian Day in UT1 and in
Terrestrial Time must be equivalent, i.e. must be related to the same instant.
This function **does not** check this.

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the PEF frame with the TOD frame. The rotation
representation is selected by the optional parameter `T`.

# Remarks

The Pseudo-Earth Fixed (PEF) frame is rotated into the True of Date (TOD) frame
considering the 1980 IAU Theory of Nutation. The IERS EOP corrections must be
added if one wants to make the rotation consistent with the Geocentric Celestial
Reference Systems (GCRS).

"""
rPEFtoTOD_fk5(JD_UT1::Number, JD_TT::Number, δΔψ_1980::Number = 0) =
    rPEFtoTOD_fk5(DCM, JD_UT1, JD_TT, δΔψ_1980)

function rPEFtoTOD_fk5(T::Type,
                       JD_UT1::Number,
                       JD_TT::Number,
                       δΔψ_1980::Number = 0)
    # Compute the nutation in the Julian Day (Terrestrial Time) `JD_TT`.
    (mϵ_1980, Δϵ_1980, Δψ_1980) = nutation_fk5(JD_TT)

    # Add the corrections to the nutation in obliquity and longitude.
    Δψ_1980 += δΔψ_1980

    # Evaluate the Delaunay parameters associated with the Moon in the interval
    # [0,2π]°.
    #
    # The parameters here were updated as stated in the errata [2].
    T_TT = (JD_TT - JD_J2000)/36525
    r    = 360
    Ω_m  = @evalpoly(T_TT, + 125.04452222,
                           - (5r + 134.1362608),
                           + 0.0020708,
                           + 2.2e-6)
    Ω_m  = mod(Ω_m, 360)*pi/180

    # Compute the equation of Equinoxes.
    #
    # According to [2], the constant unit before `sin(2Ω_m)` is also in [rad].
    Eq_equinox1982 = Δψ_1980*cos(mϵ_1980) +
        ( 0.002640*sin(1Ω_m) + 0.000063*sin(2Ω_m) )*pi/648000

    # Compute the Mean Greenwich Sidereal Time.
    θ_gmst = JDtoGMST(JD_UT1)

    # Compute the Greenwich Apparent Sidereal Time (GAST).
    #
    # TODO: Should GAST be moved to a new function as the GMST?
    θ_gast = θ_gmst + Eq_equinox1982

    # Compute the rotation matrix.
    angle_to_rot(T, -θ_gast, 0, 0, :ZYX)
end

"""
    function rTODtoPEF_fk5([T,] JD_UT1::Number, JD_TT::Number [, δΔψ_1980::Number])

Compute the rotation that aligns the True of Date (TOD) frame with the
Pseudo-Earth Fixed (PEF) frame at the Julian Day `JD_UT1` [UT1] and `JD_TT`
[Terrestrial Time]. This algorithm uses the IAU-76/FK5 theory. Notice that one
can provide correction for the nutation in longitude (`δΔψ_1980`) \\[rad] that
is usually obtained from IERS EOP Data (see `get_iers_eop`).

The Julian Day in UT1 is used to compute the Greenwich Mean Sidereal Time (GMST)
(see `JDtoGMST`), whereas the Julian Day in Terrestrial Time is used to compute
the nutation in the longitude. Notice that the Julian Day in UT1 and in
Terrestrial Time must be equivalent, i.e. must be related to the same instant.
This function **does not** check this.

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the TOD frame with the PEF frame. The rotation
representation is selected by the optional parameter `T`.

# Remarks

The True of Date (TOD) frame is rotated into the Pseudo-Earth Fixed (PEF) frame
considering the 1980 IAU Theory of Nutation. The IERS EOP corrections must be
added if one wants to make the rotation consistent with the Geocentric Celestial
Reference Systems (GCRS).

"""
rTODtoPEF_fk5(JD_UT1::Number, JD_TT::Number, δΔψ_1980::Number = 0) =
    rTODtoPEF_fk5(DCM, JD_UT1, JD_TT, δΔψ_1980)

rTODtoPEF_fk5(T::T_ROT, JD_UT1::Number, JD_TT::Number, δΔψ_1980::Number = 0) =
    inv_rotation(rPEFtoTOD_fk5(T, JD_UT1, JD_TT, δΔψ_1980))

#                                 TOD <=> MOD
# ==============================================================================

"""
    function rTODtoMOD_fk5([T,] JD_TT::Number [, δΔϵ_1980::Number, δΔψ_1980::Number])

Compute the rotation that aligns the True of Date (TOD) frame with the Mean of
Date (MOD) frame at the Julian Day `JD_TT` [Terrestrial Time]. This algorithm
uses the IAU-76/FK5 theory. Notice that one can provide corrections for the
nutation in obliquity (`δΔϵ_1980`) \\[rad] and in longitude (`δΔψ_1980`)
\\[rad] that are usually obtained from IERS EOP Data (see `get_iers_eop`).

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the TOD frame with the MOD frame. The rotation
representation is selected by the optional parameter `T`.

# Remarks

The True of Date (TOD) frame is rotated into the Mean of Date (MOD) frame
considering the 1980 IAU Theory of Nutation. The IERS EOP corrections must be
added if one wants to make the rotation consistent with the Geocentric Celestial
Reference Systems (GCRS).

"""
rTODtoMOD_fk5(JD_TT::Number, δΔϵ_1980::Number = 0, δΔψ_1980::Number = 0) =
    rTODtoMOD_fk5(DCM, JD_TT, δΔϵ_1980, δΔψ_1980)

function rTODtoMOD_fk5(T::Type,
                       JD_TT::Number,
                       δΔϵ_1980::Number = 0,
                       δΔψ_1980::Number = 0)

    # Compute the nutation in the Julian Day (Terrestrial Time) `JD_TT`.
    (mϵ_1980, Δϵ_1980, Δψ_1980) = nutation_fk5(JD_TT)

    # Add the corrections to the nutation in obliquity and longitude.
    Δϵ_1980 += δΔϵ_1980
    Δψ_1980 += δΔψ_1980

    # Compute the obliquity.
    ϵ_1980 = mϵ_1980 + Δϵ_1980

    # Compute and return the Direction Cosine DCM.
    angle_to_rot(T, ϵ_1980, Δψ_1980, -mϵ_1980, :XZX)
end

"""
    function rMODtoTOD_fk5([T,] JD_TT::Number [, δΔϵ_1980::Number, δΔψ_1980::Number])

Compute the rotation that aligns the Mean of Date (MOD) frame with the True of
Date (TOD) frame at the Julian Day `JD_TT` [Terrestrial Time]. This algorithm
uses the IAU-76/FK5 theory. Notice that one can provide corrections for the
nutation in obliquity (`δΔϵ_1980`) \\[rad] and in longitude (`δΔψ_1980`) \\[rad]
that are usually obtained from IERS EOP Data (see `get_iers_eop`).

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the MOD frame with the TOD frame. The rotation
representation is selected by the optional parameter `T`.

# Remarks

The Mean of Date (MOD) frame is rotated into the True of Date (TOD) frame
considering the 1980 IAU Theory of Nutation. The IERS EOP corrections must be
added if one wants to make the rotation consistent with the Geocentric Celestial
Reference Systems (GCRS).

"""
rMODtoTOD_fk5(JD_TT::Number, δΔϵ_1980::Number = 0, δΔψ_1980::Number = 0) =
    rMODtoTOD_fk5(DCM, JD_TT, δΔϵ_1980, δΔψ_1980)

rMODtoTOD_fk5(T::T_ROT, JD_TT::Number, δΔϵ_1980::Number = 0, δΔψ_1980::Number = 0) =
    inv_rotation(rTODtoMOD_fk5(T, JD_TT, δΔϵ_1980, δΔψ_1980))

#                                 MOD <=> GCRF
# ==============================================================================

"""
    function rMODtoGCRF_fk5([T,] JD_TT::Number)

Compute the rotation that aligns the Mean of Date (MOD) frame with the
Geocentric Celestial Reference Frame (GCRF) at the Julian Day [Terrestrial Time]
`JD_TT`. This algorithm uses the IAU-76/FK5 theory.

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the MOD frame with the GCRF frame. The rotation
representation is selected by the optional parameter `T`.

# Remarks

The Mean of Date (MOD) frame is rotated into the Geocentric Celestial Reference
Frame (GCRF) considering the IAU 1976 Precession model.

Notice that if the conversion `TOD => MOD` is performed **without** considering
the EOP corrections, then the GCRF obtained by this rotation is what is usually
called the J2000 reference frame.

"""
rMODtoGCRF_fk5(JD_TT::Number) = rMODtoGCRF_fk5(DCM,JD_TT)

function rMODtoGCRF_fk5(T::Type, JD_TT::Number)
    (ζ,Θ,z) = precession_fk5(JD_TT)
    angle_to_rot(T, z, -Θ, ζ, :ZYZ)
end

"""
    function rGCRFtoMOD_fk5([T,] JD_TT::Number)

Compute the rotation that aligns the Geocentric Celestial Reference Frame (GCRF)
with the Mean of Date (MOD) frame at the Julian Day [Terrestrial Time] `JD_TT`.
This algorithm uses the IAU-76/FK5 theory.

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the GCRF frame with the MOD frame. The rotation
representation is selected by the optional parameter `T`.

# Remarks

The Geocentric Celestial Reference Frame (GCRF) is rotated into the Mean of Date
(MOD) frame considering the IAU 1976 Precession model.

Notice that if the conversion `MOD => TOD` is performed **without** considering
the EOP corrections, then the GCRF in this rotation is what is usually called
the J2000 reference frame.

"""
rGCRFtoMOD_fk5(JD_TT::Number) = rGCRFtoMOD_fk5(DCM,JD_TT)

rGCRFtoMOD_fk5(T::T_ROT,JD_TT::Number) = inv_rotation(rMODtoGCRF_fk5(T, JD_TT))

################################################################################
#                              Multiple Rotations
################################################################################

# The functions with multiple rotations must be added only in two cases:
#
#   * ITRF <=> GCRF (Full rotation between ECI and ECEF).
#   * When the it will decrease the computational burden compared to
#     calling the functions with the single rotations.
#

#                                ITRF <=> GCRF
# ==============================================================================

"""
    function rITRFtoGCRF_fk5([T,] JD_UT1::Number, JD_TT::Number, x_p::Number, y_p::Number [, δΔϵ_1980::Number, δΔψ_1980::Number])

Compute the rotation that aligns the International Terrestrial Reference Frame
(ITRF) with the Geocentric Celestial Reference Frame (GCRF) at the Julian Day
`JD_UT1` [UT1] and `JD_TT` [Terrestrial Time], and considering the IERS EOP Data
`x_p` [rad], `y_p` [rad], `δΔϵ_1980` [rad], and `δΔψ_1980` [rad] \\(see
`get_iers_eop`). This algorithm uses the IAU-76/FK5 theory.

`x_p` is the polar motion displacement about X-axis, which is the IERS Reference
Meridian direction (positive south along the 0˚ longitude meridian). `y_p` is
the polar motion displacement about Y-axis (90˚W or 270˚E meridian). `δΔϵ_1980`
is the nutation in obliquity. `δΔψ_1980` is the nutation in longitude.

The Julian Day in UT1 is used to compute the Greenwich Mean Sidereal Time (GMST)
(see `JDtoGMST`), whereas the Julian Day in Terrestrial Time is used to compute
the nutation in the longitude. Notice that the Julian Day in UT1 and in
Terrestrial Time must be equivalent, i.e. must be related to the same instant.
This function **does not** check this.

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the ITRF frame with the GCRF frame. The rotation
representation is selected by the optional parameter `T`.

# Remarks

The EOP data related to the polar motion (`x_p` and `y_p`) is required, since
this is the only way available to compute the conversion ITRF <=> PEF (the
models are highly imprecise since the motion is still not very well understood
[1]). However, the EOP data related to the nutation of the obliquity
(`δΔϵ_1980`) and the nutation of the longitude (`δΔψ_1980`) can be omitted. In
this case, the GCRF frame is what is usually called J2000 reference frame.

"""
rITRFtoGCRF_fk5(JD_UT1::Number,
                JD_TT::Number,
                x_p::Number,
                y_p::Number,
                δΔϵ_1980::Number = 0,
                δΔψ_1980::Number = 0) =
    rITRFtoGCRF_fk5(DCM, JD_UT1, JD_TT, x_p, y_p, δΔϵ_1980, δΔψ_1980)

function rITRFtoGCRF_fk5(T::Type,
                         JD_UT1::Number,
                         JD_TT::Number,
                         x_p::Number,
                         y_p::Number,
                         δΔϵ_1980::Number = 0,
                         δΔψ_1980::Number = 0)
    # Compute the rotation ITRF => PEF.
    r_PEF_ITRF = rITRFtoPEF_fk5(T, x_p, y_p)

    # Compute the rotation PEF => MOD.
    r_MOD_PEF = rPEFtoMOD_fk5(T, JD_UT1, JD_TT, δΔϵ_1980, δΔψ_1980)

    # Compute the rotation MOD => GCRF.
    r_GCRF_MOD = rMODtoGCRF_fk5(T, JD_TT)

    # Return the full rotation.
    compose_rotation(r_PEF_ITRF, r_MOD_PEF, r_GCRF_MOD)
end

"""
    function rGCRFtoITRF_fk5([T,] JD_UT1::Number, JD_TT::Number, x_p::Number, y_p::Number [, δΔϵ_1980::Number, δΔψ_1980::Number])

Compute the rotation that aligns the Geocentric Celestial Reference Frame (GCRF)
with the International Terrestrial Reference Frame (ITRF) at the Julian Day
`JD_UT1` [UT1] and `JD_TT` [Terrestrial Time], and considering the IERS EOP Data
`x_p` [rad], `y_p` [rad], `δΔϵ_1980` [rad], and `δΔψ_1980` [rad] \\(see
`get_iers_eop`). This algorithm uses the IAU-76/FK5 theory.

`x_p` is the polar motion displacement about X-axis, which is the IERS Reference
Meridian direction (positive south along the 0˚ longitude meridian). `y_p` is
the polar motion displacement about Y-axis (90˚W or 270˚E meridian). `δΔϵ_1980`
is the nutation in obliquity. `δΔψ_1980` is the nutation in longitude.

The Julian Day in UT1 is used to compute the Greenwich Mean Sidereal Time (GMST)
(see `JDtoGMST`), whereas the Julian Day in Terrestrial Time is used to compute
the nutation in the longitude. Notice that the Julian Day in UT1 and in
Terrestrial Time must be equivalent, i.e. must be related to the same instant.
This function **does not** check this.

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the GCRF frame with the ITRF frame. The rotation
representation is selected by the optional parameter `T`.

# Remarks

The EOP data related to the polar motion (`x_p` and `y_p`) is required, since
this is the only way available to compute the conversion ITRF <=> PEF (the
models are highly imprecise since the motion is still not very well understood
[1]). However, the EOP data related to the nutation of the obliquity
(`δΔϵ_1980`) and the nutation of the longitude (`δΔψ_1980`) can be omitted. In
this case, the GCRF frame is what is usually called J2000 reference frame.

"""
rGCRFtoITRF_fk5(JD_UT1::Number,
                JD_TT::Number,
                x_p::Number,
                y_p::Number,
                δΔϵ_1980::Number = 0,
                δΔψ_1980::Number = 0) =
    rGCRFtoITRF_fk5(DCM, JD_UT1, JD_TT, x_p, y_p, δΔϵ_1980, δΔψ_1980)

rGCRFtoITRF_fk5(T::T_ROT,
                JD_UT1::Number,
                JD_TT::Number,
                x_p::Number,
                y_p::Number,
                δΔϵ_1980::Number = 0,
                δΔψ_1980::Number = 0) =
    inv_rotation(rITRFtoGCRF_fk5(T, JD_UT1, JD_TT, x_p, y_p, δΔϵ_1980, δΔψ_1980))

#                                 PEF <=> MOD
# ==============================================================================

"""
    function rPEFtoMOD_fk5([T,] JD_UT1::Number, JD_TT::Number [, δΔϵ_1980::Number, δΔψ_1980::Number])

Compute the rotation that aligns the Pseudo-Earth Fixed (PEF) frame with the
Mean of Date (MOD) at the Julian Day `JD_UT1` [UT1] and `JD_TT` [Terrestrial
Time]. This algorithm uses the IAU-76/FK5 theory. Notice that one can provide
corrections for the nutation in obliquity (`δΔϵ_1980`) \\[rad] and in longitude
(`δΔψ_1980`) \\[rad] that are usually obtained from IERS EOP Data (see
`get_iers_eop`).

The Julian Day in UT1 is used to compute the Greenwich Mean Sidereal Time (GMST)
(see `JDtoGMST`), whereas the Julian Day in Terrestrial Time is used to compute
the nutation in the longitude. Notice that the Julian Day in UT1 and in
Terrestrial Time must be equivalent, i.e. must be related to the same instant.
This function **does not** check this.

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the PEF frame with the TOD frame. The rotation
representation is selected by the optional parameter `T`.

"""
rPEFtoMOD_fk5(JD_UT1::Number,
              JD_TT::Number,
              δΔϵ_1980::Number = 0,
              δΔψ_1980::Number = 0) =
    rPEFtoMOD_fk5(DCM, JD_UT1, JD_TT, δΔϵ_1980, δΔψ_1980)

function rPEFtoMOD_fk5(T::Type,
                       JD_UT1::Number,
                       JD_TT::Number,
                       δΔϵ_1980::Number = 0,
                       δΔψ_1980::Number = 0)

    # Notice that, in this case, we will not use `rPEFtoTOD` and `rTODtoMOD`
    # because this would call the function `nutation` twice, leading to a huge
    # performance drop. Hence, the code of those two functions is almost
    # entirely rewritten here.

    # Compute the nutation in the Julian Day (Terrestrial Time) `JD_TT`.
    (mϵ_1980, Δϵ_1980, Δψ_1980) = nutation_fk5(JD_TT)

    # Add the corrections to the nutation in obliquity and longitude.
    Δϵ_1980 += δΔϵ_1980
    Δψ_1980 += δΔψ_1980

    # Compute the obliquity.
    ϵ_1980 = mϵ_1980 + Δϵ_1980

    # Evaluate the Delaunay parameters associated with the Moon in the interval
    # [0,2π]°.
    #
    # The parameters here were updated as stated in the errata [2].
    T_TT = (JD_TT - JD_J2000)/36525
    r    = 360
    Ω_m  = @evalpoly(T_TT, + 125.04452222,
                           - (5r + 134.1362608),
                           + 0.0020708,
                           + 2.2e-6)
    Ω_m  = mod(Ω_m, 360)*pi/180

    # Compute the equation of Equinoxes.
    #
    # According to [2], the constant unit before `sin(2Ω_m)` is also in [rad].
    Eq_equinox1982 = Δψ_1980*cos(mϵ_1980) +
        ( 0.002640*sin(1Ω_m) + 0.000063*sin(2Ω_m) )*pi/648000

    # Compute the Mean Greenwich Sidereal Time.
    θ_gmst = JDtoGMST(JD_UT1)

    # Compute the Greenwich Apparent Sidereal Time (GAST).
    #
    # TODO: Should GAST be moved to a new function as the GMST?
    θ_gast = θ_gmst + Eq_equinox1982

    # Compute the rotation PEF => TOD.
    r_TOD_PEF = angle_to_rot(T, -θ_gast, 0, 0, :ZYX)

    # Compute the rotation TOD => MOD.
    r_MOD_TOD = angle_to_rot(T, ϵ_1980, Δψ_1980, -mϵ_1980, :XZX)

    compose_rotation(r_TOD_PEF, r_MOD_TOD)
end

"""
    function rMODtoPEF_fk5([T,] JD_UT1::Number, JD_TT::Number [, δΔϵ_1980::Number, δΔψ_1980::Number])

Compute the rotation that aligns the Mean of Date (MOD) reference frame with the
Pseudo-Earth Fixed (PEF) frame at the Julian Day `JD_UT1` [UT1] and `JD_TT`
[Terrestrial Time]. This algorithm uses the IAU-76/FK5 theory. Notice that one
can provide corrections for the nutation in obliquity (`δΔϵ_1980`) \\[rad] and
in longitude (`δΔψ_1980`) \\[rad] that are usually obtained from IERS EOP Data
(see `get_iers_eop`).

The Julian Day in UT1 is used to compute the Greenwich Mean Sidereal Time (GMST)
(see `JDtoGMST`), whereas the Julian Day in Terrestrial Time is used to compute
the nutation in the longitude. Notice that the Julian Day in UT1 and in
Terrestrial Time must be equivalent, i.e. must be related to the same instant.
This function **does not** check this.

The rotation type is described by the optional variable `T`. If it is `DCM`,
then a DCM will be returned. Otherwise, if it is `Quaternion`, then a Quaternion
will be returned. In case this parameter is omitted, then it falls back to
`DCM`.

# Returns

The rotation that aligns the MOD frame with the PEF frame. The rotation
representation is selected by the optional parameter `T`.

"""
rMODtoPEF_fk5(JD_UT1::Number,
              JD_TT::Number,
              δΔϵ_1980::Number = 0,
              δΔψ_1980::Number = 0) =
    rMODtoPEF_fk5(DCM, JD_UT1, JD_TT, δΔϵ_1980, δΔψ_1980)

rMODtoPEF_fk5(T::T_ROT,
              JD_UT1::Number,
              JD_TT::Number,
              δΔϵ_1980::Number = 0,
              δΔψ_1980::Number = 0) =
    inv_rotation(rPEFtoMOD_fk5(T, JD_UT1, JD_TT, δΔϵ_1980, δΔψ_1980))


################
# Example data
################
N = 50
x_uncertain = [UncertainValue(Normal, x, rand(Uniform(0.1, 0.8))) for x in rand(N)]
y_uncertain = [UncertainValue(Normal, y, rand(Uniform(0.1, 0.8))) for y in rand(N)]
x = UncertainValueDataset(x_uncertain)
y = UncertainValueDataset(y_uncertain)

time_uncertain = [UncertainValue(Normal, i, 1) for i = 1:length(x)];
time_certain = [CertainValue(i) for i = 1:length(x)];
timeinds_x = UncertainIndexDataset(time_uncertain)
timeinds_y = UncertainIndexDataset(time_certain)

X = UncertainIndexValueDataset(timeinds_x, x)
Y = UncertainIndexValueDataset(timeinds_y, y);

###########################################################
# SequentialResampling
###########################################################
seq = SequentialResampling(StrictlyIncreasing())

@test resample(X, seq) isa NTuple{2, Vector{<:Real}}
@test resample(X, seq)[1] |> length == N
@test resample(X, seq)[2] |> length == N
@test resample(Y, seq) isa NTuple{2, Vector{<:Real}}
@test resample(Y, seq)[1] |> length == N
@test resample(Y, seq)[2] |> length == N

###########################################################
# SequentialInterpolatedResampling
###########################################################
seq_intp = SequentialInterpolatedResampling(StrictlyIncreasing(), RegularGrid(0, N, 2))

@test resample(X, seq_intp) isa Tuple{AbstractRange, Vector{<:Real}}
@test resample(X, seq_intp)[1] |> length == length(0:2:N)
@test resample(X, seq_intp)[2] |> length == length(0:2:N)
@test resample(Y, seq_intp) isa Tuple{AbstractRange, Vector{<:Real}}
@test resample(Y, seq_intp)[1] |> length == length(0:2:N)
@test resample(Y, seq_intp)[2] |> length == length(0:2:N)

#################################
# ConstrainedValueResampling
#################################

c = ConstrainedValueResampling(10, TruncateStd(1))

r = resample(timeinds_y, c)
@test length(r) == c.n
@test length(r[1]) == N

r = resample(timeinds_x, c)
@test length(r) == c.n
@test length(r[1]) == N

r = resample(x, c)
@test length(r) == c.n
@test length(r[1]) == N

r = resample(y, c)
@test length(r) == c.n
@test length(r[1]) == N

#################################
# ConstrainedIndexValueResampling
#################################

c = ConstrainedIndexValueResampling(10, (TruncateStd(1), TruncateStd(1)))
rx = resample(X, c)
ry = resample(Y, c)

@test rx isa Vector{Tuple{Vector{Float64},Vector{Float64}}}
@test ry isa Vector{Tuple{Vector{Float64},Vector{Float64}}}

@test length(rx) == c.n
@test length(ry) == c.n
@test length(rx[1]) == 2
@test length(ry[1]) == 2
@test length(rx[1][1]) == N
@test length(ry[1][2]) == N
@test length(rx[2][1]) == N
@test length(ry[2][2]) == N


@with_kw struct rtgM1{A,B,C} <: RTGmodel
    model::A = rtgM1_model()
    protein_lookup = get_protein_lookup(model)
    p::C = init_p(model; idx_hill_coefs=[])
    u::B = init_u(model, protein_lookup)
end

"""
Rtg Model: version 1
"""
function rtgM1_model()
    rtgM1 = @reaction_network  begin
        #=
        Input
        =#
        (1), s→s
        #=
        Modulation Layer
        =#
        (k1md*s, kn1md), Rtg2_ina_c ↔ Rtg2_act_c
        (k2md, kn2md), Rtg2_act_c + Mks ↔ Rtg2Mks_c
        (k3md, kn3md), Bmh + Mks ↔ BmhMks
        #=
        Activation Model:
        =#
        # NLSact: Nuclear Localization Signal (NLS) activation
        (k2n + k1*(BmhMks)/(k1m+BmhMks)), Rtg13_a_c → Rtg13_i_c
        (k2, k2n), Rtg3_i_c ↔ Rtg3_a_c
        # Rtg1Binding: Rtg13 formation
        (k3, kn3), Rtg1_c + Rtg3_a_c ↔ Rtg13_a_c
        (k3, kn3), Rtg1_c + Rtg3_i_c ↔ Rtg13_i_c
        # Translocation
        (k4, kn4), Rtg13_a_c ↔ Rtg13_a_n
        (k4, kn4), Rtg3_a_c ↔ Rtg3_a_n

    end k1md kn1md k2md kn2md k3md kn3md k1 k1m k2 k2n k3 kn3 k4 kn4
    return rtgM1
end


using FractionalSystems, Plots

function RF(du, u, p, t)
    du[1] = u[2]*(u[3]-1+u[1]*u[1])+0.1*u[1]
    du[2] = u[1]*(3*u[3]+1-u[1]*u[1])+0.1*u[2]
    du[3] = -2*u[3]*(0.98+u[1]*u[2])
end

LE, tspan=FOLyapunov(RF, 0.999, 0, 0.02, 300, [0.1; 0.1; 0.1], 0.005, 1000)

plot(tspan, LE[1, :])
plot!(tspan, LE[2, :])
plot!(tspan, LE[3, :])


# # # GSV: a general model for hyperspectral soil reflectance simulation
#
# Soil reflectance is crucial for canopy radiative transfer modeling. Spectral vector models are more flexible than typical soil albedo models, because they can ingest observed data and significantly improves hyperspectral soil reflectance modeling in terms of accuracy and robustness.
#
# We demonstrate the usage of the general spectral vectors (GSV) model[^1] developed to employ three dry spectral vectors and one humid spectral vector derived from global dry and humid soil reflectance databases including 23,871 soil spectra (400–2500 nm). The GSV model accurately simulates global soil reflectance with an R2 of 0.99 and RMSE of 0.01. The GSV model is highly suitable to be coupled with the CliMA model for vegetation remote sensing studies.
#
#  [^1]: Jiang, C., & Fang, H. (2019). GSV: a general model for hyperspectral soil reflectance simulation. International Journal of Applied Earth Observation and Geoinformation, 83(July), 101932. https://doi.org/10.1016/j.jag.2019.101932
#

# # Load used packages

#### Use Julia Plots package and switch to plotly js option:
using Plots
using StatsPlots
pyplot()

##----------------------------------------------------------------------------

## First, we include Revise (good for debugging) and Parameters (tools for structures)

##using Revise
using Parameters
using Statistics
using CSV
using DataStructures

##----------------------------------------------------------------------------
using LinearAlgebra
#----------------------------------------------------------------------------

# # Define used variables

## The hyperspectral wavelengths
WVL = 400:10:2501
## The general spectral vectors derived in the manuscript
GSV = [CSV.read("DryVec.csv"; header=false);CSV.read("SMVec.csv"; header=false)]
## The test hyperspectral data
hyper = CSV.read("TestSpectrum_v1.csv";header=false)
## The wavelengths of multispectral data
wvl = [450,550,650,850,1650,2150]
## The test multispectral data sliced from hyperspectral data
indices = []

for i in wvl
    push!(indices, findall(x -> x==i, WVL)[1])
end

multi = hyper[:,indices]

## Step 1: slice GSV according to the wavelengths of multispectral data
gsv = GSV[:,indices]
#----------------------------------------------------------------------------

# # Define plot

## Step 2: fit multispectral soil reflectance and reconstruct multispectral soil reflectance
X=convert(Matrix,multi)
V=convert(Matrix,gsv)
C = X * LinearAlgebra.pinv(V)
R = C * V
R[R.<0] .= 0
R[R.>1] .= 1
#----------------------------------------------------------------------------

scatter(X,R,label="")
plot!([0,0.4],[0,0.4],label="")
ylabel!("Simulations")
xlabel!("Measurements")
#----------------------------------------------------------------------------

## Step 3: reconstruct hyperspectral soil reflectance using coefficients fitted from multispectral data
X = convert(Matrix,hyper)
V = convert(Matrix,GSV)
R = C * V
R[R.<0] .= 0
R[R.>1] .= 1
#----------------------------------------------------------------------------

scatter(X,R,label="")
plot!([0,0.4],[0,0.4],label="")
ylabel!("Simulations")
xlabel!("Measurements")
#----------------------------------------------------------------------------

## Compare measured multispectral data and hyperspectral data with reconstructed ones
plot(WVL,transpose(convert(Matrix,hyper)),label="Actual hyperspectral reflectance")
scatter!(wvl,transpose(Matrix(multi)),label="Observed multispectral reflectance")
plot!(WVL,transpose(R),label="Simulated hyperspectral reflectance\nfitted from multispectral reflectance")
ylabel!("Soil albedo")
xlabel!("wl (nm)")

#----------------------------------------------------------------------------


using Sequent
using MLStyle
using HMRowUnification
using Test

@data Nat begin
    Z
    S(Nat)
end

rule_z = @sequent FOLD_Z begin
    _ ⊢ₛ Z => 0
end

rule_s = @sequent FOLD_S begin
    _ ⊢ₛ term => n
    _ ⊢ₛ S(term) => n + 1
end

@semantics [rule_z, rule_s]

@testset "Nat" begin

@test (nothing ⊢ₛ Z) == 0
@test (nothing ⊢ₛ S(S(Z))) == 2

end

@data ML begin
    LApp(ML, ML)
    LFun(Symbol, ML)
    LVar(Symbol)
    LVal(Int)
end

ImD = Base.ImmutableDict

struct Sigma{O}
    unbox :: ImD{Symbol, O}
end

Sigma{O}() where O = Sigma(ImD{Symbol, O}())

Base.getindex(s::Sigma{O}, x::Pair{Symbol, A}) where {O, A<:O} =
    Sigma(ImD(s.unbox, x))
Base.getindex(s::Sigma{O}, x::Symbol) where O =
    s.unbox[x]

(s::Sigma{O})(x::Pair{Symbol, A}) where {O, A<:O} = ImD(s, x)

r_app = @sequent APP begin
    (Γ, σ) ⊢ʷ a => ta
    (Γ, σ) ⊢ʷ f => Arrow(ta′, tr)
    true = Γ.unify(ta, ta′)
    (Γ, σ) ⊢ʷ LApp(f, a) => tr
end

r_var = @sequent VAR begin
    (Γ, σ) ⊢ʷ LVar(s) => σ[s]
end

r_fun = @sequent FUN begin
    a′ = Γ.new_tvar()
    (Γ, σ[a => a′]) ⊢ʷ r => tr
    (Γ, σ) ⊢ʷ LFun(a, r) => Arrow(a′, tr)
end

r_val = @sequent VAL begin
    _ ⊢ʷ LVal(_) => Nom(:int)
end

@semantics [r_app, r_fun, r_val, r_var]

@testset "ML" begin
    tctx = HMT[]
    Γ = mk_tcstate(tctx)
    σ = Sigma{HMT}()
    int_t = Nom(:int)
    σ = σ[:add => Arrow(int_t, Arrow(int_t, int_t))]
    term =
        LFun(:a,
            LApp(
                LApp(
                    LVar(:add),
                    LVar(:a)),
                LVar(:a)))
    infer_ty =  (Γ, σ) ⊢ʷ term
    @test Γ.prune(infer_ty) == Arrow(int_t, int_t)
end


# Test for Thompson Sampling

# Bernouli Arm Case

using MAB

ts1 = Algorithms.TS( 4 )

# For Bernoulli Bandit
for i = 1:10
    idx = Algorithms.getArmIndex( ts1 )
    print( "Playing Arm#$idx .... " )
    r = rand( Distributions.Bernoulli(0.5) )
    print( "Reward: $r\n" )
    Algorithms.updateReward!( ts1, r )
    println( ts1 )
end

Algorithms.reset!( ts1 )

# For [0,1] Arms
for i = 1:10
    idx = Algorithms.getArmIndex( ts1 )
    print( "Playing Arm#$idx .... " )
    r = rand()
    print( "Reward: $r\n" )
    Algorithms.updateReward!( ts1, r )
    println( ts1 )
end


import LinearAlgebra: svd

######### DecomposedMatrix ########

"""
Matrix in SVD decomposed form for fast and accurate fitting.

Stores a matrix ``A`` together with its thin SVD form::

    A == (u * s) * vH.

This allows for fast and accurate least squares fits using ``lstsq(A, x)``.
"""
struct DecomposedMatrix{T}
    a::Matrix{T}
    uH::Matrix{T}
    s::Vector{Float64}
    v::Matrix{T}
end

function DecomposedMatrix{T}(a::Matrix{T},
                             svd_result::Union{Tuple{Matrix{T},Vector{Float64},Matrix{T}},
                                               Nothing}=nothing) where {T}
    if svd_result === nothing
        F = svd(a)
        uH, s, v = collect(F.U'), F.S, collect(F.V)
    else
        u, s, vH = svd_result
        uH = collect(u')
        v = collect(vH')
    end
    return DecomposedMatrix(a, uH, s, v)
end

function _move_axis(arr::Array{T,N}, src::Int, dst::Int)::Array{T,N} where {T,N}
    """
    Move the axis at src to dst with keeping the order of the rest axes unchanged.

    src=1, dst=2, N=4, perm=[2, 1, 3, 4]
    src=2, dst=4, N=4, perm=[1, 3, 4, 2]
    """
    if src == dst
        return arr
    end
    perm = collect(1:N)
    deleteat!(perm, src)
    insert!(perm, dst, src)
    return permutedims(arr, perm)
end

"""
Apply a matrix operator to an array along a given axis
"""
function _matop_along_axis(op::Matrix{T}, arr::Array{S,N}, axis::Int64) where {T,S,N}
    # Move the target axis to the first position
    if axis < 0 || axis > N
        throw(DomainError("axis must be in [1,N]"))
    end
    if size(arr)[axis] != size(op)[2]
        error("Dimension mismatch!")
    end

    arr = _move_axis(arr, axis, 1)
    return _move_axis(_matop(op, arr), 1, axis)
end

function _matop(op::Matrix{T}, arr::Array{S,N}) where {T,S,N}
    # Apply op to the first axis of an array
    if size(arr)[1] != size(op)[2]
        error("Dimension mismatch!")
    end
    if N == 1
        return op * arr
    end

    rest_dims = size(arr)[2:end]
    arr = reshape(arr, (size(op)[2], prod(rest_dims)))
    return reshape(op * arr, (size(op)[1], rest_dims...))
end

function matmul(matrix::DecomposedMatrix, al::Array{T,N},
                axis::Int64)::Array{ComplexF64,N} where {T,N}
    return _matop_along_axis(matrix.a, al, axis)
end

function lstsq(matrix::DecomposedMatrix, ax::Array{T,N},
               axis::Int64)::Array{ComplexF64,N} where {T,N}
    if N == 2
        return _lstsq(matrix, axis)
    end
    ax = _move_axis(ax, axis, 1)
    rest_dims = size(ax)[2:end]
    rest_size = prod(rest_dims)
    ax = reshape(ax, size(ax)[1], rest_size)
    res = _lstsq(matrix, ax)
    res = reshape(res, size(res)[1], rest_dims...)
    return _move_axis(res, 1, axis)
end

function _lstsq(matrix::DecomposedMatrix, ax::Matrix{T}) where {T}
    r = matrix.uH * ax
    for j in 1:size(r, 2)
        for i in 1:size(r, 1)
            r[i, j] /= matrix.s[i]
        end
    end
    matrix.v * r
end

cond(a::DecomposedMatrix) = a.s[1] / a.s[length(a.s)]

############# Sampling ############
"""Base class for sparse sampling.

Encodes the "basis transformation" of a propagator from the truncated IR
basis coefficients ``G_ir[l]`` to time/frequency sampled on sparse points
``G(x[i])`` together with its inverse, a least squares fit::

         ________________                   ___________________
        |                |    evaluate     |                   |
        |     Basis      |---------------->|     Value on      |
        |  coefficients  |<----------------|  sampling points  |
        |________________|      fit        |___________________|

Attributes:
    basis: IR Basis instance
    matrix: Evaluation matrix is decomposed form
    sampling_points: Set of sampling points
"""
abstract type SamplingBase end

"""
Evaluate the basis coefficients at the sparse sampling points
"""
function evaluate(smpl::SamplingBase, al::Array{T,N};
                  axis::Int64=1)::Array{ComplexF64,N} where {T,N}
    return matmul(smpl.matrix, al, axis)
end

"""
Fit basis coefficients from the sparse sampling points
"""
function fit(smpl::SamplingBase, ax::Array{T,N};
             axis::Int64=1)::Array{ComplexF64,N} where {T,N}
    return lstsq(smpl.matrix, ax, axis)
end

"""
Condition number of the fitting problem
"""
function cond(smpl::SamplingBase)::Float64 where {S}
    return cond(smpl.matrix)
end

########### TauSampling ###########
"""
Sparse sampling in imaginary time.

Allows the transformation between the IR basis and a set of sampling points
in (scaled/unscaled) imaginary time.
"""
struct TauSampling <: SamplingBase
    basis::Basis
    sampling_points::Vector{Float64}
    matrix::DecomposedMatrix{Float64}
    cond::Float64
end

function TauSampling(basis::Basis, sampling_points::Union{Nothing,Vector{Float64}}=nothing)
    sampling_points = sampling_points === nothing ?
                      default_tau_sampling_points(basis) : sampling_points
    mat_ = Matrix{Float64}(transpose(basis.u(sampling_points)))
    matrix = DecomposedMatrix{Float64}(mat_)
    return TauSampling(basis, sampling_points, matrix, cond(matrix))
end

"""
Sampling points in (reduced) imaginary time
"""
function tau(smpl::TauSampling)
    return smpl.sampling_points
end

######## MatsubaraSampling ########
"""
Sampling in Matsubara frequency
"""
struct MatsubaraSampling <: SamplingBase
    basis::Basis
    sampling_points::Vector{Int64}
    matrix::DecomposedMatrix{ComplexF64}
    cond::Float64
end

function MatsubaraSampling(basis::Basis,
                           sampling_points::Union{Nothing,Vector{Int64}}=nothing)
    sampling_points = sampling_points === nothing ?
                      default_matsubara_sampling_points(basis) : sampling_points
    matrix = DecomposedMatrix{ComplexF64}(Matrix{ComplexF64}(transpose(basis.uhat(sampling_points))))
    return MatsubaraSampling(basis, sampling_points, matrix, cond(matrix))
end


using JuMP, ReSHOP, EMP
using Test

@testset "gnep river basin" begin

K = [100 100]
d1 = 3
d2 = 0.01
ee = [0.5, 0.25, 0.75]

c = [0.1 0.12 0.15;
    0.01 0.05 0.01]

u = [6.5    4.583;
     5.0    6.250;
     5.5    3.750]

n = 3

ctx = EMPmaster()

ag = [MathPrgm(ctx) for i in 1:n]

EquilibriumProblem(ctx, ag)

# Add the variable to the MP
for i in 1:n
    @variable(jump_model, x[i] >= 0)
end

constr = Array{Any}(undef, n, 2)

for i in 1:n
    @objective(ag[i], Min, (c[1, i] + c[2, i]*x[i])*x[i] - (d1 - d2*sum(x[j] for j in 1:n))*x[i])
    for m in 1:2
        cons = @constraint(ag[i], sum(u[j, m]*ee[j]*x[j] for j in 1:3) <= K[m])
        setindex!(constr, cons, i, m)
    end
end

solveEMP(ctx) == :Optimal

@test isapprox(value.(x), [0., 6.47333, 22.2808], atol=1e-4)
@test isapprox(dual.(constr[1,:]), [-0.8038, 0], atol=1e-3)
@test isapprox(dual.(constr[2,:]), [-1.5043, 0], atol=1e-3)
@test isapprox(dual.(constr[3,:]), [-0.4592, 0], atol=1e-3)

end


module ModelingTest
using OnlineStats, GLM, Base.Test

@testset "Modeling" begin
@testset "LinReg" begin
    n, p = 10000, 10
    x = randn(n, p)
    β = collect(1.:p)
    y = x * β + randn(n)
    o = LinReg(x, y, intercept = false)
    @test coef(o) ≈ x \ y
    o2 = LinReg(10, intercept = false)
    fit!(o2, x[1:500, :], y[1:500], 500)
    fit!(o2, x[501:1000, :], y[501:1000], 1)
    @test_approx_eq_eps coef(o) coef(o2) .5

    # vs. GLM
    l = lm(x, y)
    @test predict(l, x) ≈ predict(o, x)
    xi = randn(p)
    @test predict(l, xi') ≈ collect(predict(o, xi))
    @test loss(o, x, y) ≈ 0.5 * mean(abs2(y - predict(o, x)))
    ltab = coeftable(l)
    otab = coeftable(o)

    @test_approx_eq_eps vcov(o)[1:5]    vcov(l)[1:5]    .1
    @test_approx_eq_eps stderr(o)[1:5]  stderr(l)[1:5]  .1
    @test_approx_eq_eps confint(o)[1:5] confint(l)[1:5] .1

    @test coef(LinReg(10, intercept = false)) == zeros(10)
    @test coef(LinReg(10)) == zeros(11)

    o = LinReg(10)
    @test coef(o) == value(o)
    predict(o, randn(100, 10)) == zeros(100)
end
@testset "QuantRegMM" begin
    n, p = 10000, 10
    x = randn(n, p)
    β = collect(1.:p)
    y = x * β + randn(n)

    o = QuantRegMM(x, y)
    fit!(o, x, y, 10)

    @test coef(o) == value(o)
    @test value(o) == o.β
end
@testset "BiasVector / BiasMatrix" begin
    x = randn(100, 10)
    y = randn(100)
    yb = BiasVector(y)
    xb = BiasMatrix(x)

    @test length(yb)    == 101
    @test size(yb)      == (101,)
    @test length(xb)    == 100 * 10 + 100
    @test size(xb)      == (100, 11)
    @test yb[101]       == 1
    @test xb[1, 11]     == 1
    xb[1, 1] = 2.0
    yb[1] = 2.0
    @test xb[1, 1]      == 2.0
    @test yb[1]         == 2.0
end
@testset "TwoWayInteractionVector/Matrix" begin
    x = rand(5)
    v = TwoWayInteractionVector(x)
    @test v[5]  == x[5]
    @test v[6]  == x[1] * x[2]
    @test v[7]  == x[1] * x[3]
    @test v[8]  == x[1] * x[4]
    @test v[9]  == x[1] * x[5]
    @test v[10] == x[2] * x[3]

    x = rand(10, 5)
    m = TwoWayInteractionMatrix(x)
    @test m[1, 1]   == x[1, 1]
    @test m[1, 5]   == x[1, 5]
    @test m[1, 6]   == x[1, 1] * x[1, 2]
    @test m[1, 10]  == x[1, 2] * x[1, 3]
end
end

end#module


#=
an example on how to calculate the clebsch gordon coefficients for some su3 irreps
=#
using SUNRepresentations;

a = SUNIrrep(2,1,0);
b = SUNIrrep(2,1,0);
c = SUNIrrep(2,1,0);

CGC(a,b,c)


using CompScienceMeshes
using BEAST

using Test
using LinearAlgebra

faces = meshrectangle(1.0, 1.0, 0.5, 3)
srt_bnd_faces = sort.(boundary(faces))
edges = submesh(skeleton(faces,1)) do edge
    !(sort(edge) in srt_bnd_faces)
end

srt_bnd_nodes = sort.(skeleton(boundary(faces),0))
@test length(srt_bnd_nodes) == 8
nodes = submesh(skeleton(faces,0)) do node
    !(sort(node) in srt_bnd_nodes)
end
@test length(nodes) == 1

Conn = connectivity(nodes, edges, sign)

X = raviartthomas(faces, cellpairs(faces,edges))
@test numfunctions(X) == 8

divX = divergence(X)
Id = BEAST.Identity()
DD = assemble(Id, divX, divX)
@test rank(DD) == 7
L = divX * Conn
for sh in L.fns[1]
    @test isapprox(sh.coeff, 0, atol=1e-8)
end


using CuArrays, PyPlot
using FourierFlows
import FourierFlows.TwoDTurb

   n, L = 512, 2π   # Domain
nu, nnu = 1e-6, 1  # Viscosity
 dt, nt = 1, 100   # Time step

q0 = CuArray(rand(n, n))

prob = TwoDTurb.CuProblem(nx=n, Lx=L, nu=nu, nnu=nnu, dt=dt, stepper="FilteredRK4")
TwoDTurb.set_q!(prob, q0)

# Step forward
fig = figure(); tic()
for i = 1:10
  stepforward!(prob, nt)
  TwoDTurb.updatevars!(prob)

  cfl = maximum(prob.vars.U)*prob.grid.dx/prob.ts.dt
  @printf("step: %04d, t: %6.1f, cfl: %.2f, ", prob.step, prob.t, cfl)
  toc(); tic()

  q = Array(prob.vars.q)
  clf(); imshow(q); pause(0.01)
end

# Transfer grid and vars to device.
g = TwoDGrid(prob.grid)
U = Array(prob.vars.U)
V = Array(prob.vars.V)
q = Array(prob.vars.q)

# Calculate radial energy spectrum
E = @. 0.5*(U^2 + V^2) # energy density
Eh = rfft(E)
kr, Ehr = FourierFlows.radialspectrum(Eh, g, refinement=1)

fig, axs = subplots(ncols=2, figsize=(10, 4))

sca(axs[1]); cla()
pcolormesh(g.X, g.Y, q)
xlabel(L"x")
ylabel(L"y")
title("Vorticity")

sca(axs[2]); cla()
plot(kr, abs.(Ehr))
xlabel(L"k_r")
ylabel(L"\int | \hat{E} | k_r \mathrm{d} k_{\theta}")
title("Radial energy spectrum")

xlim(0, n/4)
axs[2][:set_yscale]("log")

tight_layout()
show()


using SparseArrays
include("./src/preprocessing.jl")
r, c = SparseReadData("../auto.mtx")

sr = sparse(r, c, ones(length(r),1), max(c...),max(c...))
num_tr = sum((sr*sr).*sr)


using CompScienceMeshes
using BEAST

Γ = readmesh(joinpath(@__DIR__,"sphere2.in"))
X = raviartthomas(Γ)

d = BEAST.CurlSingleLayerDP3D(1.0im, 1.0)
s = Helmholtz3D.singlelayer(wavenumber=1.0)

X = raviartthomas(Γ,BEAST.Continuity{:none})
Y = lagrangec0d1(Γ)

x = refspace(X)
y = refspace(Y)
charts = [chart(Γ,c) for c in cells(Γ)]

qs = BEAST.defaultquadstrat(s, x, x)
qd = quaddata(s, x, x, charts, charts, qs)
m1,m2 = 10,11
ql = quadrule(s, x, x, m1, charts[m1], m2, charts[m2], qd, qs)
@show @which quadrule(s, x, x, m1, charts[m1], m2, charts[m2], qd, qs)
@show typeof(ql)
BEAST.momintegrals!(s, x, x, charts[10], charts[20],
    zeros(ComplexF64,3,3), ql)

# sop = BEAST.singularpart(s)
ctr = CompScienceMeshes.center(charts[m1])
# BEAST.momintegrals!(sop, ctr, x, x, charts[m1], charts[m1], zeros(3,3), ql, 1.0)

dvg = divergence

@hilbertspace j p
@hilbertspace k q

κ = 1.0
curlA = strace(((x,y,z),)->-exp(-im*κ*z)*ŷ, Γ)
ndotA = dot(n, ((x,y,z),)->-x*exp(-im*κ*z)*ẑ)
a = @varform s[k,j] + s[dvg(k),p] +
    s[q,dvg(j)] - κ^2*s[q,p] == curlA[k]+ndotA[q]

Eq = @discretise a k∈X q∈Y j∈X p∈Y

u = solve(Eq)

uj = u[1:numfunctions(X)]
up = u[numfunctions(X)+1:end]

xrange = range(-2.0, stop=2.0, length=40)
# xrange = [0.0]
# yrange = [0.0]
zrange = range(-2.0, stop=2.0, length=40)
pts = [point(x,0,z) for x ∈ xrange, z ∈ zrange]

A1 = BEAST.DPVectorialTerm(1.0, 1.0*im)
A2 = BEAST.DPScalarTerm(1.0, 1.0*im)

p1 = potential(A1,pts,uj,X)
p2 = potential(A2,pts,up,Y)

p = p1 + p2


function BarycentricCoordinates(simplex1::AbstractArray{Float64, 2},
								simplex2::AbstractArray{Float64, 2},
                                    orientation1::Float64, orientation2::Float64,
                                    tolerance::Float64)
    n = size(simplex1, 1)

    numof1in2 = 0
    numof2in1 = 0

    convexexp1in2 = zeros(Float64, n+1, n+1)
    convexexp2in1 = zeros(Float64, n+1, n+1)

    ordered_vertices1 = collect(1:n+1)
    ordered_vertices2 = collect(1:n+1)

    ########################################
    # Simplex 1 with respect to simplex 2
    ########################################

    # Will contain the indices of the vertices of simplex1 that are contained in simplex2
    contained_vertices = zeros(Int, n + 1)

    for column1 = 1:n+1
        # Initialize convex expansion coefficient
        beta_min = 0.0

        for row2 = 1:n+1
            #tmp = copy(simplex2)
			tmp = similar(simplex2)
			tmp[:, :] = simplex2[:, :]
            tmp[:, row2] = simplex1[:, column1]

            beta = det([ones(1, n + 1); tmp]) / orientation2

            # If beta is close enough to zero, then the coefficient is set to zero
            if abs(beta) <= tolerance
                beta = 0.0
            end

            # If beta is close enough to one, then the coefficient is set to 1
            if abs(beta - 1) <= tolerance
                beta = 1.0
            end

            convexexp1in2[row2, column1] = beta

            if beta < beta_min
                beta_min = beta
            end
        end

        if beta_min >= 0
            numof1in2 += 1
            contained_vertices[numof1in2] = column1
        end
    end

    if numof1in2 > 0
        ordered_vertices1[1:numof1in2] = contained_vertices[1:numof1in2]

        if numof1in2 < n + 1
            ordered_vertices1[numof1in2+1:end] = complementary(ordered_vertices1[1:numof1in2], n + 1)
        end
    end

    ########################################
    # Simplex 2 with respect to simplex 1
    ########################################
    # Will contain the indices of the vertices of simplex1 that are contained in simplex2
    contained_vertices = zeros(Int, 1, n+1)

    for column2 = 1:n+1
        # Initialize convex expansion coefficient
        beta_min = 0

        for row1 = 1:n+1
            #tmp = copy(simplex1)
            tmp = similar(simplex1)
            tmp[:, :] = simplex1[:, :]
            tmp[:, row1] = simplex2[:, column2]

            beta = det([ones(1, n + 1); tmp]) / orientation1

            # If beta is close enough to zero, then the coefficient is set to zero
            if abs(beta) <= tolerance
                beta = 0
            end

            # If beta is close enough to one, then the coefficient is set to 1
            if abs(beta - 1) <= tolerance
                beta = 1
            end

            convexexp2in1[row1, column2] = beta

            if beta < beta_min
                beta_min = beta
            end
        end

        if beta_min >= 0
            numof2in1 = numof2in1 + 1
            contained_vertices[numof2in1] = column2
        end
    end

    if numof2in1 > 0
        ordered_vertices2[1:numof2in1] = contained_vertices[1:numof2in1]

        if numof2in1 < n + 1
            ordered_vertices2[numof2in1+1:end] = complementary(ordered_vertices2[1:numof2in1], n + 1)
        end
    end

    return convexexp1in2, convexexp2in1, ordered_vertices1, ordered_vertices2, numof1in2, numof2in1

end


using CUDAnative
# using Distributions
# using Distributions: zval, log2π

Base.@irrational log2π  1.8378770664093454836 log(big(2.)*π)
Base.@irrational sqrt2π 2.5066282746310005024 sqrt(big(π)*2.0)
# Base.@irrational invsqrt2π 0.3989422804014326779 inv(big(sqrt2π))

zval(μ::Real, σ::Real, x::Number) = (x - μ) / σ

# # pdf
# cunormpdf(z::Number) = CUDAnative.exp(-CUDAnative.abs2(z)/2) * invsqrt2π
# function cunormpdf(μ::Real, σ::Real, x::Number)
#     if iszero(σ)
#         if x == μ
#             z = zval(μ, one(σ), x)
#         else
#             z = zval(μ, σ, x)
#             σ = one(σ)
#         end
#     else
#         z = zval(μ, σ, x)
#     end
#     cunormpdf(z) / σ
# end

# logpdf
cunormlogpdf(x::Real) = -(x^2 + log2π)/2

function cunormlogpdf(p::Tuple{Real,Real}, x::Real)
    μ, σ = p
    if iszero(σ)
        if x == μ
            z = zval(μ, one(σ), x)
        else
            z = zval(μ, σ, x)
            σ = one(σ)
        end
    else
        z = zval(μ, σ, x)
    end
    cunormlogpdf(z) - CUDAnative.log(σ)
end


# second-order cone product; see: https://web.stanford.edu/~boyd/papers/ecos.html
function cone_product(u::AbstractVector{T}, v::AbstractVector{T}) where {T}
    [u' * v; u[1] * v[2:end] + v[1] * u[2:end]]
end

function cone_product(u::SVector{N,T}, v::SVector{N,T}) where {N,T}
    vcat(u' * v, u[1] * v[SVector{N-1}(2:end)] + v[1] * u[SVector{N-1}(2:end)])
end

function cone_product_jacobian(u::SVector{3,T}) where T
    SMatrix{3,3,T,9}(u[1], u[2], u[3], u[2], u[1], 0, u[3], 0, u[1])
end


#include("PrivateMultiplicativeWeights.jl")

#using PrivateMultiplicativeWeights

using LinearAlgebra

"""
return the lower bound for the "Query andWorkload Error"
    according to the article:
    "Ecient Batch Query Answering Under Dierential Privacy"
"""
function getLowerBound(queries::HistogramQueries, eps, delta)
    m,n = size(queries.queries)
    P_eps_delta = (2*log2(2/delta))/(eps^2)
    singular_values = svd(queries.queries).S
    ss_svd = (1/n)*(sum(singular_values)^2)

    ss_svd*P_eps_delta
end

"""
return the lower bound for the "Query andWorkload Error"
    according to the article:
    "Ecient Batch Query Answering Under Dierential Privacy"
"""
function getLowerBound(queries::RangeQueries, eps, delta)

    query_matrix = zeros(Int64, length(queries.intervals), queries.domain)

    for (index, interval) in enumerate(queries.intervals)
        query_matrix[index,:] = get_query_vector_from_interval(interval, queries.domain)
    end

    m,n = size(query_matrix)
    P_eps_delta = (2*log2(2/delta))/(eps^2)
    singular_values = svd(query_matrix).S
    ss_svd = (1/n)*(sum(singular_values)^2)

    ss_svd*P_eps_delta
end


"""
    isosurface(θ,T,Γ)

Depth of isosurface θ=T
"""
function isosurface(θ,T,Γ)
    d=NaN*similar(θ[:,1])
    nr=size(θ,2)
    for j=1:size(d,1)
        for k=1:nr-1
            i=findall(isnan.(d[j]).&(θ[j,k].>T).&(θ[j,k+1].<=T))
            a=(θ[j,k][i] .- T)./(θ[j,k][i] .- θ[j,k+1][i])
            d[j][i]=(1 .- a).*Γ.RC[k] + a.*Γ.RC[k+1]
            i=findall(isnan.(d[j]).&(θ[j,k].<=T).&(θ[j,k+1].>T))
            a=(θ[j,k+1][i] .- T)./(θ[j,k+1][i] .- θ[j,k][i])
            d[j][i]=(1 .- a).*Γ.RC[k+1] + a.*Γ.RC[k]
        end
    end
    return d
end


factorial2 = n -> n == 1 ? 1 : n * factorial2(n-1)
binomial2 = (n, p) -> sum(p .> rand(n))
pi2 = sum([x^2 + y^2 <= 1 for (x, y) in zip(rand(10000), rand(10000))])/10000*4

function pay()
  s = 0
  for i in 1:10
    1/2 > rand() ? s += 1 : s = 0
    s >= 3 && return true
  end
  return false
end

#println(pi2)
#println(pay())


nt = 1000
seed = 99
m = ABCDQR_simula(nt,seed)
y = m["y"]
u = m["u"]

#######################
print("Testing ABCDQR_em_starting: ")
wi = collect(range(0.1, length=2, stop=3))
Ai,Bi,Ci,Di,Qi,Ri,Si,m1i,P1i = ABCDQR_em_starting(y,u,wi,1,true)
if sum(Ai.^2) > 1e-6
    println("OK")
end


using SMC

function make_prior_proposal_smc_sampler(hmm::HiddenMarkovModel,
                                         observations::Vector{Int},
                                         num_particles::Int)
    function log_joint_evaluate(latent_sample::Vector{Int})
        return hmm_log_joint_probability(hmm, latent_sample, observations)
    end
    smc_scheme = HMMPriorSMCScheme(hmm, observations, num_particles)
    return SMCSampler(smc_scheme, log_joint_evaluate)
end

function make_optimal_proposal_smc_sampler(hmm::HiddenMarkovModel,
                                           observations::Vector{Int},
                                           num_particles::Int)
    function log_joint_evaluate(latent_sample::Vector{Int})
        return hmm_log_joint_probability(hmm, latent_sample, observations)
    end
    smc_scheme = HMMConditionalSMCScheme(hmm, observations, num_particles)
    return SMCSampler(smc_scheme, log_joint_evaluate)
end


#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
# This example demonstrates how to use Knitro to calculate a reentry
# trajectory for the space shuttle, which is a large and sparse
# nonlinear trajectory optimization problem.
#
# This example shows how to integrate multiple functionalities:
# - Definition of separate callbacks for constraint function
#   evaluation and constraint Jacobian evaluation;
# - Automatic detection of the Jacobian sparsity pattern;
# - Automatic differentiation for Jacobian evaluation;
# - Definition of lower and upper bounds for decision variables;
# - Setting an initial guess (for the primal values) based on a
#   linear interpolation to improve convergence speed;
# - Setup of an objective function with a linear structure;
# - Manual specification of solver user-options.
#
# This model is directly based on the optimal control example from
# chapter 6.1 in the book "Practical Methods for Optimal Control and
# Estimation Using Nonlinear Programming" by John T. Betts (2010).
#
# A companion video with a brief explanation of the formulation is
# available at https://youtu.be/fBY_yHkyU3A.
#+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

using Interpolations
using KNITRO
using SparseArrays
using SparseDiffTools
using SparsityDetection

# Global variables
const w  = 203000  # weight (lb)
const g₀ = 32.174  # acceleration (ft/sec^2)
const m  = w / g₀  # mass (slug)

# Aerodynamic and atmospheric forces on the vehicle
const ρ₀ =  0.002378
const hᵣ =  23800.0
const Rₑ =  20902900.0
const μ  =  0.14076539e17
const S  =  2690.0
const a₀ = -0.20704
const a₁ =  0.029244
const b₀ =  0.07854
const b₁ = -0.61592e-2
const b₂ =  0.621408e-3
const c₀ =  1.0672181
const c₁ = -0.19213774e-1
const c₂ =  0.21286289e-3
const c₃ = -0.10117249e-5

c_L(a) = a₀ + a₁ * rad2deg(a)
c_D(a) = b₀ + b₁ * rad2deg(a) + b₂ * rad2deg(a)^2

D(h, v, a) = 0.5 * c_D(a) * S * ρ(h) * v^2
L(h, v, a) = 0.5 * c_L(a) * S * ρ(h) * v^2

r(h) = Rₑ + h
g(h) = μ / r(h)^2
ρ(h) = ρ₀ * exp(-h / hᵣ)

# Aerodynamic heating on the vehicle wing leading edge
qₐ(a) = c₀ + c₁ * rad2deg(a) + c₂ * rad2deg(a)^2 + c₃ * rad2deg(a)^3
qᵣ(h, v) = 17700 * √ρ(h) * (0.0001 * v)^3.07
q(h, v, a) = qₐ(a) * qᵣ(h, v)

# Motion of the vehicle as a differential-algebraic system of equations (DAEs)
δh(v, γ) = v * sin(γ)
δϕ(h, θ, v, γ, ψ) = (v / r(h)) * cos(γ) * sin(ψ) / cos(θ)
δθ(h, v, γ, ψ) = (v / r(h)) * cos(γ) * cos(ψ)
δv(h, v, γ, α) = -(D(h, v, α) / m) - g(h) * sin(γ)
δγ(h, v, γ, α, β) = (L(h, v, α) / (m * v)) * cos(β) + cos(γ) * ((v / r(h)) - (g(h) / v))
δψ(h, θ, v, γ, ψ, α, β) = (1 / (m * v * cos(γ))) * L(h, v, α) * sin(β) + (v / (r(h) * cos(θ))) * cos(γ) * sin(ψ) * sin(θ)

# Initial conditions
hₛ = 2.6          # altitude (ft) / 1e5
ϕₛ = deg2rad(0)   # longitude (rad)
θₛ = deg2rad(0)   # latitude (rad)
vₛ = 2.56         # velocity (ft/sec) / 1e4
γₛ = deg2rad(-1)  # flight path angle (rad)
ψₛ = deg2rad(90)  # azimuth (rad)
αₛ = deg2rad(0)   # angle of attack (rad)
βₛ = deg2rad(0)   # banck angle (rad)
tₛ = 1.00         # time step (sec)

# Final conditions, the so-called Terminal Area Energy Management (TAEM)
hₜ = 0.8          # altitude (ft) / 1e5
vₜ = 0.25         # velocity (ft/sec) / 1e4
γₜ = deg2rad(-5)  # flight path angle (rad)

xₛ = [hₛ, ϕₛ, θₛ, vₛ, γₛ, ψₛ, αₛ, βₛ, tₛ]
xₜ = [hₜ, ϕₛ, θₛ, vₜ, γₜ, ψₛ, αₛ, βₛ, tₛ]

@show xₛ xₜ;

N = 2008  # number of segments
M = N + 1  # number of mesh points

# Linear initial guess between the boundary conditions
interp_linear = LinearInterpolation([1, M], [xₛ, xₜ])
initial_guess = [value for x = 1:M for value in interp_linear(x)]

nₓ = length(xₛ)  # mesh points dimension
n = nₓ * M  # number of decision variables
@assert n == length(initial_guess)

ind_h = 1:nₓ:n  # indices of the `h` decision variables
ind_ϕ = 2:nₓ:n  # indices of the `ϕ` decision variables
ind_θ = 3:nₓ:n  # indices of the `θ` decision variables
ind_v = 4:nₓ:n  # indices of the `v` decision variables
ind_γ = 5:nₓ:n  # indices of the `γ` decision variables
ind_ψ = 6:nₓ:n  # indices of the `ψ` decision variables
ind_α = 7:nₓ:n  # indices of the `α` decision variables
ind_β = 8:nₓ:n  # indices of the `β` decision variables
ind_t = 9:nₓ:n  # indices of the `t` decision variables
@assert length(ind_h) == length(ind_v) == length(ind_α) == M

m_dyn = 6 * N
ind_con_dyn = 1:m_dyn  # indices of the dynamics' constraints (defects)
@assert length(ind_con_dyn) == 6 * (M - 1)

EulerStep(x, δx, h) = x + h*δx

function dynamics_defects!(dx, x)
    hᵢ, ϕᵢ, θᵢ, vᵢ, γᵢ, ψᵢ, αᵢ, βᵢ, tᵢ, hⱼ, ϕⱼ, θⱼ, vⱼ, γⱼ, ψⱼ = x

    # Unit correction due to decision variable scaling
    hᵢ *= 1e5; hⱼ *= 1e5; vᵢ *= 1e4; vⱼ *= 1e4

    dx[1] = EulerStep(hᵢ, δh(        vᵢ, γᵢ            ), tᵢ) - hⱼ
    dx[2] = EulerStep(ϕᵢ, δϕ(hᵢ, θᵢ, vᵢ, γᵢ, ψᵢ        ), tᵢ) - ϕⱼ
    dx[3] = EulerStep(θᵢ, δθ(hᵢ,     vᵢ, γᵢ, ψᵢ        ), tᵢ) - θⱼ
    dx[4] = EulerStep(vᵢ, δv(hᵢ,     vᵢ, γᵢ,     αᵢ    ), tᵢ) - vⱼ
    dx[5] = EulerStep(γᵢ, δγ(hᵢ,     vᵢ, γᵢ,     αᵢ, βᵢ), tᵢ) - γⱼ
    dx[6] = EulerStep(ψᵢ, δψ(hᵢ, θᵢ, vᵢ, γᵢ, ψᵢ, αᵢ, βᵢ), tᵢ) - ψⱼ

    return nothing
end

function cb_eval_fc_con_dyn(kc, cb, evalRequest, evalResult, userParams)
    x = evalRequest.x

    for i = 0:M - 2
        ind_xᵢ = (1:nₓ) .+ i * nₓ
        ind_xⱼ = ind_xᵢ .+ nₓ
        ind_con = (1:6) .+ i * 6

        xᵢ = x[ind_xᵢ][1:9]
        xⱼ = x[ind_xⱼ][1:6]

        @views dynamics_defects!(evalResult.c[ind_con], [xᵢ; xⱼ])
    end

    return 0
end

function cb_eval_ga_con_dyn(kc, cb, evalRequest, evalResult, userParams)
    x = evalRequest.x
    jac_data = userParams

    for i = 0:M - 2
        ind_xᵢ = (1:nₓ) .+ i * nₓ
        ind_xⱼ = ind_xᵢ .+ nₓ
        ind_con = (1:6) .+ i * 6

        xᵢ = x[ind_xᵢ][1:9]
        xⱼ = x[ind_xⱼ][1:6]

        forwarddiff_color_jacobian!(jac_data.jac_dyn, dynamics_defects!, [xᵢ; xⱼ], jac_data.jac_cache)

        aux_jac = i * jac_data.length_jac
        ind_jac = aux_jac + 1 : aux_jac + jac_data.length_jac
        evalResult.jac[ind_jac] = nonzeros(jac_data.jac_dyn)
    end

    return 0
end

struct JacobianData{T1 <: SparseMatrixCSC,T2 <: ForwardColorJacCache}
    jac_dyn::T1
    jac_cache::T2
    length_jac::Int

    function JacobianData(;
                          input = rand(15), output = zeros(6))
        sparsity_pattern = jacobian_sparsity(dynamics_defects!, output, input)
        jac_dyn = convert.(Float64, sparse(sparsity_pattern))

        jac_cache = ForwardColorJacCache(dynamics_defects!, input, dx = output,
                                         colorvec = matrix_colors(jac_dyn),
                                         sparsity = sparsity_pattern)

        length_jac = nnz(jac_dyn)

        T1 = typeof(jac_dyn)
        T2 = typeof(jac_cache)

        new{T1,T2}(jac_dyn, jac_cache, length_jac)
    end
end

input = rand(15)
jac_data = JacobianData(input = input)
forwarddiff_color_jacobian!(jac_data.jac_dyn, dynamics_defects!, input, jac_data.jac_cache)

jacIndexConsCB = Cint.(hcat([rowvals(jac_data.jac_dyn) .+ i*6 for i = 0:N-1]...) .- 1)
jacIndexVarsCB = Cint.(hcat([vcat([fill(j + i*nₓ, length(nzrange(jac_data.jac_dyn, j))) for j = 1:15]...) for i = 0:N-1]...) .- 1)

# Note: the code above this point formulates the problem, and implements
# callbacks in a format compatible with Knitro's interface, but the
# actual use of Knitro only starts from this point onwards.

lm = KNITRO.LMcontext()
kc = KNITRO.KN_new_lm(lm)

KNITRO.KN_add_vars(kc, n)

!isempty(initial_guess) && KNITRO.KN_set_var_primal_init_values(kc, initial_guess)

KNITRO.KN_set_var_lobnds(kc, collect(Cint, ind_h .- 1), zeros(M))  # `h` bounds
KNITRO.KN_set_var_lobnds(kc, collect(Cint, ind_v .- 1), fill(1 / 1e4, M))  # `v` bounds

KNITRO.KN_set_var_lobnds(kc, collect(Cint, ind_θ .- 1), fill(deg2rad(-89), M))  # `θ` lower bounds
KNITRO.KN_set_var_upbnds(kc, collect(Cint, ind_θ .- 1), fill(deg2rad( 89), M))  # `θ` upper bounds

KNITRO.KN_set_var_lobnds(kc, collect(Cint, ind_γ .- 1), fill(deg2rad(-89), M))  # `γ` lower bounds
KNITRO.KN_set_var_upbnds(kc, collect(Cint, ind_γ .- 1), fill(deg2rad( 89), M))  # `γ` upper bounds

KNITRO.KN_set_var_lobnds(kc, collect(Cint, ind_α .- 1), fill(deg2rad(-90), M))  # `α` lower bounds
KNITRO.KN_set_var_upbnds(kc, collect(Cint, ind_α .- 1), fill(deg2rad( 90), M))  # `α` upper bounds

KNITRO.KN_set_var_lobnds(kc, collect(Cint, ind_β .- 1), fill(deg2rad(-89), M))  # `β` lower bounds
KNITRO.KN_set_var_upbnds(kc, collect(Cint, ind_β .- 1), fill(deg2rad(  1), M))  # `β` upper bounds

KNITRO.KN_set_var_fxbnds(kc, collect(Cint, ind_t .- 1), fill(1.00, M))  # Fix time steps

# Fix initial and final conditions
ind_fixed_vars = [ind_h[1]:ind_ψ[1] ; [ind_h[end], ind_v[end], ind_γ[end]]]
val_fixed_vars = [hₛ, ϕₛ, θₛ, vₛ, γₛ, ψₛ, hₜ, vₜ, γₜ]
KNITRO.KN_set_var_fxbnds(kc, collect(Cint, ind_fixed_vars .- 1), val_fixed_vars)

KNITRO.KN_add_cons(kc, m_dyn)

KNITRO.KN_set_con_eqbnds(kc, collect(Cint, ind_con_dyn .- 1), zeros(m_dyn))  # defects

# This callback does not evaluate the objective function. As such,
# we pass `false` as the second argument to `KN_add_eval_callback()`.
cb_dyn = KNITRO.KN_add_eval_callback(kc, false, collect(Cint, ind_con_dyn .- 1), cb_eval_fc_con_dyn)

# Similarly to above, this callback does not evaluate the gradient of the objective.
# As such, we pass `nV = 0`, and `objGradIndexVars = C_NULL` in the `KN_set_cb_grad()` call.
KNITRO.KN_set_cb_grad(kc, cb_dyn, cb_eval_ga_con_dyn,
                      nV = 0, objGradIndexVars = C_NULL,
                      jacIndexCons = jacIndexConsCB,
                      jacIndexVars = jacIndexVarsCB)

KNITRO.KN_set_cb_user_params(kc, cb_dyn, jac_data)

# Add objective
objIndex, objCoef = ind_θ[end], 1.0
KNITRO.KN_add_obj_linear_struct(kc, objIndex - 1, objCoef)
KNITRO.KN_set_obj_goal(kc, KNITRO.KN_OBJGOAL_MAXIMIZE)

KNITRO.KN_set_param(kc, KNITRO.KN_PARAM_ALG,        KNITRO.KN_ALG_BAR_DIRECT)
KNITRO.KN_set_param(kc, KNITRO.KN_PARAM_BAR_MURULE, KNITRO.KN_BAR_MURULE_QUALITY)
KNITRO.KN_set_param(kc, KNITRO.KN_PARAM_LINSOLVER,  KNITRO.KN_LINSOLVER_MA27)

KNITRO.KN_set_param(kc, KNITRO.KN_PARAM_HESSOPT, KNITRO.KN_HESSOPT_LBFGS)
KNITRO.KN_set_param(kc, KNITRO.KN_PARAM_LMSIZE, 5)  # limited-memory pairs stored

KNITRO.KN_solve(kc)
nStatus, objSol, x, lambda_ = KNITRO.KN_get_solution(kc)
cpu_time = KNITRO.KN_get_solve_time_real(kc)

KNITRO.KN_free(kc)
KNITRO.KN_release_license(lm)

print("Final latitude θ = $(round(rad2deg(x[ind_θ[end]]), digits = 2))°")


using ADCME
using AdFem

include("../chip_unstructured_solver.jl")
include("../chip_unstructured_geometry.jl")

k_mold = 0.014531
k_chip_ref = 2.60475
k_air = 0.64357

function k_exact(x, y)
    k_mold + 1000 * k_chip_ref * (x-0.49)^2 / (1 + x^2)
end

nu = 0.47893  # equal to 1/Re
power_source = 82.46295  #82.46295 = 1.0e6 divide by air rho cp   #0.0619 = 1.0e6 divide by chip die rho cp
buoyance_coef = 299102.83

u_std = 0.001
p_std = 0.000001225
T_infty = 300

NT = 15    # number of iterations for Newton's method

xy = mesh.nodes
xy2 = zeros(mesh.nedge, 2)
for i = 1:mesh.nedge
    xy2[i,:] = (mesh.nodes[mesh.edges[i,1], :] + mesh.nodes[mesh.edges[i,2], :])/2
end
xy = [xy;xy2]

x, y = xy[chip_fem_idx, 1], xy[chip_fem_idx, 2]
k_chip_exact = constant( @. k_exact(x, y) )
k_chip = k_chip_exact .+  Variable(zeros(size(chip_fem_idx, 1)))

heat_source_fem = zeros(ndof)
heat_source_fem[chip_fem_top_idx] .= power_source
heat_source_gauss = dof_to_gauss_points(heat_source_fem, mesh)
heat_source = compute_fem_source_term1(constant(heat_source_gauss), mesh)

B = constant(compute_interaction_matrix(mesh))
Laplace = constant(compute_fem_laplace_matrix1(nu * constant(ones(ngauss)), mesh))

# apply Dirichlet to velocity and temperature; set left bottom two points to zero to fix rank deficient problem for pressure
bd = [bd; bd .+ ndof;
      fvm_bd .+ 2*ndof;
      bd .+ (2*ndof+nelem)]

# add solid region into boundary condition for u, v, p, i.e. exclude solid when solving Navier Stokes
bd = [bd; solid_fem_idx; solid_fem_idx .+ ndof; solid_fvm_idx .+ 2*ndof]

S0 = constant(zeros(nelem+3*ndof))
S_data = matread("../xz_chip_unstructured_data2.mat")["V"]

S = solve_navier_stokes(S0, NT, k_chip)
S_computed = S[end, :]

loss =  mean((S_computed .- S_data)^2)
loss = loss * 1e10

# ---------------------------------------------------
_loss = Float64[]
cb = (vs, iter, loss)->begin
    global _loss
    push!(_loss, loss)
    printstyled("[#iter $iter] loss=$loss\n", color=:green)
    if mod(iter, 10)==1
        matwrite("loss$iter.mat", Dict("iter"=>iter,"loss"=>loss, "k_chip"=>vs[1]))
    end
end

sess = Session(); init(sess)
loss = BFGS!(sess, loss, vars = [k_chip], callback = cb)


using PyPlot
using LinearAlgebra
using SparseArrays
using Arpack
using ForwardDiff
using BenchmarkTools
using Formatting
using FileIO

#readfile
println("opening file... \n")
a = open("../programing/efelkismos-simple.unv","r")
#include("readfile.jl") #returns array of: nodes, edge, elem, bc
include("example-mesh.jl")
include("elastic_functions.jl") #defines basic functions

#start building global array
n = length(elem[:,1])
dofs = 2*length(nodes[:,1])
global E = Ematrix(220e9,0.5,"stress")
global K = sparse([],[],Float64[],dofs,dofs);
global M = sparse([],[],Float64[],dofs,dofs);

println("calculating stiffness/mass matrix...\n")
for i =1:n
    i1 = elem[i,:]'
    xs = nodes[i1,1]
    ys = nodes[i1,2]
    i1 = [i1 (i1 .+ Int64(0.5*dofs))]
    kre = ke(E,xs,ys)
    mre = 7e3 .* me(xs,ys)
    K[i1,i1] = K[i1,i1][1,:,1,:] + kre
    M[i1,i1] = M[i1,i1][1,:,1,:] + mre
end

include("rhs.jl") #calculate F

#gib traction nodes where (u,v) are known
#reminder, ix contains boundary condition nodes INDEXES
println("solving for U....\n")
freedofs = zeros(dofs,1)
fixedofs = ix[1][:]'#take first's b.c. nodes

fixedofs = [fixedofs[1] (fixedofs[1] .+Int64(0.5*dofs))]
freedofs[fixedofs] .= 1
freedofs = [i for i=1:dofs if freedofs[i]==0]

U = zeros(dofs,1)
rhs = F
rhs = rhs[freedofs] - K[freedofs,fixedofs'][:,:,1]*U[fixedofs']
Kfree = K[freedofs,freedofs]
Mfree = M[freedofs,freedofs]

Ufree = U[freedofs]
U[freedofs] = Kfree\rhs

n = length(elem[:,1])

println("calculating element average stress....\n")
#calculate element stress
include("stress.jl")
#println(stress')
println("DONE! \n")

i1 = elem[2,:]'
xs = nodes[i1,1]
ys = nodes[i1,2]

println(E*LN([0 0 1],xs,ys)*U[[ i1 (i1 .+ Int64(0.5*dofs))]]')
display(stress[:,1:3:18])
xs = [1 0 0 0.5 0 0.5]
ys = [0 -.5 .5 -.25 0 .25]
l = [1 0 0]

#=
F = zeros(12,1)
F[1] = 1e6
freedofs = ones(1,12)
fixedofs = [5 ]
freedofs[fixedofs] .= 0
freedofs = freedofs .==1
freedofs = freedofs[:]
kre = ke(E,xs,ys)

U  = zeros(12,1)
Ufree = kre[freedofs,freedofs]\F[freedofs]
U[freedofs] .= Ufree
println(E*LN([2 1 1]./4,xs,ys)*U)
figure(11)
for l1 = 0.:0.01:1.
    l2 = 0.5*(1-l1)
    l3 = l2
    sigma = E*LN([l1 l2 l3],xs,ys)*U
    scatter([l1],[sigma[1]/1e6])
    #scatter!([l1],[sigma[2]/1e6],legend=false,label="sy")
    #scatter!([l1],[sigma[3]/1e6],legend=false,label="txy")
end

xs = nodes[elem[2,:],1]
ys = nodes[elem[2,:],2]
xs = xs' ; ys  = ys';
g(x) = f(x)[1]
l = [1 1 0]
print(BC(g,xs,ys,l)')
=#


# Translation of minpack subroutine cstep
# Dianne O'Leary   July 1991
#
# Subroutine cstep
#
# The purpose of cstep is to compute a safeguarded step for
# a linesearch and to update an interval of uncertainty for
# a minimizer of the function.
#
# The parameter stx contains the step with the least function
# value. The parameter stp contains the current step. It is
# assumed that the derivative at stx is negative in the
# direction of the step. If bracketed is set true then a
# minimizer has been bracketed in an interval of uncertainty
# with endpoints stx and sty.
#
# The subroutine statement is
#
# subroutine cstep(stx, fx, dgx,
#                  sty, fy, dgy,
#                  stp, f, dp,
#                  bracketed, stpmin, stpmax, info)
#
# where
#
# stx, fx, and dgx are variables which specify the step,
#   the function, and the derivative at the best step obtained
#   so far. The derivative must be negative in the direction
#   of the step, that is, dgx and stp-stx must have opposite
#   signs. On output these parameters are updated appropriately
#
# sty, fy, and dgy are variables which specify the step,
#   the function, and the derivative at the other endpoint of
#   the interval of uncertainty. On output these parameters are
#   updated appropriately
#
# stp, f, and dp are variables which specify the step,
#   the function, and the derivative at the current step.
#   If bracketed is set true then on input stp must be
#   between stx and sty. On output stp is set to the new step
#
# bracketed is a logical variable which specifies if a minimizer
#   has been bracketed. If the minimizer has not been bracketed
#   then on input bracketed must be set false. If the minimizer
#   is bracketed then on output bracketed is set true
#
# stpmin and stpmax are input variables which specify lower
#   and upper bounds for the step
#
# info is an integer output variable set as follows:
#   If info = 1,2,3,4,5, then the step has been computed
#   according to one of the five cases below. Otherwise
#   info = 0, and this indicates improper input parameters
#
# Argonne National Laboratory. MINPACK Project. June 1983
# Jorge J. More', David J. Thuente

function cstep(stx::Real, fx::Real, dgx::Real,
               sty::Real, fy::Real, dgy::Real,
               stp::Real, f::Real, dg::Real,
               bracketed::Bool, stpmin::Real, stpmax::Real)

   info = 0

   #
   # Check the input parameters for error
   #

   if (bracketed && (stp <= min(stx, sty) || stp >= max(stx, sty))) ||
        dgx * (stp - stx) >= 0.0 || stpmax < stpmin
      throw(ArgumentError("Minimizer not bracketed"))
   end

   #
   # Determine if the derivatives have opposite sign
   #

   sgnd = dg * (dgx / abs(dgx))

   #
   # First case. A higher function value.
   # The minimum is bracketed. If the cubic step is closer
   # to stx than the quadratic step, the cubic step is taken,
   # else the average of the cubic and quadratic steps is taken
   #

   if f > fx
      info = 1
      bound = true
      theta = 3 * (fx - f) / (stp - stx) + dgx + dg
      s = max(theta, dgx, dg)
      gamma = s * sqrt((theta / s)^2 - (dgx / s) * (dg / s))
      if stp < stx
          gamma = -gamma
      end
      p = gamma - dgx + theta
      q = gamma - dgx + gamma + dg
      r = p / q
      stpc = stx + r * (stp - stx)
      stpq = stx + ((dgx / ((fx - f) / (stp - stx) + dgx)) / 2) * (stp - stx)
      if abs(stpc - stx) < abs(stpq - stx)
         stpf = stpc
      else
         stpf = stpc + (stpq - stpc) / 2
      end
      bracketed = true

   #
   # Second case. A lower function value and derivatives of
   # opposite sign. The minimum is bracketed. If the cubic
   # step is closer to stx than the quadratic (secant) step,
   # the cubic step is taken, else the quadratic step is taken
   #

   elseif sgnd < 0.0
      info = 2
      bound = false
      theta = 3 * (fx - f) / (stp - stx) + dgx + dg
      s = max(theta, dgx, dg)
      gamma = s * sqrt((theta / s)^2 - (dgx / s) * (dg / s))
      if stp > stx
         gamma = -gamma
      end
      p = gamma - dg + theta
      q = gamma - dg + gamma + dgx
      r = p / q
      stpc = stp + r * (stx - stp)
      stpq = stp + (dg / (dg - dgx)) * (stx - stp)
      if abs(stpc - stp) > abs(stpq - stp)
         stpf = stpc
      else
         stpf = stpq
      end
      bracketed = true

   #
   # Third case. A lower function value, derivatives of the
   # same sign, and the magnitude of the derivative decreases.
   # The cubic step is only used if the cubic tends to infinity
   # in the direction of the step or if the minimum of the cubic
   # is beyond stp. Otherwise the cubic step is defined to be
   # either stpmin or stpmax. The quadratic (secant) step is also
   # computed and if the minimum is bracketed then the the step
   # closest to stx is taken, else the step farthest away is taken
   #

   elseif abs(dg) < abs(dgx)
      info = 3
      bound = true
      theta = 3 * (fx - f) / (stp - stx) + dgx + dg
      s = max(theta, dgx, dg)
      #
      # The case gamma = 0 only arises if the cubic does not tend
      # to infinity in the direction of the step
      #
      gamma = s * sqrt(max(0.0, (theta / s)^2 - (dgx / s) * (dg / s)))
      if stp > stx
          gamma = -gamma
      end
      p = gamma - dg + theta
      q = gamma + dgx - dg + gamma
      r = p / q
      if r < 0.0 && gamma != 0.0
         stpc = stp + r * (stx - stp)
      elseif stp > stx
         stpc = stpmax
      else
         stpc = stpmin
      end
      stpq = stp + (dg / (dg - dgx)) * (stx - stp)
      if bracketed
         if abs(stp - stpc) < abs(stp - stpq)
            stpf = stpc
         else
            stpf = stpq
         end
      else
         if abs(stp - stpc) > abs(stp - stpq)
            stpf = stpc
         else
            stpf = stpq
         end
      end

   #
   # Fourth case. A lower function value, derivatives of the
   # same sign, and the magnitude of the derivative does
   # not decrease. If the minimum is not bracketed, the step
   # is either stpmin or stpmax, else the cubic step is taken
   #

   else
      info = 4
      bound = false
      if bracketed
         theta = 3 * (f - fy) / (sty - stp) + dgy + dg
         s = max(theta, dgy, dg)
         gamma = s * sqrt((theta / s)^2 - (dgy / s) * (dg / s))
         if stp > sty
             gamma = -gamma
         end
         p = gamma - dg + theta
         q = gamma - dg + gamma + dgy
         r = p / q
         stpc = stp + r * (sty - stp)
         stpf = stpc
      elseif stp > stx
         stpf = stpmax
      else
         stpf = stpmin
      end
   end

   #
   # Update the interval of uncertainty. This update does not
   # depend on the new step or the case analysis above
   #

   if f > fx
      sty = stp
      fy = f
      dgy = dg
   else
      if sgnd < 0.0
         sty = stx
         fy = fx
         dgy = dgx
      end
      stx = stp
      fx = f
      dgx = dg
   end

   #
   # Compute the new step and safeguard it
   #

   stpf = min(stpmax, stpf)
   stpf = max(stpmin, stpf)
   stp = stpf
   if bracketed && bound
      if sty > stx
         stp = min(stx + 0.66 * (sty - stx), stp)
      else
         stp = max(stx + 0.66 * (sty - stx), stp)
      end
   end

   return stx, fx, dgx, sty, fy, dgy, stp, f, dg, bracketed, info
end


# Fractional Integration --
# Author: Ziyi Yin, ziyi.yin@gatech.edu
# Date: July 2020

export apply_frac_integral, frac_int

###############################################################################
# Fractional integration

function frac_int(nt::Int,order::Number;DDT=Float32)
	# fractional integration for a trace
	x    = zeros(Int64,nt,1);
	x[1] = 1;
	y    = convert(Array{DDT,2},fgl_deriv(-order,x,1));	# fractional integration
	C    = joConvolve(nt,1,y[:];DDT=DDT,RDT=DDT);
	return C
end

function apply_frac_integral(C::joLinearFunction{ADDT,ARDT},d_obs::judiVector{avDT}) where {ADDT,ARDT,avDT}
    # fractional integration based on C
    C.n == size(d_obs.data[1],1) || throw(judiVectorException("shape mismatch"))
    jo_check_type_match(ADDT,avDT,join(["DDT for *(joLinearFunction,judiWeights):",C.name,typeof(C),avDT]," / "))
	d_out = deepcopy(d_obs)
	for i = 1:d_obs.nsrc
		d_out.data[i] = C*d_obs.data[i]
	end
	return d_out
end


# some tests for spa

# data

Random.seed!(5678)

p = 15
n = 8
k = 2

# Matrices
for T in (Float64, Float32)
    # Initialization test
    ϵ = eps(T)^(1/4)
    Wg = max.(rand(T, p, k) .- T(0.3), T(ϵ))
    Hg = max.(rand(T, k, n) .- T(0.3), T(ϵ))
    X = Wg * Hg
    w, h = NMF.spa(X, k)
    x = w * h
    @test all(w .>= zero(T))
    @test all(h .>= zero(T))
    @test x ≈ X atol=10.0*ϵ
    #println("ϵ =",ϵ," while ||x .- X||= ",maximum(abs.(x .- X)))

    # Separability test
    Wg, Hg = NMF.separable_data(p, n, k)
    X = Wg * Hg
    w, h = NMF.spa(X, k)
    x = w * h
    @test all(w .>= zero(T))
    @test all(h .>= zero(T))
    @test sqL2dist(X, x) < eps(T)
end


module EmpiricalModeDecomposition

using AbstractFFTs
using Dierckx
using Distributions
using FFTW
using ForwardDiff
using LinearAlgebra
using Random
using Statistics

include("mode-decompositions/emd.jl")
include("mode-decompositions/eemd.jl")
include("extras/hilbert.jl")

export EMDSetting, emd, EEMDSetting, eemd, hilbert_transform, hht

end # module


struct ThresholdExceedance <: EVA
    data::Variable
    logscale::paramfun
    shape::paramfun
end

"""
    ThresholdExceedance(exceedances::Vector{<:Real};
        logscalecov::Vector{<:DataItem} = Vector{Variable}(),
        shapecov::Vector{<:DataItem} = Vector{Variable}())::ThresholdExceedance

Creates a ThresholdExceedance structure.

"""
function ThresholdExceedance(exceedances::Variable;
    logscalecov::Vector{<:DataItem} = Vector{Variable}(),
    shapecov::Vector{<:DataItem} = Vector{Variable}())::ThresholdExceedance

    n = length(exceedances.value)
    validatelength(n, logscalecov)
    validatelength(n, shapecov)

    logscalefun = computeparamfunction(logscalecov)
    shapefun = computeparamfunction(shapecov)

    return ThresholdExceedance(exceedances, paramfun(logscalecov, logscalefun), paramfun(shapecov, shapefun))

end

function paramindex(model::ThresholdExceedance)::Dict{Symbol,Vector{<:Int}}

    sϕ = length(model.logscale.covariate) + 1
    sξ = length(model.shape.covariate) + 1

    return Dict{Symbol,Vector{<:Int}}(
        :ϕ => Int64[k for k in (1:sϕ)],
        :ξ => Int64[k for k in .+ sϕ .+ (1:sξ)]
    )

end

function getcovariatenumber(model::ThresholdExceedance)::Int

    return sum([length(model.logscale.covariate), length(model.shape.covariate)])

end

function nparameter(model::ThresholdExceedance)::Int

    return 2 + getcovariatenumber(model)

end

function getdistribution(model::ThresholdExceedance, θ::AbstractVector{<:Real})::Vector{<:Distribution}

    @assert length(θ)==nparameter(model) "The length of the parameter vector should be equal to the model number of parameters."

    pi = paramindex(model)
    ϕ = model.logscale.fun(θ[pi[:ϕ]])
    ξ = model.shape.fun(θ[pi[:ξ]])

    σ = exp.(ϕ)

    fd = GeneralizedPareto.(σ, ξ)

    return fd

end

function getinitialvalue(model::ThresholdExceedance)::Vector{<:Real}

    y = model.data.value

    # Compute stationary initial values
    ϕ₀,ξ₀ = getinitialvalue(GeneralizedPareto, y)
    # Store them in a dictionary
    θ₀ = Dict(:ϕ => ϕ₀, :ξ => ξ₀)

    initialvalues = zeros(nparameter(model))
    pi = paramindex(model)
    for param in [:ϕ, :ξ]
        ind = pi[param][1]
        initialvalues[ind] = θ₀[param]
    end

    return initialvalues

end

"""
    showEVA(io::IO, obj::ThresholdExceedance; prefix::String = "")

Displays a ThresholdExceedance with the prefix `prefix` before every line.

"""
function showEVA(io::IO, obj::ThresholdExceedance; prefix::String = "")

    println(io, prefix, "ThresholdExceedance")
    println(io, prefix, "data :\t\t",typeof(obj.data.value), "[", length(obj.data.value), "]")
    println(io, prefix, "logscale :\t", showparamfun("ϕ", obj.logscale))
    println(io, prefix, "shape :\t\t", showparamfun("ξ", obj.shape))

end


using Random

"""
Approximately solve the QAP through random sampling.
"""
function qap_random(A, B, obj; iter=-1)
	@assert obj in [:min, :max]
	N = size(A, 1)

	iter = (iter == -1) ? 10N : iter

	best_sol = ((obj == :min) ? Inf : -Inf, nothing)

	for i in 1:iter
		perm = randperm(N)
		s = sum(A .* B[perm, perm])

		if obj == :min && s < best_sol[1]
			best_sol = (s, perm)
		elseif obj == :max && s > best_sol[1]
			best_sol = (s, perm)
		end
	end

	perm = best_sol[2]
	P = Matrix(I, N, N)[perm,:]

	return P, perm
end