using Base.Math: @horner

# Compute the sine integral: ∫_0^x sin(t)/t dt,
# and the cosine integral: γ + log x + ∫_0^x (cos(t)-1)/t dt,
# using the rational approximants tabulated in:
#     A.J. MacLeod, "Rational approximations, software and test methods for
#     sine and cosine integrals," Numer. Algor. 12, pp. 259--272 (1996).
#         http://dx.doi.org/10.1007/BF02142806
#         https://link.springer.com/article/10.1007/BF02142806
#
# Note: the second zero of Ci(x) has a typo that is fixed:
#
# r1 = 3.38418 04228 51186 42639 78511 46402 in the article, but is in fact:
#
# r1 = 3.38418 04225 51186 42639 78511 46402.
#

function sinint(x::Float64)
    t = x*x
    if t ≤ 36.0
        return x * @horner(t, 1.00000000000000000000E0,
                             -0.44663998931312457298E-1,
                              0.11209146443112369449E-2,
                             -0.13276124407928422367E-4,
                              0.85118014179823463879E-7,
                             -0.29989314303147656479E-9,
                              0.55401971660186204711E-12,
                             -0.42406353433133212926E-15) /
                   @horner(t, 1.00000000000000000000E0,
                              0.10891556624243098264E-1,
                              0.59334456769186835896E-4,
                              0.21231112954641805908E-6,
                              0.54747121846510390750E-9,
                              0.10378561511331814674E-11,
                              0.13754880327250272679E-14,
                              0.10223981202236205703E-17)
    elseif t ≤ 144.0
        invt = inv(t)
        return copysign(π/2, x) - cos(x) *
                                         @horner(invt, 0.99999999962173909991E0,
                                                       0.36451060338631902917E3,
                                                       0.44218548041288440874E5,
                                                       0.22467569405961151887E7,
                                                       0.49315316723035561922E8,
                                                       0.43186795279670283193E9,
                                                       0.11847992519956804350E10,
                                                       0.45573267593795103181E9) /
                                    (x * @horner(invt, 1.00000000000000000000E0,
                                                       0.36651060273229347594E3,
                                                       0.44927569814970692777E5,
                                                       0.23285354882204041700E7,
                                                       0.53117852017228262911E8,
                                                       0.50335310667241870372E9,
                                                       0.16575285015623175410E10,
                                                       0.11746532837038341076E10)) -
                           sin(x)*invt * @horner(invt, 0.99999999920484901956E0,
                                                       0.51385504875307321394E3,
                                                       0.92293483452013810811E5,
                                                       0.74071341863359841727E7,
                                                       0.28142356162841356551E9,
                                                       0.49280890357734623984E10,
                                                       0.35524762685554302472E11,
                                                       0.79194271662085049376E11,
                                                       0.17942522624413898907E11) /
                                         @horner(invt, 1.00000000000000000000E0,
                                                       0.51985504708814870209E3,
                                                       0.95292615508125947321E5,
                                                       0.79215459679762667578E7,
                                                       0.31977567790733781460E9,
                                                       0.62273134702439012114E10,
                                                       0.54570971054996441467E11,
                                                       0.18241750166645704670E12,
                                                       0.15407148148861454434E12)
    elseif t < Inf
        invt = inv(t)
        return copysign(π/2, x) - cos(x) / x * (1.0 -
                                                @horner(invt, 0.19999999999999978257E1,
                                                              0.22206119380434958727E4,
                                                              0.84749007623988236808E6,
                                                              0.13959267954823943232E9,
                                                              0.10197205463267975592E11,
                                                              0.30229865264524075951E12,
                                                              0.27504053804288471142E13,
                                                              0.21818989704686874983E13) /
                                                @horner(invt, 1.00000000000000000000E0,
                                                              0.11223059690217167788E4,
                                                              0.43685270974851313242E6,
                                                              0.74654702140658116258E8,
                                                              0.58580034751805687471E10,
                                                              0.20157980379272098841E12,
                                                              0.26229141857684496445E13,
                                                              0.87852907334918467516E13)*invt) -
                         sin(x) * invt * (1.0 - @horner(invt, 0.59999999999999993089E1,
                                                              0.96527746044997139158E4,
                                                              0.56077626996568834185E7,
                                                              0.15022667718927317198E10,
                                                              0.19644271064733088465E12,
                                                              0.12191368281163225043E14,
                                                              0.31924389898645609533E15,
                                                              0.25876053010027485934E16,
                                                              0.12754978896268878403E16) /
                                                @horner(invt, 1.00000000000000000000E0,
                                                              0.16287957674166143196E4,
                                                              0.96636303195787870963E6,
                                                              0.26839734750950667021E9,
                                                              0.37388510548029219241E11,
                                                              0.26028585666152144496E13,
                                                              0.85134283716950697226E14,
                                                              0.11304079361627952930E16,
                                                              0.42519841479489798424E16)*invt)
    elseif isnan(x)
        return NaN
    else
        return copysign(π/2, x)
    end
end

function cosint(x::Float64)
    t, r0, r1 = x*x, 0.616505485620716233797110404100, 3.384180422551186426397851146402
    r01, r02 = 0.6162109375, 0.29454812071623379711E-3
    r11, r12 = 3.3837890625, 0.39136005118642639785E-3
    if x < 0.0
        throw(DomainError(x, "`x` must be nonnegative."))
    elseif x ≤ 3.0
        return log(x/r0) + ((x - r01) - r02) * (x + r0) *
                                       @horner(t, -0.24607411378767540707E0,
                                                   0.72113492241301534559E-2,
                                                  -0.11867127836204767056E-3,
                                                   0.90542655466969866243E-6,
                                                  -0.34322242412444409037E-8,
                                                   0.51950683460656886834E-11) /
                                        @horner(t, 1.00000000000000000000E0,
                                                   0.12670095552700637845E-1,
                                                   0.78168450570724148921E-4,
                                                   0.29959200177005821677E-6,
                                                   0.73191677761328838216E-9,
                                                   0.94351174530907529061E-12)
    elseif x ≤ 6.0
        return log(x/r1) + ((x - r11) - r12) * (x + r1) *
                                       @horner(t, -0.15684781827145408780E0,
                                                   0.66253165609605468916E-2,
                                                  -0.12822297297864512864E-3,
                                                   0.12360964097729408891E-5,
                                                  -0.66450975112876224532E-8,
                                                   0.20326936466803159446E-10,
                                                  -0.33590883135343844613E-13,
                                                   0.23686934961435015119E-16) /
                                        @horner(t, 1.00000000000000000000E0,
                                                   0.96166044388828741188E-2,
                                                   0.45257514591257035006E-4,
                                                   0.13544922659627723233E-6,
                                                   0.27715365686570002081E-9,
                                                   0.37718676301688932926E-12,
                                                   0.27706844497155995398E-15)
    elseif x ≤ 12.0
        invt = inv(t)
        return sin(x) * @horner(invt, 0.99999999962173909991E0,
                                      0.36451060338631902917E3,
                                      0.44218548041288440874E5,
                                      0.22467569405961151887E7,
                                      0.49315316723035561922E8,
                                      0.43186795279670283193E9,
                                      0.11847992519956804350E10,
                                      0.45573267593795103181E9) /
                   (x * @horner(invt, 1.00000000000000000000E0,
                                      0.36651060273229347594E3,
                                      0.44927569814970692777E5,
                                      0.23285354882204041700E7,
                                      0.53117852017228262911E8,
                                      0.50335310667241870372E9,
                                      0.16575285015623175410E10,
                                      0.11746532837038341076E10)) -
        cos(x) * invt * @horner(invt, 0.99999999920484901956E0,
                                      0.51385504875307321394E3,
                                      0.92293483452013810811E5,
                                      0.74071341863359841727E7,
                                      0.28142356162841356551E9,
                                      0.49280890357734623984E10,
                                      0.35524762685554302472E11,
                                      0.79194271662085049376E11,
                                      0.17942522624413898907E11) /
                        @horner(invt, 1.00000000000000000000E0,
                                      0.51985504708814870209E3,
                                      0.95292615508125947321E5,
                                      0.79215459679762667578E7,
                                      0.31977567790733781460E9,
                                      0.62273134702439012114E10,
                                      0.54570971054996441467E11,
                                      0.18241750166645704670E12,
                                      0.15407148148861454434E12)
    elseif x < Inf
        invt = inv(t)
        return sin(x)/x * (1.0 - @horner(invt, 0.19999999999999978257E1,
                                               0.22206119380434958727E4,
                                               0.84749007623988236808E6,
                                               0.13959267954823943232E9,
                                               0.10197205463267975592E11,
                                               0.30229865264524075951E12,
                                               0.27504053804288471142E13,
                                               0.21818989704686874983E13) /
                                 @horner(invt, 1.00000000000000000000E0,
                                               0.11223059690217167788E4,
                                               0.43685270974851313242E6,
                                               0.74654702140658116258E8,
                                               0.58580034751805687471E10,
                                               0.20157980379272098841E12,
                                               0.26229141857684496445E13,
                                               0.87852907334918467516E13)*invt) -
            cos(x)*invt * (1.0 - @horner(invt, 0.59999999999999993089E1,
                                               0.96527746044997139158E4,
                                               0.56077626996568834185E7,
                                               0.15022667718927317198E10,
                                               0.19644271064733088465E12,
                                               0.12191368281163225043E14,
                                               0.31924389898645609533E15,
                                               0.25876053010027485934E16,
                                               0.12754978896268878403E16) /
                                 @horner(invt, 1.00000000000000000000E0,
                                               0.16287957674166143196E4,
                                               0.96636303195787870963E6,
                                               0.26839734750950667021E9,
                                               0.37388510548029219241E11,
                                               0.26028585666152144496E13,
                                               0.85134283716950697226E14,
                                               0.11304079361627952930E16,
                                               0.42519841479489798424E16)*invt)
    elseif isnan(x)
        return NaN
    else
        return 0.0
    end
end

for f in (:sinint, :cosint)
    @eval begin
        ($f)(x::Float32) = Float32(($f)(Float64(x)))
        ($f)(x::Float16) = Float16(($f)(Float64(x)))
        ($f)(x::Real) = ($f)(float(x))
        ($f)(x::AbstractFloat) = error("not implemented for ", typeof(x))
    end
end

"""
    sinint(x)

Compute the sine integral function of `x`, defined by ``\\operatorname{Si}(x) := \\int_0^x\\frac{\\sin t}{t} dt``
for real `x`.
"""
sinint

"""
    cosint(x)

Compute the cosine integral function of `x`, defined by ``\\operatorname{Ci}(x) := \\gamma + \\log x + \\int_0^x \\frac{\\cos t - 1}{t} dt``
for real `x > 0`, where ``\\gamma`` is the Euler-Mascheroni constant.
"""
cosint


using ParticleInCell
using LinearAlgebra

@testset "FDTD solver" begin

    dimx, dimy = 2π, 2π
    nx, ny = 128, 128
    dt = 1e-4
    nstep = 8

    mesh = TwoDGrid(dimx, nx, dimy, ny)

    ex = zeros(nx+1, ny+1)
    ey = zeros(nx+1, ny+1)
    bz = zeros(nx+1, ny+1)
    jx = zeros(nx+1, ny+1)
    jy = zeros(nx+1, ny+1)

    fdtd = FDTD(mesh)

    ω = sqrt(2)

    xn = mesh.x
    yn = mesh.y |> transpose
    xc = (mesh.x[1:end-1] .+ mesh.x[2:end]) ./ 2
    yc = (mesh.y[1:end-1] .+ mesh.y[2:end]) ./ 2 |> transpose

    # Ex and Ey are set to zero at t = 0.0
    # Bz is set at  t = dt/2
    t = 0.5dt
    fdtd.bz .= -cos.(xc) .* cos.(yc) .* cos(ω * t)

    sol_ex(x, y) = +cos.(x) .* sin.(y) ./ ω
    sol_ey(x, y) = -sin.(x) .* cos.(y) ./ ω
    sol_bz(x, y) = -cos.(x) .* cos.(y)

    for istep = 1:nstep # Loop over time

        ampere_maxwell!(fdtd, mesh, jx, jy, dt)

        t = t + 0.5dt

        @test maximum(abs.(fdtd.ex .- sol_ex(xc, yn) .* sin(ω * t))) < 1e-6
        @test maximum(abs.(fdtd.ey .- sol_ey(xn, yc) .* sin(ω * t))) < 1e-6

        update_fields!(ex, ey, bz, mesh, fdtd)

        @test maximum(abs.(ex .- sol_ex(xn, yn) .* sin(ω * t))) < 1e-6
        @test maximum(abs.(ey .- sol_ey(xn, yn) .* sin(ω * t))) < 1e-6

        faraday!(fdtd, mesh, dt)

        t = t + 0.5dt

        @test maximum(abs.(fdtd.bz .- sol_bz(xc, yc) .* cos(ω * t))) < 1e-6

        update_fields!(ex, ey, bz, mesh, fdtd)

        @test maximum(abs.(bz .- sol_bz(xn, yn) .* cos(ω * t))) < 1e-3

    end # next time step

end


using Printf
using NPZ
include("spectre-defense/util.jl")

input_folder = ARGS[1]
output_path = ARGS[2]
target_label = ARGS[3]
eps = parse(Int16,ARGS[4])

reps = npzread(input_folder * "$(target_label).npy")'
n = size(reps)[2]
removed = round(Int, 1.5*eps)

println("Running PCA filter")
reps_pca, U = pca(reps, 1)
pca_poison_ind = k_lowest_ind(-abs.(mean(reps_pca[1, :]) .- reps_pca[1, :]), round(Int, 1.5*eps))
poison_removed = sum(pca_poison_ind[end-eps+1:end])
clean_removed = removed - poison_removed
@printf("%d poisons removed, %d cleans removed\n", poison_removed, clean_removed)
npzwrite(output_path, pca_poison_ind)


import SSGE
using Test

using Distributions
using LinearAlgebra
using ForwardDiff
using Distances
import Random

@testset "SSGEstimator" begin
    F = Float32
    test_dist = MvNormal([F(2),F(2)], Symmetric([F(1) F(0.3); F(0.3) F(1)]))

    test_log_pdf(x) = log.(pdf(test_dist, x))
    # Analytic solution
    ∇test_log_pdf(x) = ForwardDiff.gradient(test_log_pdf, x)

    Random.seed!(1337)
    num_samples = 100
    x_samples = rand(test_dist, num_samples)
    num_eig_fun = 3
    σ = F(1000.0)

    g = SSGE.SSGEstimator(x_samples, num_eig_fun, SSGE.SqExp(σ));

    num_tests = 100
    x_tests = rand(test_dist, num_tests)

    @test mean([norm(∇test_log_pdf(x_tests[:, i]) - g(x_tests[:, i])) for i in 1:num_tests]) < 0.4

    g_tuned = SSGE.SSGEstimator(x_samples, F(0.99));
    @test mean([norm(∇test_log_pdf(x_tests[:, i]) - g_tuned(x_tests[:, i])) for i in 1:num_tests]) < 0.45

end


@with_kw struct ParticleSystem{T}
    m::T = 87 * 1.661e-27  # in kg
    coupling::T = -32e9 * 6.626e-34 * 1e-12 * 1e12  # in kg um^6 / us^2
    k_b::T = 1.38e-23
    range::Int64 = 6
    temperature::T = 0.0

    c_m::T = coupling / m  # in um^6/us^2
end

function sum_forces!(force::AbstractVector{T}, pos_1::AbstractVector{T}, pos_2::AbstractVector{T},
    particle_system::ParticleSystem{T}) where T
    range = particle_system.range
    force .= force .- range * particle_system.c_m .* (pos_1 .- pos_2) ./ norm(pos_1 .- pos_2)^(range + 2)
end

function sum_forces!(force::AbstractVector{T}, dist::AbstractVector{T},
    particle_system::ParticleSystem{T}) where T
    range = particle_system.range
    force .= force .- range * particle_system.c_m .* dist ./ norm(dist)^(range + 2)
end

const dist_particle_system = zeros(3)
const force_particle_system = zeros(3)

function hamiltonian_explosian!(dv::Array{T, 2}, v::Array{T, 2}, u::Array{T, 2},
    system::ParticleSystem{T}, t::T) where T

    for i in 1:size(u, 2)
        force_particle_system .= 0
        for j in 1:size(u, 2)
            if i == j
                continue
            end
            pos_1 = @view u[:, i]
            pos_2 = @view u[:, j]
            dist_particle_system .= pos_1 .- pos_2
            sum_forces!(force_particle_system, dist_particle_system, system)
        end
        dv[:, i] .= force_particle_system
    end
end

function get_velocity(N::Int64; system=ParticleSystem())
    sigma = sqrt(system.k_b * system.temperature / system.m)
    gaussian_distribution = Normal(0.0, sigma)
    multivariate = product_distribution([gaussian_distribution, gaussian_distribution, gaussian_distribution])
    return rand(multivariate, N)
end

function solve_particle_system(
    positions::Matrix{T}, times::AbstractVector{T};
    system=ParticleSystem()
    ) where T
    N = size(positions)[2]
    velocities = get_velocity(N, system=system)
    prob = SecondOrderODEProblem(
        hamiltonian_explosian!, velocities, positions, (times[1], times[end]), system
        )
    return solve(prob, DPRKN6())
end

function get_J(sol::DifferentialEquations.ODESolution, t)
    pos = sol(t).x[2]
    J = get_J(pos, PowerLaw(6, 60e3))
    return @SMatrix[J]
end

function get_J_itp(
    positions, times;
    system::ParticleSystem=ParticleSystem()
    )

    sol = solve_particle_system(positions, times, system=system)
    itp = interpolate([get_J(sol, t) for t in times], BSpline(Linear()))
    f_itp = Interpolations.scale(itp, times)
    return (t) -> f_itp(t).data[1]
end


struct BasicFootTrajectory{T}
    p0::SVector{3, T}
    xaxis::SVector{2, T}
    Δx::Polynomial{4, T}
    Δz::Polynomial{3, T}
    s::Polynomial{6, T}
    tf::T
end

function BasicFootTrajectory(t0::Number, tf::Number, p0::SVector{3}, Δzmid::Number, pf::SVector{3}, zdf::Number)
    # Coordinates of final position relative to initial, projected onto swing plane (x-z)
    Δpf = pf - p0
    Δxf = sqrt(Δpf[1]^2 + Δpf[2]^2)
    T = eltype(Δxf)
    Δzf = convert(T, Δpf[3])
    xaxis = SVector(Δpf[1], Δpf[2]) / Δxf

    # Trajectories as function of interpolation variable `s`
    s0 = zero(T)
    sf = oneunit(T)
    smid = (s0 + sf) / 2
    Δx = fit_cubic(x0=s0, xf=sf, y0=zero(T), yd0=zero(T), yf=Δxf, ydf=zero(T))
    Δz = let
        num_coeffs = Val(3)
        A = hcat(
            SVector(SUP.coefficient_gradient(s0, num_coeffs, Val(0))),
            SVector(SUP.coefficient_gradient(smid, num_coeffs, Val(0))),
            SVector(SUP.coefficient_gradient(sf, num_coeffs, Val(0))),
        ) |> transpose
        b = SVector(tuple(zero(T), max(0, Δzf) + Δzmid, Δzf))
        Polynomial(Tuple(A \ b))
    end

    # Final value of derivative of height as a function of `s`
    dzds_f = derivative(Δz)(sf)

    # Final value of derivative of s as a function of t
    sdf = zdf / dzds_f

    # Interpolation trajectory
    s = fit_quintic(x0=t0, xf=tf, y0=s0, yd0=zero(s0), ydd0=zero(s0), yf=sf, ydf=sdf, yddf=zero(sf))

    BasicFootTrajectory(p0, xaxis, Δx, Δz, s, tf)
end

function Base.convert(::Type{BasicFootTrajectory{T}}, p::SVector{3}, tf::Number) where {T}
    xaxis = SVector(zero(T), zero(T))
    Δx = Polynomial(ntuple(i -> zero(T), Val(4)))
    Δz = Polynomial(ntuple(i -> zero(T), Val(3)))
    s = Polynomial(ntuple(i -> zero(T), Val(6)))
    BasicFootTrajectory(p, xaxis, Δx, Δz, s, tf)
end

function (traj::BasicFootTrajectory)(t::Number, ::Val{2})
    s = traj.s(t)
    sd = derivative(traj.s)(t)
    sdd = derivative(derivative(traj.s))(t)
    Δx = traj.Δx(s)
    Δxd = derivative(traj.Δx)(s) * sd
    Δxdd = derivative(traj.Δx)(s) * sdd + derivative(derivative(traj.Δx))(s) * sd^2
    Δz = traj.Δz(s)
    Δzd = derivative(traj.Δz)(s) * sd
    Δzdd = derivative(traj.Δz)(s) * sdd + derivative(derivative(traj.Δz))(s) * sd^2
    p = traj.p0 + SVector(Δx * traj.xaxis[1], Δx * traj.xaxis[2], Δz)
    pd = SVector(Δxd * traj.xaxis[1], Δxd * traj.xaxis[2], Δzd)
    pdd = SVector(Δxdd * traj.xaxis[1], Δxdd * traj.xaxis[2], Δzdd)
    p, pd, pdd
end

(traj::BasicFootTrajectory)(t::Number) = first(traj(t, Val(2)))


@testset "detectors" begin
  Random.seed!(42)
  x = vcat(rand(8,2), 5*rand(1,2), 30*rand(1,2))
  x1 = vcat(rand(20,3), 5*rand(1,3), 30*rand(1,3))
  @test rxdetect(x, 0.8) == [false, false, false, false, false, false, false, false, true, true]
  @test hosvdc4detect(x, 4., 2; b=2) == [false, false, false, false, false, false, false, false, true, true]
  @test hosvdc4detect(x1, 3.9, 1; b=1) == vcat(fill(false, 20), [true, true])
  x2 = [[0. 0.]; [1. 2.]; [1. 1.]]
  @test hosvdc4detect(x2, .1, 2; b=2) == [false, false, false]
  @test rxdetect(x1, 0.9) == vcat(fill(false, 20), [true, true])
  ls = fill(true, 10)
  ls1 = [true, true, true, true, true, true, true, true, false, false]
  c = cumulants(x, 4)
  @test hosvdstep(x, ls, 4., 2, c[4])[1] == ls1
  @test hosvdstep(x, ls, 3., 1, c[4])[2] ≈ 1.15 atol= 0.1
end


export findw
"""
function findw(param,M;maxIterQP,out,pwr)

Find a frequency vector w in Omega_W that solves the constrained linear least
squares problem

min_w | M*w - d |_{Gamma,pwr}

Input:

	1. param     - StrainReconParam
	2. M         - binary matrix
    3. maxIterQP - maximum number of iterations for asqp
	4. out       - show intermediate results for asqp/iplp if > 1
	5. pwr       - exponent in the likelihood function

Output:

	1. wsol      - solution vector for constrained least squares problem

"""
function findw(param::StrainReconParam,M::BitArray{2};maxIterQP::Int=5,out=-1,pwr::Float64=param.pwr)

	(m,n) = size(M)
	R     = param.R
	d  = param.d

	# equality constraint
	(Ci,ci,Ce,ce) = getConstraints(param)

	if pwr==2.0
		# solve equality constrained QP problem and see if constraints are satisfied
		Q   = M'*((R'*R)*M) # diagm(collect(0:n-1));
		c   = M'*(R'*(R*d))
		x0  = 1./collect(1:n)
		x0  ./= sum(x0)
		wsol, iter, Wc = asqp(Q,-c,Ce,ce,Ci,ci,x0,maxIter=maxIterQP,out=out)
	elseif pwr==1.0
        # solve equality constrained linear programming with interior point method
        # return wsol, Lagrange multipliers, slack variables, history of iterations
        w0  = 1./collect(1:n)
		w0  ./= sum(w0)
		# set up linear system to be solved by IPLP
	    ni = size(Ci,1)
	    ne = size(Ce,1)
	    Ae = [M -eye(m) eye(m) zeros(m,ni);
	          Ce zeros(ne,2*m) zeros(ne,ni);
	          Ci zeros(ni,2*m) -eye(ni)]
	    ae = [vec(d); vec(ce); vec(ci); ]

	    c  = [zeros(n); diag(R); diag(R); zeros(ni)]
	    # how to initialize Lagrange mutlipliers and slack variables?
		x0 = rand(n+2*m+ni)
		y0 = rand(size(Ae,1))
	    s0 = rand(size(x0))

        wsol, lm, s, his = iplpPathFollowing(Ae,ae,c,x0,y0,s0,maxIter=100,out=-1,ftol=1e-12,mutol=1e-12)
		wsol = wsol[1:n]
	end

	return wsol
end


Base.signbit(x::Double64) = signbit(Hi(x))
signbits(x::Double64) = (signbit(Hi(x), signbit(Lo(x))))
Base.sign(x::Double64) = sign(Hi(x))
signs(x::Double64) = (sign(Hi(x), sign(Lo(x))))
Base.significand(x::Double64) = (significand(Hi(x)), significand(Lo(x)))
Base.exponent(x::Double64) = (exponent(Hi(x)), exponent(Lo(x)))

significant_bits(::Type{Float64})    =   53
significant_bits(::Type{Double64})   = 2*53
significant_bits(::Type{ComplexF64}) =   53
significant_bits(::Type{ComplexD64}) = 2*53

Base.frexp(x::Double64) = (frexp(Hi(x)), frexp(Lo(x)))

function Base.ldexp(x::Tuple{Tuple{Float64,Int64},Tuple{Float64,Int64}})
    hi = ldexp(x[1]...)
    lo = ldexp(x[2]...)
    return Double64((hi, lo))
end

# like frexp, returns an integer-valued significand
function ntexp(x::Float64)
    fr, ex = frexp(x)
    nt = Int128(ldexp(fr, 53))
    ex -= 53
    return nt, ex
end

#=
    num, xp2, den = Base.decompose(x)
    if isone(den)
       BigFloat(x) == ldexp(BigFloat(num), xp2)
    else
       BigFloat(x) == ldexp(BigFloat(num), xp2) / den
    end
=#
function Base.decompose(x::Double64)
    hi = Hi(x)
    isnan(hi)  && return 0, 0, 0
    isinf(hi)  && return sign(x), 0, 0
    iszero(hi) && return 0, 0, sign(x)

    nt_hi, ex_hi = ntexp(Hi(x))
    nt_lo, ex_lo = ntexp(Lo(x))
    num = (nt_hi << abs(ex_hi-ex_lo)) + nt_lo
    xp2 = ex_lo
    return num, xp2, 1
end

Base.copysign(x::Double64, y) = signbit(y) ? -abs(x) : abs(x)
Base.flipsign(x::Double64, y) = signbit(y) ? -x : x

Base.:(-)(x::Double64) = Double64((-Hi(x), -Lo(x)))
Base.abs(x::Double64) = signbit(x) ? -x : x
Base.abs2(x::Double64) = x*x
fastabs(x::Double64) = abs(Hi(x))
fastabs2(x::Double64) = abs2(Hi(x))

Base.:(-)(x::ComplexD64) = ComplexD64((-Hi(x), -Lo(x)))
fastabs(x::ComplexD64) = abs(Hi(x))
fastabs2(x::ComplexD64) = abs2(Hi(x))

fastabs(x::Float64) = abs(x)
fastabs(x::ComplexF64) = abs(x)
fastabs2(x::Float64) = abs2(x)
fastabs2(x::ComplexF64) = abs2(x)

function Base.ceil(x::Double64)
    xhi = Hi(x)
    hi = ceil(Hi(x))
    if hi === xhi
        t = ceil(Lo(x))
        hi = hi + t
        lo = t - (hi - xhi)
    else
        lo = 0.0
    end
    return Double64((hi, lo))
end

Base.ceil(::Type{T}, x::Double64) where {T<:Integer} = T(ceil(x))

function Base.floor(x::Double64)
    xhi = Hi(x)
    hi = floor(Hi(x))
    if hi === xhi
        t = floor(Lo(x))
        hi = hi + t
        lo = t - (hi - xhi)
    else
        lo = 0.0
    end
    return Double64((hi, lo))
end

Base.floor(::Type{T}, x::Double64) where {T<:Integer} = T(floor(x))

function Base.trunc(x::Double64)
    return signbit(x) ? ceil(x) : floor(x)
end

Base.trunc(::Type{T}, x::Double64) where {T<:Integer} = T(trunc(x))

function Base.div(x::Double64, y::Double64)
    (!isfinite(x) || isnan(y)) && return NaND64
    !isfinite(y) && return zero(Double64)
    return trunc(x / y)
end

function Base.fld(x::Double64, y::Double64)
    (!isfinite(x) || isnan(y)) && return NaND64
    !isfinite(y) && return zero(Double64)
    return floor(x / y)
end

function Base.cld(x::Double64, y::Double64)
    (!isfinite(x) || isnan(y)) && return NaND64
    !isfinite(y) && return zero(Double64)
    return ceil(x / y)
end

function Base.rem(x::Double64, y::Double64)
    (!isfinite(x) || isnan(y)) && return NaND64
    !isfinite(y) && return x
    return x - div(x,y) * y
end

function Base.mod(x::Double64, y::Double64)
    (!isfinite(x) || isnan(y)) && return NaND64
    !isfinite(y) && return x
    return x - fld(x,y) * y
end

function Base.divrem(x::Double64, y::Double64)
    (!isfinite(x) || isnan(y)) && return NaND64
    !isfinite(y) && return zero(Double64)
    dv = trunc(x / y)
    rm = x - dv * y
    return dv, rm
end

function Base.fldmod(x::Double64, y::Double64)
    (!isfinite(x) || isnan(y)) && return NaND64
    !isfinite(y) && return zero(Double64)
    fr = floor(x / y)
    md = x - fr * y
    return fr, md
end


module BoatDynamics
using LinearAlgebra
using DifferentialEquations
using StaticArrays
using Plots

# Write your package code here.
include("matlab_base/model_constants.jl")
include("matlab_base/funshapeex.jl")
include("matlab_base/kinematics.jl")
include("matlab_base/kinematic_excitation.jl")
export Boat_timing

include("kinetic_energy.jl")
export kinetic_energy

include("helper.jl")

include("matlab_base/oar_force.jl")
include("matlab_base/forces.jl")

include("symbolic/sym_kinetic_energy.jl")
include("symbolic/sym_generate_equations.jl")
export generate_equations_of_motion # For tests under development

if !isfile(joinpath(@__DIR__, "generated", "inertia_lhs.jl"))
    generate_equations_of_motion()
end
inertia_lhs = include("generated/inertia_lhs.jl")
inertia_rhs = include("generated/inertia_rhs.jl")

include("solver/system_eqs.jl")
export system_lhs, system_rhs

include("solver/boat_settings.jl")
export Boat_settings
include("solver/boat_odefun.jl")
include("solver/solve_boat.jl")
export solve_boat, find_the0

include("postprocess/animate.jl")
export animate_boat

end


using TensorFlow
using Distributions

# Generate some synthetic data
x = randn(100, 50)
w = randn(50, 10)
y_prob = exp.(x*w)
y_prob ./= sum(y_prob,2)

function draw(probs)
    y = zeros(size(probs))
    for i in 1:size(probs, 1)
        idx = rand(Categorical(probs[i, :]))
        y[i, idx] = 1
    end
    return y
end

y = draw(y_prob)

# Build the model
sess = Session(Graph())

X = placeholder(Float64, shape=[-1, 50])
Y_obs = placeholder(Float64, shape=[-1, 10])

variable_scope("logisitic_model"; initializer=Normal(0, .001)) do
    global W = get_variable("W", [50, 10], Float64)
    global B = get_variable("B", [10], Float64)
end

Y=nn.softmax(X*W + B)

Loss = -reduce_sum(log(Y).*Y_obs)
optimizer = train.AdamOptimizer()
minimize_op = train.minimize(optimizer, Loss)
saver = train.Saver()

# Run training
run(sess, global_variables_initializer())
checkpoint_path = mktempdir()
info("Checkpoint files saved in $checkpoint_path")
for epoch in 1:100
    cur_loss, _ = run(sess, [Loss, minimize_op], Dict(X=>x, Y_obs=>y))
    println(@sprintf("Current loss is %.2f.", cur_loss))
    train.save(saver, sess, joinpath(checkpoint_path, "logistic"), global_step=epoch)
end


function solve_exp_ode(
    M::AbstractManifold,
    p,
    X;
    basis::AbstractBasis=DefaultOrthonormalBasis(),
    solver=AutoVern9(Rodas5()),
    backend=default_differential_backend(),
    kwargs...,
)
    n = length(p)
    iv = SVector{n}(1:n)
    ix = SVector{n}((n + 1):(2n))
    u0 = allocate(p, 2n)
    u0[iv] .= X
    u0[ix] .= p

    function exp_problem(u, params, t)
        M = params[1]
        dx = u[iv]
        p = u[ix]
        ddx = allocate(u, Size(n))
        du = allocate(u)
        Γ = christoffel_symbols_second(M, p, basis; backend=backend)
        @einsum ddx[k] = -Γ[k, i, j] * dx[i] * dx[j]
        du[iv] .= ddx
        du[ix] .= dx
        return Base.convert(typeof(u), du)
    end

    params = (M,)
    prob = ODEProblem(exp_problem, u0, (0.0, 1.0), params)
    sol = solve(prob, solver; kwargs...)
    q = sol.u[1][(n + 1):(2 * n)]
    return q
end


module Jordan
using Hecke

function rref_with_trans(M::T) where {T <: MatElem{S} where {S <: FieldElem}}
  #does row ops
  n = hcat(M, identity_matrix(base_ring(M), nrows(M)))
  rref!(n)
  s = nrows(n)
  while s>0 && iszero(sub(n, s:s, 1:ncols(M)))
    s -= 1
  end
  return s, sub(n, 1:nrows(M), 1:ncols(M)), sub(n, 1:nrows(M), ncols(M)+1:ncols(n))
end

function quo_gens(A, B)
  n, B = rref(B')
  B = sub(B, 1:n, 1:ncols(B))'
  fl, T = can_solve_with_solution(A, B)  # A T = B
  d, r = rref(T')
  t = sub(r, 1:d, 1:ncols(r))
  t = Hecke.reduce_mod(identity_matrix(base_ring(t), ncols(t)), t)
  d = rref!(t)
  t = t[1:d, :]
  return A*t'
end

function rational_normal_form(M::T) where {T <: MatElem{S} where {S <: FieldElem}}
  f = minpoly(M)
  lf = factor(f)
  all_b = zero_matrix(base_ring(M), nrows(M), 0)
  for (g, v) = lf.fac
    m = (g^v)(M)
    this_g = zero_matrix(base_ring(M), nrows(M), 0)
    while true
      d, km = nullspace(m)
      km = quo_gens(km, this_g)
      if ncols(km) == 0
        break
      end
      ex = v
      while true
        b = quo_gens(km, g(M)*km)
        if ncols(b) > 0
          break
        end
        @show ex -= 1
        km = g(M)*km
        d, km = rref(km')
        km = km[1:d, :]'
      end
      b = b[:, 1]
    #any 0 ne b in km/g(M)km will generate a sigma cyclic subspace
    # b, Mb M^2b ... g(M)b, M g(M) b, ... g^2(M)b, ....
      ve = deepcopy(b)
      b = zero_matrix(base_ring(M), nrows(M), 0)
      for k = 1:ex
        for j = 1:degree(g)
          @show k, j
          b = hcat(b, (M^(j-1))*((g^(k-1))(M))*ve)
        end
      end
      @show this_g = hcat(this_g, b)
    end
    all_b = hcat(all_b, this_g)
  end
  return all_b
end

end


using Sundials

## Define the system residual function.
function resrob(t, y, yp)
    r[1]  = -0.04*y[1] + 1.0e4*y[2]*y[3]
    r[2]  = -r[1] - 3.0e7*y[2]*y[2] - yp[2]
    r[1] -=  yp[1]
    r[3]  =  y[1] + y[2] + y[3] - 1.0
    return r
end

t = [0.0, 4 * logspace(-1., 5., 7)]
yout, ypout = Sundials.idasol(resrob, [1.0, 0, 0], [-0.04, 0.04, 0.0], t)


using MittagLefflerFunctions
using PyPlot
using Polynomials

x = range(0, 10, length=201)
α_vals = [0.2, 0.4, 0.6, 0.8, 1.0]

figure(1)
r = 5
m = r+1
n = r
style = ("-", "--", ":", "-.", "-")
for row = 1:length(α_vals)
    α = α_vals[row]
    Eα_approx = MLPade(α, 1.0, m, n, :svd)
    Eα_ref = MLQuad(α, 1.0, 14, :hyperbola)
    labelstring = latexstring("\$\\alpha=$α\$")
    semilogy(x, abs.(Eα_ref.(-x) - Eα_approx.(x)),
             style[row], label=labelstring)
end
xlabel(L"$x$")
legend()
grid(true)

savefig("figure10.pdf")

α = 1.85
β = 1.0
println("Setting α = $α, β = $β.")
for (m, n) in [(8,7), (10,9)]
    println("\nRoots of q when m = $m, n = $n:")
    E = MLPade(α, β, m, n, :svd)
    q = Polynomial(E.q)
    display(roots(q))
end


using FEniCS
using PyPlot

L = 1; W = 0.2
mu = 1
rho = 1
delta = W/L
gamma = 0.4*delta^2
beta = 1.25
lambda_ = beta
g = gamma

# Create mesh and define function space
mesh = BoxMesh(Point([0, 0, 0]), Point([L, W, W]), 10, 3, 3)
V = VectorFunctionSpace(mesh, "P", 1)
c = Constant((0,0,0))
bc = DirichletBC(V, c, "on_boundary && x[1]<1E-14")
tol = 1E-14

#bc1 = fenics.DirichletBC(V.pyobject, c.pyobject, "on_boundary and x[1]<tol",tol=1E-14)
function epsilon(u)
     0.5*(nabla_grad(u)+Transpose(nabla_grad(u)))
end

function sigma(u)
     lambda_*nabla_div(u)*Identity(d) + 2*mu*epsilon(u)
end

u = TrialFunction(V)
d = geometric_dimension(u)# space dimension
v = TestFunction(V)
f = Constant((0, 0, -rho*g))
T = Constant((0, 0, 0))
a = inner(sigma(u), epsilon(v))*dx
L = dot(f, v)*dx + dot(T, v)*ds
u=FeFunction(V)
lvsolve(a,L,u,bc)
s = sigma(u) - (1.0/3)*tr(sigma(u))*Identity(d)
#fenics.plot(U.pyobject,mode="displacement")
von_Mises = sqrt(3.0/2*inner(s, s))
V = FunctionSpace(mesh, "P", 1)

von_Mises = project(von_Mises, V)
u_magnitude = sqrt(dot(u, u))
u_magnitude = project(u_magnitude, V)

# How the heck do we plot these things!!!
# FEniCS.Plot(von_Mises, title="Stress intensity")
# FEniCS.Plot(u_magnitude)


using Primes

function carmichael(pmax::Integer)
    if pmax ≤ 0 throw(DomainError("pmax must be strictly positive")) end
    car = Vector{typeof(pmax)}(0)
    for p in primes(pmax)
        for h₃ in 2:(p-1)
            m = (p - 1) * (h₃ + p)
            pmh = mod(-p ^ 2, h₃)
            for Δ in 1:(h₃+p-1)
                if m % Δ != 0 || Δ % h₃ != pmh continue end
                q = m ÷ Δ + 1
                if !isprime(q) continue end
                r = (p * q - 1) ÷ h₃ + 1
                if !isprime(r) || mod(q * r, p - 1) == 1 continue end
                append!(car, [p, q, r])
            end
        end
    end
    return reshape(car, 3, length(car) ÷ 3)
end


using MultivariateStats: PCA, pcacov
import IterTools: takenth, product
export PCAEmbedding

"""
	PCAEmbedding(s, em::AbstractSpatialEmbedding; kwargs...) → embedding
A spatio temporal delay coordinates structure with
Principal Component Analysis as a means of dimension reduction,
`embedding` can be used as a functor:
```julia
embedding(rvec, s, t, α)
```
which operates inplace on `rvec` and reconstructs vector from spatial time series `s` at
timestep `t` and cartesian index `α`.

To instantiate this `embedding`, give the data to be reconstructed `s` as well as an
instance of [`SpatioTemporalEmbedding`](@ref) to `PCAEmbedding`.

## Keyword Arguments
* `pratio = 0.99` : Ratio of variances that needs to be preserved in low-dimensional
  PCA-reconstruction.
* `maxoutdim = 25`: Upper limit for output dimension. May break `pratio` criterion.
* `every_t = 1` : Speed up computation by only using every n-th point in time.
* `every_α = 1` : Speed up computation further by only using every n-th point in space
  (linear indexing).

To set the output dimension to a certain value `X`, pass `pratio=1, maxoutdim=X`.
"""
struct PCAEmbedding{T,Φ,BC,X,Y} <: AbstractSpatialEmbedding{Φ,BC,X}
	stem::AbstractSpatialEmbedding{Φ,BC,Y}
	meanv::T
	covmat::Matrix{T}
	drmodel::PCA{T}
	tmp::Vector{T} #length Y
end

function PCAEmbedding(
		s::AbstractArray{<:AbstractArray{T,Φ}},
		stem::AbstractSpatialEmbedding{Φ,BC,Y};
		pratio   = 0.99,
		maxoutdim= 25,
		every_t::Int    = 1,
		every_α::Int    = 1) where {T,Φ,BC,Y}
	meanv = mean(mean.(s))
	tsteps = (length(s) - get_τmax(stem))
	usable_idxs = get_usable_idxs(stem)
	num_pt = length(usable_idxs)
	subset = Iterators.product(takenth(usable_idxs, every_α), 1:every_t:tsteps)
	L      = length(subset)

	recv = zeros(T,Y)
	covmat = zeros(T,Y,Y)
	for (α, t) ∈ subset
		stem(recv, s,t,α)
		recv  .-= meanv
		@inbounds for j=1:Y, i=1:Y
			covmat[i,j] += recv[i]*recv[j] / L
		end
	end
	drmodel = compute_pca(covmat,pratio, maxoutdim)
	X = size(drmodel.proj,2)
	tmp = zeros(Y)
	return PCAEmbedding{T,Φ,BC,X,Y}(stem, meanv, covmat, drmodel,tmp)
end

function (r::PCAEmbedding{T,Φ,BC,X,Y})(data, s, t, α) where {T,Φ,BC,X,Y}
	r.stem(r.tmp, s,t,α)
	r.tmp .-= r.meanv
	mul!(data,transpose(r.drmodel.proj),r.tmp)
end

Base.show(io::IO, em::PCAEmbedding) = (show(io, em.stem); show(io, em.drmodel))

compute_pca(covmat::Matrix{T}, pratio, maxoutdim) where T=
	pcacov(covmat, T[]; maxoutdim=maxoutdim, pratio=pratio)

get_τmax(em::PCAEmbedding) = get_τmax(em.stem)::Int #somehow type inference doesn't figure this out
get_num_pt(em::PCAEmbedding) = get_num_pt(em.stem)::Int

outdim(em::PCAEmbedding{T,Φ,BC,X}) where {T,Φ,BC,X} = X


# Matrix setup
#==========================================================================================#

"""
	influence_coefficient(r, horseshoe, normal, V_hat, symmetry)

Compute the influence coefficient of the velocity of a Horseshoe with trailing lines in a given direction ``\\hat V`` at a point ``r`` projected to a normal vector.
"""
influence_coefficient(horseshoe :: Horseshoe, r, normal, V_hat, finite_core = false) = dot(velocity(r, horseshoe, 1., V_hat, finite_core), normal)

"""
	influence_matrix(horseshoes, collocation_points, normals, V_hat)

Assemble the Aerodynamic Influence Coefficient (AIC) matrix given horseshoes, collocation points, associated normal vectors, and a unit vector representing the freestream.
"""
influence_matrix(horseshoes :: Vector{<: Horseshoe}, collocation_points, normals, V_hat, finite_core = false) = [ influence_coefficient(horsie_j, r_i, n_i, V_hat, finite_core) for (r_i, n_i) ∈ zip(collocation_points, normals), horsie_j ∈ horseshoes ]

"""
	boundary_condition(velocities, normals)

Compute the projection of a velocity vector with respect to normal vectors of panels. Corresponds to construction of the boundary condition for the RHS of the AIC system.
"""
boundary_condition(velocities, normals) = dot.(velocities, normals)

## Pre-allocated versions
#====================================================#

function matrix_assembly!(AIC, RHS, horseshoes :: Vector{<: Horseshoe}, collocation_points, normals, V, Ω)
    for i ∈ 1:length(collocation_points)
        for j ∈ 1:length(horseshoes)
            AIC[i,j] = dot(total_horseshoe_velocity(r1(collocation_points[i], horseshoes[j]), r2(collocation_points[i], horseshoes[j]), 1., -normalize(V)), normals[i])
        end
        RHS[i] = dot(V + Ω × collocation_points[i], normals[i])
    end
    nothing
end


struct STE <: AbstractLoss
    σ::Real
    constant::Float64

    function STE(;σ::T = 1/sqrt(2)) where T <: Real
        σ > 0 || throw(ArgumentError("σ in STE loss must be > 0"))
        new(σ, 1/σ^2)
    end

end

@doc raw"""
    function kernel(loss::STE, X::AbstractMatrix)

Computes:

```math
K = \exp(\|X_i - X_j\|^2/(2\sigma^2)) \forall (i,j) \in {1,\ldots,n} \times {1,\ldots,n}.
```
"""
function kernel(loss::STE, X::Embedding)
    K = pairwise(SqEuclidean(), X.X, dims=2)
    c = -loss.constant / 2

    for ij in eachindex(K)
        @inbounds K[ij] = exp(c * K[ij])
    end
    return K
end

function gradient(loss::STE, triplets, X::AbstractMatrix)

    K = kernel(loss, X) # Triplet kernel values (in the STE loss)

    # We need to create an array to prevent race conditions
    # This is the best average solution for small and big Embeddings
    nthreads = Threads.nthreads()
    triplets_range = partition(length(triplets), nthreads)

    C = zeros(Float64, nthreads)
    ∇C = [zeros(Float64, size(X)) for _ in 1:nthreads]

    Threads.@threads for tid in 1:nthreads
        C[tid] = tgradient!(∇C[tid], loss, triplets, X, K, triplets_range[tid])
    end

    return sum(C), -sum(∇C)
end

function tgradient!(
    ∇C::Matrix{<:AbstractFloat},
    loss::STE,
    triplets::Triplets,
    X::AbstractMatrix,
    K::Matrix{<:AbstractFloat},
    triplets_range::UnitRange{Int64})

    C = 0.0

    for t in triplets_range
        @views @inbounds i, j, k = triplets[t][:i], triplets[t][:j], triplets[t][:k]

        @inbounds P = K[i,j] / (K[i,j] + K[i,k])
        C += -log(P)

        for d in 1:ndims(X)
            @inbounds ∂x_j = (1 - P) * (X[d,i] - X[d,j])
            @inbounds ∂x_k = (1 - P) * (X[d,i] - X[d,k])

            @inbounds ∇C[d,i] += - loss.constant * (∂x_j - ∂x_k)
            @inbounds ∇C[d,j] +=   loss.constant *  ∂x_j
            @inbounds ∇C[d,k] += - loss.constant *  ∂x_k
        end
    end
    return C
end

function tgradient!(
    ∇C::Matrix{<:AbstractFloat},
    loss::STE,
    triplets::LabeledTriplets,
    X::AbstractMatrix,
    K::Matrix{<:AbstractFloat},
    triplets_range::UnitRange{Int64})

    C = 0.0

    for t in triplets_range
        @views @inbounds i, j, k = if triplets[t][:y] == -1
            triplets[t][:i], triplets[t][:j], triplets[t][:k]
        else
            triplets[t][:i], triplets[t][:k], triplets[t][:j]
        end

        @inbounds P = K[i,j] / (K[i,j] + K[i,k])
        C += -log(P)

        for d in 1:ndims(X)
            @inbounds ∂x_j = (1 - P) * (X[d,i] - X[d,j])
            @inbounds ∂x_k = (1 - P) * (X[d,i] - X[d,k])

            @inbounds ∇C[d,i] += - loss.constant * (∂x_j - ∂x_k)
            @inbounds ∇C[d,j] +=   loss.constant *  ∂x_j
            @inbounds ∇C[d,k] += - loss.constant *  ∂x_k
        end
    end
    return C
end


mutable struct PDataMA97{T} <: PDataFact{T}
    PLD::Ma97          # H = (PL)D(PL)'  or  LDL' = P'HP
    Δ::Array{T,1}  # diagonal, eigenvalues of D
    D::Any
    H::Any
    pivot::Any
    #Q::Array{Float64,2}  # orthogonal matrix, eigenvectors of D:  should be sparse
    # QΔQ'  =  D
    Q::SparseMatrixCSC{T,Int64}
    g̃::Array{T,1}  # transformed gradient
    g::Array{T,1}  # untransformed gradient
    λ::T
    success::Bool        # previous iteration was successfull
    OK::Bool             # preprocess success

    PDataMA97() = new{Nothing}()
    PDataMA97(PLD, Δ, D, H, pivot, Q, g̃, g, l, success, OK) =
        new{eltype(g̃)}(PLD, Δ, D, H, pivot, Q, g̃, g, l, success, OK)
end

function preprocess(PData::PDataMA97, H, g, n1, n2)
    PLD = Ma97
    pivot = Array{Int32,1}
    vD1 = Array{Float64,2}
    vD2 = Array{Float64,2}

    H97 = convert(SparseMatrixCSC{Float64,Int}, H)
    try
        PLD = Ma97(H97, print_level = -1)
        ma97_factorize!(PLD)
    catch
        println("*******   Problem in MA97")
        res = PDataMA97()
        res.OK = false
        return res
    end
    try
        ret = ma97_inquire(PLD)
        vD = ret[2]
        pivot = ret[1]
        # Take care of zero valued pivots
        z(a) = a == 0
        pivot[findall(z, pivot)] = collect(1:length(pivot))[findall(z, pivot)]

        vD1 = copy(vD[1:1, :])'[:, 1:1]
        vD2 = copy(vD[2:2, 1:end-1])'[:, 1:1]
    catch
        println("*******   Problem after MA97")
        println(" Cond(H) = $(cond(full(H)))")
        res = PDataMA97()
        res.OK = false
        return res
    end

    vD1 = vec(vD1)
    vD2 = vec(vD2)
    D = SparseArrays.spdiagm(0 => vD1, -1 => vD2, 1 => vD2)

    #################  Future object BlockDiag operator?
    vQ1 = ones(length(vD1))       # vector representation of orthogonal matrix Q
    vQ2 = zeros(length(vD2))      #
    vQ2 = zeros(length(vD2))      #
    vQ2m = zeros(length(vD2))     #
    veig = copy(vD1)      # vector of eigenvalues of D, initialized to diagonal of D
    # if D diagonal, nothing more will be computed

    i = 1
    while i < length(vD1)
        if vD2[i] == 0.0
            i += 1
        else
            mA = [vD1[i] vD2[i]; vD2[i] vD1[i+1]]  #  2X2 submatrix
            # DiagmA, Qma = eig(mA)                 #  spectral decomposition of mA
            X = eigen(mA)
            DiagmA = X.values
            Qma = X.vectors
            veig[i] = DiagmA[1]
            vQ1[i] = Qma[1, 1]
            vQ2[i] = Qma[1, 2]
            vQ2m[i] = Qma[2, 1]
            vQ1[i+1] = Qma[2, 2]
            veig[i+1] = DiagmA[2]
            i += 2
        end
    end

    Q = sparse(SparseArrays.spdiagm(0 => vQ1, -1 => vQ2m, 1 => vQ2)) # sparse representation of Q

    Q = Q[abs.(pivot), abs.(pivot)]
    veig = veig[abs.(pivot)]

    Δ = 1.0 ./ veig #'[:,1]             # don't forget inquire returns D^-1
    l_m, = findmin(Δ)

    ĝ = copy(g)
    ma97_solve!(PLD, ĝ, job = :PL)

    g̃ = Q' * ĝ

    λ = max(-l_m, 0.0)
    return PDataMA97(PLD, Δ, D, H, pivot, Q, g̃, g, λ, true, true)
end

function AInv(X::PDataMA97, d̃::Array{T,1}) where {T}
    d̂ = X.Q * d̃

    ma97_solve!(X.PLD, d̂, job = :LPS)

    return d̂
end


module Perturbativematrix
    using LinearAlgebra

    """
        PDmatrix{T} <: AbstractMatrix{T}
        A = A0 + t A1
        A0:Diagonal matrix
        A1:Offdiagonal matrix
        t:parameter

        A0::AbstractVector{T}
        A1::AbstractMatrix{T}
        t::T
    """
    struct PDmatrix <: AbstractMatrix
        A0::AbstractVector
        A1::AbstractMatrix
        t::T

        function PDmatrix(A0,A1,t)
            return new(A0,A1,t)
        end

    end

    function Base.getindex(A::PDmatrix,i::Int,j::Int)
        if i==j
            diag = A.A0[i,i]
        else
            diag = 0
        end
        return diag + A.t*A.A1[i,j]
    end

    function Base.:*(A::PDmatrix{T},B::PDmatrix{T})
        error("not supported yet")
    end

    function Base.:*(A::PDmatrix,v::T) where T <: AbstractVector
        x = A.A0 .* v
        x += A.t * A.A1*v
        return x
    end

    function LinearAlgebra.mul!(y,A::PDmatrix,x)
        N = length(y)
        for i=1:N
            y[i] = A.A0[i]*x[i]
        end
        mul!(y,A.A1,x,A.t,1)
    end

end


function shock_loading(m::HetDSGELag, TTT_jump::Matrix{Float64})
    exo = m.exogenous_shocks
    endo = m.endogenous_states

    _RRR = zeros(n_backward_looking_states(m), n_shocks_exogenous(m))
    RRR  = zeros(n_model_states(m), n_shocks_exogenous(m))

    _RRR[endo[:B′], exo[:b_sh]] .= 1.
    _RRR[endo[:G′], exo[:g_sh]] .= 1.
    _RRR[endo[:z′_t], exo[:z_sh]] .= 1.
    _RRR[endo[:MU′], exo[:μ_sh]] .= 1.
    _RRR[endo[:LAMW′], exo[:λ_w_sh]] .= 1.
    _RRR[endo[:LAMF′], exo[:λ_f_sh]] .= 1.
    _RRR[endo[:MON′], exo[:MON_sh]] .= 1.

    # Loading on states and jumps
    RRR[1:n_backward_looking_states(m), :] = _RRR
    RRR[n_backward_looking_states(m)+1:end, :] = TTT_jump*_RRR

    return RRR
end


include("euclidean-1-NN.jl")
include("../algoritmos-busqueda/busqueda-local.jl")
include("../utils/funcion-objetivo.jl")

"""
    BL_LearnerOneNN(data::Matrix{<:Real}, labels::Vector{<:Real})
Devuelve un clasificado 1-NN refinado por unos pesos en la distancia euclídea.
Ese vector de pesos se ha obtenido a partir de un algoritmo de búsqueda local
"""
function BL_LearnerOneNN(data::Matrix{<:Real}, labels)
    # Creamos función objetivo
    a = 0.5
    umbral_tasa_reduccion = 0.1
    F = CrearFuncionObjetivo(data, labels, a, umbral_tasa_reduccion )

    numero_atributos = size(data)[2] # los atributos son las columnas
    # Criterios de acorde a los requisitos
    numero_maximo_vecinos_sin_mejora = 20 * numero_atributos
    nuero_maximo_evaluaciones = 15000
    # Buscamos pesos de acorde a parámetros calculados
    w = PrimeroMejor(numero_maximo_vecinos_sin_mejora,
    nuero_maximo_evaluaciones,numero_atributos,umbral_tasa_reduccion, F)

    return WeightedLearnerEuclideanOneNN(w, data, labels), F(w), w
end


using Test
using StaticArrays, IntervalSets, LinearAlgebra

import ClimaCore: slab, Domains, Meshes, Topologies, Spaces, Fields

import ClimaCore.Geometry: Geometry
import ClimaCore.DataLayouts: IJFH

@testset "1d domain space" begin
    FT = Float64
    domain = Domains.IntervalDomain(
        Geometry.XPoint{FT}(-3) .. Geometry.XPoint{FT}(5),
        periodic = true,
    )
    mesh = Meshes.IntervalMesh(domain; nelems = 1)
    topology = Topologies.IntervalTopology(mesh)

    quad = Spaces.Quadratures.GLL{4}()
    points, weights = Spaces.Quadratures.quadrature_points(FT, quad)

    space = Spaces.SpectralElementSpace1D(topology, quad)

    coord_data = Spaces.coordinates_data(space)
    @test eltype(coord_data) == Geometry.XPoint{Float64}

    array = parent(Spaces.coordinates_data(space))
    @test size(array) == (4, 1, 1)
    coord_slab = slab(Spaces.coordinates_data(space), 1)
    @test coord_slab[1] == Geometry.XPoint{FT}(-3)
    @test coord_slab[4] == Geometry.XPoint{FT}(5)

    local_geometry_slab = slab(space.local_geometry, 1)
    dss_weights_slab = slab(space.dss_weights, 1)

    for i in 1:4
        @test Geometry.components(local_geometry_slab[i].∂x∂ξ) ≈
              @SMatrix [8 / 2]
        @test Geometry.components(local_geometry_slab[i].∂ξ∂x) ≈
              @SMatrix [2 / 8]
        @test local_geometry_slab[i].J ≈ (8 / 2)
        @test local_geometry_slab[i].WJ ≈ (8 / 2) * weights[i]
        if i in (1, 4)
            @test dss_weights_slab[i] ≈ 1 / 2
        else
            @test dss_weights_slab[i] ≈ 1
        end
    end

end

@testset "1×1 domain space" begin
    FT = Float32
    domain = Domains.RectangleDomain(
        Geometry.XPoint{FT}(-3) .. Geometry.XPoint{FT}(5),
        Geometry.YPoint{FT}(-2) .. Geometry.YPoint{FT}(8),
        x1periodic = true,
        x2periodic = false,
        x2boundary = (:south, :north),
    )
    mesh = Meshes.RectilinearMesh(domain, 1, 1)
    grid_topology = Topologies.Topology2D(mesh)

    quad = Spaces.Quadratures.GLL{4}()
    points, weights = Spaces.Quadratures.quadrature_points(FT, quad)

    space = Spaces.SpectralElementSpace2D(grid_topology, quad)

    array = parent(Spaces.coordinates_data(space))
    @test size(array) == (4, 4, 2, 1)
    coord_slab = slab(Spaces.coordinates_data(space), 1)
    @test coord_slab[1, 1] ≈ Geometry.XYPoint{FT}(-3.0, -2.0)
    @test coord_slab[4, 1] ≈ Geometry.XYPoint{FT}(5.0, -2.0)
    @test coord_slab[1, 4] ≈ Geometry.XYPoint{FT}(-3.0, 8.0)
    @test coord_slab[4, 4] ≈ Geometry.XYPoint{FT}(5.0, 8.0)

    local_geometry_slab = slab(space.local_geometry, 1)
    dss_weights_slab = slab(space.dss_weights, 1)

    for i in 1:4, j in 1:4
        @test Geometry.components(local_geometry_slab[i, j].∂x∂ξ) ≈
              @SMatrix [8/2 0; 0 10/2]
        @test Geometry.components(local_geometry_slab[i, j].∂ξ∂x) ≈
              @SMatrix [2/8 0; 0 2/10]
        @test local_geometry_slab[i, j].J ≈ (10 / 2) * (8 / 2)
        @test local_geometry_slab[i, j].WJ ≈
              (10 / 2) * (8 / 2) * weights[i] * weights[j]
        if i in (1, 4)
            @test dss_weights_slab[i, j] ≈ 1 / 2
        else
            @test dss_weights_slab[i, j] ≈ 1
        end
    end

    @test length(space.boundary_surface_geometries) == 2
    @test keys(space.boundary_surface_geometries) == (:south, :north)
    @test sum(parent(space.boundary_surface_geometries.north.sWJ)) ≈ 8
    @test parent(space.boundary_surface_geometries.north.normal)[1, :, 1] ≈
          [0.0, 1.0]
end

@testset "dss on 2×2 rectangular mesh (unstructured)" begin
    FT = Float64
    n1, n2 = 2, 2
    domain = Domains.RectangleDomain(
        Geometry.XPoint{FT}(0) .. Geometry.XPoint{FT}(4),
        Geometry.YPoint{FT}(0) .. Geometry.YPoint{FT}(4),
        x1periodic = false,
        x2periodic = false,
        x1boundary = (:west, :east),
        x2boundary = (:south, :north),
    )
    mesh = Meshes.RectilinearMesh(domain, n1, n2)
    grid_topology = Topologies.Topology2D(mesh)

    quad = Spaces.Quadratures.GLL{4}()
    points, weights = Spaces.Quadratures.quadrature_points(FT, quad)

    space = Spaces.SpectralElementSpace2D(grid_topology, quad)

    array = parent(Spaces.coordinates_data(space))
    @test size(array) == (4, 4, 2, 4)

    Nij = length(points)
    field = Fields.Field(IJFH{FT, Nij}(ones(Nij, Nij, 1, n1 * n2)), space)
    field_values = Fields.field_values(field)
    Spaces.horizontal_dss!(field)

    @testset "dss should not modify interior degrees of freedom of any element" begin
        result = true
        for el in 1:(n1 * n2)
            slb = slab(field_values, 1, el)
            for i in 2:(Nij - 1), j in 2:(Nij - 1)
                if slb[i, j] ≠ 1
                    result = false
                end
            end
        end
        @test result
    end
    s1 = slab(field_values, 1, 1)
    s2 = slab(field_values, 1, 2)
    s3 = slab(field_values, 1, 3)
    s4 = slab(field_values, 1, 4)

    @testset "vertex common to all (4) elements" begin
        @test (s1[Nij, Nij] == s2[1, Nij] == s3[Nij, 1] == s4[1, 1])
    end

    @testset "vertices common to (2) elements" begin
        @test s1[Nij, 1] == s2[1, 1]
        @test s1[1, Nij] == s3[1, 1]
        @test s2[Nij, Nij] == s4[Nij, 1]
        @test s3[Nij, Nij] == s4[1, Nij]
    end

    @testset "boundary faces" begin
        for fc in 2:(Nij - 1)
            @test s1[1, fc] == 1 # element 1 face 1
            @test s1[fc, 1] == 1 # element 1 face 3
            @test s2[Nij, fc] == 1 # element 2 face 2
            @test s2[fc, 1] == 1 # element 2 face 3
            @test s3[1, fc] == 1 # element 3 face 1
            @test s3[fc, Nij] == 1 # element 3 face 4
            @test s4[Nij, fc] == 1 # element 4 face 2
            @test s4[fc, Nij] == 1 # element 4 face 4
        end
    end

    @testset "interior faces" begin
        for fc in 2:(Nij - 1)
            @test (s1[Nij, fc] == s2[1, fc] == 2) # (e1, f2) == (e2, f1) == 2
            @test (s1[fc, Nij] == s3[fc, 1] == 2) # (e1, f4) == (e3, f3) == 2
            @test (s2[fc, Nij] == s4[fc, 1] == 2) # (e2, f4) == (e4, f3) == 2
            @test (s3[Nij, fc] == s4[1, fc] == 2) # (e3, f2) == (e4, f1) == 2
        end
    end
end

@testset "dss on 2×2 rectangular mesh" begin
    FT = Float64
    n1, n2 = 2, 2
    Nij = 4
    domain = Domains.RectangleDomain(
        Geometry.XPoint{FT}(0) .. Geometry.XPoint{FT}(4),
        Geometry.YPoint{FT}(0) .. Geometry.YPoint{FT}(4),
        x1periodic = false,
        x2periodic = false,
        x1boundary = (:west, :east),
        x2boundary = (:south, :north),
    )
    mesh = Meshes.RectilinearMesh(domain, n1, n2)
    grid_topology = Topologies.Topology2D(mesh)

    quad = Spaces.Quadratures.GLL{Nij}()
    points, weights = Spaces.Quadratures.quadrature_points(FT, quad)

    space = Spaces.SpectralElementSpace2D(grid_topology, quad)

    array = parent(Spaces.coordinates_data(space))
    @test size(array) == (Nij, Nij, 2, n1 * n2)

    data = zeros(Nij, Nij, 3, n1 * n2)
    data[:, :, 1, :] .= 1:Nij
    data[:, :, 2, :] .= (1:Nij)'
    data[:, :, 3, :] .= reshape(1:(n1 * n2), 1, 1, :)
    field = Fields.Field(IJFH{Tuple{FT, FT, FT}, Nij}(data), space)
    field_dss = Spaces.horizontal_dss!(copy(field))
    data_dss = parent(field_dss)

    @testset "slab 1" begin
        @test data_dss[1:(Nij - 1), 1:(Nij - 1), :, 1] ==
              data[1:(Nij - 1), 1:(Nij - 1), :, 1]
        @test data_dss[Nij, 1:(Nij - 1), :, 1] ==
              data[Nij, 1:(Nij - 1), :, 1] .+ data[1, 1:(Nij - 1), :, 2]
        @test data_dss[1:(Nij - 1), Nij, :, 1] ==
              data[1:(Nij - 1), Nij, :, 1] .+ data[1:(Nij - 1), 1, :, 3]
        @test data_dss[Nij, Nij, :, 1] ==
              data[Nij, Nij, :, 1] .+ data[1, Nij, :, 2] .+
              data[Nij, 1, :, 3] .+ data[1, 1, :, 4]
    end

    @testset "slab 2" begin
        @test data_dss[2:Nij, 1:(Nij - 1), :, 2] ==
              data[2:Nij, 1:(Nij - 1), :, 2]
        @test data_dss[1, 1:(Nij - 1), :, 2] ==
              data[Nij, 1:(Nij - 1), :, 1] .+ data[1, 1:(Nij - 1), :, 2]
        @test data_dss[2:Nij, Nij, :, 2] ==
              data[2:Nij, Nij, :, 2] .+ data[2:Nij, 1, :, 4]
        @test data_dss[1, Nij, :, 2] ==
              data[Nij, Nij, :, 1] .+ data[1, Nij, :, 2] .+
              data[Nij, 1, :, 3] .+ data[1, 1, :, 4]
    end

    @testset "slab 3" begin
        @test data_dss[1:(Nij - 1), 2:Nij, :, 3] ==
              data[1:(Nij - 1), 2:Nij, :, 3]
        @test data_dss[Nij, 2:Nij, :, 3] ==
              data[Nij, 2:Nij, :, 3] .+ data[1, 2:Nij, :, 4]
        @test data_dss[1:(Nij - 1), 1, :, 3] ==
              data[1:(Nij - 1), Nij, :, 1] .+ data[1:(Nij - 1), 1, :, 3]
        @test data_dss[Nij, 1, :, 3] ==
              data[Nij, Nij, :, 1] .+ data[1, Nij, :, 2] .+
              data[Nij, 1, :, 3] .+ data[1, 1, :, 4]
    end

    @testset "slab 3" begin
        @test data_dss[2:Nij, 2:Nij, :, 4] == data[2:Nij, 2:Nij, :, 4]
        @test data_dss[1, 2:Nij, :, 4] ==
              data[Nij, 2:Nij, :, 3] .+ data[1, 2:Nij, :, 4]
        @test data_dss[2:Nij, 1, :, 4] ==
              data[2:Nij, Nij, :, 2] .+ data[2:Nij, 1, :, 4]
        @test data_dss[1, 1, :, 4] ==
              data[Nij, Nij, :, 1] .+ data[1, Nij, :, 2] .+
              data[Nij, 1, :, 3] .+ data[1, 1, :, 4]
    end
end


function CL(ALPHA,BETA)
    A=[   0.     0.     0.     0.     0.     0.     0.     0.     0.     0.     0.     0.
       -.001  -.004  -.008  -.012  -.016  -.019  -.020  -.020  -.015  -.008  -.013  -.015
       -.003  -.009  -.017  -.024  -.030  -.034  -.040  -.037  -.016  -.002  -.010  -.019
       -.001  -.010  -.020  -.030  -.039  -.044  -.050  -.049  -.023  -.006  -.014  -.027
        .000  -.010  -.022  -.034  -.047  -.046  -.059  -.061  -.033  -.036  -.035  -.035
        .007  -.010  -.023  -.034  -.049  -.046  -.068  -.071  -.060  -.058  -.062  -.059
        .009  -.011  -.023  -.037  -.050  -.047  -.074  -.079  -.091  -.076  -.077  -.076]'
      #
      S= 0.2 * ALPHA
      K= floor(S)+3
      if(K <= 1)
          K= 2
      end
      if(K >=  12)
          K=  11
      end
      DA= S - K +3
      L = K + fix( 1.1*sign(DA) )
      #
      S= .2* abs(BETA)
      M= floor(S)+1
      if(M <=  1)
          M= 2
      end
      if(M >=  7)
          M= 6
      end
      DB= S - M +1
      N= M + fix( 1.1*sign(DB) )
      #
      K=convert(Int64, K)
      L=convert(Int64, L)
      M=convert(Int64, M)
      N=convert(Int64, N)
      T= A[K,M]
      U= A[K,N]
      V= T + abs(DA) * (A[L,M] - T)
      W= U + abs(DA) * (A[L,N] - U)
      DUM= V + (W-V)  * abs(DB)
      return clv = DUM * sign(BETA)
end


##
# Abstract Algebra.
abstract type AbstractAlgebra end

# abstract type HeytingAlgebra end

# TODO remove domain from Boolean Algebra?
# TODO remove parametric types?
# TODO add AbstractHeytingAlgebra whose subtypes are ChainHeytingAlgebra and CustomHeytingAlgebra
struct BooleanAlgebra{DT,MT<:Function,JT<:Function,IT<:Function,BT,TT} <: AbstractAlgebra
    name::String
    domain::DT
    meet::MT
    join::JT
    impl::IT
    bottom::BT
    top::TT
end

struct HeytingAlgebra{DT,MT<:Function,JT<:Function,IT<:Function,BT,TT} <: AbstractAlgebra
    name::String
    domain::DT
    meet::MT
    join::JT
    impl::IT
    bottom::BT
    top::TT
end

# Boolean and
∧(a::Bool,b::Bool)          = ifelse(a == true && a == b, true, false)
∧(a::Int64,b::Int64)        = ifelse(a == 1 && a == b, 1, 0)
∧(a::Float64,b::Float64)    = ifelse(a == 1.0 && a == b, 1.0, 0.0)
∧(a,b,args...)              = ∧(∧(a,b),args...)

# Boolean or
∨(a::Bool,b::Bool)          = ifelse(a == false && a == b, false, true)
∨(a::Int64,b::Int64)        = ifelse(a == 0 && a == b, 0, 1)
∨(a::Float64,b::Float64)    = ifelse(a == 0.0 && a == b, 0.0, 1.0)
∨(a,b,args...)              = ∨(∨(a,b),args...)

# Boolean implication
→(a::Bool,b::Bool)          = ifelse(a == true && b == false, false, true)
→(a::Int64,b::Int64)        = ifelse(a == 1 && b == 0, 0, 1)
→(a::Float64,b::Float64)    = ifelse(a == 1.0 && b == 0.0, 0.0, 1.0)
→(a,b,args...)              = →(→(a,b),args...)

# Heyting chain implication
(↪)(a::Float64, b::Float64) = a ≤ b ? float(1) : b

const BAlgebra = BooleanAlgebra("Boolean Algebra",[0,1],∧,∨,→,0,1)
const HCAlgebra(step::Float64) = HeytingAlgebra("Heyting Chain Algebra", [x for x ∈ 0.0:step:1.0],min,max,↪,0.0,1.0)

# function ==̃ₕ(x::Int64, y::Int64; h::Int64=4)
#     # |x-y| ≥ h
#     if abs(x-y) ≥ h
#         return float(0)
#     # |x-y| < h
#     else
#         return (h - abs(x-y))/h
#     end
# end

# function <̃ₕ(x::Int64, y::Int64; h::Int64=4)
#     # y-x > h
#     if y-x > h
#         return float(1)
#     # y ≦ x
#     elseif y ≤ x
#         return float(0)
#     # y-x ≦ h
#     elseif y-x ≤ h
#         return (y-x)/h
#     end
# end


function rand(rng::AbstractRNG, ::Random.SamplerTrivial{Random.CloseOpen01{DoubleFloat{T}}}) where {T<:IEEEFloat}
    hi, lo  = rand(rng, T, 2)
    if hi === zero(T)
        if lo === zero(T)
            return zero(DoubleFloat(T))
        end
        hi, lo = lo, hi
    end
    frlo, xplo  = frexp(lo)
    xplo = Base.exponent(hi) - min(1, fld(xplo,4)) - abs(Base.exponent(eps(hi)))
    lo = ldexp(frlo, xplo)
    lo = rand(rng, Bool) ? lo : -lo

    DoubleFloat(hi, lo)
end

function rand(rng::AbstractRNG, ::Random.SamplerTrivial{Random.CloseOpen01{Complex{DoubleFloat{T}}}}) where {T<:IEEEFloat}
    re = rand(rng, DoubleFloat{T})
    im = rand(rng, DoubleFloat{T})
    return Complex{DoubleFloat{T}}(re, im)
end

function randpm(::Type{DoubleFloat{T}}) where {T<:IEEEFloat}
    r = rand(DoubleFloat{T})
    r = rand(Bool) ? r : -r
    return r
end

function randpm(rng::MersenneTwister, ::Type{DoubleFloat{T}}) where {T<:IEEEFloat}
    r = rand(rng, DoubleFloat{T})
    r = rand(rng, Bool) ? r : -r
    return r
end

function randpm(::Type{Complex{DoubleFloat{T}}}) where {T<:IEEEFloat}
    re = randpm(DoubleFloat{T})
    im = randpm(DoubleFloat{T})
    return Complex{DoubleFloat{T}}(re, im)
end

function randpm(rng::MersenneTwister, ::Type{Complex{DoubleFloat{T}}}) where {T<:IEEEFloat}
    re = randpm(rng, DoubleFloat{T})
    im = randpm(rng, DoubleFloat{T})
    return Complex{DoubleFloat{T}}(re, im)
end

function randpm(::Type{DoubleFloat{T}}, n::Int) where {T<:IEEEFloat}
    rs = rand(DoubleFloat{T}, n)
    sgns = ones(n) .- (2 .* rand(Bool, n))
    return rs .* sgns
end

function randpm(::Type{Complex{DoubleFloat{T}}}, n::Int) where {T<:IEEEFloat}
    res = rand(DoubleFloat{T}, n)
    ims = rand(DoubleFloat{T}, n)
    o = ones(n)
    rsgns = o .- (2 .* rand(Bool,n))
    isgns = o .- (2 .* rand(Bool,n))
    res = res .* rsgns
    ims = ims .* isgns
    return map((re,im)->Complex{DoubleFloat{T}}(re,im), res, ims)
end

function randpm(rng::MersenneTwister, ::Type{DoubleFloat{T}}, n::Int) where {T<:IEEEFloat}
    rs = rand(rng, DoubleFloat{T}, n)
    sgns = ones(n) .- (2 .* rand(rng, Bool, n))
    return rs .* sgns
end

function randpm(rng::MersenneTwister, ::Type{Complex{DoubleFloat{T}}}, n::Int) where {T<:IEEEFloat}
    res = rand(rng, DoubleFloat{T}, n)
    ims = rand(rng, DoubleFloat{T}, n)
    o = ones(n)
    rsgns = o .- (2 .* rand(rng, Bool,n))
    isgns = o .- (2 .* rand(rng, Bool,n))
    res = res .* rsgns
    ims = ims .* isgns
    return map((re,im)->Complex{DoubleFloat{T}}(re,im), res, ims)
end

# normal variates

function randn(rng::AbstractRNG, ::Type{DoubleFloat{T}}) where {T<:IEEEFloat}
    urand1, urand2 = rand(rng, DoubleFloat{T}, 2)
    urand1 = urand1 + urand1 - 1
    urand2 = urand2 + urand2 - 1
    s = urand1*urand1 + urand2*urand2

    while s >= 1 || s === 0
        urand1, urand2 = rand(rng, DoubleFloat{T}, 2)
        urand1 = urand1 + urand1 - 1
        urand2 = urand2 + urand2 - 1
        s = urand1*urand1 + urand2*urand2
    end

    s = sqrt( -log(s) / s )
    return (urand1 + urand2) * s
end


using ClusterTrees, BEAST, CompScienceMeshes

a, h = 1.0, 0.061
Γ1 = meshsphere(a, h)
Γ2 = Γ1
#Γ2 = CompScienceMeshes.translate(Γ1, point(2.10,0,0))

X1 = raviartthomas(Γ1)
X2 = X1
#X2 = raviartthomas(Γ2)

@show numfunctions(X1)
@show numfunctions(X2)

κ = 1.0; γ = κ*im;
T = Maxwell3D.singlelayer(gamma=γ)

μ = (τ,σ) -> assemble(T, subset(X1,τ), subset(X2,σ))
A = ClusterTrees.h1compress(T,X1,X2)

sz = storedentries(A)
println("Compression: ", sz/numfunctions(X1)/numfunctions(X2))


module Source

using SparseArrays

"""
Determine the indices and values of the source discretization stencil that
spans `p` cells.

Input arguments:
x : List of coordinates
xs : Source location
p : Polynomial degree.
s : Number of smoothness conditions.

Return values:
stencil : An array of the stencil indices that maps to the `x` coordinates.
coef : An array of stencil coefficients (values).
alpha : The signed distance from the source location to the nearest grid point
"""
function source_smoothness(x, xs, p, s)

        idx = argmin(abs.(x .- xs))

        # Determine signed distance from source location to nearest point.
        alpha = x[idx] - xs

        shift::Int64 = 0
        if alpha < 0
                shift = 0
        else
                shift = -1
        end

        if p == 0
                coef = 1
                stencil = idx
                return
        end

        if mod(p,1) == 1
                error("Polynomial degree must be odd")
        end

        left = round((p-1)/2) + round(s/2);
        right = p + s - left - 1;

        idx_l::Int64 = idx - left + shift;
        idx_r::Int64 = idx + right + shift + 1;

        adjust::Int64 = 0;
        if idx_l < 1
                adjust = 1 - idx_l;
        end
        if idx_r > length(x)
                adjust = length(x) - idx_r;
        end

        stencil = collect((adjust + idx_l):(adjust + idx_r));

        # Moment conditions
        e = ones(size(stencil))
        xl = x[stencil] .- x[idx];
        M = zeros(p+1, size(xl, 1))
        for i in 1:p+1
        for j in 1:size(xl, 1)
                M[i,j] = xl[j] ^ (i - 1)
        end
        end
        b1 = (xs - x[idx]).^(0:p);

        if s > 0
                # Smoothness conditions
                S = zeros(s,size(xl, 1))
                for i in 1:s
                for j in 1:size(xl, 1)
                        S[i,j] = (-1)^j * xl[j] ^ (i - 1)
                end
                end
                A = [M; S]
                b = [b1; zeros(s,1)]
        else
                A = M
                b = b1
        end
        coef = A\b;
return stencil, coef, alpha
end

function source_discretize_1d(r1_s, p, s, r1, h1)
        d = spzeros(length(r1))
        stencil1, coef1, alpha1 = source_smoothness(r1, r1_s, p, s)
        for i=1:length(stencil1)
                d[stencil1[i]] = coef1[i] / h1
        end

        return d
end
"""
d = source_disc(xs,ys, h, gr, J)
Discretize point source using nearest grid point

Arguments:
xs, ys : Source location
h : Grid spacing in parameter space
gr : grid object that contains X,Y meshgrids.
J : Jacobian

Returns
A sparse vector that is zero everywhere except for one index, which is set to
a weight d = 1/(cell-area).
"""
function source_discretize_2d(r1_s, r2_s, p, s, r1, r2, h1, h2)
        d = spzeros(length(r1)*length(r2))
        stencil1, coef1, alpha1 = source_smoothness(r1, r1_s, p, s)
        stencil2, coef2, alpha2 = source_smoothness(r2, r2_s, p, s)

        n2 = length(r2)
        reci_area = 1 / (h1 * h2)
        for i=1:length(stencil1)
        for j=1:length(stencil2)
          d[stencil2[j] + n2 * (stencil1[i] - 1)] = coef1[i] * coef2[j] * reci_area
        end
        end

        return d
end

end


function fac(x::BigInt)
    if x < 2
        return 1
    end
    return x * fac(x - 1)
end

print(fac(BigInt(100999)))


using GeoPhyInv
using Statistics
using Test

medium = Medium(:elastic_homo2D, 5)

medium_new = similar(medium)
update!(medium_new, [:vp, :rho, :vs], rectangle = [[-500, -500], [500, 500]], perc = 5.0) # perturb velocity

ageom = AGeom(medium.mgrid, :surf, SSrcs(3), Recs(100)); # surface seismic

tgrid = range(0.0, stop = 2.0, length = 2500) # generate a temporal grid
wav = ricker(15.0, tgrid, tpeak = 0.25); # Choose a source wavelet
srcwav = SrcWav(tgrid, ageom, [:vz]) # initialize
update!(srcwav, [:vz], wav) # distribute to all supersources

pa = SeisForwExpt(
    FdtdElastic(),
    medium = medium,
    ageom = ageom,
    srcwav = srcwav,
    tgrid = tgrid,
    rfields = [:vz],
    verbose = true,
);

@time update!(pa);
d1 = copy(pa[:data][:vz])

update!(pa, medium_new)

@time update!(pa);
d2 = copy(pa[:data][:vz])

@test d1 ≠ d2


struct AffineStrategy{nx, nu, TP<:SMatrix{nu, nx}, TA<:SVector{nu}}
    "The proportional gain."
    P::TP
    "The feed-forward term."
    α::TA
end

n_states(γ::AffineStrategy{nx}) where {nx} = nx
n_controls(γ::AffineStrategy{nx, nu}) where {nx, nu} = nu

Base.zero(γ::AffineStrategy) = zero(typeof(γ))
function Base.zero(::Type{<:AffineStrategy{nx, nu, TP, TA}}) where {nx, nu, TP, TA}
    return AffineStrategy(zero(TP), zero(TA))
end

"""
    $(TYPEDSIGNATURES)

Computes the control input for a given affine strategy γ, for a state reference
deviation of Δx and a control reference of ũ.
"""
control_input(γ::AffineStrategy, Δx::AbstractVector, ũ::AbstractVector) = ũ - γ.P*Δx - γ.α


using MultivariateStats
using LinearAlgebra
using Test
using StableRNGs
using Statistics: mean, cov, cor

@testset "CCA" begin

    rng = StableRNG(34568)

    dx = 5
    dy = 6
    p = 3

    # CCA with zero means

    X = rand(rng, dx, 100)
    Y = rand(rng, dy, 100)

    Px = qr(randn(rng, dx, dx)).Q[:, 1:p]
    Py = qr(randn(rng, dy, dy)).Q[:, 1:p]

    M = CCA(Float64[], Float64[], Px, Py, [0.8, 0.6, 0.4])

    @test size(M)[1] == dx
    @test size(M)[2] == dy
    @test mean(M, :x) == zeros(dx)
    @test mean(M, :y) == zeros(dy)
    @test_throws ArgumentError mean(M, :z)
    @test projection(M, :x) == Px
    @test projection(M, :y) == Py
    @test_throws ArgumentError projection(M, :z)
    @test cor(M) == [0.8, 0.6, 0.4]

    @test predict(M, X, :x) ≈ Px'X
    @test predict(M, Y, :y) ≈ Py'Y

    ## CCA with nonzero means

    ux = randn(rng, dx)
    uy = randn(rng, dy)

    M = CCA(ux, uy, Px, Py, [0.8, 0.6, 0.4])

    @test size(M)[1] == dx
    @test size(M)[2] == dy
    @test mean(M, :x) == ux
    @test mean(M, :y) == uy
    @test projection(M, :x) == Px
    @test projection(M, :y) == Py
    @test cor(M) == [0.8, 0.6, 0.4]

    @test predict(M, X, :x) ≈ Px' * (X .- ux)
    @test predict(M, Y, :y) ≈ Py' * (Y .- uy)

    ## prepare data

    n = 1000
    dg = 10
    G = randn(rng, dg, n)

    X = randn(rng, dx, dg) * G + 0.2 * randn(rng, dx, n)
    Y = randn(rng, dy, dg) * G + 0.2 * randn(rng, dy, n)
    xm = vec(mean(X, dims=2))
    ym = vec(mean(Y, dims=2))
    Zx = X .- xm
    Zy = Y .- ym

    Cxx = cov(X, dims=2)
    Cyy = cov(Y, dims=2)
    Cxy = cov(X, Y, dims=2)
    Cyx = Cxy'

    ## ccacov

    # X ~ Y
    M = fit(CCA, X, Y; method=:cov, outdim=p)
    Px = projection(M, :x)
    Py = projection(M, :y)
    rho = cor(M)
    @test size(M)[1] == dx
    @test size(M)[2] == dy
    @test size(M)[3] == p
    @test mean(M, :x) == xm
    @test mean(M, :y) == ym
    @test issorted(rho; rev=true)

    @test Px' * Cxx * Px ≈ Matrix(I, p, p)
    @test Py' * Cyy * Py ≈ Matrix(I, p, p)
    @test Cxy * (Cyy \ Cyx) * Px ≈ Cxx * Px * Diagonal(rho.^2)
    @test Cyx * (Cxx \ Cxy) * Py ≈ Cyy * Py * Diagonal(rho.^2)
    @test Px ≈ MultivariateStats.qnormalize!(Cxx \ (Cxy * Py), Cxx)
    @test Py ≈ MultivariateStats.qnormalize!(Cyy \ (Cyx * Px), Cyy)

    # Y ~ X
    M = fit(CCA, Y, X; method=:cov, outdim=p)
    Py = projection(M, :x)
    Px = projection(M, :y)
    rho = cor(M)
    @test size(M)[1] == dy
    @test size(M)[2] == dx
    @test size(M)[3] == p
    @test mean(M, :x) == ym
    @test mean(M, :y) == xm
    @test issorted(rho; rev=true)

    @test Px' * Cxx * Px ≈ Matrix(I, p, p)
    @test Py' * Cyy * Py ≈ Matrix(I, p, p)
    @test Cxy * (Cyy \ Cyx) * Px ≈ Cxx * Px * Diagonal(rho.^2)
    @test Cyx * (Cxx \ Cxy) * Py ≈ Cyy * Py * Diagonal(rho.^2)
    @test Px ≈ MultivariateStats.qnormalize!(Cxx \ (Cxy * Py), Cxx)
    @test Py ≈ MultivariateStats.qnormalize!(Cyy \ (Cyx * Px), Cyy)

    ## ccasvd

    # n > d
    M = fit(CCA, X, Y; method=:svd, outdim=p)
    Px = projection(M, :x)
    Py = projection(M, :y)
    rho = cor(M)
    @test size(M)[1] == dx
    @test size(M)[2] == dy
    @test size(M)[3] == p
    @test mean(M, :x) == xm
    @test mean(M, :y) == ym
    @test issorted(rho; rev=true)

    @test Px' * Cxx * Px ≈ Matrix(I, p, p)
    @test Py' * Cyy * Py ≈ Matrix(I, p, p)
    @test Cxy * (Cyy \ Cyx) * Px ≈ Cxx * Px * Diagonal(rho.^2)
    @test Cyx * (Cxx \ Cxy) * Py ≈ Cyy * Py * Diagonal(rho.^2)
    @test Px ≈ MultivariateStats.qnormalize!(Cxx \ (Cxy * Py), Cxx)
    @test Py ≈ MultivariateStats.qnormalize!(Cyy \ (Cyx * Px), Cyy)

    # different input types
    XX = convert.(Float32, X)
    YY = convert.(Float32, Y)

    MM = fit(CCA, view(XX, :, 1:400), view(YY, :, 1:400); method=:svd, outdim=p)

    # test that mixing types doesn't error
    predict(M, XX, :x)
    predict(M, YY, :y)
    predict(MM, XX, :x)
    predict(MM, YY, :y)

    # type stability
    for func in (M->mean(M, :x), M->mean(M, :y),
                 M->projection(M, :x),
                 M->projection(M, :y), cor)
        @test eltype(func(M)) == Float64
        @test eltype(func(MM)) == Float32
    end
end


# Can we approximate power-2 limb-darkening with polynomial?

using PyPlot
include("/Users/ericagol/Computer/Julia/regress.jl")

alpha = [0.2,0.25,0.3,0.35,0.4,0.45,0.5,0.55,0.6,0.65,0.7,0.75,0.8,0.85,0.9,0.95,0.99,1.01,1.05,1.1,1.15,1.2,1.25,1.3,1.35,1.4,1.45,1.5,1.55,1.6,1.65,1.7,1.75,1.8]
#nmu = 1000; mu = logspace(-big(2.5),big(0.0),nmu)
nalpha = length(alpha)
nmu = 1000; mu = linspace(1e-3,1.0,nmu)
nord = 30
coeff = zeros(nalpha,nord,nord-1)
scat = zeros(nalpha,nord)
for ialpha=1:nalpha
  fnon = mu.^alpha[ialpha]-mu
  for i=2:nord
    fn = zeros(i-1,nmu)
#    fn[1,:] .= big(1.0)
#    fn[1,:] .= 1.0
#    fn[2,:] .= mu
    for j=2:i
      fn[j-1,:] .= (j+2)*mu.^j-j*mu.^(j-2)-2mu
    end
    ci,cov = regress(fn,fnon,ones(Float64,nmu))
    fmod = zeros(Float64,nmu)
    for j=1:i-1
      fmod += ci[j]*fn[j,:]
    end
    clf()
    plot(mu,convert(Array{Float64,1},fnon))
    plot(mu,convert(Array{Float64,1},fmod))
    plot(mu,convert(Array{Float64,1},fmod-fnon))
    scat[ialpha,i]=convert(Float64,std(fmod-fnon))
    coeff[ialpha,i,1:i-1]=convert(Array{Float64,1},ci)
    println("alpha: ",convert(Float64,alpha[ialpha])," i: ",i," scatter: ",scat[ialpha,i]," max: ",maximum(abs,convert(Array{Float64,1},fmod-fnon)))
  end
end


using LinearAlgebraicRepresentation, SparseArrays
Lar = LinearAlgebraicRepresentation;
using IntervalTrees,LinearAlgebra
# using Revise,
# using OhMyREPL

#=
Method to compute an internal point to a polyhedron.
----------------------------------------------------

1. Take two of points close to the opposite sides of any face of a polyhedron, e.g., the first face.
2. For each of the two points compute the intersections of a (vertical) ray with the planes (of the faces) intersected by the ray (positive direction of the half-line).
3. Transform each such plane and face (and intersection point) to 2D space.
4. Test for point-in-polygon intersection.
5. Compute the parity of the intersection points for each ray.
6. Invariant:  if one is even; the other is odd.
7. The initial point with odd number of intersection points is interior to the polyhedron. The other is exterior.
=#

"""
	spaceindex(point3d)(model)

Compute the set of face boxes of possible intersection with a point-ray.
Work in 3D, where the ray direction is parallel to the z-axis.
Return an array of indices of face.

#	Example
```
julia> V,(VV,EV,FV,CV) = Lar.cuboidGrid([1,1,1],true)

julia> spaceindex([.5,.5,.5])((V,FV))
3-element Array{Int64,1}:
 5
 6
```
"""
function spaceindex(point3d::Array{Float64,1})::Function
	function spaceindex0(model::Lar.LAR)::Array{Int,1}
				V,CV = copy(model[1]),copy(model[2])
		V = [V point3d]
		dim, idx = size(V)
		push!(CV, [idx,idx,idx])
		cellpoints = [ V[:,CV[k]]::Lar.Points for k=1:length(CV) ]
		#----------------------------------------------------------
		bboxes = [hcat(Lar.boundingbox(cell)...) for cell in cellpoints]
		xboxdict = Lar.coordintervals(1,bboxes)
		yboxdict = Lar.coordintervals(2,bboxes)
		# xs,ys are IntervalTree type
		xs = IntervalTrees.IntervalMap{Float64, Array}()
		for (key,boxset) in xboxdict
			xs[tuple(key...)] = boxset
		end
		ys = IntervalTrees.IntervalMap{Float64, Array}()
		for (key,boxset) in yboxdict
			ys[tuple(key...)] = boxset
		end
		xcovers = Lar.boxcovering(bboxes, 1, xs)
		ycovers = Lar.boxcovering(bboxes, 2, ys)
		covers = [intersect(pair...) for pair in zip(xcovers,ycovers)]

		# add new code part

		# remove each cell from its cover
		pointcover = setdiff(covers[end],[idx+1])
		return pointcover[1:end-1]
	end
	return spaceindex0
end

"""
	rayintersection(point3d::Array{Float64})(V,FV,face::Int)

Compute the intersection point of the vertical line through `point3d` w `face`.
If the face is parallel to `z axis` return `false`.
# Example
```
julia> V,(VV,EV,FV,CV) = Lar.simplex(3,true);

julia> V
3×4 Array{Float64,2}:
 0.0  1.0  0.0  0.0
 0.0  0.0  1.0  0.0
 0.0  0.0  0.0  1.0

julia> FV
4-element Array{Array{Int64,1},1}:
 [1, 2, 3]
 [1, 2, 4]
 [1, 3, 4]
 [2, 3, 4]

 julia> Lar.rayintersection([.333,.333,0])(V,FV,4)
 3-element Array{Float64,1}:
  0.333
  0.333
  0.3340000000000001
```
"""
function rayintersection(point3d)
	function rayintersection0(V, FV, face::Int)
						l0, l = point3d, [0,0,1.]
		ps = V[:,FV[face]]  # face points
		p0 = ps[:,1]
		v1, v2 = ps[:,2]-p0, ps[:,3]-p0
		n = LinearAlgebra.normalize(cross( v1,v2  ))

		denom = dot(n, l)
		if (abs(denom) > 1e-8) #1e-6
			p0l0 = p0 - l0
			t = dot(p0l0, n) / denom
			if t>0 return l0 + t*l end
		else
			#error("ray and face are parallel")
			return false
	 	end
	end
	return rayintersection0
end

"""
	planemap(V,copEV,copFE,face)(point)

Tranform the 3D face and the 3D point in their homologous 2D, in order to test for containment.
"""
function planemap(V,copEV,copFE,face)
	fv, edges = Lar.vcycle(copEV, copFE, face)
	# Fv = Dict(zip(1:length(fv),fv))
	# edges = [[Fv[u],Fv[v]] for (u,v) in edges]
	function planemap0(point)
		vs = V[:,fv]
		# Plasm.view(Plasm.numbering(0.5)((vs,[[[k] for k=1:4],edges])))
		#translation
		point = point .- vs[:,1]
		vs = vs .- vs[:,1]
		u,v = edges[1]
		z,w = [[z,w] for (z,w) in edges if z==v][1]
		v1 = vs[:,u]-vs[:,v]
		v2 = vs[:,w]-vs[:,v]
		v3 = cross(v2,v1)
		M = [v1 v2 v3]
		vs = inv(M) * [vs point]
		outvs = vs[1:2, 1:end-1]
		outpoint = vs[1:2, end]
		return outvs, edges, outpoint
	end
	return planemap0
end

function settestpoints(V,EV,FV,Fs, copEV,copFE)
	f = Fs[1]
	e = findnz(copFE[f,:])[1][1] # first (global) edge of first (global) face
	f1,f2 = findnz(copFE[:,e])[1] # two (global) faces incident on it
	v1,v2 = findnz(copEV[e,:])[1] # two (global) verts incident on in
	fdict = Dict(zip(Fs,1:length(Fs)))
	V1 = FV[fdict[f1]]
	V2 = FV[fdict[f2]]
	v1,v2 = intersect(V1,V2) # verified ... !
	t1 = V[:,v1], V[:,v2], V[:,[v for v in V1 if v≠v1 && v≠v2][1]]
	t2 = V[:,v2], V[:,v1], V[:,[v for v in V2 if v≠v1 && v≠v2][1]]
	n1 = LinearAlgebra.normalize(cross( t1[2]-t1[1], t1[3]-t1[1] ))
	n2 = LinearAlgebra.normalize(cross( t2[2]-t2[1], t2[3]-t2[1] ))
	p0 = (V[:,v1] + V[:,v2]) ./ 2
	n = n1 + n2
	ϵ = 1.0e-4
	ptest1 = p0 + ϵ * n
	ptest2 = p0 - ϵ * n
	return ptest1, ptest2
end

"""
	testinternalpoint(V::Lar.Points, EV::Lar.Cells, FV::Lar.Cells)

"""
function testinternalpoint(V,EV,FV)
	copEV = Lar.lar2cop(EV)
	copFV = Lar.lar2cop(FV)
	copFE = copFV * copEV'
	I,J,Val = findnz(copFE)
	triple = zip([(i,j,1) for (i,j,v) in zip(I,J,Val) if v==2]...)
	I,J,Val = map(collect,triple)
	Val = convert(Array{Int8,1},Val)
	copFE = sparse(I,J,Val)
	function testinternalpoint0(testpoint)
		intersectedfaces = Int64[]
		# spatial index for possible intersections with ray
		faces = spaceindex(testpoint)((V,FV))
		depot = []
		# face in faces :  indices of faces of possible intersection with ray
		for face in faces
			value = rayintersection(testpoint)(V,FV,face)
			if typeof(value) == Array{Float64,1} push!(depot, (face,value)) end
		end
		# actual containment test of ray point in faces within depot
		for (face,point3d) in depot
			vs, edges, point2d = planemap(V,copEV,copFE,face)(point3d)
			classify = Lar.pointInPolygonClassification(vs,edges)
			inOut = classify(point2d)
			if inOut!="p_out"
				push!(intersectedfaces,face)
			end
		end
		return intersectedfaces
	end
	return testinternalpoint0
end

"""
	getinternalpoint(V,EV,FV,Fs, copEV,copFE)

"""
function getinternalpoint(V,EV,FV,Fs, copEV,copFE)
	#edges for v1=FV[1][1]
	ptest1, ptest2 = Lar.settestpoints(V,EV,FV,Fs, copEV,copFE)
	intersectedfaces = Int64[]
	#GL.VIEW([ GL.GLFrame, GL.GLLines(V,EV), GL.GLPoints([ptest1'; ptest2']) ]);

	# for each test point compute the face planes intersected by vertical ray
	dep1, dep2 = [],[]
	# face in Fs : global indices of faces of current solid
	for (f,face) in enumerate(Fs)
		ret1 = rayintersection(ptest1)(V,FV,f)
		ret2 = rayintersection(ptest2)(V,FV,f)
		if typeof(ret1) == Array{Float64,1} push!(dep1, (face,ret1)) end
		if typeof(ret2) == Array{Float64,1} push!(dep2, (face,ret2)) end
	end

	# transform each plane in 2D and look whether the intersection point is internal
	# return the test point with odd numeber of ray intersections
	k1,k2 = 0,0
	for (face,point3d) in dep1
		vs, edges, point2d = planemap(V,copEV,copFE,face)(point3d)
		classify = Lar.pointInPolygonClassification(vs,edges)
		inOut = classify(point2d)
		if inOut!="p_out"
			k1+=1
			push!(intersectedfaces,face)
		end
	end
	if k1 % 2 == 1 return ptest1,intersectedfaces
	else
		for (face,point3d) in dep2
			vs, edges, point2d = planemap(V,copEV,copFE,face)(point3d)
			classify = Lar.pointInPolygonClassification(vs,edges)
			inOut = classify(point2d)
			if inOut!="p_out"
				k2+=1
				push!(intersectedfaces,face)
			end
		end
		if k2 % 2 == 1 return ptest2,intersectedfaces
		else println("error: while computing internal point of 3-cell") end
	end
end

# high level function

function chainbasis2solids(V,copEV,copFE,copCF)
	CF = [findnz(copCF[k,:])[1] for k=1:copCF.m]
	FE = [findnz(copFE[k,:])[1] for k=1:copFE.m]
	EV = [findnz(copEV[k,:])[1] for k=1:copEV.m]

	FEs = Array{Array{Int64,1},1}[]
	EVs = Array{Array{Array{Int64,1},1},1}[]
	FVs = Array{Array{Int64,1},1}[]
	for k=1:copCF.m
		push!( FEs, [collect(Set(cat([e for e in FE[f]]))) for f in CF[k]] )
		# edges in EVs are aggregated by face, in order to answer point-classifications
		push!( EVs, [[EV[e] for e in FE[f]] for f in CF[k]] )
		push!( FVs, [collect(Set(cat([EV[e] for e in FE[f]]))) for f in CF[k]] )
	end
	pols = collect(zip(EVs,FVs,FEs))
	W = convert(Lar.Points,V')
	return W,pols,CF
end

function internalpoints(V,copEV,copFE,copCF)
	# transform each 3-cell in a solid (via Lar model)
	#-------------------------------------------------------------------------------
	U,pols,CF = Lar.chainbasis2solids(V,copEV,copFE,copCF)
	# compute, for each `pol` (3-cell) in `pols`, one `internalpoint`.
	#-------------------------------------------------------------------------------
	innerpoints = []
	intersectedfaces = []
	for k=1:length(pols)
		(EV,FV,FE),Fs = pols[k],CF[k]
		EV = convert(Lar.Cells,collect(Set(cat(EV))))
		#GL.VIEW([ GL.GLFrame, GL.GLLines(V,EV) ]);
		points,facenumber = Lar.getinternalpoint(V,EV,FV,Fs, copEV,copFE)
		push!(innerpoints,points)
		push!(intersectedfaces,facenumber)
	end
	return innerpoints,intersectedfaces
end

################################################################################
#=
After the arrangement, extract all the d-cells from (d-1)-coboundary as isolated polyhedra.
Then compute a single interior point for each of them.
Then compare each such point against all input boundaries, in order to compute those which it was interior to. Extend this point membership as 3-cell containment within the relative input solids.
The point membership with a boundary consists in the parity count of the intersection points of a vertical ray starting at the test point, with the boundary surface.
=#
################################################################################

function bool3d(assembly)
	# input of affine assembly
	#-------------------------------------------------------------------------------
	V,FV,EV = Lar.struct2lar(assembly)
	cop_EV = convert(Lar.ChainOp, Lar.coboundary_0(EV::Lar.Cells));
	cop_FE = Lar.coboundary_1(V, FV::Lar.Cells, EV::Lar.Cells); ## TODO: debug
	W = convert(Lar.Points, V');
	# generate the 3D space arrangement
	#-------------------------------------------------------------------------------
	V, copEV, copFE, copCF = Lar.Arrangement.spatial_arrangement( W, cop_EV, cop_FE)
	W = convert(Lar.Points, V');
	#V,CVs,FVs,EVs = Lar.pols2tria(W, copEV, copFE, copCF)
	innerpoints,intersectedfaces = Lar.internalpoints(W,copEV,copFE,copCF[2:end,:])
	# associate internal points to 3-cells
	#-------------------------------------------------------------------------------
	listOfModels = Lar.evalStruct(assembly)
	inputfacenumbers = [length(listOfModels[k][2]) for k=1:length(listOfModels)]
	cumulative = cumsum([0;inputfacenumbers]).+1
	fspans = collect(zip(cumulative[1:end-1], cumulative[2:end].-1))
	span(h) = [j for j=1:length(fspans) if fspans[j][1]<=h<=fspans[j][2] ]
	# test input data for containment of reference points
	#-------------------------------------------------------------------------------
	V,FV,EV = Lar.struct2lar(assembly)
	containmenttest = Lar.testinternalpoint(V,EV,FV)
	# currently copCF contains the outercell in first column ...
	# TODO remove first row and column, in case (look at file src/)
	boolmatrix = BitArray(undef, length(innerpoints)+1, length(fspans)+1)
	boolmatrix[1,1] = 1
	for (k,point) in enumerate(innerpoints) # k runs on columns
		cells = containmenttest(point) # contents of columns
		#println(k," ",faces)
		rows = [span(h) for h in cells]
		for l in cat(rows)
			boolmatrix[k+1,l+1] = 1
		end
	end
	return W, (copEV, copFE, copCF), boolmatrix
end
function bool3d(expr, assembly)
	V, copEV, copFE, copCF, boolmatrix = bool3d(assembly)
	# TODO please see template "bool2d()" in src/bool2d.jl
	return V, (copEV, copFE, copCF), boolmatrix, result
end

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

#
# #pointcover = spaceindex([0,0,0.])((V,FV))


#=Generator function for TSP that takes the weight Matrix
and returns a function that evaluates the fitness of a single path=#

function fullFit(costs::AbstractMatrix{Float64})
  N = size(costs,1)
  function fFit(tour::Vector{T}) where T<:Integer
    @assert length(tour) == N "Tour must be of length $N"
    @assert isperm(tour) "Not a valid tour, not a permutation"
    #distance = weights[from,to] (from,to) in tour
    distance = costs[tour[N],tour[1]]
    for i in 1:N-1
      @inbounds distance += costs[tour[i],tour[i+1]]
    end
    return distance
  end
  return fFit
end

function partFit(costs::AbstractMatrix{Float64})
  N = size(costs,1)
  function pFit(tour::Vector{T}) where T<:Integer
    n = length(tour)
    #distance = weights[from,to] (from,to) in tour
    distance = n == N ? costs[tour[N],tour[1]] : zero(Float64)
    for i in 1:n-1
      @inbounds distance += costs[tour[i],tour[i+1]]
    end
    return distance
  end
  return pFit
end


# The references to special cases are to Table of Integrals, Series, and Products, § 9.121, followed by NIST's DLMF.

"""
Compute the Gauss hypergeometric function `₂F₁(a,b;c;z)`.
"""
function _₂F₁(a::Number,b::Number,c::Number,z::Number)
    if real(a) > real(b)
        return _₂F₁(b,a,c,z) # ensure a ≤ b
    elseif isequal(a,c) # 1. 15.4.6
        return exp(-b*log1p(-z))
    elseif isequal(b,c) # 1. 15.4.6
        return exp(-a*log1p(-z))
    elseif isequal(c,0.5)
        if isequal(a+b,0) # 31. 15.4.11 & 15.4.12
            return cosnasinsqrt(2b,z)
        elseif isequal(a+b,1) # 32. 15.4.13 & 15.4.14
            return cosnasinsqrt(1-2b,z)*exp(-0.5log1p(-z))
        elseif isequal(b-a,0.5) # 15.4.7 & 15.4.8
            return expnlog1pcoshatanhsqrt(-2a,z)
        end
    elseif isequal(c,1.5)
        if abeqcd(a,b,0.5) # 13. 15.4.4 & 15.4.5
            return sqrtasinsqrt(z)
        elseif abeqcd(a,b,1) # 14.
            return sqrtasinsqrt(z)*exp(-0.5log1p(-z))
        elseif abeqcd(a,b,0.5,1) # 15. 15.4.2 & 15.4.3
            return sqrtatanhsqrt(z)
        elseif isequal(a+b,1) # 29. 15.4.15 & 15.4.16
            return sinnasinsqrt(1-2b,z)
        elseif isequal(a+b,2) # 30.
            return sinnasinsqrt(2-2b,z)*exp(-0.5log1p(-z))
        elseif isequal(b-a,0.5) # 4. 15.4.9 & 15.4.10
            return expnlog1psinhatanhsqrt(1-2a,z)
        end
    elseif isequal(c,2)
        if abeqcd(a,b,1) # 6. 15.4.1
            return (s = -z; log1p(s)/s)
        elseif a ∈ ℤ && b == 1 # 5.
            return expm1nlog1p(1-a,-z)
        elseif a == 1 && b ∈ ℤ # 5.
            return expm1nlog1p(1-b,-z)
        end
    elseif isequal(c,2.5) && abeqcd(a,b,1,1.5)
         return speciallog(z)
    end
    _₂F₁general(a,b,c,z) # catch-all
end
_₂F₁(a::Number,b::Number,c::Number,z::AbstractArray) = reshape(promote_type(typeof(a),typeof(b),typeof(c),eltype(z))[ _₂F₁(a,b,c,z[i]) for i in eachindex(z) ], size(z))

"""
Compute the Gauss hypergeometric function `₂F₁(a,b;c;z)` with general parameters a,b, and c.
This polyalgorithm is designed based on the paper

    N. Michel and M. V. Stoitsov, Fast computation of the Gauss hypergeometric function with all its parameters complex with application to the Pöschl–Teller–Ginocchio potential wave functions, Comp. Phys. Commun., 178:535–551, 2008.
"""
function _₂F₁general(a::Number,b::Number,c::Number,z::Number)
    T = promote_type(typeof(a),typeof(b),typeof(c),typeof(z))

    real(b) < real(a) && (return _₂F₁general(b,a,c,z))
    real(c) < real(a)+real(b) && (return exp((c-a-b)*log1p(-z))*_₂F₁general(c-a,c-b,c,z))

    if abs(z) ≤ ρ || -a ∈ ℕ₀ || -b ∈ ℕ₀
        _₂F₁maclaurin(a,b,c,z)
    elseif abs(z/(z-1)) ≤ ρ && absarg(1-z) < convert(real(T),π)
        exp(-a*log1p(-z))_₂F₁maclaurin(a,c-b,c,z/(z-1))
    elseif abs(inv(z)) ≤ ρ && absarg(-z) < convert(real(T),π)
        _₂F₁Inf(a,b,c,z)
    elseif abs(1-inv(z)) ≤ ρ && absarg(z) < convert(real(T),π) && absarg(1-z) < convert(real(T),π)
        exp(-a*log1p(-z))*_₂F₁Inf(a,c-b,c,z/(z-1))
    elseif abs(1-z) ≤ ρ && absarg(z) < convert(real(T),π) && absarg(1-z) < convert(real(T),π)
        _₂F₁one(a,b,c,z)
    elseif abs(inv(1-z)) ≤ ρ && absarg(-z) < convert(real(T),π)
        exp(-a*log1p(-z))*_₂F₁one(a,c-b,c,z/(z-1))
    elseif absarg(1-z) < convert(real(T),π)
        _₂F₁taylor(a,b,c,z)
    else
        zero(T)#throw(DomainError())
    end
end

"""
Compute the Gauss hypergeometric function `₂F₁(a,b;c;z)` with general parameters a,b, and c.
This polyalgorithm is designed based on the review

    J. W. Pearson, S. Olver and M. A. Porter, Numerical methos for the computation of the confluent and Gauss hypergeometric functions, arXiv:1407.7786, 2014.
"""
function _₂F₁general2(a::Number,b::Number,c::Number,z::Number)
    T = promote_type(typeof(a),typeof(b),typeof(c),typeof(z))
    if abs(z) ≤ ρ || -a ∈ ℕ₀ || -b ∈ ℕ₀
        _₂F₁maclaurin(a,b,c,z)
    elseif abs(z/(z-1)) ≤ ρ && absarg(1-z) < convert(real(T),π) # 15.8.1
        w = z/(z-1)
        _₂F₁maclaurin(a,c-b,c,w)*exp(-a*log1p(-z))
    elseif abs(inv(z)) ≤ ρ && absarg(-z) < convert(real(T),π)
        w = inv(z)
        if isapprox(a,b) # 15.8.8
            gamma(c)/gamma(a)/gamma(c-a)*(-w)^a*_₂F₁logsumalt(a,c-a,z,w)
        elseif a-b ∉ ℤ # 15.8.2
            gamma(c)*((-w)^a*gamma(b-a)/gamma(b)/gamma(c-a)*_₂F₁maclaurin(a,a-c+1,a-b+1,w)+(-w)^b*gamma(a-b)/gamma(a)/gamma(c-b)*_₂F₁maclaurin(b,b-c+1,b-a+1,w))
        else
            zero(T) # TODO: full 15.8.8
        end
    elseif abs(inv(1-z)) ≤ ρ && absarg(-z) < convert(real(T),π)
        w = inv(1-z)
        if isapprox(a,b) # 15.8.9
            gamma(c)*exp(-a*log1p(-z))/gamma(a)/gamma(c-b)*_₂F₁logsum(a,c-a,z,w,1)
        elseif a-b ∉ ℤ # 15.8.3
            gamma(c)*(exp(-a*log1p(-z))*gamma(b-a)/gamma(b)/gamma(c-a)*_₂F₁maclaurin(a,c-b,a-b+1,w)+exp(-b*log1p(-z))*gamma(a-b)/gamma(a)/gamma(c-b)*_₂F₁maclaurin(b,c-a,b-a+1,w))
        else
            zero(T) # TODO: full 15.8.9
        end
    elseif abs(1-z) ≤ ρ && absarg(z) < convert(real(T),π) && absarg(1-z) < convert(real(T),π)
        w = 1-z
        if isapprox(c,a+b) # 15.8.10
            gamma(c)/gamma(a)/gamma(b)*_₂F₁logsum(a,b,z,w,-1)
        elseif c-a-b ∉ ℤ # 15.8.4
            gamma(c)*(gamma(c-a-b)/gamma(c-a)/gamma(c-b)*_₂F₁maclaurin(a,b,a+b-c+1,w)+exp((c-a-b)*log1p(-z))*gamma(a+b-c)/gamma(a)/gamma(b)*_₂F₁maclaurin(c-a,c-b,c-a-b+1,w))
        else
            zero(T) # TODO: full 15.8.10
        end
    elseif abs(1-inv(z)) ≤ ρ && absarg(z) < convert(real(T),π) && absarg(1-z) < convert(real(T),π)
        w = 1-inv(z)
        if isapprox(c,a+b) # 15.8.11
            gamma(c)*z^(-a)/gamma(a)*_₂F₁logsumalt(a,b,z,w)
        elseif c-a-b ∉ ℤ # 15.8.5
            gamma(c)*(z^(-a)*gamma(c-a-b)/gamma(c-a)/gamma(c-b)*_₂F₁maclaurin(a,a-c+1,a+b-c+1,w)+z^(a-c)*(1-z)^(c-a-b)*gamma(a+b-c)/gamma(a)/gamma(b)*_₂F₁maclaurin(c-a,1-a,c-a-b+1,w))
        else
            zero(T) # TODO: full 15.8.11
        end
    elseif abs(z-0.5) > 0.5
        if isapprox(a,b) && !isapprox(c,a+0.5)
            gamma(c)/gamma(a)/gamma(c-a)*(0.5-z)^(-a)*_₂F₁continuationalt(a,c,0.5,z)
        elseif a-b ∉ ℤ
            gamma(c)*(gamma(b-a)/gamma(b)/gamma(c-a)*(0.5-z)^(-a)*_₂F₁continuation(a,a+b,c,0.5,z) + gamma(a-b)/gamma(a)/gamma(c-b)*(0.5-z)^(-b)*_₂F₁continuation(b,a+b,c,0.5,z))
        else
            zero(T)
        end
    else
        zero(T)#throw(DomainError())
    end
end

"""
Compute the generalized hypergeometric function `₃F₂(a₁,a₂,a₃;b₁,b₂;z)` with the Maclaurin series.
"""
function _₃F₂(a₁::Number,a₂::Number,a₃::Number,b₁::Number,b₂::Number,z::Number)
    if abs(z) ≤ ρ
        _₃F₂maclaurin(a₁,a₂,a₃,b₁,b₂,z)
    else
        zero(z)
    end
end
_₃F₂(a₁::Number,a₂::Number,a₃::Number,b₁::Number,b₂::Number,z::AbstractArray) = reshape(promote_type(typeof(a₁),typeof(a₂),typeof(a₃),typeof(b₁),typeof(b₂),eltype(z))[ _₃F₂(a₁,a₂,a₃,b₁,b₂,z[i]) for i in eachindex(z) ], size(z))
_₃F₂(a₁::Number,b₁::Number,z) = _₃F₂(1,1,a₁,2,b₁,z)

"""
Compute the generalized hypergeometric function `mFn(a;b;z)` with the Maclaurin series.
"""
function mFn{S<:Number,V<:Number}(a::AbstractVector{S},b::AbstractVector{V},z::Number)
    if abs(z) ≤ ρ || length(a) ≤ length(b)
        mFnmaclaurin(a,b,z)
    else
        zero(promote_type(S,V,typeof(z)))
    end
end
mFn{S<:Number,V<:Number}(a::AbstractVector{S},b::AbstractVector{V},z::AbstractArray) = reshape(promote_type(S,V,eltype(z))[ mFn(a,b,z[i]) for i in eachindex(z) ], size(z))


@testset "Test Dof handlers" begin

using jFEMTools
using SparseArrays

mesh = unitSquareMesh(TriangleCell, (2,2));
dim = 2
element = PoissonVirtualElement(dim,1);
u = TrialFunction(VEMFunctionSpace(mesh,element))
dh = DofHandler(mesh, [u]);

@test dh.cell_dofs == [1, 2, 3, 2, 4, 3, 2, 5, 4, 5, 6, 4, 3, 4, 7, 4, 8, 7, 4, 6, 8, 6, 9, 8]
@test dh.cell_dofs_offset == [1, 4, 7, 10, 13, 16, 19, 22, 25]
@test jFEMTools.dof_range(dh, u, 1) == 1:3
@test jFEMTools.vertexdofs(dh, u) == [1,2,5,3,4,6,7,8,9]
K = jFEMTools.create_sparsity_pattern(dh)
@test size(K) == (9,9)
@test nnz(K) == 41
end