using Test, LinearAlgebra

@testset "test/integration/local_polytope_facets.jl" begin

using BellScenario: LocalPolytope, ConvexPolytope, Degeneracy, Behavior

dir = "./test/integration/files"

@testset "21->12 polytope" begin
    α = (2,1)
    β = (1,2)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    # There should be (1*2)(1 + 1*(2 - 1)) vertices for fixed direction
    @test length(vertices) == 4
    @test size(vertices[1]) == (2,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 4
    @test constraints[1] == [[0 -1 0],[0 0 -1],[-1 0 1],[-1 1 0]]
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"normalized")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 1
    @test issetequal(canonical_groups[1], [
        [0 -1 0],[0 0 -1],[-1 0 1],[-1 1 0]
    ])
end

@testset "31->12 polytope" begin
    α = (3,1)
    β = (1,2)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    # There should be (1*2)(1 + 1*(2 - 1)) vertices for fixed direction
    @test length(vertices) == 8
    @test size(vertices[1]) == (3,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 6
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"normalized")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 1
    @test issetequal(canonical_groups[1], [
        [0 -1 0 0],[0 0 -1 0],[0 0 0 -1],[-1 0 1 0],[-1 1 0 0],[-1 0 0 1]
    ])
end

@testset "31->13 polytope" begin
    α = (3,1)
    β = (1,3)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    # There should be (1*3)(1 + 3*(3 - 1)) vertices for fixed direction
    @test length(vertices) == 21
    @test size(vertices[1]) == (6,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 15
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"normalized")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 2
    @test issetequal(canonical_groups[1], [
        [0 -1 0 0 0 0 0], [0 0 -1 0 0 0 0], [0 0 0 -1 0 0 0],
        [0 0 0 0 -1 0 0], [0 0 0 0 0 -1 0], [0 0 0 0 0 0 -1],
        [-1 0 0 1 0 0 1], [-1 0 1 0 0 1 0], [-1 1 0 0 1 0 0]
    ])
    @test issetequal(canonical_groups[2], [
        [-1 -1 0 1 -1 1 0], [-1 -1 1 0 -1 0 1], [-1 0 -1 1 1 -1 0],
        [-1 0 1 -1 1 0 -1], [-1 1 -1 0 0 -1 1], [-1 1 0 -1 0 1 -1]
    ])
end

@testset "41->14 polytope" begin
    α = (4,1)
    β = (1,4)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    # There should be (1*4)(1 + 7*(4 - 1)) vertices for fixed direction
    @test length(vertices) == 88
    @test size(vertices[1]) == (12,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 664
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"normalized")

    ggame_facet = [-2 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1]*Behavior.norm_to_gen_proj(α,β)
    @test in(ggame_facet, canonical_groups[2])

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 6
end

@testset "22->12 polytope" begin
    α = (2,2)
    β = (1,2)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    # There should be (4*2)(1 + 1*(2 - 1)) vertices for fixed direction
    @test length(vertices) == 16
    @test size(vertices[1]) == (6,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 8
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"fixed-direction")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 1
end

@testset "22->22 polytope (fixed-direction)" begin
    α = (2,2)
    β = (2,2)

    vertices = LocalPolytope.vertices(α, β, bits=1, rep="fixed-direction")

    # There should be (4*4)(1 + 1*(4 - 1)) vertices for fixed direction
    @test length(vertices) == 64
    @test size(vertices[1]) == (10,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)
    @test constraints[2] == []
    @test constraints[3] == []

    @test length(constraints[1]) == 16

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"fixed-direction")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 1
end

@testset "33->13 polytope" begin
    α = (3,3)
    β = (1,3)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    # There should be (27*3)(1 + 3*(3 - 1)) vertices for fixed direction
    @test length(vertices) == 567
    @test size(vertices[1]) == (24,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 33
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"fixed-direction")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 2
end

@testset "32->22 polytope" begin
    α = (3,2)
    β = (2,2)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    # There should be (27*3)(1 + 3*(3 - 1)) vertices for fixed direction
    @test length(vertices) == 320
    @test size(vertices[1]) == (15,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 864
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"fixed-direction")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 9
end

@testset "32->12 polytope" begin
    α = (3,2)
    β = (1,2)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    @test length(vertices) == 64
    @test size(vertices[1]) == (9,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 12
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"fixed-direction")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 1
end

@testset "41->12 polytope" begin
    α = (4,1)
    β = (1,2)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    @test length(vertices) == 16
    @test size(vertices[1]) == (4,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 8
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"normalized")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 1
end

@testset "33->12 polytope" begin
    α = (3,3)
    β = (1,2)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    @test length(vertices) == 216
    @test size(vertices[1]) == (15,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 18
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"fixed-direction")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 1
end

@testset "32->13 polytope" begin
    α = (3,2)
    β = (1,3)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    @test length(vertices) == 168
    @test size(vertices[1]) == (15,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 24
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"fixed-direction")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 2
end

@testset "41->13 polytope" begin
    α = (4,1)
    β = (1,3)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    @test length(vertices) == 45
    @test size(vertices[1]) == (8,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 36
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"normalized")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 2
end

@testset "31->14 polytope" begin
    α = (3,1)
    β = (1,4)

    vertices = LocalPolytope.vertices(α, β, bits=1)

    @test length(vertices) == 40
    @test size(vertices[1]) == (9,1)

    constraints = ConvexPolytope.facet_constraints(vertices, dir=dir)

    @test length(constraints[1]) == 72
    @test constraints[2] == []
    @test constraints[3] == []

    canonical_groups = Degeneracy.canonical_facets(α,β,constraints[1],"normalized")

    @test issetequal(constraints[1], collect(Iterators.flatten(canonical_groups)))
    @test length(canonical_groups) == 3
end

end


using NumericExtensions
function main()
	N = big(100)
	n = sum([1:N])^2 - sumsq([1:N])
	println(n)
end

main()


using GeomDAE
using PyPlot

const Δt    = 0.1
const ntime = 100000
const neps  = 1E-14
const nmax  = 20

function f(t, x, fx)
    fx[1] = x[2]
    fx[2] = sin(x[1])
    nothing
end

function run_pendulum(tableau, filename)
    x0 = [acos(0.4), 0.0]
    ode = ODE(f, x0)
    int = Integrator(ode, tableau, Δt)
    sol = Solution(ode, Δt, ntime)

    integrate!(int, sol)
    set_initial_conditions!(sol, ode)

    print("Running ", tableau.name, "...")
    @time integrate!(int, sol)

    fig = figure(figsize=(6,6))
    plot(sol.x[1,:,1], sol.x[2,:,1])
    xlim(0, 6)
    ylim(-2,+2)
    savefig(filename)
end

run_pendulum(getTableauExplicitEuler(), "pendulum_explicit_euler.png")
run_pendulum(getTableauExplicitMidpoint(), "pendulum_explicit_midpoint.png")
run_pendulum(getTableauHeun(), "pendulum_heun.png")
run_pendulum(getTableauKutta(), "pendulum_kutta.png")
run_pendulum(getTableauERK4(), "pendulum_explicit_rk4.png")
run_pendulum(getTableauERK438(), "pendulum_explicit_rk438.png")

run_pendulum(getTableauImplicitEuler(), "pendulum_implicit_euler.png")
run_pendulum(getTableauGLRK1(), "pendulum_implicit_glrk1.png")
run_pendulum(getTableauGLRK2(), "pendulum_implicit_glrk2.png")
run_pendulum(getTableauGLRK3(), "pendulum_implicit_glrk3.png")

function qf(x, fx)
    fx[1] = x[1]
    nothing
end

function pf(t, x, fx)
    fx[1] = sin(x[1])
    nothing
end

function run_pendulum_partitioned(tableau, filename)
    q0 = [acos(0.4)]
    p0 = [0.0]

    ode = PODE(qf, pf, q0, p0)
    int = Integrator(ode, tableau, Δt)
    sol = Solution(ode, Δt, ntime)

    integrate!(int, sol)
    set_initial_conditions!(sol, ode)

    print("Running ", tableau.name, "...")
    @time integrate!(int, sol)

    fig = figure(figsize=(6,6))
    plot(sol.x[1,1,:,1], sol.x[2,1,:,1])
    # plot(sol[1], sol[2])
    xlim(0, 6)
    ylim(-2,+2)
    savefig(filename)
end

run_pendulum_partitioned(getTableauSymplecticEulerA(), "pendulum_symplectic_euler_a.png")
run_pendulum_partitioned(getTableauSymplecticEulerB(), "pendulum_symplectic_euler_b.png")
run_pendulum_partitioned(TableauEPRK(:PERK4, 4, getTableauERK4(), getTableauERK4()), "pendulum_explicit_prk4.png")


infile = "./src/Day9/input.txt"
input = parse.(Int,readlines(infile))

# part 1, check that each number can be the sum of any 2 of the prior 25
# find first anomaly

function checkpairs(bank, target)
    for i in 1:(length(bank)-1), j in (i+1):length(bank)
        check = bank[i] + bank[j] == target
        if check
            return true
        end
    end
    return false
end

bank = input[1:25]
target = input[26]
checkpairs(bank, target)

function findanomaly(input)
    bank = input[1:25]
    for test in 26:length(input)
        target = input[test]
        if !checkpairs(bank, target)
            return target, test
        end
        discard = popfirst!(bank)
        push!(bank, target)
    end
    return 0, 0
end
anomaly, pos = findanomaly(input)

function breakencryption(input, anomaly)
    for start in 1:(length(input) - 1)
        for finish in (start+1):length(input)
            adds = sum(input[start:finish])
            if adds > anomaly
                break # end finish loop
            end
            if adds == anomaly
                return start, finish, minimum(input[start:finish]), maximum(input[start:finish])
            end
        end
    end
    return 0,0,0,0
end

start, finish, small, big = breakencryption(input, anomaly)
sum(input[518:534])
small + big


#############################################################################
#  Copyright 2016, Iain Dunning, Joey Huchette, Miles Lubin, and contributors
#  This Source Code Form is subject to the terms of the Mozilla Public
#  License, v. 2.0. If a copy of the MPL was not distributed with this
#  file, You can obtain one at http://mozilla.org/MPL/2.0/.
#
# Author: Louis Luangkesorn
# Date: February 26, 2015
#
# JuMP implementation of the multicommodity transportation model
#   AMPL: A Modeling Language for Mathematical Programming, 2nd ed
#   by Robert Fourer, David Gay, and Brian W. Kernighan
#   4-1 multi.mod and multi.dat
#############################################################################

using Compat

function PrintSolution(status, Trans, ORIG, DEST, PROD)
    println("RESULTS:")
    if status == :Optimal
      for i = 1:length(ORIG)
        for j = 1:length(DEST)
          for p = 1:length(PROD)
            print(" $(PROD[p]) $(ORIG[i]) $(DEST[j]) = $(getvalue(Trans[i,j, p])) \t")
          end
          println()
        end
      end
    else
        println("  No solution")
    end
    println("")
end

using JuMP

# PARAMETERS

orig = ["GARY" "CLEV" "PITT"]
dest = ["FRA" "DET" "LAN" "WIN" "STL" "FRE" "LAF"]
prod = ["bands" "coils" "plate"]
numorig = length(orig)
numdest = length(dest)
numprod = length(prod)

# supply(prod, orig) amounts available at origins
supply = [[ 400    700    800]
          [ 800   1600   1800]
          [ 200    300    300]]

# demand(prod, dest) amounts required at destinations
demand = [[  300   300   100    75   650   225   250]
          [ 500   750   400   250   950   850   500]
          [ 100   100     0    50   200   100   250]]

# limit(orig, dest) of total units from any origin to destination
defaultlimit = float(625)

limit = [[defaultlimit::AbstractFloat for j=1:numdest] for i=1:numorig]

# cost(dest, orig, prod) Shipment cost per unit
cost = reshape([[[  30,   10,   8 ,  10,   11 ,  71,    6];
         [  22,    7,   10,    7,   21,   82,   13];
         [  19,   11,   12,   10,   25,   83,   15]];
        [[  39,   14,   11,   14,   16,   82,    8];
         [  27,    9,   12,    9,   26,   95,   17];
         [  24,   14,   17,   13,   28,   99,   20]];
        [[  41,   15,   12,   16,   17,   86,    8];
         [  29,    9,   13,    9,   28,   99,   18];
         [  26,   14,   17,   13,   31,  104,   20]]],
               7, 3, 3)

#  DECLARE MODEL

multi = Model()

#  VARIABLES

@variable(multi, Trans[1:numorig, 1:numdest, 1:numprod] >= 0)

#  OBJECTIVE

length(cost)

@objective(multi, Max,
              sum{sum{sum{cost[j, i, p] * Trans[i,j, p],
                        i=1:numorig}, j=1:numdest}, p=1:numprod})

#  CONSTRAINTS

# Supply constraint
@constraint(multi, xyconstr[i=1:numorig, p=1:numprod],
               sum{Trans[i,j,p], j=1:numdest} == supply[p,i])

# Demand constraint
@constraint(multi, xyconstr[j=1:numdest, p=1:numprod],
               sum{Trans[i,j,p], i=1:numorig} == demand[p,j])

# Total shipment constraint
@constraint(multi, xyconstr[i=1:numorig, j=1:numdest],
               sum{Trans[i,j,p], p=1:numprod} - limit[i][j] <= 0)
limit[2][3]
status = solve(multi)
if status == :Optimal
  result = Trans
  PrintSolution(status, Trans, orig, dest, prod)
else
  result = status
end


using EmpiricalCDFs

struct Pareto
    alpha::Float64
    x0::Float64
end

Pareto(alpha) = Pareto(alpha,1.0)
Pareto() = Pareto(1.0)

Base.rand(p::Pareto) = p.x0 * rand()^(-one(p.alpha)/p.alpha)

function Base.rand(p::Pareto, n::Int)
    a = Array(Float64, n)
    for i in 1:n
        a[i] = rand(p)
    end
    a
end

const odir = joinpath(Pkg.dir("EmpiricalCDFs"), "test")
const ofile = joinpath(odir, "paretocdf.bin")

function write_test_data()
    d = Pareto()
    cdf = EmpiricalCDF()
    append!(cdf, rand(d,10^5))
    sort!(cdf)
    write(ofile, cdf)
    nothing
end

import EmpiricalCDFs: mleKS, scanmle

# julia> read_test_data()
# EmpiricalCDFs.MLEKS{Float64}(2.0018144538762677,0.0010018144538762677,0.2488588894720618)

function read_test_data()
    cdf = readcdf(ofile)
    mleks = mleKS(cdf)
    println(mleks)
    scanmle(cdf,15)
end


abstract type AbstractResistor end

"""
    r = Resistor(R0=1e3, α=0.0, T₀=293.15)

A resistor with resistance varying linearly with temperature:

```math
R = R₀ (1 + α (T - T₀))
```

**IMPORTANT** In the constructor, coefficient `α` is a *fraction*, not percentage.
 linear.

# Examples

```jldoctest
julia> R = Resistor(1e3, 0.01)
THW.Resistor(1000.0, 0.0001, 293.15)

julia> resistance(R)
1000.0

julia> resistance(R, 120+273.15)
1010.0

julia> R()
1000.0

julia> R(120+273.15)
1010.0

julia> temperature(R)
293.15

julia> temperature(R, 1010)
120.00000000000009

```
"""
struct Resistor <: AbstractResistor
    "Resistance at reference temperature (Ω)"
    R₀::Float64
    "Linear resistance coefficient"
    α::Float64
    "Reference temperature (K)"
    T₀::Float64
    Resistor(R₀=1e3, α=0.0, T₀=293.15) = new(R₀, α, T₀)
end
Base.broadcastable(R::Resistor) = Ref(R)

"""
    R = Thermistor(R₀, B, T₀)

Models a NTC thermistor. In the constructor, `R0` is the resistance of the thermistor at the reference temperature  `T₀`. For convenience, the temperature should be expressed in K. `B` is a coefficient that characterizes the temperature dependence of the thermistor. In the model used her, the resistance varies with temperature according to the following equation:

```math
R = R₀ \\exp B \\left(\\frac{1}{T₀} - \\frac{1}{T}\\right)
```

where
 * R₀ is the reference resistance in Ω
 * T₀ is the reference temperature in K, usually the adopted value is   293.15 K
 * T is the temperature in K, where the value of resistance should be calculated
 * B is an empirical coefficient in K that characterizes the temperature dependency of the resistance

    When using the struct `Thermistor`, beware: in the constructor, the unit of T₀ is °C NOT K.

# Examples
```jldoctest
julia> R = Thermistor(5e3, 3200, 293.15) # Create an object `Thermistor`
Thermistor(5000.0, 3200.0, 293.15)

julia> resistance(R) # Resistance at the reference temperature
5000.0

julia> resistance(R, 298.15) # Resistance at 25°C
4163.587774917559

julia> R() # The same as resistance(R)
5000.0

julia> R(298.15) # The same as resistance(R, 298.15)
4163.587774917559

julia> temperature(R)
293.15

julia> temperature(R, 4163.588)
298.15

```

"""
struct Thermistor <: AbstractResistor
    "Reference resistance in Ω at temperature `T₀`"
    R₀::Float64
    "Thermistor's B coefficient in K "
    B::Float64
    "Reference temperature in K"
    T₀::Float64
    Thermistor(R₀=5e3, B=0.0, T₀=25.0) = new(R₀, B, T₀)
end
Base.broadcastable(R::Thermistor) = Ref(R)

"""
    resistance(th::AbstractResistor)
    resistance(th::AbstractResistor, Tc)

Calculates the resistance for `AbstractResistor`objects` at temperature `Tc`.
If argumento `Tc` is not provided, the function returns the reference resistance `R0`.

See [`Thermistor`](@ref) to see more details of the  type  `Thermistor`. For more details on
type `Resistor`, see [`Thermistor`](@ref).

"""
resistance(r::AbstractResistor) = r.R₀
resistance(r::Resistor, T) = r.R₀ * (1.0 + r.α * (T - r.T₀))
resistance(th::Thermistor, T) = th.R₀ * exp( -th.B * (1/th.T₀ - 1/T ) )

temperature(r::AbstractResistor) = r.T₀
temperature(r::Resistor, R) = 1/r.α * (R/r.R₀ - 1.0) + r.T₀
temperature(th::Thermistor, R) = 1/( 1/th.T₀ + 1/th.B * log(R/th.R₀) )
temperature(th::Thermistor) = th.T₀

(th::Thermistor)(T) = resistance(th, T)
(th::Thermistor)() = th.R₀
(r::Resistor)(T) = resistance(r, T)
(r::Resistor)() = resistance(r)


fft_frequencies(fs::Real, nfft::Int) = LinRange(0f0, fs / 2f0, (nfft >> 1) + 1)

"""
    MelSpectrogram{T, F, Ti} <: DSP.Periodograms.TFR{T}

# Arguments:
- `power::Matrix{T}`
- `mels::F`
- `time::Ti`
"""
struct MelSpectrogram{T, F,Ti} <: DSP.Periodograms.TFR{T}
    power::Matrix{T}
    mels::F
    time::Ti
end
DSP.freq(tfr::MelSpectrogram) = tfr.mels
Base.time(tfr::MelSpectrogram) = tfr.time

"""
    MFCC{T, F, Ti} <: DSP.Periodograms.TFR{T}

# Arguments:
- `mfcc::Matrix{T}`
- `number::F`
- `time::Ti`
"""
struct MFCC{T, F, Ti} <: DSP.Periodograms.TFR{T}
    mfcc::Matrix{T}
    number::F
    time::Ti
end
DSP.freq(tfr::MFCC) = tfr.number
Base.time(tfr::MFCC) = tfr.time

function hz_to_mel(frequencies)
    f_min = 0f0
    f_sp = 200f0 / 3

    mels = collect((frequencies .- f_min) ./ f_sp)
    min_log_hz = 1000f0
    min_log_mel = (min_log_hz .- f_min) ./ f_sp
    logstep = log(6.4f0) / 27f0

    @inbounds for i = 1:length(mels)
        if frequencies[i] >= min_log_hz
            mels[i] = min_log_mel + log(frequencies[i] / min_log_hz) / logstep
        end
    end

    mels
end

function mel_to_hz(mels)
    f_min = 0f0
    f_sp = 200f0 / 3
    frequencies = collect(f_min .+ f_sp .* mels)

    min_log_hz = 1000f0
    min_log_mel = (min_log_hz .- f_min) ./ f_sp
    logstep = log(6.4f0) / 27f0

    @inbounds for i = 1:length(frequencies)
        if mels[i] >= min_log_mel
            frequencies[i] = min_log_hz * exp(logstep * (mels[i] - min_log_mel))
        end
    end

    frequencies
end

function mel_frequencies(nmels::Int = 128, fmin::Real = 0.0f0, fmax::Real = 11025f0)
    min_mel = hz_to_mel(fmin)[1]
    max_mel = hz_to_mel(fmax)[1]

    mels = LinRange(min_mel, max_mel, nmels)
    mel_to_hz(mels)
end

"""
    M = mel(fs::Real, nfft::Int; nmels::Int = 128, fmin::Real = 0f0, fmax::Real = fs/2f0)

Returns a Mel matrix `M` such that `M*f` is a mel spectrogram if `f` is a vector of spectrogram powers, e.g.,
```julia
M*abs2.(rfft(sound))
```
"""
function mel(fs::Real, nfft::Int; nmels::Int = 128, fmin::Real = 0f0, fmax::Real = fs/2f0)
    weights = zeros(Float32, nmels, (nfft >> 1) + 1)
    fftfreqs = fft_frequencies(fs, nfft)
    melfreqs = mel_frequencies(nmels + 2, fmin, fmax)
    enorm = 2f0 ./ (melfreqs[3:end] - melfreqs[1:nmels])

    for i in 1:nmels
        lower = (fftfreqs .- melfreqs[i]) ./ (melfreqs[i+1] - melfreqs[i])
        upper = (melfreqs[i+2] .- fftfreqs) ./ (melfreqs[i+2] - melfreqs[i+1])

        weights[i, :] = max.(0, min.(lower, upper)) * enorm[i]
    end

    weights
end

"""
    melspectrogram(s, n=div(length(s), 8), args...; fs=1, nmels::Int=128, fmin::Real=0.0f0, fmax::Real=fs / 2.0f0, window=hanning, kwargs...)

DOCSTRING

#Arguments:
- `s`: signal
- `n`: number of points in each window
- `args`: are sent to `spectrogram`
- `fs`: sample frequency
- `nmels`: number of mel frequencies
- `fmin`: minimum freq
- `fmax`: maximum freq
- `window`: window function, defaults to hanning
- `kwargs`: are sent to `spectrogram`
"""
function melspectrogram(S::DSP.Periodograms.Spectrogram; fs=1, nmels::Int = 128, fmin::Real = 0f0, fmax::Real = fs / 2f0)
    n = 2size(S.power,1)-1
    data = mel(fs, n; nmels=nmels, fmin=fmin, fmax=fmax) * S.power
    nframes = size(data, 2)
    MelSpectrogram(data, LinRange(hz_to_mel(fmin)[1], hz_to_mel(fmax)[1], nmels), S.time)
end

function melspectrogram(s, n=div(length(s), 8), args...; fs=1, nmels::Int = 128, fmin::Real = 0f0, fmax::Real = fs / 2f0, window=hanning, kwargs...)
    S = DSP.spectrogram(s, n, args...; fs=fs, window=window, kwargs...)
    melspectrogram(S,fs=fs,nmels=nmels,fmin=fmin,fmax=fmax)
end

"""
    mfcc(s, args...; nmfcc::Int=20, nmels::Int=128, window=hanning, kwargs...)

DOCSTRING

#Arguments:
- `s`: signal
- `args`: are sent to `spectrogram`
- `nmfcc`: number of coeffs
- `nmels`: number of mel frequencies
- `window`: window function, defaults to hanning
- `kwargs`: are sent to `spectrogram`
"""
function mfcc(s, args...; nmfcc::Int = 20, nmels::Int = 128, window=hanning, kwargs...)
    if nmfcc >= nmels
        error("number of mfcc components should be less than the number of mel frequency bins")
    end
    M = melspectrogram(s, args...; nmels=nmels, window=window, kwargs...)
    mfcc = dct_matrix(nmfcc, nmels) * power(M)

    for frame in 1:size(mfcc, 2)
        mfcc[:, frame] /= norm(mfcc[:, frame])
    end

    MFCC(mfcc, 1:nmfcc, time(M))
end

"""returns the DCT filters"""
function dct_matrix(nfilters::Int, ninput::Int)
    basis = Array{Float32}(undef, nfilters, ninput)
    samples = (1f0:2f0:2ninput) * π / 2ninput
    for i = 1:nfilters
        basis[i, :] = cos.(i * samples)
    end

    basis *= sqrt(2f0/ninput)
    basis
end


function backTrackingLS(f,g,maxLSiter,c1,θ,∇,fval)
    α = 1
    lineSearchIter = 0
    for i in 1:maxLSiter
        f_xk_alpha_k = f( θ + α * -∇)
        f_xk_plus_grad = fval + ( c1 * α * ∇' * -∇)[1]
		#@printf("%f \t < \t %f\n",f_xk_alpha_k,f_xk_plus_grad)
        if  ! (f_xk_alpha_k < f_xk_plus_grad[1])
            α = α * 0.5
            lineSearchIter += 1
        else
            break
        end
    end
    return (α,lineSearchIter)
end

# function / gradient-function / direction / ls iter / zoom iter / c1 / v2 / theta
function wolfeLineSearch(f,g,pk,maxLSiter,maxZoomIter,c1,c2,xk,grad,fval)
    # NEED ITERATION COUNTER
    a_0 = 0.0
    a_iminus1 = a_0
    a_i = 0.1
    a_max = 65536.0

    phi_0 = fval
    phi_a_iminus1 = phi_0
    phi_a_i = NaN

    # phi'(α) = innerProduct(pk, grad(x))
    phi_prime_0 = (pk'*grad)[1]
    phi_prime_a_i = NaN

    for i in 1:maxLSiter

        x_new = xk + a_i * pk
        phi_a_i = f(x_new)

        # check wolfe condition
        if phi_a_i > phi_0 + c1 * a_i * phi_prime_0 ||
            (i > 1 && phi_a_i >= phi_a_iminus1)
            (a_star,iter) = zoom(a_iminus1,a_i,maxZoomIter,phi_0,phi_prime_0,f,g,pk,xk,c1,c2)
            return (a_star, i , iter)
        end

        phi_prime_a_i = (pk'*g(x_new))[1]
        # check condition
        if abs(phi_prime_a_i) <= -c2*phi_prime_0
            return (a_i,i,0)
        end

        if phi_prime_a_i >= 0
            (a_star,iter) = zoom(a_i,a_iminus1,maxZoomIter,phi_0,phi_prime_0,f,g,pk,xk,c1,c2)
            return (a_star, i , iter)
        end

        # choose alphai+1 in (a_i,a_max)
        a_iminus1 = a_i
        a_i = a_i * 2

        phi_a_iminus1 = phi_a_i
    end
    return (a_i,maxLSiter,0)
end

function zoom(alpha_l, alpha_u,maxzoomiter,phi_0,phi_prime_0,f,g,pk,xk,c1,c2)

   phi_prime_al = (pk'*g(xk + alpha_l * pk))[1]
   phi_prime_au = (pk'*g(xk + alpha_u * pk))[1]

   for i in 1:maxzoomiter
        phi_al = f(xk + alpha_l*pk)
        phi_au = f(xk + alpha_u*pk)

        d1 = phi_prime_al + phi_prime_au - 3 * (phi_al - phi_au) / (alpha_l - alpha_u)
        d2 = sign(alpha_u-alpha_l)*sqrt(d1*d1-phi_prime_al*phi_prime_au)

        alpha_j = alpha_u - (alpha_u-alpha_l) * (phi_prime_au + d2 - d1) / (phi_prime_au - phi_prime_al + 2*d2)

        phi_alpha_j = f( xk + alpha_j * pk)
        # check condition
        if phi_alpha_j > phi_0 + c1 * alpha_j * phi_prime_0 || phi_alpha_j >= phi_al
            alpha_u = alpha_j
        else
            phi_prime_aj = (pk' * g(xk + alpha_j * pk))[1]
            if abs(phi_prime_aj) <= -c2*phi_prime_0
                return (alpha_j,i)
            end
            if phi_prime_aj* (alpha_u-alpha_l) >= 0
                alpha_u = alpha_l
            end
            alpha_l = alpha_j
        end

        if i == maxzoomiter
            return (alpha_j,maxzoomiter)
        end
        phi_prime_al = (pk'*g(xk + alpha_l))[1]
        phi_prime_au = (pk'*g(xk + alpha_u))[1]

    end
end


function gauleg(n::Int, xmin::FT, xmax::FT; norm=false) where FT
    ξ, w = gausslegendre(n)
    ξ = (xmax - xmin) / FT(2) * ξ .+ (xmin + xmax) / FT(2)
    norm ? w /= sum(w) : w *= (xmax - xmin) / FT(2)
    return FT.(ξ), FT.(w)
end


export FSimGate

# https://arxiv.org/pdf/1711.04789.pdf
# Google supremacy paper
mutable struct FSimGate{T<:Number} <: PrimitiveBlock{2}
    theta::T
    phi::T
end

function Base.:(==)(fs1::FSimGate, fs2::FSimGate)
    return fs1.theta == fs2.theta && fs1.phi == fs2.phi
end

function YaoBlocks.mat(::Type{T}, fs::FSimGate) where T
    θ, ϕ = fs.theta, fs.phi
    T[1 0          0          0;
     0 cos(θ)     -im*sin(θ) 0;
     0 -im*sin(θ) cos(θ)     0;
     0 0          0          exp(-im*ϕ)]
end

YaoBlocks.iparams_eltype(::FSimGate{T}) where T = T
YaoBlocks.getiparams(fs::FSimGate{T}) where T = (fs.theta, fs.phi)
function YaoBlocks.setiparams!(fs::FSimGate{T}, θ, ϕ) where T
    fs.theta = θ
    fs.phi = ϕ
    return fs
end


#=
  SEND+MOST=MONEY in Julia + ConstraintSolver.jl

  Alphametic problem were we maximize MONEY.

  This version do two things:
    - find the maximum of MONEY
    - and then find all solutions for the maximum value of MONEY.

  Problem from the lecture notes:
  http://www.ict.kth.se/courses/ID2204/notes/L01.pdf

  Model created by Hakan Kjellerstrand, hakank@gmail.com
  See also my Julia page: http://www.hakank.org/julia/

=#
using ConstraintSolver, JuMP
const CS = ConstraintSolver
include("constraints_utils.jl")

function send_most_money()
    model = Model(optimizer_with_attributes(CS.Optimizer,
                                                        # "all_solutions"=>true,
                                                        "all_optimal_solutions"=>true,
                                                        "logging"=>[]))
    @variable(model, 0 <= x[1:8] <= 9, Int)
    s,e,n,d,m,o,t,y = x

    @variable(model, 10000 <= MONEY <= 99999, Int)

    @constraint(model, x in CS.AllDifferentSet())
    @constraint(model, s != 0)
    @constraint(model, m != 0)
    @constraint(model, MONEY == 10000*m + 1000*o + 100*n + 10*e + y)
    @constraint(model,
                              1000*s + 100*e + 10*n + d
                           +  1000*m + 100*o + 10*s + t
                 == MONEY
        )

    @objective(model,Max,MONEY)

    # Solve the problem
    optimize!(model)

    status = JuMP.termination_status(model)
    println("status:$status")
    if status == MOI.OPTIMAL
        num_sols = MOI.get(model, MOI.ResultCount())
        println("num_sols:$num_sols\n")
        for sol in 1:num_sols
            println("solution #$sol")
            xx = convert.(Integer,JuMP.value.(x; result=sol))
            MONEYx = convert.(Integer,JuMP.value.(MONEY; result=sol))
            println("x:$xx MONEY:$MONEYx\n")
        end

    end
end

send_most_money()


# Standard stanh layer: f(A,x,b)=stanh(A*x+b)
# Note that the bias (a column vector) gets expanded here to match the dimension of A*x
sf=2.5

FstanhAXplusBias(A,X,b)=begin; a=A*X+b*ones(1,size(X,2)); return (sf*tanh(a),[]); end

function FstanhAXplusBias_inplace(value,aux,A,X,b)
    a=A*X+b*ones(1,size(X,2))
    copy!(value,sf*tanh(a))
end

function DstanhAXplusBias(derivativeIDX,f_c,faux_c,grad_c,grad_n,A,X,b)

    if derivativeIDX==1
        axpy!(1.0,(sf*grad_c.*(1.0-(f_c/sf).^2))*X',grad_n)
    elseif derivativeIDX==2
        axpy!(1.0,sf*A'*(grad_c.*(1.0-(f_c/sf).^2)),grad_n)
    elseif derivativeIDX==3
        axpy!(1.0,sf*sum(grad_c.*(1.0-(f_c/sf).^2),2),grad_n)
    end
end

if PROC=="GPU"
    function FstanhAXplusBias(A::CudaArray,X::CudaArray,b::CudaArray)
        tmp=FAXplusBias(A,X,b)[1]
        stanh!(sf,tmp,tmp)
        return(tmp,[])
    end

    function FstanhAXplusBias_inplace(value,aux,A::CudaArray,X::CudaArray,b::CudaArray)
        stanh!(sf,FAXplusBias(A,X,b)[1],value)
    end

    function DstanhAXplusBias(derivativeIDX,f_c,faux_c,grad_c,grad_n,A::CudaArray,X::CudaArray,b::CudaArray)
        tmp=CudaArray(Float64,size(grad_c)); fill!(tmp,0.0)
        Dstanh!(sf,grad_c,f_c,tmp)
        if derivativeIDX==1
            gemm!('N','T',1.0,tmp,X,1.0,grad_n)
        elseif derivativeIDX==2
            gemm!('T','N',1.0,A,tmp,1.0,grad_n)
        elseif derivativeIDX==3
            ons=CudaArray(Float64,(size(X,2),1)); fill!(ons,1.0)
            gemm!('N','N',1.0,tmp,ons,1.0,grad_n)
            free(ons)
        end
        free(tmp)
    end
end

Derivative[FstanhAXplusBias]=DstanhAXplusBias
Inplace[FstanhAXplusBias]=FstanhAXplusBias_inplace

stanhAXplusBias(A,X,b)=ADnode(FstanhAXplusBias,[A X b])
export stanhAXplusBias


export Bloating

"""
    Bloating{N, S<:LazySet{N}} <:LazySet{N}

Type that represents a uniform expansion of a set in a given norm (also
known as *bloating*).

### Fields

- `X` -- set
- `ε` -- (positive) bloating factor
- `p` -- ``p``-norm (should be ``≥ 1``; default: ``2``)

### Notes

If `ε` is positive, then `Bloating(X, ε, p)` is equivalent to the Minkowski sum
of `X` and a ball in the `p`-norm of radius `ε` centered in the origin `O`
(i.e., `X ⊕ Ballp(p, O, ε)`).

The `Bloating` operation preserves convexity: if `X` is convex, then any
bloating of `X` is convex as well.
"""
struct Bloating{N, S<:LazySet{N}} <:LazySet{N}
    X::S
    ε::N
    p::N

    function Bloating(X::S, ε::N, p::N=N(2)) where {N, S<:LazySet{N}}
        @assert p >= one(N) "bloating requires a norm >= 1, but $p was given"

        return new{N, S}(X, ε, p)
    end
end

isoperationtype(::Type{<:Bloating}) = true
isconvextype(::Type{Bloating{N, S}}) where {N, S} = isconvextype(S)

"""
    dim(B::Bloating)

Return the dimension of a bloated set.

### Input

- `B` -- bloated set

### Output

The ambient dimension of the bloated set.
"""
function dim(B::Bloating)
    return dim(B.X)
end

# helper function to compute the bloating ball
function _bloating_ball(B::Bloating{N}) where {N}
    return Ballp(B.p, zeros(N, dim(B)), abs(B.ε))
end

"""
    σ(d::AbstractVector, B::Bloating)

Return the support vector of a bloated set in a given direction.

### Input

- `d` -- direction
- `B` -- bloated set

### Output

The support vector of the bloated set in the given direction.
"""
function σ(d::AbstractVector, B::Bloating)
    @assert !iszero(d) "the support vector in the zero direction is undefined"
    @assert B.ε >= 0 || B.p > 1 "the support vector for negative bloating " *
        "in the 1-norm is not implemented"

    return σ(d, B.X) + sign_cadlag(B.ε) * σ(d, _bloating_ball(B))
end

"""
    ρ(d::AbstractVector, B::Bloating)

Return the support function of a bloated set in a given direction.

### Input

- `d` -- direction
- `B` -- bloated set

### Output

The support function of the bloated set in the given direction.
"""
function ρ(d::AbstractVector, B::Bloating)
    @assert !iszero(d) "the support function in the zero direction is undefined"

    return ρ(d, B.X) + sign_cadlag(B.ε) * ρ(d, _bloating_ball(B))
end

"""
    isbounded(B::Bloating)

Determine whether a bloated set is bounded.

### Input

- `B` -- bloated set

### Output

`true` iff the wrapped set is bounded.
"""
function isbounded(B::Bloating)
    return isbounded(B.X)
end

function isboundedtype(::Type{<:Bloating{N, S}}) where {N, S}
    return isboundedtype(S)
end

"""
    isempty(B::Bloating)

Determine whether a bloated set is empty.

### Input

- `B` -- bloated set

### Output

`true` iff the wrapped set is empty.
"""
function isempty(B::Bloating)
    return isempty(B.X)
end

"""
    an_element(B::Bloating)

Return some element of a bloated set.

### Input

- `B` -- bloated set

### Output

An element in the bloated set.

### Algorithm

The implementation returns the result of `an_element` for the wrapped set.
"""
function an_element(B::Bloating)
    return an_element(B.X)
end

"""
    constraints_list(B::Bloating)

Return the list of constraints of a bloated set.

### Input

- `B` -- bloated set

### Output

The list of constraints of the bloated set.

### Notes

The constraints list is only available for bloating in the `p`-norm for
``p = 1`` or ``p = ∞`` and if `constraints_list` is available for the unbloated
set.

### Algorithm

We call `constraints_list` on the lazy Minkowski sum.
"""
function constraints_list(B::Bloating)
    @assert (B.p == 1 || B.p == Inf) "the constraints list is only available " *
        "for bloating in the 1-norm or in the infinity norm"
    if B.ε < 0
        throw(ArgumentError("computing the constraints list of a negatively " *
                            "bloated set is not supported yet"))
    end

    return constraints_list(MinkowskiSum(B.X, _bloating_ball(B)))
end


using MarginalLogDensities
using Distributions
using StatsPlots
using Random
using Optim
using BenchmarkTools

Random.seed!(1234)
ncategories = 8
categories = 1:ncategories
μ0 = 5.0
σ0 = 5.0
aa = rand(Normal(μ0, σ0), ncategories)
b = 4.5
σ = 0.5
const category = repeat(categories, inner=50)
const n = length(category)
const x = rand(Uniform(-1, 1), n)
μ = [aa[category[i]] + b * x[i] for i in 1:n]
const y = rand.(Normal.(μ, σ))

scatter(x, y, color=category, label="")

function loglik(θ::Vector{T}) where T
    μ0 = θ[1]
    σ0 = exp(θ[2])
    aa = θ[3:10]
    b = θ[11]
    σ = exp(θ[12])
    μ = [aa[category[i]] + b * x[i] for i in 1:n]
    return loglikelihood(Normal(μ0, σ0), aa) + sum(logpdf.(Normal.(μ, σ), y))
end

θtrue = [μ0; log(σ0); aa; b; log(σ)]
nθ = length(θtrue)
loglik(θtrue)
θmarg = θtrue[[1, 2, 11, 12]]

mld = MarginalLogDensity(loglik, nθ, collect(3:10))
@btime mld($aa, $θmarg) # 5.3 μs
@btime mld($θmarg) # 115 μs
@profiler for i in 1:5000
    mld([μ0, log(σ0), b, log(σ)])
end

opt = optimize(θjoint -> -mld(θjoint), ones(4))
μ0_opt, logσ0_opt, b_opt, logσ_opt = opt.minimizer

                    # should be:
μ0_opt              # 5.0
exp(logσ0_opt)      # 5.0
b_opt               # 4.5
exp(logσ_opt)       # 0.5

θ_opt = [μ0_opt, exp(logσ0_opt), b_opt, exp(logσ_opt)]

using FiniteDiff, LinearAlgebra
H = FiniteDiff.finite_difference_hessian(θjoint -> -mld(θjoint), opt.minimizer)
std_errors = 1 ./ sqrt.(diag(H))

θ_names = ["μ₀", "σ₀", "b", "σ"]
scatter(θ_names, θ_opt, yerror=2*std_errors, label="Estimate",
    xlabel="Parameter", ylabel="Value")
scatter!(θ_names, [μ0, σ0, b, σ], label="Truth")


module Newton
using LinearAlgebra
using RecursiveFactorization
using DiffResults
using ForwardDiff
using StaticArrays

struct NewtonCache{Tres,Tcfg}
    result::Tres
    config::Tcfg
    lupivot::Vector{Int}
end

"""
    function NewtonCache(x::AbstractVector, rf!)

Create the cache used by the `newtonsolve!` and `linsolve!`
to find `x` such that `rf!(r,x)` yields `r=0`.
"""
function NewtonCache(x::AbstractVector, rf!)
    result = DiffResults.JacobianResult(x)
    cfg = ForwardDiff.JacobianConfig(rf!, x, result.value, ForwardDiff.Chunk(length(x)))
    lupivot = Vector{Int}(undef, length(x))
    return NewtonCache(result, cfg, lupivot)
end

"""
    get_drdx(cache::NewtonCache) = DiffResults.jacobian(cache.result)

"""
get_drdx(cache::NewtonCache) = DiffResults.jacobian(cache.result)

"""
    linsolve!(K::AbstractMatrix, b::AbstractVector, cache::NewtonCache)

Solves the linear equation system `Kx=b`, mutating both `K` and `b`.
`b` is mutated to the solution `x`
"""
function linsolve!(K::AbstractMatrix, b::AbstractVector, cache::NewtonCache)
    LU = RecursiveFactorization.lu!(K, cache.lupivot)
    ldiv!(LU, b)
    return b
end

"""
    newtonsolve!(x::AbstractVector, drdx::AbstractMatrix, rf!, cache::ResidualCache; tol=1.e-6, maxiter=100)

Solve the nonlinear equation system r(x)=0 using the newton-raphson method. Returns `true` if converged and `false` otherwise.

# args
- `x`: Vector of unknowns. Provide as initial guess, mutated to solution.
- `drdx`: Jacobian matrix. Only provided as preallocation. Can be aliased to `DiffResults.jacobian(cache.result)`
- `rf!`: Residual function. Signature `rf!(r, x)` and mutating the residual `r`
- `cache`: Optional cache that can be preallocated by calling `ResidualCache(x, rf!)`

# kwargs
- `tol=1.e-6`: Tolerance on `norm(r)`
- `maxiter=100`: Maximum number of iterations before no convergence

"""
function newtonsolve!(x::AbstractVector, drdx::AbstractMatrix, rf!, cache::NewtonCache = NewtonCache(x,rf!); tol=1.e-6, maxiter=100)
    diffresult = cache.result
    cfg = cache.config
    for i = 1:maxiter
        # Disable checktag using Val{false}(). solve_residual should never be differentiated using dual numbers!
        # This is required when using a different (but equivalent) anynomus function for caching than for running.
        ForwardDiff.jacobian!(diffresult, rf!, diffresult.value, x, cfg, Val{false}())
        err = norm(DiffResults.value(diffresult))
        i == 1 && @assert !(typeof(err) <: ForwardDiff.Dual)    # Check that we don't try to differentiate
        if err < tol
            drdx .= DiffResults.jacobian(diffresult)
            return true
        end
        linsolve!(DiffResults.jacobian(diffresult), DiffResults.value(diffresult), cache)
        x .-= DiffResults.value(diffresult)
    end
    # No convergence
    return false
end

"""
    linsolve(drdx::SMatrix{dim,dim}, r::SVector{dim}) where{dim}

Solves the linear equation system `drdx*x=r` without mutating and returns the solution x
"""
@inline linsolve(drdx::SMatrix{dim,dim}, r::SVector{dim}) where{dim} = drdx\r

"""
    newtonsolve(x0::SVector, rf; tol=1.e-6, maxiter=100)

Solve the nonlinear equation system `r(x)=0` using the newton-raphson method.
Returns type: `(converged, x, drdx)`, SVector, SMatrix)` where
- `converged::Bool` is `true` if converged and `false` otherwise
- `x::SVector` is the solution vector such that `r(x)=0`
- `drdx::SMatrix` is the jacobian at `x`

# args
- `x0`: Vector of with initial guess for unknowns.
- `rf`: Residual function. Signature `r=rf(x::SVector{dim})::SVector{dim}`

# kwargs
- `tol=1.e-6`: Tolerance on `norm(r)`
- `maxiter=100`: Maximum number of iterations before no convergence

"""
function newtonsolve(x::SVector{dim}, rf; tol=1.e-6, maxiter=100) where{dim}
    for _ = 1:maxiter
        r = rf(x)
        err = norm(r)
        drdx = ForwardDiff.jacobian(rf, x)
        if err < tol
            return true, x, drdx
        end
        x -= linsolve(drdx, r)
    end
    return false, zero(SVector{dim})*NaN, zero(SMatrix{dim,dim})*NaN
end

export newtonsolve!, newtonsolve
export NewtonCache
export get_drdx

end


using MarketCycles
using CSV
using Statistics

@static if VERSION < v"0.7.0-DEV.2005"
    using Base.Test
else
    using Test
end

######################################################################
# Validate against John Ehlers Original TradeStation Easylanguage code
# Run against dummy data loaded in Tradestation
######################################################################

# Note - for some tests irratic results for the lead in period #

# 3-3 SuperSmoother Test
@testset "Ehlers" begin
    @testset "Super Smoother - Equation 3-3" begin
        filename = joinpath(dirname(@__FILE__), "test_3-3_Supersmoother.csv")
        test = CSV.read( filename)
        dat = Float64.(test[:, :x])
        super_smoother_benchmark = Float64.(test[:,:Ten_Period_Supersmoother])
        Super_Smoother = SuperSmoother(dat,n=10)
        Super_Smoother = round.(Super_Smoother; digits=2) # round same as tradestation output
        valid = ifelse.(Super_Smoother .== super_smoother_benchmark,1,0)
        valid = valid[28:length(valid)] # remove indicator lead in period
        @test sum(valid) == length(valid)
        # Passed but takes 28 lead in bars to do so
    end

    @testset "Decycler - Equation 4-1" begin
        filename = joinpath(dirname(@__FILE__), "test_4-1_Decycler.csv")
        test = CSV.read( filename)
        dat = Float64.(test[:, :x])
        decycler_benchmark = Float64.(test[:,:Sixty_Period_Decycler])
        decycler = Decycler(dat, n=60)
        decycler = round.(decycler; digits=2) # round same as tradestation output
        valid = ifelse.(decycler .== decycler_benchmark,1,0)
        @test sum(valid) == length(valid)
    end

    @testset "Decycle Oscillator - Equation 4-2" begin
        filename = joinpath(dirname(@__FILE__), "test_4-2_Decycle_Oscillator.csv")
        test = CSV.read( filename)
        dat = Float64.(test[:, :x])
        decycler_osc_benchmark = Float64.(test[:,:thirty_sixty_decycle_osc])
        decycler_osc = Decycle_OSC(dat, LPPeriod=30,HPPeriod=60)
        decycler_osc = round.(decycler_osc; digits=2) # round same as tradestation output
        valid = ifelse.(decycler_osc .== decycler_osc_benchmark,1,0)
        @test sum(valid) == length(valid)-148 # minus lead in
    end

    @testset "Band Pass Filter - Equation 5-1" begin
        filename = joinpath(dirname(@__FILE__), "test_5-1_Band_Pass_Filter.csv")
        test = CSV.read( filename)
        dat = Float64.(test[:, :x])
        BP_filt_benchmark = Float64.(test[:,:Trigger])
        BP_filt = BandPassFilter(dat,n=20,bandwidth=.3)
        BP_filt = round.(BP_filt;digits=2) # round same as tradestation output
        valid = ifelse.(BP_filt .== BP_filt_benchmark,1,0)
        @test sum(valid) == length(valid)-92  # 92 bar lead in period
    end

    @testset "Hurst Coefficient - Equation 6-1" begin
        filename = joinpath(dirname(@__FILE__), "test_6-1_Hurst_Coefficient.csv")
        test = CSV.read( filename)
        dat = Float64.(test[:, :x])
        Hurst_benchmark = Float64.(test[:,:Hurst_Coefficient])
        Hurst = HurstCoefficient(dat,n=30)
        Hurst = round.(Hurst;digits=4) # round same as tradestation output
        valid = ifelse.(Hurst .== Hurst_benchmark,1,0)
        @test sum(valid) == length(valid)-37 # 37 bar lead in period
    end

    @testset "HP LP Roofing Filter - Equation 7-1" begin
        filename = joinpath(dirname(@__FILE__), "test_7-1_HP_LP_Roofing_Filter.csv")
        test = CSV.read( filename)
        dat = Float64.(test[:, :x])
        HP_LP_Roof_benchmark = Float64.(test[:,:HP_LP_Roofing_Filter])
        HP_LP_Roof = HPLPRoofingFilter(dat)
        HP_LP_Roof = round.(HP_LP_Roof;digits=2) # round same as tradestation output
        valid = ifelse.(HP_LP_Roof .== HP_LP_Roof_benchmark,1,0)
        @test sum(valid) == length(valid)-48 # 48 bar lead in period
    end

    @testset "Zero Mean Roofing Filter - Lag 0 - Equation 7-2" begin
        filename = joinpath(dirname(@__FILE__), "test_7-2_Zero_Mean_Roofing_Filter.csv")
        test = CSV.read( filename)
        dat = Float64.(test[:, :x])
        Zero_Mean_Roofing_Filter_lag_0_benchmark = Float64.(test[:,:Filt])
        Zero_Mean_Roofing_Filter_lag_0 = HPLPRoofingFilter(dat)
        Zero_Mean_Roofing_Filter_lag_0 = round.(Zero_Mean_Roofing_Filter_lag_0;digits=2) # round same as tradestation output
        valid = ifelse.(Zero_Mean_Roofing_Filter_lag_0 .== Zero_Mean_Roofing_Filter_lag_0_benchmark,1,0)
        @test sum(valid) == length(valid)-48 # 48 bar lead in period
    end

    @testset "Zero Mean Roofing Filter - Lag 1 - Equation 7-2" begin
        filename = joinpath(dirname(@__FILE__), "test_7-2_Zero_Mean_Roofing_Filter.csv")
        test = CSV.read( filename)
        dat = Float64.(test[:, :x])
        Zero_Mean_Roofing_Filter_lag_1_benchmark = Float64.(test[:,:Filt2])
        Zero_Mean_Roofing_Filter_lag_1 = ZeroMeanRoofingFilterK1(dat)
        Zero_Mean_Roofing_Filter_lag_1 = round.(Zero_Mean_Roofing_Filter_lag_1;digits=2) # round same as tradestation output
        valid = ifelse.(Zero_Mean_Roofing_Filter_lag_1 .== Zero_Mean_Roofing_Filter_lag_1_benchmark,1,0)
        @test sum(valid) == length(valid)-60 # 48 bar lead in period
    end

    @testset "Roofing Filter Indicator Equation 7-3" begin
        filename = joinpath(dirname(@__FILE__), "test_7-3_Roofing_Filter_Indicator.csv")
        test = CSV.read(filename)
        dat = Float64.(test[:, :x])
        Roofing_Filter_Indicator_benchmark = Float64.(test[:,:Filt])
        Roofing_Filter_Indicator = RoofingFilterIndicator(dat)
        Roofing_Filter_Indicator = round.(Roofing_Filter_Indicator;digits=2) # round same as tradestation output
        valid = ifelse.(Roofing_Filter_Indicator .== Roofing_Filter_Indicator_benchmark,1,0)
        @test sum(valid) == length(valid)-213 # 213 bar lead in period
    end

    @testset "Modified Stochastic - Equation 7-4" begin
        filename = joinpath(dirname(@__FILE__), "test_7-4_Modified_Stochastic.csv")
        test = CSV.read(filename)
        dat = Float64.(test[:, :x])
        Modified_stochastic_benchmark = Float64.(test[:,:Modified_stochasitc])
        Modified_stochastic = ModifiedStochastic(dat,n=20)
        Modified_stochastic = round.(Modified_stochastic; digits=2) # round same as tradestation output
        valid = ifelse.(Modified_stochastic .== Modified_stochastic_benchmark,1,0)
        @test sum(valid) == length(valid)-72 # 72 bar lead in period
    end

    @testset "Modified RSI - Equation 7-5" begin
        filename = joinpath(dirname(@__FILE__), "test_7-5_Modified_RSI.csv")
        test = CSV.read(filename)
        dat = Float64.(test[:, :x])
        Modified_RSI_benchmark = Float64.(test[:,:mod_rsi])
        Modified_RSI = ModifiedRSI(dat,n=10)
        Modified_RSI = round.(Modified_RSI;digits=2) # round same as tradestation output
        valid = ifelse.(Modified_RSI .== Modified_RSI_benchmark,1,0)
        @test sum(valid) == length(valid)-96 # 96 bar lead in period
    end

    @testset "Autocorrelation Indicator - Equation 8-2" begin
        filename = joinpath(dirname(@__FILE__), "test_8-3_Autocorrelation_Indicator.csv")
        test = CSV.read(filename)
        dat = Float64.(test[:, :x])
        Autocor_p48_benchmark = Float64.(test[:,:cor_48])
        Autocor_p10_benchmark = Float64.(test[:,:cor_10])
        Autocor_ind_out = AutoCorrelationIndicator(dat)
        autocorr_p48 = Autocor_ind_out[:,48]
        autocorr_p10 = Autocor_ind_out[:,10]
        autocorr_p48 = round.(autocorr_p48;digits=4) # round same as tradestation output
        autocorr_p10 = round.(autocorr_p10;digits=4) # round same as tradestation output
        valid = ifelse.(autocorr_p10 .== Autocor_p10_benchmark,1,0)
        @test sum(valid) == length(valid)-172  # 124 bar lead in period
        valid = ifelse.(autocorr_p48 .== Autocor_p48_benchmark,1,0)
        @test sum(valid) == length(valid)-233 # 185 bar lead in period
    end

    @testset "Single Lag Autocorrelation - Equation 8-3" begin
        filename = joinpath(dirname(@__FILE__), "test_8-3_Autocorrelation_Indicator.csv")
        test = CSV.read(filename)
        dat = Float64.(test[:, :x])
        Autocor_p10_benchmark = Float64.(test[:,:cor_10])
        Autocor_ind_out = AutoCorrelationIndicator(dat)
        autocorr_p10 = Autocor_ind_out[:,10]
        autocorr_p10 = round.(autocorr_p10;digits=4) # round same as tradestation output
        valid = ifelse.(autocorr_p10 .== Autocor_p10_benchmark,1,0)
        @test sum(valid) == length(valid)-172  # 124 bar lead in period
    end

    # to do - fix MaxPwr calculation for Autocorrelation Peeriodogram 8-3

    @testset "Autocorrelation Reversals - Equation 8-4" begin
        filename = joinpath(dirname(@__FILE__), "test_8-4_Autocorrelation_Reversals.csv")
        test = CSV.read(filename)
        dat = Float64.(test[:, :x])
        Autocor_reversals_benchmark = Float64.(test[:,:autocor_reversal])
        Autocor_reversals_out = AutoCorrelationReversals(dat)
        valid = ifelse.(Autocor_reversals_out .== Autocor_reversals_benchmark,1,0)
        @test sum(valid) == length(valid)-8  # adjust for lead in times
    end

    # TO DO- fix MaxPwr calculation for DFT Spectral Estimate

    # TO DO -  Complete Comb Filter Spectral Estimate - Equation 10-1

end


# # Conjugate Gradient
# In this tutorial we describe the basics of using the conjugate gradient iterative solvers
# At the end you should be able to
# 1. Use Conjugate Gradient to solve a linear system
# 2. Know when to not use it
# 3. Contruct a column-wise linear solver with Conjugate Gradient

# ## What is it?
# Conjugate Gradient is an iterative method for solving special kinds of linear systems:
# ```math
#  Ax = b
# ```
# via iterative methods.
# !!! warning
#     The linear operator need to be symmetric positive definite and the preconditioner must be symmetric.
# See the [wikipedia](https://en.wikipedia.org/wiki/Conjugate_gradient_method) for more details.

# ## Basic Example
# First we must load a few things
using ClimateMachine,
    ClimateMachine.LinearSolvers, ClimateMachine.ConjugateGradientSolver
using LinearAlgebra, Random

# Next we define a 3x3 symmetric positive definite linear system. (In the ClimateMachine code a symmetric positive definite system could arise from treating diffusion implicitly.)
A = [
    2.0 -1.0 0.0
    -1.0 2.0 -1.0
    0.0 -1.0 2.0
];
# We define the matrix `A` here as a global variable for convenience later.

# We can see that it is symmetric. We can check that it is positive definite by checking the spectrum
eigvals(A)

# The linear operators that are passed into the abstract iterative solvers need to be defined as functions that act on vectors. Let us do that with our matrix. We are using function closures for type stability.
function closure_linear_operator!(A)
    function linear_operator!(x, y)
        mul!(x, A, y)
    end
    return linear_operator!
end;

# We now define our linear operator using the function closure

linear_operator! = closure_linear_operator!(A)

# We now define our `b` in the linear system
b = ones(typeof(1.0), 3);

# The exact solution to the system `Ax = b` is
x_exact = [1.5, 2.0, 1.5];

# Now we can set up the ConjugateGradient struct
linearsolver = ConjugateGradient(b);
# and an initial guess for the iterative solver.
x = ones(typeof(1.0), 3);
# To solve the linear system we just need to pass to the linearsolve! function
iters = linearsolve!(linear_operator!, linearsolver, x, b)
# The variable `x` gets overwitten during the linear solve
# The norm of the error is
norm(x - x_exact) / norm(x_exact)
# The relative norm of the residual is
norm(A * x - b) / norm(b)
# The number of iterations is
iters
# Conjugate Gradient is guaranteed to converge in 3 iterations with perfect arithmetic in this case.

# ## Non-Example

# Conjugate Gradient is not guaranteed to converge with nonsymmetric matrices. Consider
A = [
    2.0 -1.0 0.0
    0.0 2.0 -1.0
    0.0 0.0 2.0
];
# We define the matrix `A` here as a global variable for convenience later.

# We can see that it is not symmetric, but it does have all positive eigenvalues
eigvals(A)

# The linear operators that are passed into the abstract iterative solvers need to be defined as functions that act on vectors. Let us do that with our matrix. We are using function closures for type stability.
function closure_linear_operator!(A)
    function linear_operator!(x, y)
        mul!(x, A, y)
    end
    return linear_operator!
end;

# We define the linear operator using our function closure

linear_operator! = closure_linear_operator!(A)

# We now define our `b` in the linear system
b = ones(typeof(1.0), 3);

# The exact solution to the system `Ax = b` is
x_exact = [0.875, 0.75, 0.5];

# Now we can set up the ConjugateGradient struct
linearsolver = ConjugateGradient(b, max_iter = 100);
# We also passed in the keyword argument "max_iter" for the maximum number of iterations of the iterative solver. By default it is assumed to be the size of the vector.
# As before we need to define an initial guess
x = ones(typeof(1.0), 3);
# To (not) solve the linear system we just need to pass to the linearsolve! function
iters = linearsolve!(linear_operator!, linearsolver, x, b)
# The variable `x` gets overwitten during the linear solve
# The norm of the error is
norm(x - x_exact) / norm(x_exact)
# The relative norm of the residual is
norm(A * x - b) / norm(b)
# The number of iterations is
iters
# Conjugate Gradient is guaranteed to converge in 3 iterations with perfect arithmetic for a symmetric positive definite matrix. Here we see that the matrix is not symmetric and it didn't converge even after 100 iterations.

# ## More Complex Example
# Here we show how to construct a column-wise iterative solver similar to what is is in the ClimateMachine code. The following is not for the faint of heart.
# We must first define a linear operator that acts like one in the ClimateMachine
function closure_linear_operator!(A, tup)
    function linear_operator!(y, x)
        alias_x = reshape(x, tup)
        alias_y = reshape(y, tup)
        for i6 in 1:tup[6]
            for i4 in 1:tup[4]
                for i2 in 1:tup[2]
                    for i1 in 1:tup[1]
                        tmp = alias_x[i1, i2, :, i4, :, i6][:]
                        tmp2 = A[i1, i2, i4, i6] * tmp
                        alias_y[i1, i2, :, i4, :, i6] .=
                            reshape(tmp2, (tup[3], tup[5]))
                    end
                end
            end
        end
    end
end;

# Now that we have this function, we can define a linear system that we will solve columnwise
# First we define the structure of our array as ```tup``` in a manner that is similar to a stacked brick topology
tup = (3, 4, 7, 2, 20, 2);
# where
# 1. tup[1] is the number of Gauss–Lobatto points in the x-direction
# 2. tup[2] is the number of Gauss–Lobatto points in the y-direction
# 3. tup[3] is the number of Gauss–Lobatto points in the z-direction
# 4. tup[4] is the number of states
# 6. tup[5] is the number of elements in the vertical direction
# 7. tup[6] is the number of elements in the other directions

# Now we define our linear operator as a random matrix.
Random.seed!(1235);
B = [
    randn(tup[3] * tup[5], tup[3] * tup[5])
    for i1 in 1:tup[1], i2 in 1:tup[2], i4 in 1:tup[4], i6 in 1:tup[6]
];
columnwise_A = [
    B[i1, i2, i4, i6] * B[i1, i2, i4, i6]' + 10I
    for i1 in 1:tup[1], i2 in 1:tup[2], i4 in 1:tup[4], i6 in 1:tup[6]
];
columnwise_inv_A = [
    inv(columnwise_A[i1, i2, i4, i6])
    for i1 in 1:tup[1], i2 in 1:tup[2], i4 in 1:tup[4], i6 in 1:tup[6]
];
columnwise_linear_operator! = closure_linear_operator!(columnwise_A, tup);
columnwise_inverse_linear_operator! =
    closure_linear_operator!(columnwise_inv_A, tup);

# We define our `x` and `b` with matrix structures similar to an MPIStateArray
mpi_tup = (tup[1] * tup[2] * tup[3], tup[4], tup[5] * tup[6]);
b = randn(mpi_tup);
x = randn(mpi_tup);

# Now we solve the linear system columnwise
linearsolver = ConjugateGradient(
    x,
    max_iter = tup[3] * tup[5],
    dims = (3, 5),
    reshape_tuple = tup,
);
# The keyword arguments dims is the reduction dimension for the linear solver. In this case dims = (3,5) are the ones associated with a column. The reshape_tuple argument is to convert the shapes of the array `x` in the a form that is more easily usable for reductions in the linear solver

# Now we can solve it
iters = linearsolve!(columnwise_linear_operator!, linearsolver, x, b);
x_exact = copy(x);
columnwise_inverse_linear_operator!(x_exact, b);
# The norm of the error is
norm(x - x_exact) / norm(x_exact)
# The number of iterations is
iters
# The algorithm converges within `tup[3]*tup[5] = 140` iterations

# ## Tips
# 1. The convergence criteria should be changed, machine precision is too small and the maximum iterations is often too large
# 2. Use a preconditioner if possible
# 3. Make sure that the linear system really is symmetric and positive-definite


import Measurements: Measurement
import ..UncertainValues: UncertainValue
import Distributions: Normal

resample(m::Measurement{T}) where T = resample(UncertainValue(Normal, m.val, m.err))

function resample(m::Measurement{T}, n::Int) where T
    uval = UncertainValue(Normal, m.val, m.err)

    [resample(uval) for i = 1:n]
end


@assert begin
    local_interp=InterpolationFunction(
        :(a0+a1*xi+a2*eta),
        [:xi, :eta],
        [:a0, :a1, :a2],
        [
            [0, 0]
        ]
    )

    abs_interp=InterpolationFunction(
        :(a0+a1*x+a2*y),
        [:x, :y],
        [:a0, :a1, :a2],
        [
            [0, 0]
        ]
    )

    dmn=Domain(
        local_interp,
        [
            (0.0, 1.0),
            (0.0, :(1.0-xi))
        ],
        [
            [0.0, 0.0],
            [1.0, 0.0],
            [0.0, 1.0]
        ]
    )

    fin=Finel(
        [
            [1.0, 1.0],
            [2.0, 1.0],
            [2.0, 2.0]
        ],
        dmn
    )

    vals=[
        0.0, 0.0, 1.0
    ]

    regr=finel_get_regr(fin, abs_interp)

    coefs=regress(regr, vals)

    isright=isapprox(coefs, [-1.0, 0.0, 1.0])

    Jac=Matrix{Float64}(undef, 2, 2)
    from_finel_jac!(Jac, fin, 0.0, 0.0)

    isright && isapprox(
        Jac,
        [1.0 1.0; 0.0 1.0]
    )
end


function linsolve(operator, b, x₀, alg::CG, a₀::Real = 0, a₁::Real = 1)
    # Initial function operation and division defines number type
    y₀ = apply(operator, x₀)
    T = typeof(dot(b, y₀)/norm(b)*one(a₀)*one(a₁))
    α₀ = convert(T, a₀)
    α₁ = convert(T, a₁)
    # Continue computing r = b - a₀ * x₀ - a₁ * operator(x₀)
    r = copyto!(similar(b, T), b)
    r = α₀ == 0 ? r : axpy!(-α₀, x₀, r)
    r = axpy!(-α₁, y₀, r)
    x = copyto!(similar(r), x₀)
    normr = norm(r)
    S = typeof(normr)

    # Algorithm parameters
    maxiter = alg.maxiter
    tol = convert(S, alg.rtol == 0 ? alg.atol : max(alg.atol, alg.rtol*norm(b)))
    numops = 1 # operator has been applied once to determine r
    numiter = 0

    # Check for early return
    normr < tol && return (x, ConvergenceInfo(1, r, normr, numiter, numops))

    # First iteration
    ρ = normr^2
    p = copyto!(similar(r), r)
    q = apply(operator, p)
    if α₀ != zero(α₀) || α₁ != one(α₁)
        axpby!(α₀, p, α₁, q)
    end
    α = ρ / dot(p,q)
    axpy!(+α, p, x)
    axpy!(-α, q, r)
    normr = norm(r)
    ρold = ρ
    ρ = normr^2
    β = ρ/ρold
    numops += 1
    numiter += 1

    # Check for early return
    normr < tol && return (x, ConvergenceInfo(1, r, normr, numiter, numops))

    while numiter < maxiter
        axpby!(1, r, β, p)
        q = apply(operator, p)
        if α₀ != zero(α₀) || α₁ != one(α₁)
            axpby!(α₀, p, α₁, q)
        end
        α = ρ / dot(p,q)
        axpy!(+α, p, x)
        axpy!(-α, q, r)
        normr = norm(r)
        if normr < tol # recompute to account for buildup of floating point errors
            r = copyto!(similar(b, T), b)
            r = α₀ == 0 ? r : axpy!(-α₀, x, r)
            r = axpy!(-α₁, apply(operator, x), r)
            normr = norm(r)
            ρ = normr^2
            β = 0 # restart CG
        else
            ρold = ρ
            ρ = normr^2
            β = ρ/ρold
        end
        normr < tol && return (x, ConvergenceInfo(1, r, normr, numiter, numops))
        numops += 1
        numiter += 1
    end
    return (x, ConvergenceInfo(0, r, normr, numiter, numops))
end


export getConvMatPeriodic, getAverageMatrix, getCoupledConvMat

function getAverageMatrix(TYPE::Type,nImg,nc)

    A1 = spdiagm((fill(convert(TYPE,.5),nImg[1]),fill(convert(TYPE,.5),nImg[1]-1)),(0,1),nImg[1],nImg[1])
    A1 = A1[1:2:end,:]
    A2 = spdiagm((fill(convert(TYPE,.5),nImg[2]),fill(convert(TYPE,.5),nImg[2]-1)),(0,1),nImg[2],nImg[2])
    A2 = A2[1:2:end,:]
    Av    = kron(A2,A1);
    return kron(speye(TYPE,nc),Av);
end
function getCoupledConvMat(TYPE::Type,K::Array,nImg,sK;stride=[1;1])
    # build single 2D convolution operator
    nK  = prod(sK[1:2])
    K   = reshape(K,tuple(sK...)) # basic kernel
    K = flipdim(K,2)
    K = flipdim(K,1)
    # K   = permutedims(K,(2,1,3,4))
    # apply stride
    idx = falses(nImg[1],nImg[2])
    idx[1:stride[1]:end,:] = true
    idx[:,1:stride[2]:end] = true

    A = spzeros(TYPE,0,prod(nImg[1:2])*sK[3])
    for i=1:sK[4]
        Ak = spzeros(TYPE,prod(nImg[1:2]),0)
        for j=1:sK[3]
            Ak = [Ak getConvMatPeriodic(TYPE,K[:,:,j,i],[nImg[1:2]; 1])]
        end
        A = [A; Ak[vec(idx).==true,:]]
    end
    return A
end

function getConvMatPeriodic(TYPE::Type,K::Array,n::Array{Int64})

G  = reshape(collect(1:prod(n)),tuple(n...))

jj = zeros(Int64,0); ii = zeros(Int64,0); vv = zeros(TYPE,0);
I1,I2,I3 = ndgrid(1:n[1],1:n[2],1:n[3]);
for i=1:size(K,1)
    for j=1:size(K,2)
        for k=1:size(K,3)
            ofi = i - div(size(K,1)-1,2) - 1
            ofj = j - div(size(K,2)-1,2) - 1
            ofk = k - div(size(K,3)-1,2) - 1

            t1 = vec(I1.+ofi)
            t1[t1.<=0]   += n[1]
            t1[t1.>n[1]] -= n[1]

            t2 = vec(I2.+ofj)
            t2[t2.<=0]   += n[2]
            t2[t2.>n[2]] -= n[2]

            t3 = vec(I3.+ofk)
			t3[t3.<=0]   += n[3]
            t3[t3.>n[3]] -= n[3]

            jj = [jj; vec(G)];
            ii = [ii; vec(G[sub2ind(tuple(n...),t1, t2,t3)])];
            vv = [vv; ones(prod(n))*K[i,j,k]];
        end
    end
end
A = convert(SparseMatrixCSC{TYPE},sparse(ii, jj ,vv,prod(n),prod(n)));
return A
end


#=
DelayABM method for system of fractional delay differential equations.

```tex
@inproceedings{Bhalekar2011APS,
  title={A PREDICTOR-CORRECTOR SCHEME FOR SOLVING NONLINEAR DELAY DIFFERENTIAL EQUATIONS OF FRACTIONAL ORDER},
  author={Sachin Bhalekar and Varsha Daftardar-Gejji},
  year={2011}
}
```
=#

function solve(FDDESys::FDDESystem, h, ::DelayABM)
    @unpack f, ϕ, α, τ, T = FDDESys
    len = length(ϕ)
    N::Int = round(Int, T/h)
    Ndelay = round(Int, τ/h)
    x1 = zeros(Ndelay+N+1, len)
    x = zeros(Ndelay+N+1, len)
    du = zeros(len)

    # Put the delay term in the array
    for i=1:len
        x[1:Ndelay, i] = ϕ[i]*ones(Ndelay)
    end

    x0 = copy(x[Ndelay, :])

    for i=1:len
        αi = α[i]
        f(du, x0, x[1, :], 0)
        x1[Ndelay+1, i] = x0[i] + h^αi*du[i]/(gamma(αi)*αi)
    end

    for i=1:len
        αi = α[i]
        f(du, x[Ndelay+1, :], x[1, :], 0)
        du1 = copy(du)
        f(du, x0, x[1, :], 0)
        x[Ndelay+1, i] = x0[i] + h^αi*(du1[i] + αi*du[i])/gamma(αi+2)
    end

    M1=zeros(len); N1=zeros(len)
    @fastmath @inbounds @simd for n=1:N
        for i=1:len
            αi = α[i]
            f(du, x0, x[1, :], 0)
            M1[i]=(n^(αi+1)-(n-αi)*(n+1)^αi)*du[i]
            N1[i]=((n+1)^αi-n^αi)*du[i]
        end

        @fastmath @inbounds @simd for j=1:n
            for i=1:len
                αi = α[i]
                f(du, x[Ndelay+j, :], x[j, :], 0)
                M1[i] = M1[i]+((n-j+2)^(αi+1)+(n-j)^(αi+1)-2*(n-j+1)^(αi+1))*du[i]
                N1[i] = N1[i]+((n-j+1)^αi-(n-j)^αi)*du[i]
            end
        end

        for i=1:len
            αi = α[i]
            x1[Ndelay+n+1, i] = x0[i]+h^αi*N1[i]/(gamma(αi)*αi)
            f(du, x[Ndelay+n+1, :], x[n+1, :], 0)
            x[Ndelay+n+1, i] = x0[i]+h^αi*(du[i]+M1[i])/gamma(αi+2)
        end
    end

    xresult = zeros(N-Ndelay+1, len)
    yresult = zeros(N-Ndelay+1, len)

    xresult[N-Ndelay+1, :] .= 0
    yresult[N-Ndelay+1, :] .= 0

    @fastmath @inbounds @simd for n=2*Ndelay+1:N+Ndelay+1
       xresult[n-2*Ndelay, :] = x[n, :]
       yresult[n-2*Ndelay, :] = x[n-Ndelay, :]
    end

    return xresult, yresult
end


"""
    build_QP_JuMP_model(Q,R,A,B,N;...) -> JuMP.Model(...)
Return a `JuMP.jl` Model for the quadratic problem

min 1/2 ( sum_{i=1}^{N-1} s_i^T Q s + sum_{i=1}^{N-1} u^T R u + s_N^T Qf s_n  )
s.t. s_{i+1} = As_i + Bs_i for i = 1,..., N-1
Optional Arguments
- `Qf = []`: matrix multiplied by s_N in objective function (defaults to Q if not given)
- `c = zeros(N*size(Q,1) + N*size(R,1)`:  linear term added to objective funciton, c^T z
- `sl = fill(-Inf, size(Q,1))`: lower bound on state variables
- `su = fill(Inf,  size(Q,1))`: upper bound on state variables
- `ul = fill(-Inf, size(Q,1))`: lower bound on input variables
- `uu = fill(Inf,  size(Q,1))`: upper bound on input variables
- `s0 = []`: initial state of the first state variables
"""
function build_QP_JuMP_model(
        Q,R,A,B, N;
        s0 = [],
        sl = [],
        su = [],
        ul = [],
        uu = [],
        Qf = Q,
        E  = [],
        F  = [],
        gl = [],
        gu = [],
        S  = zeros(size(Q, 1), size(R, 1)),
        K  = zeros(size(R, 1), size(Q, 1))
        )

    ns = size(Q,1) # Number of states
    nu = size(R,1) # Number of inputs

    NS = 1:ns # set of states
    NN = 1:N # set of times
    NU = 1:nu # set of inputs

    model = Model(MadNLP.Optimizer) # define model

    @variable(model, s[NS, 0:N])     # define states
    @variable(model, u[NU, 0:(N-1)]) # define inputs

    if !iszero(K)
        @variable(model, v[NU, 0:(N-1)])
    end

    # Bound states/inputs
    if length(sl) > 0
        for i in NS
            for j in 0:N
                if sl[i] != -Inf
                    @constraint(model, s[i,j] >= sl[i])
                end
            end
        end
    end
    if length(su) > 0
        for i in NS
            for j in 0:N
                if su[i] != Inf
                    @constraint(model, s[i,j] <= su[i])
                end
            end
        end
    end
    if length(ul) > 0
        for i in NU
            for j in 0:(N-1)
                if ul[i] != -Inf
                    @constraint(model,  u[i,j] >= ul[i])
                end
            end
        end
    end
    if length(uu) > 0
        for i in NU
            for j in 0:(N-1)
                if uu[i] != Inf
                    @constraint(model, u[i,j] <= uu[i])
                end
            end
        end
    end

    if length(s0) >0
        for i in NS
            JuMP.fix(s[i,0], s0[i])
        end
    end

    # Give constraints from A, B, matrices
    @constraint(model, [t in 0:(N - 1), s1 in NS], s[s1, t + 1] == sum(A[s1, s2] * s[s2, t] for s2 in NS) + sum(B[s1, u1] * u[u1, t] for u1 in NU) )

    # Constraints for Kx + v = u
    if !iszero(K)
        for u1 in NU
            @constraint(model, [t in 0:(N - 1)], u[u1, t] == v[u1, t] + sum( K[u1, s1] * s[s1,t]  for s1 in NS))
        end
    end

    # Add E, F constraints
    if length(E) > 0
        for i in 1:size(E,1)
            @constraint(model,[t in 0:(N-1)], gl[i] <= sum(E[i, s1] * s[s1, t] for s1 in NS) + sum(F[i,u1] * u[u1, t] for u1 in NU))
            @constraint(model,[t in 0:(N-1)], gu[i] >= sum(E[i, s1] * s[s1, t] for s1 in NS) + sum(F[i,u1] * u[u1, t] for u1 in NU))
        end
    end

    # Give objective function as xT Q x + uT R u where x is summed over T and u is summed over T-1
    @objective(model,Min,  sum( 1/2 * Q[s1, s2]*s[s1,t]*s[s2,t] for s1 in NS, s2 in NS, t in 0:(N-1)) +
            sum( 1/2 * R[u1,u2] * u[u1, t] * u[u2,t] for t in 0:(N-1) , u1 in NU, u2 in NU) +
            sum( 1/2 * Qf[s1,s2] * s[s1,N] * s[s2, N]  for s1 in NS, s2 in NS) +
            sum( S[s1, u1] * s[s1, t] * u[u1, t] for s1 in NS, u1 in NU, t in 0:(N-1))
            )

    return model
end


"the effective low frequency bulk modulus and density of a material filled with particles"
function effective_medium(medium::Medium{T}, species::Array{Specie{T}}; dim = 2) where T<:Number
    φ = sum(volume_fraction.(species; dim = dim))

    # calculate effective properties
    β = medium.ρ*medium.c^2 # medium bulk modulus
    if abs(β) == zero(T) || abs(prod(s -> s.ρ*s.c^2, species)) == zero(T)
        β_eff = zero(T)
    else
        β_eff = one(T)/((1-φ)/β + sum(volume_fraction(s; dim = dim)/(s.ρ*s.c^2) for s in species))
    end
    ρ_frac = sum(volume_fraction(s; dim = dim)*(medium.ρ - s.ρ)/(medium.ρ + s.ρ) for s in species)
    ρ_eff = medium.ρ*(1 - ρ_frac)/(1 + T(dim - 1) * ρ_frac)

    return Medium(ρ=ρ_eff, c=sqrt(β_eff/ρ_eff))
end

"the reflection from a homogenious halfspace, which is also the low frequency reflection from a particulate material when using the effective_medium."
function reflection_coefficient_halfspace(incident_medium::Medium{T}, reflect_medium::Medium{T};
        θin::T = zero(T), tol = 1e-8) where T<:Number

    k_in = T(1)/incident_medium.c
    k_r = T(1)/reflect_medium.c

    q = real(reflect_medium.c*reflect_medium.ρ/(incident_medium.c*incident_medium.ρ))
    θ_trans = transmission_angle(k_in, k_r, θin; tol=tol)
    R = (q*cos(θin) - cos(θ_trans))/(q*cos(θin) + cos(θ_trans))
end


using SpecialFunctions: gamma
using GSL: sf_gamma_inc_P
incomplete_gamma = sf_gamma_inc_P

"""
The displaced Poisson distribution (a.k.a the hyper-Poisson distribution).

Ref: https://www.jstor.org/stable/2283992
"""
struct DisplacedPoisson{T<:Real} <: Distributions.DiscreteUnivariateDistribution
    λ::T
    r::T
    function DisplacedPoisson{T}(λ, r) where {T<:Real}
        @assert λ > 0 "λ is not positve"
        @assert r >= 0 "r is not non-negative"
        return new(λ, r)
    end
end
DisplacedPoisson(λ::T, r::T) where {T<:Real} = DisplacedPoisson{T}(λ, r)

function pdf(dp::DisplacedPoisson, k::Int)
    if k < dp.r
        return 0
    else
        p = dp.λ^(k - dp.r) * exp(-dp.λ) / gamma(k - dp.r + 1)
        diff = ceil(dp.r) - dp.r
        # incomplete_gamma(a, b) is undefined for a = 0
        if !iszero(diff)
            return p / incomplete_gamma(diff, dp.λ)
        else
            return p
        end
    end
end

function rand(dp::DisplacedPoisson{T}) where {T}
    u = rand(T)
    cm = 0
    k = -1
    while cm < u
        k += 1
        cm += pdf(dp, k)
    end
    return k
end

function mode(dp::DisplacedPoisson)
    k = 0
    p = pdf(dp, k)
    while true
        p_next = pdf(dp, k + 1)
        if p_next < p
            break
        end
        p = p_next
        k += 1
    end
    return k
end

minimum(dp::DisplacedPoisson) = ceil(Integer, dp.r)
maximum(dp::DisplacedPoisson) = Inf

Base.show(io::IO, dp::DisplacedPoisson) =
    print(io, "DisplacedPoisson($(round(dp.λ; sigdigits=3)), $(round(dp.r; sigdigits=3)))")


using Test, Random, LSHFunctions

include(joinpath("..", "utils.jl"))

#==================
Tests
==================#
@testset "LpHash tests" begin
    Random.seed!(RANDOM_SEED)

    @testset "Can construct an ℓ^p distance hash function" begin
        # Construct a hash for L^1 distance
        L1_hash = L1Hash(5; scale = 2)
        @test n_hashes(L1_hash) == 5
        @test L1_hash.scale == 2
        @test L1_hash.power == 1
        @test similarity(L1_hash) == ℓ1
        @test hashtype(L1_hash) == Int32

        # Construct a hash for L^2 distance
        L2_hash = L2Hash(12; scale = 3.4)
        @test n_hashes(L2_hash) == 12
        @test L2_hash.scale == Float32(3.4)
        @test L2_hash.power == 2
        @test similarity(L2_hash) == ℓ2

        # Construct a hash using a specified dtype
        @test isa(L1Hash(1; dtype=Float32), LSHFunctions.LpHash{Float32})
        @test isa(L2Hash(1; dtype=Float64), LSHFunctions.LpHash{Float64})
    end

    @testset "Hashes are correctly computed" begin
        n_hashes = 8
        scale = 2

        hashfn = L2Hash(n_hashes; scale=scale)

        # Test on a single input
        x = randn(8)
        hashes = hashfn(x)
        manual_hashes = floor.(Int32, hashfn.coeff * x ./ scale .+ hashfn.shift)

        @test isa(hashes, Vector{Int32})
        @test hashes == manual_hashes

        # Test on many inputs, simultaneously
        x = randn(8, 128)
        hashes = hashfn(x)
        manual_hashes = floor.(Int32, hashfn.coeff * x ./ scale .+ hashfn.shift)

        @test isa(hashes, Matrix{Int32})
        @test hashes == manual_hashes
    end

    @testset "Hashes have the correct dtype" begin
        hashfn = L1Hash(5)

        # Test 1: Vector{Float64} -> Vector{Int32}
        hashes = hashfn(randn(5))
        @test eltype(hashes) == hashtype(hashfn)
        @test isa(hashes, Vector{Int32})

        # Test 2: Matrix{Float64} -> Matrix{Int32}
        hashes = hashfn(randn(5, 10))
        @test isa(hashes, Matrix{Int32})
    end

    @testset "Nearby points experience more frequent collisions" begin
        hashfn = L2Hash(1024; dtype=Float64, scale=4)

        x1 = randn(128)
        x2 = x1 + 0.05 * randn(length(x1))
        x3 = x1 + 0.50 * randn(length(x1))
        x4 = x1 + 2.00 * randn(length(x1))

        h1, h2, h3, h4 = hashfn(x1), hashfn(x2), hashfn(x3), hashfn(x4)
        @test sum(h1 .== h4) < sum(h1 .== h3) < sum(h1 .== h2)
    end

    @testset "Hash collision frequency matches probability" begin
        hashfn = L2Hash(1024; scale = 4)

        # Dry run
        @test test_collision_probability(hashfn, 0.05)

        # Full test
        @test all(test_collision_probability(hashfn, 0.05) for ii = 1:128)
    end

    @testset "Hash inputs of different sizes" begin
        n_hashes = 10
        for hashfn_type in (:L1Hash, :L2Hash)
            hashfn = eval(hashfn_type)(n_hashes)
            @test size(hashfn.coeff) == (n_hashes, 0)

            hashfn(rand(5))
            @test size(hashfn.coeff) == (n_hashes, 5)

            hashfn(rand(20))
            @test size(hashfn.coeff) == (n_hashes, 20)

            hashfn(rand(10))
            @test size(hashfn.coeff) == (n_hashes, 20)
        end
    end

    @testset "Hash inputs of different sizes with resize_pow2 = true" begin
        n_hashes = 20
        for hashfn_type in (:L1Hash, :L2Hash)
            hashfn = eval(hashfn_type)(n_hashes; resize_pow2=true)
            @test size(hashfn.coeff) == (n_hashes, 0)

            hashfn(rand(4))
            @test size(hashfn.coeff) == (n_hashes, 4)

            hashfn(rand(5))
            @test size(hashfn.coeff) == (n_hashes, 8)

            hashfn(rand(10))
            @test size(hashfn.coeff) == (n_hashes, 16)

            hashfn(rand(8))
            @test size(hashfn.coeff) == (n_hashes, 16)
        end
    end

    @testset "collision_probability works correctly" begin
        hashfn = L1Hash()

        # collision_probability should be 1 for two inputs of distance zero
        x = rand(4)
        @test collision_probability(hashfn, x, x) ≈ 1.0

        # collision_probability with n_hashes=N should be the same as
        # collision_probability with n_hashes=1, raised to the power N
        y = rand(4)
        @test collision_probability(hashfn, x, y; n_hashes=10) ≈
              collision_probability(hashfn, x, y; n_hashes=1)^10
    end
end


module DesignStructureMatrix

using Gadfly
using LinearAlgebra

export plotDSM
export toReachableMatrix
export OrderReachable
export Sequencing
export Clustering

## plot DSM by using Gadfly
function plotDSM(DSM,label)

    cDSM = copy(DSM);
    cDSM[diagind(cDSM)] .= -1

     spy(cDSM, Scale.y_discrete(labels = i->label[i]), Scale.x_discrete(labels = i->label[i]),
        Guide.ylabel(nothing), Guide.xlabel(nothing, orientation=:vertical), Guide.xticks(orientation=:vertical),
        Scale.color_continuous(colormap=Scale.lab_gradient( "darkgrey","aliceblue", "navy")),
        Theme(minor_label_font_size=16pt, key_position=:none, bar_spacing= 0.5mm, panel_fill="darkgrey"))

end

## convert DSM to ReachableMatrix
function toReachableMatrix(DSM)

    I = zeros(Int64,size(DSM)) + Diagonal(ones(Int64,size(DSM))); #identity matrix
    R1 = zeros(Int64,size(DSM))
    calcmax = 1000

    for i  = 1 : calcmax

        R1 = (DSM+I)^i
        (Rindex) =findall(x-> 1<x , R1);
        R1[Rindex] = ones(size(Rindex));

        R2 = (DSM+I)^(i+1)
        (Rindex) =findall(x-> 1<x , R2);
        R2[Rindex] = ones(size(Rindex));

        if R1 == R2
            break
        end

    end

    return R1
end

##order Reachable matrix for seaqencing
function OrderReachable(Reachable)

    original = copy(Reachable);
    (DSMsize,) = size(Reachable);
    valuesDSM = collect(1:DSMsize);
    count = 0;
    level = zeros(Int,DSMsize);

    while count < DSMsize

        R = Reachable;
        A = Reachable';
        RA = R .* A;
        (RAsize,) = size(RA);
        elements = collect(1:RAsize);

        for i = 1 : RAsize

            if RA[i,:] == R[i,:];
                    count += 1;
                    level[count] = valuesDSM[i];
                    elements[i] = 0;
            end
        end

        elements = filter(x-> x > 0, elements);
        Reachable = Reachable[elements , elements];
        valuesDSM = valuesDSM[elements];

    end

    orderedReachable = original[level,level]

    return orderedReachable , level

end

## Sequencing DSM
function Sequencing(DSM,label)
    cDSM = copy(DSM);
    clabel = copy(label);

    Reachable = toReachableMatrix(cDSM);

    (OrderedReachable,level) = OrderReachable(Reachable);

    SequencedDSM = cDSM[level,level];
    Sequencedlabel = clabel[level];

    return SequencedDSM, Sequencedlabel

end

##Clustering DSM
function Clustering(DSM,label)

    powcc = 1;
    powbid = 0;
    powdep = 1;
    rand_accept = 30;
    rand_bid = 30;
    times = 5;
    stable_limit = 2;

    original_DSM = copy(DSM);
    original_label = copy(label);
    (DSMsize,) = size(original_DSM);

    Clustermatrix = zeros(Int64,DSMsize,DSMsize) + Diagonal(ones(Int64,DSMsize,DSMsize)) ;

    cDSM = copy(original_DSM);

    function ClusterBid(Clustermatrix,cDSM,DSMsize,powdep,powbid,element)

        bestCluster = 0;
        bestBid = -1;

        for i = 1 : DSMsize

            Clusterlist= findall(x-> x == 1 , Clustermatrix[i,:]);
            (Clustersize,) = size(Clusterlist);
            bid = 0;

            for j = 1 : Clustersize

                bid += cDSM[element,Clusterlist[j]];

            end

            bid = bid ^ powdep / Clustersize ^ powbid;

            if bid > bestBid

                 bestBid = bid;
                bestCluster = i;
            end

        end

        return bestCluster

    end

    function TCC(cDSM,DSMsize,Clustermatrix,powcc)

        intraCost=0;
        extraCost=0;

        for i = 1: DSMsize

            for j = 1 : DSMsize

                Clusterofi = findall(x-> x == 1 , Clustermatrix[:,i]);
                Clusterofj = findall(x-> x == 1 , Clustermatrix[:,j]);
                (Clustersizeofi,) = size(Clustermatrix[Clusterofi,:]);

                cost = cDSM[i,j] + cDSM[j,i];

                if Clusterofi == Clusterofj

                    intraCost += cost*Clustersizeofi^powcc;

                else

                    extraCost += cost*DSMsize^powcc;
                end

            end

        end

        totalCost = intraCost + extraCost;

        return totalCost
    end

    TCCost = TCC(cDSM,DSMsize,Clustermatrix,powcc)

    costlist = zeros(DSMsize*times,1);

    for i = 1 : DSMsize * times;

        preClustermatrix = Clustermatrix;
        element = rand(1:DSMsize);
        bestCluster = ClusterBid(Clustermatrix,cDSM,DSMsize,powdep,powbid,element);
        Clustermatrix[:,element] = zeros(DSMsize,1);
        Clustermatrix[bestCluster,element] = 1;
        newCost = TCC(cDSM,DSMsize,Clustermatrix,powcc);
        if newCost <= maximum([TCCost,rand_accept])
            TCCost = newCost;
        else
            Clustermatrix = preClustermatrix;
        end

        costlist[i] = newCost
    end

    orderCluster = [sum(Clustermatrix,dims=2) Clustermatrix]
    orderedCluster = sortslices(orderCluster,dims = 1, rev=true)
    reorderedCluster = orderedCluster[:,2:DSMsize+1]

    Order = [];

    for i = 1 : DSMsize
        Order = [Order ; findall(x-> x == 1 , reorderedCluster[i,:])]
    end

    ## reorder DSM
    I =zeros(DSMsize,DSMsize) + Diagonal(ones(DSMsize,DSMsize))
    P =zeros(DSMsize,DSMsize) + Diagonal(ones(DSMsize,DSMsize))
    Clusteredlabel = copy(original_label)
    copylabel= copy(original_label)

    ## Permutation matrix
    for i = 1 : DSMsize
        P[i,:] = I[Order[i],:]
        Clusteredlabel[i] = copylabel[Order[i]]
    end

    ## order DSM
    ClusteredDSM = P*original_DSM*P'

    return ClusteredDSM, Clusteredlabel;

end

end # module


"""
    constraint_equations(f::ODEFunction)

Returns the constraint equations from an ODEFunction f used in DifferentialEquations.jl.

f must be in Mass Matrix form meaning: M ẋ = f(x).
"""
function constraint_equations(f::ODEFunction)
    M = f.mass_matrix
    if M == true*I
        error("There are no constraints in the system!")
    end
    len_M = size(M, 1)
    g_idx = findall([M[i,i] for i in 1:len_M] .== 0)
    g(x) = (dx = similar(x);
            f(dx, x, nothing, 0.0);
            dx[g_idx])
end

"""
    AmbientForcing(f::ODEFunction, z, tau, Frand)
Draws random initial condition form the constraint manifold M.

This consists of the following steps:
    1. Draw a random vector Frand from the ambient space
    2. Projetion back onto the tangetial space TxM by using PN

# Arguments
    f: An ODEFunction in mass matrix form
    initial_con: A point which lays on the manifold M, could e.g. be a fixed point
    dist_args: Arguments of the distribution function dist
    dist: Distribution function e.g. Uniform, Normal....
    tau: Integration time
"""
function ambient_forcing(f::ODEFunction, z, tau, Frand)
    g = constraint_equations(f)
    prob = ODEProblem(ambient_forcing_ODE, z, (0.0, tau), (g, Frand))
    sol = solve(prob, Tsit5())
    return sol[end]
end

"""
    Proj_N(A)

Calculates the orthogonal projection matrix on a subspace N.

A is a matrix containg the basis vectors of N as columns.
The matrix (A^T*A)^-1 recovers the norm.
"""
Proj_N(A) = A * inv(transpose(A) * A) * transpose(A)

"""
    ambient_forcing_ODE(u, p, t)
The manifold preserving ODE version of a vector h.

ż = Pn(ker(Jg)) * h
# Arguments
    `p[1]`: g the function defining the manifold
    `p[2]` h constant vector from ambient space
    `u0`: Inital condition
    `t`: Integration time
"""
function ambient_forcing_ODE(u, p, t)
    g, h = p
    Jacg = jacobian(g, u)
    N = nullspace(Jacg)
    du = Proj_N(N) * h # Projection back onto the Manifold
end


function fweight(a, b; d=3)
    return length(a)^d + length(b)^d - length(union(a, b))^d
end

function mergeclique!(cliques, cliqueG, i, j; d=3)
    cliques[i] = unique(sort([cliques[i]; cliques[j]]))
    cliques[j] = cliques[end]
    cliques = cliques[1:end-1]
    for neighbor in neighbors(cliqueG, j)
        if neighbor != i
            add_edge!(cliqueG, i, neighbor)
        end
    end
    rem_vertex!(cliqueG, j)
    for neighbor in neighbors(cliqueG, i)
        weight = fweight(cliques[i], cliques[neighbor], d=d)
        set_prop!(cliqueG, i, neighbor, :weight, weight)
    end
    return cliques,cliqueG
end

function clique_merge!(cliques; d=3, QUIET=true)
    cql = length(cliques)
    cliqueG = MetaGraph(cql)
    for i = 1:cql, j = i+1:cql
        if intersect(cliques[i], cliques[j]) != []
            add_edge!(cliqueG, i, j)
            weight = fweight(cliques[i], cliques[j], d=d)
            set_prop!(cliqueG, i, j, :weight, weight)
        end
    end
    merge = true
    while merge == true
        mweight = 0
        medge = 0
        for edge in edges(cliqueG)
            weight = get_prop(cliqueG, edge, :weight)
            if weight > mweight
                mweight = weight
                medge = edge
            end
        end
        if mweight > 0
            cliques,cliqueG = mergeclique!(cliques, cliqueG, src(medge), dst(medge), d=d)
        else
            break
        end
    end
    cliquesize = length.(cliques)
    cql = length(cliquesize)
    if QUIET == false
        uc = sort(unique(cliquesize), rev=true)
        sizes = [sum(cliquesize.== i) for i in uc]
        println("-------------------------------------------")
        println("The size of blocks after merging:\n$uc\n$sizes")
        println("-------------------------------------------")
    end
    return cliques,cql,cliquesize
end


# von Mises distribution

immutable VonMises <: ContinuousUnivariateDistribution
    μ::Float64  # mean
    κ::Float64  # concentration

    function VonMises(μ::Real, κ::Real)
        κ > zero(κ) || error("kappa must be positive")
        new(float64(μ), float64(κ))
    end

    VonMises(κ::Real) = VonMises(0.0, float64(κ))
    VonMises() = VonMises(0.0, 1.0)
end

## Properties
circmean(d::VonMises) = d.μ
circmedian(d::VonMises) = d.μ
circmode(d::VonMises) = d.μ
circvar(d::VonMises) = 1.0 - besselix(1, d.κ) / besselix(0, d.κ)

function entropy(d::VonMises)
	I0κ = besselix(0.0, d.κ)
	log(twoπ * I0κ) - d.κ * (besselix(1, d.κ) / I0κ - 1.0)
end

## Functions
pdf(d::VonMises, x::Real) = exp(d.κ * (cos(x - d.μ) - 1.0)) / (twoπ * besselix(0, d.κ))
logpdf(d::VonMises, x::Real) = d.κ * (cos(x - d.μ) - 1.0) - log2π - log(besselix(0, d.κ))
cf(d::VonMises, t::Real) = besselix(abs(t), d.k) / besselix(0.0, d.κ) * exp(im * t * d.μ)
cdf(d::VonMises, x::Real) = cdf(d, x, d.μ - π)

function cdf(d::VonMises, x::Real, from::Real)
	const tol = 1.0e-20
	x = mod(x - from, twoπ)
	μ = mod(d.μ - from, twoπ)
	if μ == 0.0
		return vmcdfseries(d.κ, x, tol)
	elseif x <= μ
		upper = mod(x - μ, twoπ)
		if upper == 0.0
			upper = twoπ
		end
		return vmcdfseries(d.κ, upper, tol) - vmcdfseries(d.κ, mod(-μ, twoπ), tol)
	else
		return vmcdfseries(d.κ, x - μ, tol) - vmcdfseries(d.κ, μ, tol)
	end
end

function vmcdfseries(κ::Real, x::Real, tol::Real)
	j, s = 1, 0.0
	while true
		sj = besselix(j, κ) * sin(j * x) / j
		s += sj
		j += 1
		abs(sj) >= tol || break
	end
	x / twoπ + s / (π * besselix(0, κ))
end

## Sampling
sampler(d::VonMises) = VonMisesSampler(d.μ, d.κ)


# ---
# title: Histograms
# cover: assets/histograms.png
# description: Computing 1- and 2-dimensional histograms
# author: "[Pietro Vertechi](https://github.com/piever)"
# ---

using AlgebraOfGraphics, CairoMakie
using AlgebraOfGraphics: density
set_aog_theme!() #src

df = (x=rand(0:99, 1000),)
plt = data(df) * mapping(:x) * histogram(bins=20)
fg = draw(plt)

#

df = (x=randn(1000), c=rand(["a", "b"], 1000))
plt = data(df) * mapping(:x, color=:c, stack=:c) * histogram(bins=20)
fg = draw(plt)

#

df = (x=rand(1000), y=randn(1000))
plt = data(df) * mapping(:x, :y) * histogram(bins=20)
draw(plt)

# save cover image #src
mkpath("assets") #src
save("assets/histograms.png", fg) #src


using LinearAlgebra
function fit(x, t; alpha = 0.1)
    w = nothing
    if size(x)[1] != size(t)[1]#Processing when the matrix is organized by dependent variable
        x = x'
        t = t'
    end
    x2 = ones(size(x)[1], 1)
    x = hcat(x2, x)
    i = Matrix{Float64}(I, size(x)[2], size(x)[2])
    w = inv(x' * x + alpha * i) * x' * t
    return w
end
function predict(x, w)
    x2 = ones(1, size(x)[2])
    x = vcat(x2, x)
    return w' * x
end


using DynamicalSystemsToolkit
using ModelingToolkit
using NonlinearSolve

function hopf()
    @parameters t β σ
    @variables y₁(t) y₂(t)
    D = Differential(t)
    return ODESystem(
        [
            D(y₁) ~ β * y₁ - y₂ + σ * y₁ * (y₁^2 + y₂^2),
            D(y₂) ~ y₁ + β * y₂ + σ * y₂ * (y₁^2 + y₂^2),
        ],
        t,
        [y₁, y₂],
        [β, σ];
        defaults=[β => 1, σ => -1],
        name=:hopf,
    )
end

function hopf_test(; n_mesh=5, n_coll=4)
    coll = CollocationSystem(hopf(), n_mesh, n_coll)
    add_boundarycondition!(coll, (u₀, t₀, u₁, t₁, p) -> u₀ .- u₁)  # periodicity
    add_boundarycondition!(coll, (u₀, t₀, u₁, t₁, p) -> [0 ~ t₀, 0 ~ u₀[2]])  # start time and phase condition
    sol = t -> (cos(t), sin(t))
    ics = generate_initialconditions(coll, sol, (0, 2π))
    prob = NonlinearProblem(NonlinearSystem(coll), ics, [], jac=true)
    return (coll, prob)
end

function test2()
    @variables x y
    sys = NonlinearSystem([x ~ y], [x, y], []; defaults=[y => 1])
    return NonlinearProblem(sys, [x => 0])
end


# Test dissipator definition
@test diss(q('x'))(q('z')) ≈ q('x') * q('z') * q('x')' - (1/2)*(q('x')'*q('x')*q('z') + q('z')*q('x')'*q('x'))
@test diss(o('x'))(o('n')) ≈ o('x') * o('n') * o('x')' - (1/2)*(o('x')'*o('x')*o('n') + o('n')*o('x')'*o('x'))
@test diss(o('y'))(o('n')) ≈ o('y') * o('n') * o('y')' - (1/2)*(o('y')'*o('y')*o('n') + o('n')*o('y')'*o('y'))
@test unsuperket(sdiss(o('y')) * superket(o('n'))) ≈ o('y') * o('n') * o('y')' - (1/2)*(o('y')'*o('y')*o('n') + o('n')*o('y')'*o('y'))

# Test innovation definition
@test inn(q('x'))(q('z')) == q('x') * q('z') +  q('z') * q('x')' - tr((q('x') + q('x')')*q('z'))*q('z')
@test inn(o('x'))(o('n')) == o('x') * o('n') +  o('n') * o('x')' - tr((o('x') + o('x')')*o('n'))*o('n')
@test inn(o('y'))(o('n')) == o('y') * o('n') +  o('n') * o('y')' - tr((o('y') + o('y')')*o('n'))*o('n')


@testset " Maxwell 1D FEM solver " begin
    using GEMPIC
    using LinearAlgebra

    """
    Test 1D Maxwell spline finite element solver on a periodic grid

    `L_x` domain dimensions and M is an integer.

    ```math
    B_z(x,y,t) = \\cos(\\frac{2 M \\pi}{L_x} x)  \\cos(\\frac{2 M \\pi}{L_x} t)
    ```
    ```math
    E_y(x,y,t) = \\sin(\\frac{2 M \\pi}{L_x} x)  \\sin(\\frac{2 M \\pi}{L_x} t)
    ```

    """

    mode = 2
    eta1_min = 0.0
    eta1_max = 2π
    nc_eta1 = 256

    Lx = eta1_max - eta1_min

    delta_eta1 = Lx / nc_eta1

    mesh = OneDGrid(eta1_min, eta1_max, nc_eta1)

    deg = 3

    maxwell_1d = Maxwell1DFEM(mesh, deg)

    ex = zeros(Float64, nc_eta1)
    ey = zeros(Float64, nc_eta1)
    bz = zeros(Float64, nc_eta1)

    bz_exact = zeros(Float64, nc_eta1)
    ex_exact = zeros(Float64, nc_eta1)
    ey_exact = zeros(Float64, nc_eta1)
    rho = zeros(Float64, nc_eta1)
    sval = zeros(Float64, nc_eta1)

    cos_k(x) = cos(mode * 2 * pi * x / Lx)
    sin_k(x) = sin(mode * 2 * pi * x / Lx)

    # Test Poisson
    #-------------
    # Set exact solution
    for i in 1:nc_eta1
        xi = eta1_min + (i - 1) * delta_eta1
        ex_exact[i] = sin_k(xi) / (2.0 * mode * pi / Lx)
    end

    compute_rhs_from_function!(rho, maxwell_1d, cos_k, deg)

    compute_e_from_rho!(ex, maxwell_1d, rho)

    # Evaluate spline curve at grid points and compute error
    # Ex is a 1-form, i.e. one spline degree lower
    sval = eval_uniform_periodic_spline_curve(deg - 1, ex)
    err_ex = maximum(abs.(sval .- ex_exact))
    println(" error Poisson  $err_ex ")
    @test err_ex ≈ 0.0 atol = 1e-6

    # Test Ampere
    #-------------
    # Set time step
    dt = 0.5 * delta_eta1
    # Set exact solution
    for i in 1:nc_eta1
        xi = eta1_min + (i - 1) * delta_eta1
        ex_exact[i] = -cos_k(xi) * dt
    end

    compute_rhs_from_function!(rho, maxwell_1d, cos_k, deg - 1)
    fill!(ex, 0.0)
    compute_e_from_j!(ex, maxwell_1d, dt .* rho, 1)

    # Evaluate spline curve at grid points and compute error
    # Ex is a 1-form, i.e. one spline degree lower
    sval = eval_uniform_periodic_spline_curve(deg - 1, ex)
    err_ex2 = maximum(abs.(sval .- ex_exact))
    #println( " error Ampere  $err_ex2 ")
    @test err_ex2 ≈ 0.0 atol = 1e-6

    l2norm = l2norm_squared(maxwell_1d, ex, deg - 1)
    err_l2norm = l2norm - dt^2 * pi
    #println( " error l2 norm $err_l2norm ")

    # Test Maxwell on By and Ez
    #--------------------------
    # Set time stepping parameters

    time = 0.0
    dt = 0.5 * delta_eta1
    nstep = 10

    # Compute initial fields
    ex = 0.0 # 0-form -> splines of degree deg
    l2projection!(bz, maxwell_1d, cos_k, deg - 1) # 0-form -> splines of degree deg-1

    for istep in 1:nstep
        compute_b_from_e!(bz, maxwell_1d, 0.5 * dt, ey)
        compute_e_from_b!(ey, maxwell_1d, dt, bz)
        compute_b_from_e!(bz, maxwell_1d, 0.5 * dt, ey)

        time = time + dt

        for i in 1:nc_eta1
            xi = eta1_min + (i - 1) * delta_eta1
            ey_exact[i] = sin(mode * 2π * xi / Lx) * sin(mode * 2π * time / Lx)
            bz_exact[i] = cos(mode * 2π * xi / Lx) * cos(mode * 2π * time / Lx)
        end

        sval = eval_uniform_periodic_spline_curve(deg, ey)
        err_ey = norm(sval .- ey_exact)

        sval = eval_uniform_periodic_spline_curve(deg - 1, bz)
        err_bz = norm(sval .- bz_exact)

        @test err_ey ≈ 0.0 atol = 1e-2
        @test err_bz ≈ 0.0 atol = 1e-2
    end
end


function init_V_gauss_G(strf, Ω, Ns, G2, A, alpha)
    Npoints = size(strf)[1]
    Nspecies = size(strf)[2]
    Vg = zeros(ComplexF64,Npoints)
    V = zeros(Float64,Npoints)
    for isp = 1:Nspecies
        pf = -A[isp]*(pi/alpha[isp])^1.5/Ω
        @printf("Gaussian parameters:\n")
        @printf("A[%2d]     = %18.10f\n", isp, A[isp])
        @printf("alpha[%2d] = %18.10f\n", isp, alpha[isp])
        for ig = 1:Npoints
            Vg[ig] = pf*exp(-0.25*G2[ig]/alpha[isp])*strf[ig,isp]
        end
        V = V + real( G_to_R(Ns,Vg) )*Npoints
    end
    return V
end


using Omega
using Test
using Statistics: mean

function testreplace2()
  θ = normal(0.0, 1.0)
  x = normal(θ, 1.0)
  θnew = normal(100.0, 1.0)
  xnew = Omega.replace(x, θ => θnew)
  @test isapprox(mean(rand(xnew, 1000)), 100, atol = 1.0)
  θnewnew = normal(200.0, 1.0)
  xnewnew = replace(xnew, θnew => θnewnew)
  @test isapprox(mean(rand(xnewnew, 1000)), 200, atol = 1.0)
  y = xnewnew^2
  ynew = replace(y, xnewnew => 10.0)
  @test rand(ynew) == 10.0^2
end

testreplace2()

function replaceconst()
  μ = uniform(1.0, 2.0)
  σ = uniform(1.0, 2.0)
  x = normal(μ, σ)
  xnew = replace(x, σ => μ)
  rand(xnew)
  xnew = replace(x, σ => 3.432)
  rand(xnew)
  xnew = replace(x, μ => 3.432, σ => 21.2)
  rand(xnew)
end

replaceconst()