"""
````
function simplexgrid(coord::Array{Tc,2},
                     cellnodes::Array{Ti,2},
                     cellregions::Array{Ti,1},
                     bfacenodes::Array{Ti,2},
                     bfaceregions::Array{Ti,1}
                     ) where {Tc,Ti}
````

    Create simplex grid from five arrays.
"""
function simplexgrid(coord::Array{Tc,2},
                     cellnodes::Array{Ti,2},
                     cellregions::Array{Ti,1},
                     bfacenodes::Array{Ti,2},
                     bfaceregions::Array{Ti,1}
                     ) where {Tc,Ti}
    @assert size(coord,2)>0
    dim=size(coord,1)
    if dim==1
        eltype=Edge1D
        btype=Vertex0D
        csys=Cartesian1D
    elseif dim==2
        eltype=Triangle2D
        btype=Edge1D
        csys=Cartesian2D
    elseif dim==3
        eltype=Tetrahedron3D
        btype=Triangle2D
        csys=Cartesian3D
    end

    grid=ExtendableGrid{Tc,Ti}()
    grid[Coordinates]=coord
    grid[CellNodes]=cellnodes
    grid[CellRegions]=cellregions
    grid[CellGeometries]=VectorOfConstants(eltype,length(cellregions))
    grid[BFaceNodes]=bfacenodes
    grid[BFaceRegions]=bfaceregions
    grid[BFaceGeometries]=VectorOfConstants(btype,length(bfaceregions))
    grid[CoordinateSystem]=csys
    return grid
end

"""
$(TYPEDSIGNATURES)

(Try to) create a subdivision of interval (a,b) stored in the
returned array X such that
  - `X[1]==a, X[end]==b`
  - `(X[2]-X[1])<=ha+tol*(b-a)`
  - `(X[end]-X[end-1])<=hb+tol*(b-a)`
  - There is a number q such that  `X[i+1]-X[i] == q*(X[i]-X[i-1])`
  - X is the array with the minimal possible number of points with the above property

Caveat: the algorithm behind this is  well tested but unproven.

Returns an Array containing the points of the subdivision.
"""
function geomspace(a, b, ha, hb ; tol=1.0e-10)

    function _geomspace0(l,h0, hl, tol=1.0e-10)
        @assert (l>0.0)
        @assert (h0>0.0)
        @assert (hl>=h0)
        @assert((hl+h0)<l)

        #  We need to  adjust two things:

        # The sum of the geometric progression must
        # match the length of the interval, so lmismatch
        # should be zero:
        function lmismatch(q,k)
            return l - h0*(1-q^k)/(1-q)
        end

        # The claim from experimenral evidence (Wolfram)
        # is that, if written as a polynomial,
        # it has two real zeros: one and the value searched for which
        # is slightly larger than one. All other zeros are on one circle.

        # The size of the last interval should be close to
        # to hl, so hmismatch should be close to one and not larger than one
        function  hmismatch(q,k)
            return  h0*q^(k-1)/hl
        end

        # define initial number of intervals from
        # average of minmal and maximal h
        n=Int(ceil((2.0*l/(h0+hl))))

        if n==1
            n=2
        end

        # define initial q such that hmismatch is one.
        q=(hl/h0)^(1.0/(n-1.0))

        # Iteration until both mismatches are satisfactory
        # Outer loop runs until hmismatch is less than 1
        hmiss=10.0 # some initial value >1 just to run the loop at least once
        if abs(q-1.0)<tol
            hmiss=1.0
        end

        while  hmiss>1.0
            # increase number of intervals until
            # lmismatch becomes less than zero
            while  lmismatch(q,n)>0.0
                n+=1
            end

            # find initial interval for q containing
            # value with zero lmismatch
            ns=0
            nsmax=100

            while lmismatch(q,n)<0 &&  ns<nsmax
                q*=0.99
                ns+=1
            end

            xl=q
            xr=q/0.99
            @assert ns<nsmax

            # bisection to define q with zero lmismatch
            ns=0
            xm=0.5*(xl+xr)
            while !(xr≈xl) && ns<nsmax
                ns+=1
                mmm=lmismatch(xm,n)
                if mmm≈0.0
                    break
                elseif   lmismatch(xl,n)*mmm<0
                    xr=xm
                else
                    xl=xm
                end
                xm=0.5*(xl+xr)
            end
            q=xm
            @assert ns<nsmax
            hmiss=hmismatch(q,n)
            if hmiss>1.0+tol
                n=n+1
            end
        end
        #  printf("%d %g %g %g\n",n,q,lmismatch(q,n),hmismatch(q,n))

        X = Array{Float64,1}(undef,n+1)
        X[1]=0
        h=h0
        for i=1:n
            X[i+1]=X[i]+h
            h*=q
        end
        X[n+1]=l
        return X
    end

    # Map things to [0,b-a]
    @assert (ha>0.0)
    @assert (hb>0.0)
    @assert (a<b)

    tol=tol*(b-a)
    if ha<=hb
        X=_geomspace0(b-a,ha,hb,tol)
        X.+=a
    else
        X=-reverse(_geomspace0(b-a,hb,ha,tol))
        X.+=b
    end

    @assert (X[2]-X[1])<=ha+tol
    @assert (X[end]-X[end-1])<=hb+tol

    return X
end

collect_or_assign(X)=X
collect_or_assign(X::AbstractRange)=collect(X)
is_monotone(X)=all(X[2:end]-X[1:end-1].>0)

"""
$(TYPEDSIGNATURES)

Glue together two vectors a and b resulting in a vector c. They last element
of a shall be equal (up to tol) to the first element of b.
The result fulfills `length(c)=length(a)+length(b)-1`
"""
function glue(_a::AbstractVector, _b::AbstractVector; tol=1.0e-10)
    a=collect_or_assign(_a)
    b=collect_or_assign(_b)

    is_monotone(a) || error("a not monotone")
    is_monotone(b) || error("b not monotone")
    Tv=promote_type(eltype(a),eltype(b))
    na=length(a)
    nb=length(b)

    d=b[1]-a[na-1]
    @assert(d>0)
    d=b[1]-a[na]
    @assert(d>-tol)
    @assert(d<tol)

    c=Vector{Tv}(undef,na+nb-1)
    ic=0
    for ia=1:na
        ic+=1
        c[ic]=a[ia]
    end
    for ib=2:nb
        ic+=1
        c[ic]=b[ib]
    end
    return c
end

##########################################################
abstract type XCoordinates <: AbstractGridFloatArray1D end
abstract type YCoordinates <: AbstractGridFloatArray1D end
abstract type ZCoordinates <: AbstractGridFloatArray1D end

"""
$(TYPEDSIGNATURES)

Constructor for 1D grid.

Construct 1D grid from an array of node cordinates.
It creates two boundary regions with index 1 at the left end and
index 2 at the right end.

Primal grid holding unknowns: marked by `o`, dual
grid marking control volumes: marked by `|`.

```@raw html
 o-----o-----o-----o-----o-----o-----o-----o-----o
 |--|-----|-----|-----|-----|-----|-----|-----|--|
```
"""
function simplexgrid(_X::AbstractVector)
    X=collect_or_assign(_X)
    #    is_monotone(X) || error("X not monotone")
    coord=reshape(X,1,length(X))
    cellnodes=zeros(Int32,2,length(X)-1)
    cellregions=zeros(Int32,length(X)-1)
    for i=1:length(X)-1
        cellnodes[1,i]=i
        cellnodes[2,i]=i+1
        cellregions[i]=1
    end
    bfacenodes=Array{Int32}(undef,1,2)
    bfaceregions=zeros(Int32,2)
    bfacenodes[1,1]=1
    bfacenodes[1,2]=length(X)
    bfaceregions[1]=1
    bfaceregions[2]=2
    grid=simplexgrid(coord,
                     cellnodes,
                     cellregions,
                     bfacenodes,
                     bfaceregions)
    grid[XCoordinates]=X
    grid
end

##########################################################
"""
$(TYPEDSIGNATURES)

Constructor for 2D grid
from coordinate arrays.
Boundary region numbers count counterclockwise:

| location  |  number |
| --------- | ------- |
| south     |       1 |
| east      |       2 |
| north     |       3 |
| west      |       4 |

"""
function  simplexgrid(_X::AbstractVector, _Y::AbstractVector)
    X=collect_or_assign(_X)
    Y=collect_or_assign(_Y)
    is_monotone(X) || error("X not monotone")
    is_monotone(Y) || error("Y not monotone")

    Tc=promote_type(eltype(X),eltype(Y))

    function leq(x, x1, x2)
        if (x>x1)
            return false
        end
        if (x>x2)
            return false
        end
        return true
    end

    function geq(x, x1, x2)
        if (x<x1)
            return false
        end
        if (x<x2)
            return false
        end
        return true
    end

    nx=length(X)
    ny=length(Y)

    hmin=X[2]-X[1]
    for i=1:nx-1
        h=X[i+1]-X[i]
        if h <hmin
            hmin=h
        end
    end
    for i=1:ny-1
        h=Y[i+1]-Y[i]
        if h <hmin
            hmin=h
        end
    end

    @assert(hmin>0.0)
    eps=1.0e-5*hmin

    x1=X[1]+eps
    xn=X[nx]-eps
    y1=Y[1]+eps
    yn=Y[ny]-eps

    function  check_insert_bface(ibface,coord,n1,n2)
        if (geq(x1,coord[1,n1],coord[1,n2]))
            ibface=ibface+1
            bfacenodes[1,ibface]=n1
            bfacenodes[2,ibface]=n2
	    bfaceregions[ibface]=4
        elseif (leq(xn,coord[1,n1],coord[1,n2]))
            ibface=ibface+1
            bfacenodes[1,ibface]=n1
            bfacenodes[2,ibface]=n2
	    bfaceregions[ibface]=2
        elseif (geq(y1,coord[2,n1],coord[2,n2]))
            ibface=ibface+1
            bfacenodes[1,ibface]=n1
            bfacenodes[2,ibface]=n2
	    bfaceregions[ibface]=1
        elseif (leq(yn,coord[2,n1],coord[2,n2]))
            ibface=ibface+1
            bfacenodes[1,ibface]=n1
            bfacenodes[2,ibface]=n2
	    bfaceregions[ibface]=3
        end
        ibface
    end

    num_nodes=nx*ny
    num_cells=2*(nx-1)*(ny-1)
    num_bfacenodes=2*(nx-1)+2*(ny-1)

    coord=zeros(Tc,2,num_nodes)
    cellnodes=zeros(Int32,3,num_cells)
    cellregions=zeros(Int32,num_cells)
    bfacenodes=Array{Int32}(undef,2,num_bfacenodes)
    bfaceregions=zeros(Int32,num_bfacenodes)

    ipoint=0
    for iy=1:ny
        for ix=1:nx
            ipoint=ipoint+1
            coord[1,ipoint]=X[ix]
            coord[2,ipoint]=Y[iy]
        end
    end
    @assert(ipoint==num_nodes)

    icell=0
    for iy=1:ny-1
        for ix=1:nx-1
	    ip=ix+(iy-1)*nx
	    p00 = ip
	    p10 = ip+1
	    p01 = ip  +nx
	    p11 = ip+1+nx

            icell=icell+1
            cellnodes[1,icell]=p00
            cellnodes[2,icell]=p10
            cellnodes[3,icell]=p11
            cellregions[icell]=1

            icell=icell+1
            cellnodes[1,icell]=p11
            cellnodes[2,icell]=p01
            cellnodes[3,icell]=p00
            cellregions[icell]=1
        end
    end
    @assert(icell==num_cells)

    #lazy way to  create boundary grid

    ibface=0
    for icell=1:num_cells
        n1=cellnodes[1,icell]
	n2=cellnodes[2,icell]
	n3=cellnodes[3,icell]
        ibface=check_insert_bface(ibface,coord,n1,n2)
	ibface=check_insert_bface(ibface,coord,n1,n3)
	ibface=check_insert_bface(ibface,coord,n2,n3)
    end
    @assert(ibface==num_bfacenodes)

    grid=simplexgrid(coord,
                     cellnodes,
                     cellregions,
                     bfacenodes,
                     bfaceregions)

    grid[XCoordinates]=X
    grid[YCoordinates]=Y
    grid
end

##########################################################
"""
$(TYPEDSIGNATURES)

Constructor for 3D grid
from coordinate arrays.
Boundary region numbers:

| location  |  number |
| --------- | ------- |
| south     |       1 |
| east      |       2 |
| north     |       3 |
| west      |       4 |
| bottom    |       5 |
| top       |       6 |

"""
function  simplexgrid(_X::AbstractVector,_Y::AbstractVector,_Z::AbstractVector)
    X=collect_or_assign(_X)
    Y=collect_or_assign(_Y)
    Z=collect_or_assign(_Z)

    is_monotone(X) || error("X not monotone")
    is_monotone(Y) || error("Y not monotone")
    is_monotone(Z) || error("Z not monotone")

    Tc=promote_type(eltype(X),eltype(Y),eltype(Z))

    leq(x, x1, x2, x3)=x≤x1 && x≤x2 && x≤x3
    geq(x, x1, x2, x3)=x≥x1 && x≥x2 && x≥x3

    nx=length(X)
    ny=length(Y)
    nz=length(Z)

    hmin=X[2]-X[1]
    for i=1:nx-1
        h=X[i+1]-X[i]
        if h <hmin
            hmin=h
        end
    end
    for i=1:ny-1
        h=Y[i+1]-Y[i]
        if h <hmin
            hmin=h
        end
    end

    for i=1:nz-1
        h=Z[i+1]-Z[i]
        if h <hmin
            hmin=h
        end
    end

    @assert(hmin>0.0)
    eps=1.0e-5*hmin

    x1=X[1]+eps
    xn=X[end]-eps

    y1=Y[1]+eps
    yn=Y[end]-eps

    z1=Z[1]+eps
    zn=Z[end]-eps

    function  check_insert_bface(ibface,coord,bfacenodes,n1,n2,n3)
        if geq(x1,coord[1,n1],coord[1,n2],coord[1,n3])
            ibface=ibface+1
            bfacenodes[1,ibface]=n1
            bfacenodes[2,ibface]=n2
            bfacenodes[3,ibface]=n3
	    bfaceregions[ibface]=4
        elseif leq(xn,coord[1,n1],coord[1,n2],coord[1,n3])
            ibface=ibface+1
            bfacenodes[1,ibface]=n1
            bfacenodes[2,ibface]=n2
            bfacenodes[3,ibface]=n3
	    bfaceregions[ibface]=2
        elseif geq(y1,coord[2,n1],coord[2,n2],coord[2,n3])
            ibface=ibface+1
            bfacenodes[1,ibface]=n1
            bfacenodes[2,ibface]=n2
            bfacenodes[3,ibface]=n3
	    bfaceregions[ibface]=1
        elseif leq(yn,coord[2,n1],coord[2,n2],coord[2,n3])
            ibface=ibface+1
            bfacenodes[1,ibface]=n1
            bfacenodes[2,ibface]=n2
            bfacenodes[3,ibface]=n3
	    bfaceregions[ibface]=3
        elseif geq(z1,coord[3,n1],coord[3,n2],coord[3,n3])
            ibface=ibface+1
            bfacenodes[1,ibface]=n1
            bfacenodes[2,ibface]=n2
            bfacenodes[3,ibface]=n3
	    bfaceregions[ibface]=5
        elseif leq(zn,coord[3,n1],coord[3,n2],coord[3,n3])
            ibface=ibface+1
            bfacenodes[1,ibface]=n1
            bfacenodes[2,ibface]=n2
            bfacenodes[3,ibface]=n3
	    bfaceregions[ibface]=6
        end
        ibface
    end

    num_nodes=nx*ny*nz
    num_cells=6*(nx-1)*(ny-1)*(nz-1)
    num_bfacenodes=4*(nx-1)*(ny-1)+4*(nx-1)*(nz-1)+4*(ny-1)*(nz-1)

    Ti=Int32
    coord=zeros(Tc,3,num_nodes)
    cellnodes=zeros(Ti,4,num_cells)
    cellregions=zeros(Ti,num_cells)
    bfacenodes=zeros(Ti,3,num_bfacenodes)
    bfaceregions=zeros(Ti,num_bfacenodes)

    ipoint=0
    for iz=1:nz
        for iy=1:ny
            for ix=1:nx
                ipoint=ipoint+1
                coord[1,ipoint]=X[ix]
                coord[2,ipoint]=Y[iy]
                coord[3,ipoint]=Z[iz]
            end
        end
    end

    @assert(ipoint==num_nodes)

    icell=0
    nxy=nx*ny
    for iz=1:nz-1
        for iy=1:ny-1
            for ix=1:nx-1

	        ip=ix+(iy-1)*nx+(iz-1)*nxy;

	        p000 = ip ;
	        p100 = ip+1;
	        p010 = ip  +nx;
	        p110 = ip+1+nx;
	        p001 = ip	+nxy;
	        p101 = ip+1	+nxy;
	        p011 = ip  +nx+nxy;
	        p111 = ip+1+nx+nxy;

                icell=icell+1; cellregions[icell]=1; @. cellnodes[:,icell]=(p000,p100,p110,p111)
                icell=icell+1; cellregions[icell]=1; @. cellnodes[:,icell]=(p000,p100,p101,p111)
                icell=icell+1; cellregions[icell]=1; @. cellnodes[:,icell]=(p000,p010,p011,p111)
                icell=icell+1; cellregions[icell]=1; @. cellnodes[:,icell]=(p000,p010,p110,p111)
                icell=icell+1; cellregions[icell]=1; @. cellnodes[:,icell]=(p000,p001,p101,p111)
                icell=icell+1; cellregions[icell]=1; @. cellnodes[:,icell]=(p000,p001,p011,p111)
            end
        end
    end
    @assert(icell==num_cells)
    #lazy way to  create boundary grid
    # lazy but easy... bad for partitioning !!!

    ibface=0
    for icell=1:num_cells
        n1=cellnodes[1,icell]
	n2=cellnodes[2,icell]
	n3=cellnodes[3,icell]
	n4=cellnodes[4,icell]
        ibface=check_insert_bface(ibface,coord,bfacenodes,n1,n2,n3)
	ibface=check_insert_bface(ibface,coord,bfacenodes,n1,n2,n4)
	ibface=check_insert_bface(ibface,coord,bfacenodes,n1,n3,n4)
	ibface=check_insert_bface(ibface,coord,bfacenodes,n2,n3,n4)
    end
    @assert(ibface==num_bfacenodes)

    grid=simplexgrid(coord,
                     cellnodes,
                     cellregions,
                     bfacenodes,
                     bfaceregions)

    grid[XCoordinates]=X
    grid[YCoordinates]=Y
    grid[ZCoordinates]=Z
    grid
end

######################################################
"""
$(TYPEDSIGNATURES)

Read grid from file. Currently for pdelib sg format only
"""
function simplexgrid(file::String;format="")
    (fbase,fext)=splitext(file)
    if format==""
        format=fext[2:end]
    end
    @assert format=="sg"

    tks=TokenStream(file)
    expecttoken(tks,"SimplexGrid")
    version=parse(Float64,gettoken(tks))
    version20=false;

    if (version==2.0)
        version20=true;
    elseif (version==2.1)
        version20=false;
    else
        error("Read grid: wrong format version: $(version)")
    end

    dim::Int32=0
    coord=Array{Float64,2}(undef,0,0)
    cells=Array{Int32,2}(undef,0,0)
    regions=Array{Int32,1}(undef,0)
    faces=Array{Int32,2}(undef,0,0)
    bregions=Array{Int32,1}(undef,0)
    while(true)
        if (trytoken(tks,"DIMENSION"))
            dim=parse(Int32,gettoken(tks));
        elseif (trytoken(tks,"NODES"))
            nnodes=parse(Int32,gettoken(tks));
            embdim=parse(Int32,gettoken(tks));
            if(dim!=embdim)
                error("Dimension error (DIMENSION $(dim)) in section NODES")
            end
            coord=Array{Float64,2}(undef,dim,nnodes)
            for inode=1:nnodes
                for idim=1:embdim
                    coord[idim,inode]=parse(Float64,gettoken(tks))
                end
            end
        elseif (trytoken(tks,"CELLS"))
            ncells=parse(Int32,gettoken(tks));
            cells=Array{Int32,2}(undef,dim+1,ncells)
            regions=Array{Int32,1}(undef,ncells)
            for icell=1:ncells
                for inode=1:dim+1
                    cells[inode,icell]=parse(Int32,gettoken(tks));
                end
                regions[icell]=parse(Int32,gettoken(tks));
	        if version20
		    for j=1:dim+1
		        gettoken(tks);  # skip file format garbage
                    end
                end
            end
        elseif (trytoken(tks,"FACES"))
            nfaces=parse(Int32,gettoken(tks));
            faces=Array{Int32,2}(undef,dim,nfaces)
            bregions=Array{Int32,1}(undef,nfaces)
            for iface=1:nfaces
                for inode=1:dim
                    faces[inode,iface]=parse(Int32,gettoken(tks));
                end
                bregions[iface]=parse(Int32,gettoken(tks));
	        if (version20)
		    for j=1:dim+2
		        gettoken(tks); #skip file format garbage
                    end
                end
            end
        else
            expecttoken(tks,"END")
            break
        end
    end
    simplexgrid(coord,cells,regions,faces,bregions);
end

"""
$(TYPEDSIGNATURES)

Write grid to file. Currently for pdelib sg format only
"""
function Base.write(fname::String, g::ExtendableGrid; format="")
    (fbase,fext)=splitext(fname)
    if format==""
        format=fext[2:end]
    end
    @assert format=="sg"

    dim_g=dim_grid(g)
    dim_s=dim_space(g)
    nn=num_nodes(g)
    nc=num_cells(g)
    nbf=num_bfaces(g)
    coord=g[Coordinates]
    cellnodes=g[CellNodes]
    bfacenodes=g[BFaceNodes]
    cellregions=g[CellRegions]
    bfaceregions=g[BFaceRegions]

    # TODO: replace @sprintf by someting non-allocating
    open(fname, "w") do file
        write(file,@sprintf("SimplexGrid"))
        write(file,@sprintf(" "))
        write(file,@sprintf("2.1\n"))
        write(file,@sprintf("#created by ExtendableGrids.jl (c) J.Fuhrmann et al\n"))
        write(file,@sprintf("#mailto:{fuhrmann|streckenbach}@wias-berlin.de\n"))
        write(file,@sprintf("#%s\n",Dates.format(Dates.now(),"yyyy-mm-ddTHH-mm-SS")))

        write(file,@sprintf("DIMENSION\n%d\n",dim_g))
        write(file,@sprintf("NODES\n%d %d\n",nn,dim_s))

        for inode=1:nn
            for idim=1:dim_s
	        write(file,@sprintf("%.20e ",coord[idim,inode]))
                write(file,@sprintf("\n"))
            end
        end

        write(file,@sprintf("CELLS\n%d\n",nc))
        for icell=1:nc
            for inode=1:dim_g+1
	        write(file,@sprintf("%d ", cellnodes[inode,icell]))
            end
            write(file,@sprintf("%d\n",cellregions[icell]))
        end

        write(file,@sprintf("FACES\n%d\n",nbf))
        for ibface=1:nbf
            for inode=1:dim_g
	        write(file,@sprintf("%d ", bfacenodes[inode,ibface]))
            end
            write(file,@sprintf("%d\n",bfaceregions[ibface]))
        end
        write(file,@sprintf("END\n"))
        flush(file)
        flush(file)
    end
    nothing
end


using QuantileRegressions, CSV, DataFrames
@testset "npqreg" begin
	mycyle = CSV.read("../examples/mcycle.csv", DataFrame)
	x = mycyle.times
	y = mycyle.accel

	for t = range(0.01,0.99,length=99)
		res = QuantileRegressions.npqreg(Array(y), Array(x), t; h=2)
		@test all(!isnan, res[2])
		@test all(!isnan, res[3])
		xrange = sort(unique(x))
		res = QuantileRegressions.npqreg(Array(y), Array(x), t; xrange=xrange, h=0.5)
	end
	res = QuantileRegressions.npqreg([y[1]], [x[1]], 0.4; xrange=[x[1]], h=2)
	@test !isnan(res[2][1])
	@test isnan(res[3][1])
	res = QuantileRegressions.npqreg([y[1]], [x[1]], 0.8; xrange=[x[1]], h=0.0001)
	@test !isnan(res[2][1])
	@test isnan(res[3][1])
	res = QuantileRegressions.npqreg([y[1]], [x[1]], 0.8; xrange=[x[1].+1], h=0.1)
	@test isnan(res[2][1])
	@test isnan(res[3][1])

	@test all(QuantileRegressions.npqreg([1.0,2.0,4.0,5.0], [0.0,0.0,6.0,12.0], 0.3; h=0.001, xrange=[0.0,6.0,12.0])[2] .≈ [1.0, 4.0, 5.0])

	yrand = randn(100)
	# Integer xrange gives same results as float xrange
	@test all(QuantileRegressions.npqreg(yrand, collect(1:100), 0.4; xrange=1:1:10) .== QuantileRegressions.npqreg(yrand, collect(1:100), 0.4; xrange=1.0:1.0:10.0))

	# Integer x gives same results as float x
	@test all(QuantileRegressions.npqreg(yrand, collect(1:100), 0.4) .== QuantileRegressions.npqreg(yrand, collect(1.0:100.0), 0.4))

	# Combinations
	@test all(QuantileRegressions.npqreg(yrand, collect(1.0:100.0), 0.4; xrange=1:1:10) .== QuantileRegressions.npqreg(yrand, collect(1:100), 0.4; xrange=1.0:1.0:10.0))
	@test all(QuantileRegressions.npqreg(yrand, collect(1:100), 0.4; xrange=1.0:1.0:10.0) .== QuantileRegressions.npqreg(yrand, collect(1:100), 0.4; xrange=1.0:1.0:10.0))
end


###
# DDUS tests relying on QP funcitonality
####
# Merge these into main test suite when default solver in JuMPeR supports QPs

using DDUS
using FactCheck
using JuMPeR

include("test_helpers.jl")  #loads up generic functionality

facts("portTest2  UCS bounds") do
	srand(8675309); data = randn(500, 2)
	w = UCSOracle(data, .1, .1, .1)
	portTest(w, -2.4604223389522866, [0.530985977658841, 0.46901402234115896], TOL=1e-6,
			unc_lower=[-1e6, -1e6], unc_upper=[1e6, 1e6])
end


### A Pluto.jl notebook ###
# v0.12.21

using Markdown
using InteractiveUtils

# ╔═╡ 1d1c0a56-7946-11eb-051f-fb214769d3f0
using PyPlot, BandedMatrices, SparseArrays, LinearAlgebra; PyPlot.svg(true);

# ╔═╡ 20390c4e-7981-11eb-16e4-a5f422480175
md"""We're implementing the method of [Oliver 1968](https://link.springer.com/article/10.1007/BF02166688), a method for solving recurrence relations of the form
```math
P_n(i) y(i) + P_{n-1}(i) y(i+1) + \cdots + P_0(i) y(i+n) = R(i)
```
where the recurrence is unstable in both the forward and backwards solutions. Oliver's method transforms an unstable forward recurrence into a boundary value problem. This can be accomplished by solving a linear system,

```math
\mathbf{P} \vec{y} = \vec{\rho}.
```
We consider the general case in which we specify ``n`` initial values, such that the terms ``Y(a+1), Y(a+2), \ldots, Y(a+n-m)`` are specified, and on the other end, ``Y(b-m+1), \ldots, Y(b)`` are specified for some integer ``m \in [0,n]``, the number of boundary values specified on the end of the recurrence. In this case, the solution is ``\vec{y}``,

```math
\vec{y} = [ Y(a + n - m + 1), \cdots , Y(b - m) ]^T.
```

"""

# ╔═╡ ca5b9a8e-7981-11eb-2dec-dfb58c50ef7d
"""
Construct a boundary value problem from a recurrence relation. The functions for coefficients and the right side dispatch on this abstract type.

`N` and `M` correspond to the total number of specified initial boundary conditions, and the number specified on the end of the recurrence, respectively.
"""
abstract type OliverProblem{T,N,M} end

# ╔═╡ c82988f2-7986-11eb-184c-4dc4685efe09
struct Example61161{T,N,M} <: OliverProblem{T,N,M} end

# ╔═╡ 1a9b22ea-7989-11eb-1123-d923b0f891f1
md"""
Consider an example recurrence relation, which we will call `Example61161`
```math
6 y(s) - 11 y(s+1) + 6 y(s+2) - y(s+3) = 0.
```
with initial conditions ``y(1) = 2``, ``y(2)=5``, and ``y(3) = 15``. This recurrence relation has an explicit solution
```math
y(s) = 1 - 2^{s-1} + 2 \cdot 3^{s-1}.
```
In this case, we actually have constant coefficients so ``P_0(i) = -1``, ``P_1(i) = 6``, ``P_2(i) = -11``, and ``P_3(i) = 6``. Further, ``R(i) = 0``.
"""

# ╔═╡ c4023f50-799d-11eb-18a4-f98202a8f420
y(s) = 1 - 2^(s-1) + 2 * 3^(s-1)

# ╔═╡ c00af4c4-7988-11eb-1dee-4f042a18d53b
"""
	OliverP(OP::OliverProblem, s, i)

Computes ``P_s(i)``, the coefficient ``s`` at solution index ``i``.
"""
function OliverP(OP::Example61161, s, i)
	if s == 0
		return -1
	elseif s == 1
		return 6
	elseif s == 2
		return -11
	elseif s == 3
		return 6
	end
end

# ╔═╡ 67cda80a-798b-11eb-020d-7de13497098c
"""
	OliverR(OP::OliverProblem, i)

Computes ``R(i)``, the right side of the recurrence relation.
"""
OliverR(OP::Example61161, i) = 0

# ╔═╡ c87eb55c-798b-11eb-05db-ab39057c8d20
"""
	assembleP{T}(OP::OliverProblem{M,N}, a, b) where {M, N}

Generic description of the ``\\mathbf{P}`` matrix within the linear system of Oliver's method.
"""
function assembleP(OP::OliverProblem{T,N,M}, a, b) where {T,N,M}
	Psize = b - N - a
	𝐏 = BandedMatrix{T}(undef, (Psize, Psize), (N-M,M))
	for row in 1:Psize
		for col in max(1, row-N+M):min(row + M, Psize)
			Δm = col - row
			𝐏[row, col] = OliverP(OP, M - Δm, a + row)
		end
	end
	return 𝐏
end

# ╔═╡ 20107d58-7981-11eb-1823-b3f867edeb85
assembleP(Example61161{Float64,3,1}(), 0, 10)

# ╔═╡ aaa88a3e-7946-11eb-3301-2720cf0f2301
"""
	assembleρ{T}(OP::OliverProblem{M,N}, a, b) where {M, N}

Generic description of the ``\\vec{\\rho}`` vector within the linear system of Oliver's method.
"""
function assembleρ(OP::OliverProblem{T,N,M}, a, b, YBC) where {T,N,M}
	Psize = b - N - a
	ρ = zeros(T, Psize)

	for row in 1:(N-M)
		Σ = zero(T)
		for s in 1:(N - M - row + 1)
			# every row, increase by 1 the sum UB, YBC arg, P arg
			Σ = Σ + OliverP(OP, N - s + 1, a + row) * YBC[a + s + row - 1]
		end
		ρ[row] = OliverR(OP, a + row) - Σ
	end

	rₛ, rₑ = (b - N - M + 1 - a), (b - N - a)
	for row in rₛ:rₑ
		row_from_end = rₑ - row + 1
		Σ = zero(T)
		for s in 0:(M - row_from_end)
			# every row, decrease by 1 the sum UB, YBC arg, P arg
			Σ = Σ + OliverP(OP, s, b - N - row_from_end + 1) *
				YBC[b - s - row_from_end + 1]
		end
		ρ[row] = OliverR(OP, a+row) - Σ
	end

	return ρ
end

# ╔═╡ f29b3902-7946-11eb-3fcc-0be7bcbf2c6c
md"""Recall the initial conditions ``y(1) = 2``, ``y(2)=5``, and ``y(3) = 15``."""

# ╔═╡ 22514dac-799a-11eb-2940-db93f451d9df
begin
	a, b, n, m = 1, 9, 3, 3
	# YBC = Dict([1,2,3] .=> [2,5,15])
	YBC = Dict( (a+1):(b+1) .=> y.((a+1):(b+1)))
	ex = Example61161{Float64,n,m}()
	P = assembleP(ex, a, b)
	ρ = assembleρ(ex, a, b, YBC)

	# """$(P * y.((a+n-m+1):(b-m))) $(ρ)"""
	sol = P \ ρ
	ref = y.((a+n-m+1):(b-m))
	"""$((sol .- ref) ./ ref)"""
end

# ╔═╡ 3e4fe168-7a07-11eb-0a47-0b31536f8773

# ╔═╡ Cell order:
# ╠═1d1c0a56-7946-11eb-051f-fb214769d3f0
# ╟─20390c4e-7981-11eb-16e4-a5f422480175
# ╠═ca5b9a8e-7981-11eb-2dec-dfb58c50ef7d
# ╠═c82988f2-7986-11eb-184c-4dc4685efe09
# ╟─1a9b22ea-7989-11eb-1123-d923b0f891f1
# ╠═c4023f50-799d-11eb-18a4-f98202a8f420
# ╠═c00af4c4-7988-11eb-1dee-4f042a18d53b
# ╠═67cda80a-798b-11eb-020d-7de13497098c
# ╠═c87eb55c-798b-11eb-05db-ab39057c8d20
# ╠═20107d58-7981-11eb-1823-b3f867edeb85
# ╠═aaa88a3e-7946-11eb-3301-2720cf0f2301
# ╟─f29b3902-7946-11eb-3fcc-0be7bcbf2c6c
# ╠═22514dac-799a-11eb-2940-db93f451d9df
# ╠═3e4fe168-7a07-11eb-0a47-0b31536f8773


using HigherOrderClustering
using Base.Test
using Combinatorics

function test1()
    I = [1, 1, 1, 2, 2, 3]
    J = [2, 3, 4, 3, 4, 4]
    A = convert(SparseMatrixCSC{Int64,Int64},
                sparse(I, J, ones(length(I)), 4, 4))
    A = max.(A, A')

    @test kcliques(A, 2) == [3, 3, 3, 3]
    @test kcliques(A, 3) == [3, 3, 3, 3]
    @test kcliques(A, 4) == [1, 1, 1, 1]
    @test kcliques(A, 5) == [0, 0, 0, 0]

    ccf2 = higher_order_ccfs(A, 2)
    @test ccf2.global_hoccf == 1.0
    @test ccf2.avg_hoccf    == 1.0
    @test ccf2.avg_hoccf2   == 1.0

    ccf3 = higher_order_ccfs(A, 3)
    @test ccf3.global_hoccf == 1.0
    @test ccf3.avg_hoccf    == 1.0
    @test ccf3.avg_hoccf2   == 1.0

    push!(I, 3, 4, 4)
    push!(J, 7, 7, 4)
    A = convert(SparseMatrixCSC{Int64,Int64},
                sparse(I, J, ones(length(I)), 7, 7))
    A = max.(A, A')

    @test sum(kcliques(A, 2)) == 8 * 2
    @test sum(kcliques(A, 3)) == 5 * 3
    @test sum(kcliques(A, 4)) == 1 * 4
    @test sum(kcliques(A, 5)) == 0 * 5

    @test kcliques(A, 4) == [1, 1, 1, 1, 0, 0, 0]
end

function test2()
    # Test k-cliques on a small network
    A = load_example_data("celegans.txt")
    degs = vec(sum(A, 2))
    n = size(A, 2)
    deg_order = zeros(Int64, n)
    deg_order[sortperm(vec(sum(A, 1)))] = collect(1:n)

    function higher_neighbors(j::Int64)
        oj = deg_order[j]
        nbrs = find(A[:, j])
        return filter(x -> deg_order[x] > oj, nbrs)
    end

    counts = zeros(Int64, size(A, 2))
    for j = 1:n
        for w in higher_neighbors(j)
            counts[[j, w]] += 1
        end
    end
    counts == kcliques(A, 2)

    counts = zeros(Int64, size(A, 2))
    for j = 1:n
        for (w, x) in combinations(higher_neighbors(j), 2)
            if A[w, x] > 0
                counts[[j, w, x]] += 1
            end
        end
    end
    @test counts == kcliques(A, 3)

    counts = zeros(Int64, size(A, 2))
    for j = 1:n
        for (w, x, y) in combinations(higher_neighbors(j), 3)
            if A[w, x] > 0 && A[w, y] > 0 && A[x, y] > 0
                counts[[j, w, x, y]] += 1
            end
        end
    end
    @test counts == kcliques(A, 4)

    counts = zeros(Int64, size(A, 2))
    for j = 1:n
        for (w, x, y, z) in combinations(higher_neighbors(j), 4)
            if A[w, x] > 0 && A[w, y] > 0 && A[x, y] > 0 &&
                A[w, z] > 0 && A[x, z] > 0 && A[y, z] > 0
                counts[[j, w, x, y, z]] += 1
            end
        end
    end
    @test counts == kcliques(A, 5)
end

test1()
test2()


#####################################################
#nonlinearities

function relu!(inp, outp)
	for i in 1:length(inp)
		outp[i] = max(0, inp[i])
	end
end

function relu!(inp, outp, t)
	for i in 1:length(inp)
		outp[i] = max(0, inp[i] - t[i])
	end
end

function relu_diff!(ax, error)
	for i in 1:length(ax)
		error[i] *= ax[i] > 0
	end
end

function lin!(inp, outp)
	for i in 1:length(inp)
		outp[i] = inp[i]
	end
end

function lin_diff!(ax, error)
end

function sigm!(inp, outp)
	for i in 1:length(inp)
		outp[i] = 1. / (1. + exp(-inp[i]))
	end
end

function sigm_diff!(ax, error)
	for i in 1:length(ax)
		error[i] *= 1. / (1 + exp(-ax[i]))*(1. - 1. / (1 + exp(-ax[i])))
	end
end

function pwl!(inp, outp, t)
	for i in 1:length(inp)
		outp[i] = clamp(inp[i] - t[i],0.,1.) #piece-wise linear
	end
end

function sigm!(inp, outp, t; λ = 10.) #10
	for i in 1:length(inp)
		outp[i] = 1. / (1. + exp(- λ * (inp[i] - t[i])))
	end
end


struct LinearScale{T}
    fmin::T
    fmax::T
    tmin::T
    tmax::T
    clamp::Bool
end

"""
    scale_linear(fmin, fmax, tmin, tmax; clamp=true)

Creating a mapping which takes values from `fmin` to `fmax`
and outputs values ranging from `tmin` to `tmax`.
If the input is outside the range it will be by default clamped to the `fmin` - `fmax`.
This can be prevented by setting `clamp=false`.

# Example
```julia
scale = scale_linear(0, 10, 0, 100)
scale(5) # returns 50

scale_point = scale_linear(O, Point(10, 10), O, Point(100, 100))
scale_point(Point(7,8)) # returns Point(70, 80)
```
"""
function scale_linear(fmin, fmax, tmin, tmax; clamp = true)
    return LinearScale(fmin, fmax, tmin, tmax, clamp)
end

function (ls::LinearScale)(x)
    if ls.clamp
        # clamp needs the values in low high order
        x = clamp(x, min(ls.fmin, ls.fmax), max(ls.fmin, ls.fmax))
    end
    return (x - ls.fmin) / (ls.fmax - ls.fmin) * (ls.tmax - ls.tmin) + ls.tmin
end

function (ls::LinearScale{T})(p::Point) where {T<:Point}
    px = p.x
    py = p.y
    if ls.clamp
        # clamp needs the values in low high order
        px = clamp(px, min(ls.fmin.x, ls.fmax.x), max(ls.fmin.x, ls.fmax.x))
        py = clamp(py, min(ls.fmin.y, ls.fmax.y), max(ls.fmin.y, ls.fmax.y))
    end
    nx = (px - ls.fmin.x) / (ls.fmax.x - ls.fmin.x) * (ls.tmax.x - ls.tmin.x) + ls.tmin.x
    ny = (py - ls.fmin.y) / (ls.fmax.y - ls.fmin.y) * (ls.tmax.y - ls.tmin.y) + ls.tmin.y
    return Point(nx, ny)
end

macro scale_layer(scale_mapping, body)
    return esc(
        quote
            @layer begin
                # compute the center of both rectangles
                fcenterx = ($scale_mapping.fmax.x + $scale_mapping.fmin.x) / 2
                fcentery = ($scale_mapping.fmax.y + $scale_mapping.fmin.y) / 2

                tcenterx = ($scale_mapping.tmax.x + $scale_mapping.tmin.x) / 2
                tcentery = ($scale_mapping.tmax.y + $scale_mapping.tmin.y) / 2

                # scale in x and y
                sx =
                    ($scale_mapping.tmax.x - $scale_mapping.tmin.x) /
                    ($scale_mapping.fmax.x - $scale_mapping.fmin.x)
                sy =
                    ($scale_mapping.tmax.y - $scale_mapping.tmin.y) /
                    ($scale_mapping.fmax.y - $scale_mapping.fmin.y)
                Luxor.scale(sx, sy)

                # translate such that inputting the center from the "from mapping" is at 0,0
                Luxor.translate(-fcenterx, -fcentery)

                # shift center of canvas to center of new region
                Luxor.translate(tcenterx / sx, tcentery / sy)
                $body
            end
        end,
    )
end


#Cylinder impedance BC
function Z(t , θ::Float64 , p::Param)

    Z0 = p.Z0
    ϵ  = p.ϵ
    Ω  = p.Ω

    return Z0*(1 + ϵ*cos(θ - Ω*t)^2)
end

function dZ(t,θ,p::Param)

    Z0 = p.Z0
    ϵ  = p.ϵ
    Ω  = p.Ω

    return 2 * Z0 * Ω * ϵ * cos(θ - Ω * t) * sin(θ - Ω * t)
end


# Copyright 2017, Iain Dunning, Joey Huchette, Miles Lubin, and contributors    #src
# This Source Code Form is subject to the terms of the Mozilla Public License   #src
# v.2.0. If a copy of the MPL was not distributed with this file, You can       #src
# obtain one at https://mozilla.org/MPL/2.0/.                                   #src

# # The urban planning problem

# An "urban planning" problem based on an [example from puzzlor](http://www.puzzlor.com/2013-08_UrbanPlanning.html).

using JuMP
import GLPK
import Test

function example_urban_plan()
    model = Model(GLPK.Optimizer)
    ## x is indexed by row and column
    @variable(model, 0 <= x[1:5, 1:5] <= 1, Int)
    ## y is indexed by R or C, the points, and an index in 1:5. Note how JuMP
    ## allows indexing on arbitrary sets.
    rowcol = ["R", "C"]
    points = [5, 4, 3, -3, -4, -5]
    @variable(model, 0 <= y[rowcol, points, 1:5] <= 1, Int)
    ## Objective - combine the positive and negative parts
    @objective(model, Max, sum(
          3 * (y["R", 3, i] + y["C", 3, i])
        + 1 * (y["R", 4, i] + y["C", 4, i])
        + 1 * (y["R", 5, i] + y["C", 5, i])
        - 3 * (y["R", -3, i] + y["C", -3, i])
        - 1 * (y["R", -4, i] + y["C", -4, i])
        - 1 * (y["R", -5, i] + y["C", -5, i])
        for i in 1:5)
    )
    ## Constrain the number of residential lots
    @constraint(model, sum(x) == 12)
    ## Add the constraints that link the auxiliary y variables to the x variables
    for i = 1:5
        @constraints(model, begin
            ## Rows
            y["R", 5, i] <= 1 / 5 * sum(x[i, :]) # sum = 5
            y["R", 4, i] <= 1 / 4 * sum(x[i, :]) # sum = 4
            y["R", 3, i] <= 1 / 3 * sum(x[i, :]) # sum = 3
            y["R", -3, i] >= 1 - 1 / 3 * sum(x[i, :]) # sum = 2
            y["R", -4, i] >= 1 - 1 / 2 * sum(x[i, :]) # sum = 1
            y["R", -5, i] >= 1 - 1 / 1 * sum(x[i, :]) # sum = 0
            ## Columns
            y["C", 5, i] <= 1 / 5 * sum(x[:, i]) # sum = 5
            y["C", 4, i] <= 1 / 4 * sum(x[:, i]) # sum = 4
            y["C", 3, i] <= 1 / 3 * sum(x[:, i]) # sum = 3
            y["C", -3, i] >= 1 - 1 / 3 * sum(x[:, i]) # sum = 2
            y["C", -4, i] >= 1 - 1 / 2 * sum(x[:, i]) # sum = 1
            y["C", -5, i] >= 1 - 1 / 1 * sum(x[:, i]) # sum = 0
        end)
    end
    ## Solve it
    optimize!(model)
    Test.@test termination_status(model) == MOI.OPTIMAL
    Test.@test primal_status(model) == MOI.FEASIBLE_POINT
    Test.@test objective_value(model) ≈ 14.0
    return
end

example_urban_plan()


using LinearAlgebra
using Random
using NNlib: softplus
using Roots # for inverse

################################################################################
#                            Planar and Radial Flows                           #
#             Ref: Variational Inference with Normalizing Flows,               #
#               D. Rezende, S. Mohamed(2015) arXiv:1505.05770                  #
################################################################################

###############
# RadialLayer #
###############

mutable struct RadialLayer{T1 <: Real, T2 <: AbstractVector{<:Real}} <: Bijector{1}
    α_::T1
    β::T1
    z_0::T2
end

function RadialLayer(dims::Int, container=Array)
    α_ = randn()
    β = randn()
    z_0 = container(randn(dims))
    return RadialLayer(α_, β, z_0)
end

h(α, r) = 1 ./ (α .+ r)     # for radial flow from eq(14)
#dh(α, r) = .- (1 ./ (α .+ r)) .^ 2   # for radial flow; derivative of h()

# An internal version of transform that returns intermediate variables
function _transform(flow::RadialLayer, z::AbstractVecOrMat)
    return _radial_transform(flow.α_, flow.β, flow.z_0, z)
end
function _radial_transform(α_, β, z_0, z)
    α = softplus(α_)            # from A.2
    β_hat = -α + softplus(β)    # from A.2
    if z isa AbstractVector
        r = norm(z .- z_0)
    else
        r = vec(sqrt.(sum(abs2, z .- z_0; dims = 1)))
    end
    transformed = z .+ β_hat ./ (α .+ r') .* (z .- z_0)   # from eq(14)
    return (transformed = transformed, α = α, β_hat = β_hat, r = r)
end

(b::RadialLayer)(z::AbstractMatrix{<:Real}) = _transform(b, z).transformed
(b::RadialLayer)(z::AbstractVector{<:Real}) = vec(_transform(b, z).transformed)

function forward(flow::RadialLayer, z::AbstractVecOrMat)
    transformed, α, β_hat, r = _transform(flow, z)
    # Compute log_det_jacobian
    d = size(flow.z_0, 1)
    h_ = h(α, r)
    if transformed isa AbstractVector
        T = eltype(transformed)
    else
        T = typeof(vec(transformed))
    end
    log_det_jacobian::T = @. (
        (d - 1) * log(1 + β_hat * h_)
        + log(1 +  β_hat * h_ + β_hat * (- h_ ^ 2) * r)
    )   # from eq(14)
    return (rv = transformed, logabsdetjac = log_det_jacobian)
end

function (ib::Inverse{<:RadialLayer})(y::AbstractVector{<:Real})
    flow = ib.orig
    T = promote_type(eltype(flow.α_), eltype(flow.β), eltype(flow.z_0), eltype(y))
    TV = vectorof(T)
    α = softplus(flow.α_)            # from A.2
    β_hat = - α + softplus(flow.β)   # from A.2
    # Define the objective functional
    f(y) = r -> norm(y .- flow.z_0) - r * (1 + β_hat / (α + r))   # from eq(26)
    # Run solver
    rs::T = find_zero(f(y), zero(T), Order16())
    return (flow.z_0 .+ (y .- flow.z_0) ./ (1 .+ β_hat ./ (α .+ rs)))::TV
end
function (ib::Inverse{<:RadialLayer})(y::AbstractMatrix{<:Real})
    flow = ib.orig
    T = promote_type(eltype(flow.α_), eltype(flow.β), eltype(flow.z_0), eltype(y))
    TM = matrixof(T)
    α = softplus(flow.α_)            # from A.2
    β_hat = - α + softplus(flow.β)   # from A.2
    # Define the objective functional
    f(y) = r -> norm(y .- flow.z_0) - r * (1 + β_hat / (α + r))   # from eq(26)
    # Run solver
    rs = mapvcat(eachcol(y)) do c
        find_zero(f(c), zero(T), Order16())
    end
    return (flow.z_0 .+ (y .- flow.z_0) ./ (1 .+ β_hat ./ (α .+ rs')))::TM
end

logabsdetjac(flow::RadialLayer, x::AbstractVecOrMat) = forward(flow, x).logabsdetjac
isclosedform(b::Inverse{<:RadialLayer}) = false


# Lax-Wendroff Scheme

struct LaxWendroffScheme <: AbstractFVAlgorithm end

function update_flux_value(uold,node_idx,dt,dx,mesh,Flux,alg::LaxWendroffScheme)
    # Local speeds of propagation
    @inbounds ul=cellval_at_left(node_idx,uold,mesh)
    @inbounds ur=cellval_at_right(node_idx,uold,mesh)
    aa = fluxρ(0.5*(ul+ur),Flux)
    # Numerical Fluxes
    return 0.5*(Flux(ul)+Flux(ur))-aa*dt/(2*dx)*(Flux(ur)-Flux(ul))
end
# Ritchmeyer Two-step Lax-Wendroff Method
# Reference:
# R. Leveque. Finite Volume Methods for Hyperbolic Problems.Cambridge University
# Press. New York 2002

struct LaxWendroff2sScheme <: AbstractFVAlgorithm end

function update_flux_value(uold,node_idx,dt,dx,mesh,Flux,alg::LaxWendroff2sScheme)
    # Local speeds of propagation
    @inbounds ul=cellval_at_left(node_idx,uold,mesh)
    @inbounds ur=cellval_at_right(node_idx,uold,mesh)
    # Numerical Fluxes
    return Flux(0.5*(ul+ur)-dx/(2*dt)*(Flux(ur)-Flux(ul)))
end


function XC_c_vwn_spin( Rhoe, zeta )

    third = 1.0/3.0
    pi34 = 0.6203504908994  # pi34=(3/4pi)^(1/3)
    rs = pi34/Rhoe^third

    A      = ( 0.0310907, 0.01554535, -0.01688686394039 )
    x0     = ( -0.10498, -0.32500, -0.0047584 )
    b      = ( 3.72744, 7.06042, 1.13107 )
    c      = ( 12.9352, 18.0578, 13.0045 )
    Q      = ( 6.15199081975908, 4.73092690956011, 7.12310891781812 )
    tbQ    = ( 1.21178334272806, 2.98479352354082, 0.31757762321188 )
    fx0    = ( 12.5549141492, 15.8687885, 12.99914055888256 )
    bx0fx0 = ( -0.03116760867894, -0.14460061018521, -0.00041403379428 )

    # N.B.: A is expressed in Hartree
    # Q = sqrt(4*c - b^2)
    # tbQ = 2*b/Q
    # fx0 = X(x_0) = x_0^2 + b*x_0 + c
    # bx0fx0 = b*x_0/X(x_0)

    cfz = 2.0^(4.0/3.0) - 2.0
    cfz1 = 1.0 / cfz
    cfz2 = 4.0/3.0 * cfz1
    iddfz0 = 9.0 / 8.0 *cfz
    sqrtrs = sqrt(rs)
    zeta3 = zeta^3
    zeta4 = zeta3*zeta
    trup = 1.0 + zeta
    trdw = 1.0 - zeta
    trup13 = trup^(1.0/3.0)
    trdw13 = trdw^(1.0/3.0)
    fz = cfz1 * (trup13*trup + trdw13*trdw - 2.0)         # f(zeta)
    dfz = cfz2 * (trup13 - trdw13)     # d f / d zeta

    ecP, vcP = padefit( sqrtrs, 1, x0, Q, b, c, A, tbQ, bx0fx0 )    # ecF = e_c Paramagnetic
    ecF, vcF = padefit( sqrtrs, 2, x0, Q, b, c, A, tbQ, bx0fx0 )    # ecP = e_c Ferromagnetic
    ac, dac  = padefit( sqrtrs, 3, x0, Q, b, c, A, tbQ, bx0fx0 )    # ac = "spin stiffness"

    ac = ac * iddfz0
    dac = dac * iddfz0
    De = ecF - ecP - ac # e_c[F] - e_c[P] - alpha_c/(ddf/ddz(z=0))
    fzz4 = fz * zeta4
    ec = ecP + ac * fz  + De * fzz4

    dec1 = vcP + dac*fz + (vcF - vcP - dac) * fzz4       # e_c - (r_s/3)*(de_c/dr_s)
    dec2 = ac*dfz + De*(4.0*zeta3*fz + zeta4*dfz)      # de_c/dzeta

    # v_c[s] = e_c - (r_s/3)*(de_c/dr_s) + [sign(s)-zeta]*(de_c/dzeta)
    vcup = dec1 + (1.0 - zeta)*dec2
    vcdw = dec1 - (1.0 + zeta)*dec2

    return ec, vcup, vcdw
end

function padefit(x, i, x0, Q, b, c, A, tbQ, bx0fx0)
    # implements formula [4.4] in:
    # S.H. Vosko, L. Wilk, and M. Nusair, Can. J. Phys. 58, 1200 (1980)

    # Pade fit calculated in x and its derivative w.r.t. rho
    # rs = inv((rho*)^(1/3)) = x^2
    # fit  [eq. 4.4]
    # dfit/drho = fit - (rs/3)*dfit/drs = ec - (x/6)*dfit/dx

    sqx = x * x                          # x^2 = r_s
    xx0 = x - x0[i]                      # x - x_0
    Qtxb = Q[i] / (2.0*x + b[i])      # Q / (2x+b)
    atg = atan(Qtxb)                     # tan^-1(Q/(2x+b))
    fx = sqx + b[i]*x + c[i]             # X(x) = x^2 + b*x + c

    fit = A[i] * (  log(sqx/fx) + tbQ[i]*atg -
            bx0fx0[i] * ( log(xx0*xx0/fx) + (tbQ[i] + 4.0*x0[i]/Q[i]) * atg )  )

    txb = 2.0*x + b[i]
    txbfx = txb / fx
    itxbQ = 1.0 / (txb*txb + Q[i]*Q[i])

    dfit = fit - A[i] / 3.0 + A[i]*x/6.0 * (  txbfx + 4.0*b[i]*itxbQ +
              bx0fx0[i] * ( 2.0/xx0 - txbfx - 4.0*( b[i] + 2.0*x0[i])*itxbQ )  )

    return fit, dfit
end


using Polynomials: Poly, polyder, polyint

struct LegendreBasis{T, N, fT, pT, dT, iT} <: PolynomialBasis{T,N}
    factors::fT
    polys::pT
    derivs::dT
    ints::iT
end

function LegendreBasis(T,N)

    factors = Tuple{}()
    polys   = Tuple{}()
    derivs  = Tuple{}()
    ints    = Tuple{}()

    for p in 0:N-1
        tfac, tpol = legendre_polynomial(p)
        tder = polyder(tpol)
        tint = polyint(tpol)

        factors = (factors..., tfac)
        polys   = (polys..., tpol)
        derivs  = (derivs..., tder)
        ints    = (ints..., tint)
    end

    LegendreBasis{T, N, typeof(factors), typeof(polys), typeof(derivs), typeof(ints)}(factors, polys, derivs, ints)
end

nodes(b::LegendreBasis{T,N})  where {T,N} = Vector{T}([])

Base.hash(b::LegendreBasis{T,N}, h::UInt) where {T,N} = hash(T, hash(N, h))

Base.:(==)(b1::LegendreBasis{T1,N1}, b2::LegendreBasis{T2,N2}) where {T1,N1,T2,N2} = (T1 == T2 && N1 == N2)

Base.isequal(b1::LegendreBasis{T1,N1}, b2::LegendreBasis{T2,N2}) where {T1,N1,T2,N2} = (T1 == T2 && N1 == N2)

function legendre_polynomial(p, T=Float64)
    if p == 0
        (T(sqrt(1)), Poly([1]))
    elseif p == 1
        (T(sqrt(3)), Poly([-1, +2]))
    elseif p == 2
        (T(sqrt(5)), Poly([+1, -6, +6]))
    elseif p == 3
        (T(sqrt(7)), Poly([-1, +12, -30, +20]))
    elseif p == 4
        (T(sqrt(9)), Poly([+1, -20, +90, -140, +70]))
    elseif p == 5
        (T(sqrt(11)), Poly([-1, +30, -210, +560, -630, +252]))
    elseif p == 6
        (T(sqrt(13)), Poly([+1, -42, +420, -1680, +3150, -2772, +924]))
    elseif p == 7
        (T(sqrt(15)), Poly([-1, +56, -756, +4200, -11550, +16632, -12012, +3432]))
    elseif p == 8
        (T(sqrt(17)), Poly([+1, -72, +1260, -9240, +34650, -72072, +84084, -51480, +12870]))
    elseif p == 9
        (T(sqrt(19)), Poly([-1, +90, -1980, +18480, -90090, +252252, -420420, +411840, -218790, +48620]))
    elseif p == 10
        (T(sqrt(21)), Poly([+1, -110, +2970, -34320, +210210, -756756, +1681680, -2333760, +1969110, -923780, +184756]))
    else
        error("Legendre polynomials only implemented for 0 ≤ p ≤ 10.")
    end
end

function eval_basis(b::LegendreBasis{T,N}, j::Int, x::T) where {T,N}
    return b.factors[j] * b.polys[j](x)
end

function deriv_basis(b::LegendreBasis{T,N}, j::Int, x::T) where {T,N}
    return b.factors[j] * b.derivs[j](x)
end

function int_basis(b::LegendreBasis{T,N}, j::Int, x::T) where {T,N}
    return b.factors[j] * b.ints[j](x)
end


using LinearAlgebra, SparseArrays, ForwardDiff
using Convex, ECOS

# Dimensions
n = 2 # state
m = 1 # control
T = 2 # horizon
num_var = n * T + m * (T - 1)
num_con = n * T
num_dec = num_var + num_con
z0 = rand(num_dec)

function traj(z)
    x1 = view(z, 1:n)
    u1 = view(z, n .+ (1:m))
    x2 = view(z, n + m .+ (1:n))
    λ1 = view(z, 2 * n + m .+ (1:n))
    λ2 = view(z, 3 * n + m .+ (1:n))
    return x1, u1, x2, λ1, λ2
end

x1, u1, x2, λ1, λ2 = traj(z0)

# Dynamics
x_init = ones(n)

A = [1.0 1.0;
     0.0 1.0]

B = [0.1;
     0.0]

In = Diagonal(ones(n))

# Objective
Q = [1.0 0.0; 0.0 1.0]
R = Array([1.0e-1])

# Convex.jl
y = Variable(num_var) # decision variables
objective = 0.5 * quadform(y[1:n], Q) + 0.5 * quadform(y[n + m .+ (1:n)], Q) + 0.5 * R[1] * square(y[n .+ (1:m)][1])

problem = minimize(objective) # setup problem

# constraint setup
problem.constraints += A * y[1:n] + B * y[n .+ (1:m)][1] - y[n + m .+ (1:n)] == 0.0
problem.constraints += y[1:n] - x_init == 0.0

solve!(problem, ECOS.Optimizer) # solve

# Convex solution
x1_sol = y.value[1:n]
u1_sol = y.value[n .+ (1:m)]
x2_sol = y.value[n + m .+ (1:n)]
λ1_sol = -1.0 * problem.constraints[1].dual
λ2_sol = -1.0 * problem.constraints[2].dual

z_sol = vec([x1_sol; u1_sol; x2_sol; λ1_sol; λ2_sol])

# kkt system r + Hz = 0
function r(z)
    x1, u1, x2, λ1, λ2 = traj(z)
    [Q * x1 + A' * λ1 + λ2;
     (R * u1[1])[1] + B' * λ1;
     Q * x2 - λ1;
     A * x1 + B * u1[1] - x2;
     x1 - x_init]
end
r(z_sol)

H = [Q zeros(n, m) zeros(n, n) A' In;
     zeros(m, n) R zeros(m, n) B' zeros(m, n);
     zeros(n, n) zeros(n, m) Q -In zeros(n, n);
     A B -In zeros(n, n) zeros(n, n);
     In zeros(n, m) zeros(n, n) zeros(n, n) zeros(n, n)]

r(z0)
ForwardDiff.jacobian(r, z0) - H

# function con(z)
#     x1, u1, x2, λ1, λ2 = traj(z)
#
#     [A * x1 + B * u1[1] - x2
#      x1 - ones(2);]
# end
#
# C = [A B -In;
#      In zeros(n, m + n)]
#
# con(z0)
# ForwardDiff.jacobian(con, z0)[:, 1:num_var] - C

# initialize variables
x1 = rand(n) #copy(x_init) #
u1 = rand(m)
x2 = A * x1 + B * u1[1]
λ1 = rand(n) # copy(λ1_sol)
λ2 = rand(n) # copy(λ2_sol)

z = [x1; u1; x2; λ1; λ2]

# kkt solve
Δz = -1.0 * H \ r(z)
Δx1, Δu1, Δx2, Δλ1, Δλ2 = traj(Δz)

# iLQR solve
function compute_Δu1(Δx1)
    Quu = R[1] + B' * Q * B
    Qux = B' * Q * A
    Qu = (R * u1[1])[1] + B' * Q * x2
    -1.0 * Quu \ (Qux * Δx1 + Qu)
end

Δu1[1] - compute_Δu1(Δx1)


@testset "Defining and evaluating Gaussian SPNs" begin

    import SPNetworks: GaussianDistribution

    GSPN = SumProductNetwork([
        SumNode([2,3,4],[4/20,9/20,7/20]),# 1 Sum P(A, B) 0.2,0.5,0.3
        ProductNode([5,7]),               # 2 Prod P(A, B)=P(A)*P(B)
        ProductNode([5,8]),               # 3 Prod P(A,B)=P(A)*P(B)
        ProductNode([6,8]),               # 4 Prod P(A,B)=P(A)*P(B)
        GaussianDistribution(1, 2, 18),   # 5 Normal(A, mean=2, var=18)
        GaussianDistribution(1, 11, 8),   # 6 Normal(A, mean=11, var=8)
        GaussianDistribution(2, 3, 10),   # 7 Normal(B, mean=3, var=10)
        GaussianDistribution(2, -4, 7)    # 8 Normal(B, mean=-4, var=7)
    ])

    @testset "Evaluation" begin
        res = GSPN([11.0,-4.0])
        @test res ≈ 0.008137858167261642
        @test logpdf(GSPN,[11.0,-4.0]) ≈ -4.811228258006023
    end
    @testset "Sampling" begin
        #N = 500000
        N = 1000
        data = rand(GSPN,N)

        max = -Inf
        amax = 0
        for n=1:N
            v = logpdf(GSPN,view(data,n,:))  # GSPN(data[n,:])
            if v > max
                max = v
                amax = n
            end
        end
        a, b = 11.0, -4.0
        ref = logpdf(GSPN,[a,b]) #GSPN([a,b])
        # println("max ln S($(data[amax,:])) = $max ≈ ln S($a,$b) = $ref")
    end
end # END of test set


@testset "SGD training" begin

    Random.seed!(1)

    # x y   z

    # 0 0   0
    # 0 1   1
    # 1 0   1
    # 1 1   0

    # x y   p(x, y | z=0)
    # 0 0   0.5
    # 0 1   0
    # 1 0   0
    # 1 1   0.5

    # x y   p(x, y | z=1)
    # 0 0   0
    # 0 1   0.5
    # 1 0   0.5
    # 1 1   0

    @gen function teacher()
        x = @trace(bernoulli(0.5), :x)
        y = @trace(bernoulli(0.5), :y)
        z::Bool = xor(x, y)
        @trace(bernoulli(z ? 1.0 : 0.0), :z)
    end

    sigmoid(val) = 1.0 / (1.0 + exp(-val))

    @gen function student(z::Bool)
        @param theta1::Float64
        @param theta2::Float64
        @param theta3::Float64
        @param theta4::Float64
        @param theta5::Float64
        x = @trace(bernoulli(sigmoid(theta1)), :x)
        if z && x
            prob_y = sigmoid(theta2) # should be near 0
        elseif z && !x
            prob_y = sigmoid(theta3) # should be near 1
        elseif !z && x
            prob_y = sigmoid(theta4) # should be near 1
        elseif !z && !x
            prob_y = sigmoid(theta5) # should be near 0
        end
        @trace(bernoulli(prob_y), :y)
    end

    function data_generator()
        (choices, _, retval) = propose(teacher, ())
        local inputs
        local constraints
        inputs = (choices[:z],)
        constraints = choicemap()
        constraints[:x] = choices[:x]
        constraints[:y] = choices[:y]
        (inputs, constraints)
    end

    # theta1 = 0.0
    # theta2 -> inf (prob_y -> 1)
    # theta3 -> -inf (prob_y -> 0)

    init_param!(student, :theta1, 0.)
    init_param!(student, :theta2, 0.)
    init_param!(student, :theta3, 0.)
    init_param!(student, :theta4, 0.)
    init_param!(student, :theta5, 0.)

    # check gradients using finite differences on a simulated batch
    minibatch_size = 100
    inputs = Vector{Any}(undef, minibatch_size)
    constraints = Vector{Any}(undef, minibatch_size)
    for i=1:minibatch_size
        (inputs[i], constraints[i]) = data_generator()
        (student_trace, _) = generate(student, inputs[i], constraints[i])
        accumulate_param_gradients!(student_trace, nothing)
    end
    for name in [:theta1, :theta2, :theta3, :theta4, :theta5]
        actual = get_param_grad(student, name)
        dx = 1e-6
        value = get_param(student, name)

        # evaluate total log density at value + dx
        set_param!(student, name, value + dx)
        lpdf_pos = 0.
        for i=1:minibatch_size
            (incr, _) = assess(student, inputs[i], constraints[i])
            lpdf_pos += incr
        end

        # evaluate total log density at value - dx
        set_param!(student, name, value - dx)
        lpdf_neg = 0.
        for i=1:minibatch_size
            (incr, _) = assess(student, inputs[i], constraints[i])
            lpdf_neg += incr
        end

        expected = (lpdf_pos - lpdf_neg) / (2 * dx)
        @test isapprox(actual, expected, atol=1e-4)

        set_param!(student, name, value)
    end

    # use stochastic gradient descent
    update = ParamUpdate(GradientDescent(0.01, 1000000), student)
    train!(student, data_generator, update,
        num_epoch=2000, epoch_size=50, num_minibatch=1, minibatch_size=50,
        verbose=false)

    # p(x | z=0) = p(x | z=1) = 0.5
    @test isapprox(get_param(student, :theta1), 0., atol=0.2)

    # y | z, x = xor(x, z)
    @test get_param(student, :theta2) < -5
    @test get_param(student, :theta3) > 5
    @test get_param(student, :theta4) > 5
    @test get_param(student, :theta5) < -5
end


function getIncomeGrid()

# A function that returns:
# 1. an income grid
# 2. A Markovian transition matrix (Q) over income realisations
# 3. A vector of minimum incomes in each year
# 4. A vector of maximum incomes in each year

# ------------------------------------------------------------------------
#  Declare the global variables that will be necessary in this function
#global T Tretire sigma mu rho isUncertainty        % Structural economic paramters
#global hcIncome hcIncPDF                        % Income process (hardcoded)
#global normBnd numPointsY uncertaintyMethod     % Numerical methods

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

#----------------------------------------%
# Scenario where there is no uncertainty
#----------------------------------------%
if isUncertainty == 0
    y = exp(mu);                # income is set equal to the exp of the log mean
    minInc = y;
    maxInc = y;
    Q = [1];                    # The transition matrix Q is simply a constant 1
                                # with prob 1 each perio\d income is 1

#----------------------------------------%
# Scenario where there is uncertainty - we have hardcoded the discrete
# number of income draws and their pdf
#----------------------------------------%
elseif isUncertainty == 1

    if uncertaintyMethod == 0
        y = hcIncome;           # The hard-coded incomes

        # hcIncPDF contains a single PDF. But we need a Markov transition
        # matrix that gives probability of each income next period
        # conditional on income this period
        Q = repmat(hcIncPDF', numPointsY,1);

        minInc = minimum(y);
        maxInc = maximum(y);

#----------------------------------------%
# Scenario where there is uncertainty - income draws are log normally distributed
#----------------------------------------%
    elseif uncertaintyMethod == 1
        #First get the standard deviation of income (from sigma and rho)
        sig_inc = sigma/((1-rho^2)^0.5);

        (lNormDev, ly ) = getEquiprobNormalDeviates(mu, sig_inc, numPointsY ); # thanks to Alex

        #---------------------%
        #Get transition matrix Q(i, j). The prob of income j in t+1
        #conditional on income i in t
        #---------------------%
         Q = Array(Float64,numPointsY, numPointsY);             #initialise the transition matrix

        for i = 1:1:numPointsY
            for j = 1:1:numPointsY
                hiDraw = lNormDev[j+1] - (1-rho)*mu - rho * ly[i]; #highest innovation that will give us income j tomorrow
                loDraw = lNormDev[j]   - (1-rho)*mu - rho * ly[i]; #lowest  innovation that will give us income j tomorrow

                P_part1=cdf(Normal(), hiDraw/sigma );
                P_part2=cdf(Normal(), loDraw/sigma );

                Q[i,j] = P_part1 - P_part2;
            end #j

            #Each of the rows of Q should add up to 1. But
            #due to the truncation of the normal distribution they will
            #not. So we divide through by the sum of elements in the row
            Q[i, :] = Q[i, :] ./ sum(Q[i, :]);
        end #i

        y = exp(ly);                # Get y from log y
        minInc = exp(-normBnd * sig_inc); # Get the minimum income in each year
        maxInc = exp(normBnd * sig_inc);  # Get the maximum income in each year

        if (y[1] < 1e-4) || (y[numPointsY] > 1e5)
            println("Combination of sigma and rho give a very high income variance. Numerical instability possible")
        end

     end  # if uncertaintyMethod == 0
 end  # if isUncertainty == 0

#----------------------------------------%
# Now get a matrix, T * numPointsY that holds the grid for each income in
# each year. Do likewise with minimum and maximum income
#----------------------------------------%
Ygrid = repmat(y', T, 1);
minInc = repmat([minInc]', T, 1);
maxInc = repmat([maxInc]', T, 1);

#----------------------------------------%
# Replace these arays with zeros for all years after retirement
#----------------------------------------%
if Tretire == 0         # no work (retired at birth)
    Ygrid[:, :] = 0;
    minInc[:, :] = 0;
    maxInc[:, :] = 0;
elseif (Tretire > 0) && (Tretire <=T)  #retire at some age
     Ygrid[Tretire:T, :] = 0;
     minInc[Tretire:T, :] = 0;
     maxInc[Tretire:T, :] = 0;
end

println("Income Grid Done")

return Ygrid, Q, minInc, maxInc

end


using ModelingToolkit, LinearAlgebra, SparseArrays

# Define the constants for the PDE
const α₂ = 1.0
const α₃ = 1.0
const β₁ = 1.0
const β₂ = 1.0
const β₃ = 1.0
const r₁ = 1.0
const r₂ = 1.0
const _DD = 100.0
const γ₁ = 0.1
const γ₂ = 0.1
const γ₃ = 0.1
const N = 8
const X = reshape([i for i in 1:N for j in 1:N],N,N)
const Y = reshape([j for i in 1:N for j in 1:N],N,N)
const α₁ = 1.0.*(X.>=4*N/5)

const Mx = Tridiagonal([1.0 for i in 1:N-1],[-2.0 for i in 1:N],[1.0 for i in 1:N-1])
const My = copy(Mx)
Mx[2,1] = 2.0
Mx[end-1,end] = 2.0
My[1,2] = 2.0
My[end,end-1] = 2.0

# Define the initial condition as normal arrays
@variables du[1:N,1:N,1:3] u[1:N,1:N,1:3] MyA[1:N,1:N] AMx[1:N,1:N] DA[1:N,1:N]

# Define the discretized PDE as an ODE function
function f(du,u,p,t)
   A = @view  u[:,:,1]
   B = @view  u[:,:,2]
   C = @view  u[:,:,3]
  dA = @view du[:,:,1]
  dB = @view du[:,:,2]
  dC = @view du[:,:,3]
  mul!(MyA,My,A)
  mul!(AMx,A,Mx)
  @. DA = _DD*(MyA + AMx)
  @. dA = DA + α₁ - β₁*A - r₁*A*B + r₂*C
  @. dB = α₂ - β₂*B - r₁*A*B + r₂*C
  @. dC = α₃ - β₃*C + r₁*A*B - r₂*C
end

f(du,u,nothing,0.0)

multithreadedf = eval(ModelingToolkit.build_function(du,u,multithread=true)[2])
_du = rand(N,N,3)
_u = rand(N,N,3)
multithreadedf(_du,_u)

jac = sparse(ModelingToolkit.jacobian(vec(du),vec(u),simplify=false))
multithreadedjac = eval(ModelingToolkit.build_function(vec(jac),u,multithread=true)[2])

#_jac = similar(jac,Float64)
#multithreadedjac(_jac,_u)


#=
Copyright (c) 2018-2022 Chris Coey, Lea Kapelevich, and contributors

This Julia package Hypatia.jl is released under the MIT license; see LICENSE
file in the root directory or at https://github.com/chriscoey/Hypatia.jl

problem 1 (linear_dynamics = true)
eigenvalue problem related to Lyapunov stability example from sec 2.2.2, 6.3.2
"Linear Matrix Inequalities in System and Control Theory" by
S. Boyd, L. El Ghaoui, E. Feron, and V. Balakrishnan:
minimize    t
subject to  P in S_+
            [-A'*P - P*A - C'C, P*B;
            B'*P, tI] in S_+
for the system with linear dynamics x_dot = A*x

problem 2 (linear_dynamics = false)
Lyapunov stability example
see https://stanford.edu/class/ee363/sessions/s4notes.pdf:
minimize    t
subject to  P - I in S_+
            [-A'*P - P*A - alpha*P - t*gamma^2*I, -P;
            -P, tI] in S_+
originally a feasibility problem, a feasible P and t prove the existence of a
Lyapunov function for the system x_dot = A*x+g(x), norm(g(x)) <= gamma*norm(x)
=#

struct LyapunovStabilityJuMP{T <: Real} <: ExampleInstanceJuMP{T}
    num_rows::Int
    num_cols::Int
    linear_dynamics::Bool # solve problem 1 in the description, else problem 2
    use_matrixepipersquare::Bool # use matrixepipersquare cone, else PSD cone
end

function build(inst::LyapunovStabilityJuMP{T}) where {T <: Float64}
    (num_rows, num_cols) = (inst.num_rows, inst.num_cols)

    model = JuMP.Model()
    JuMP.@variable(model, t)
    JuMP.@objective(model, Min, t)

    if inst.linear_dynamics
        A = randn(num_rows, num_rows)
        A = -A * A'
        B = randn(num_rows, num_cols)
        C = randn(num_rows, num_rows)
        JuMP.@variable(model, P[1:num_rows, 1:num_rows], PSD)
        U = -A' * P - P * A - C' * C / 100
        W = P * B
    else
        @assert num_rows == num_cols
        # P = -A is a feasible solution, with alpha and gamma sufficiently small
        A = randn(num_rows, num_rows)
        A = -A * A' - I
        alpha = 0.01
        gamma = 0.01
        JuMP.@variable(model, P[1:num_rows, 1:num_rows], Symmetric)
        JuMP.@constraint(model, Symmetric(P - I) in JuMP.PSDCone())
        U = -A' * P - P * A - alpha * P - (t * gamma ^ 2) .*
            Matrix(I, num_rows, num_rows)
        W = -P
    end

    if inst.use_matrixepipersquare
        U_svec = Cones.smat_to_svec!(zeros(eltype(U),
            Cones.svec_length(num_rows)), U, sqrt(2))
        coneT = Hypatia.MatrixEpiPerSquareCone{T, T}
        JuMP.@constraint(model, vcat(U_svec, t / 2, vec(W)) in
            coneT(num_rows, num_cols))
    else
        JuMP.@constraint(model, Symmetric(
            [t .* Matrix(I, num_cols, num_cols) W'; W U]) in JuMP.PSDCone())
    end

    return model
end


@testset "Condition StraightLineHomotopy" begin
    @polyvar x
    f = [x^2-1]
    g = [x^2+1]

    z=[1.0]
    t=1.0

    H = StraightLineHomotopy(f,g)
    @test κ(H,z,t) ≈ 1/sqrt(2)
    @test κ_norm(H,z,t) ≈ 1.0
    @test μ_norm(H,z,t) ≈ 1.0
end

@testset "Condition GeodesicOnTheSphere" begin
    @polyvar x
    f = [x^2-1]
    g = [x^2+1]

    z=[1.0, 1.0]
    t=1.0

    H = GeodesicOnTheSphere(f,g)
    @test κ(H,z,t) ≈ 1/sqrt(2)
    @test κ_norm(H,z,t) ≈ 1.0
    @test μ_norm(H,z,t) ≈ 1.0
end


function GBlackScholes(CallPutFlag, S, X, T, r, b, v)

  # b = r, give the BS stock option model
  # b = r - q, gives Merton 73, stock option with dividend
  # b = 0, give black 76
  # b = 0 && r = 0, gives Asay for margined futures options
  # b = r- r_f, gives garman kohlhagen currency options model

  d1 = (log(S / X) + (b + v^2 / 2) * T) / (v * sqrt(T))
  d2 = d1 - v * sqrt(T)

  if CallPutFlag == "c"
    GBS = S * exp((b - r) * T) * CND(d1) - X * exp(-r * T) * CND(d2)
  elseif CallPutFlag == "p"
    GBS = X * exp(-r * T) * CND(-d2) - S * exp((b - r) * T) * CND(-d1)
  end

end

function KirkSpreadOption(CallPutFlag, S1, S2, X, T, r, b1, b2, v1, v2, cor)

        S = (S1 * exp((b1 - r) * T)) / (S2 * exp((b2 - r) * T) + X * exp((-r) * T))
        F = (S2 * exp((b2 - r) * T)) / (S2 * exp((b2 - r) * T) + X * exp((-r) * T))
        sig = sqrt(v1^2 + (v2 * F)^2 - 2 * cor * v1 * v2 * F)

        d1 = (log(S) + (sig^2 / 2) * T) / (sig * sqrt(T))
        d2 = d1 - sig * sqrt(T)

        if CallPutFlag == "c"

                return (S2 * exp((b2 - r) * T) + X * exp((-r) * T)) * (S * CND(d1) - CND(d2))

        elseif CallPutFlag == "p"

                return (S2 * exp((b2 - r) * T) + X * exp((-r) * T)) * (CND(-d2) - S * CND(-d1))

        else

                println(STDERR, "Falsche Eingabe! Abbruch\n")
                return 0.0 #eventuell nochmal checken ob das sinn macht

        end

end

function GeoAveRate(CallPutFlag, S, X, T, r, b, v)

        sigAdj = v / sqrt(3)
        bAdj = (b - v^2 / 6) / 2

        d1 = (log(S / X) + (bAdj + (sigAdj^2 / 2)) * T) / (sigAdj * sqrt(T))
        d2 = d1 - sigAdj * sqrt(T)

        if CallPutFlag == "c"

                return S * exp((bAdj - r) * T) * CND(d1) - X * exp((-r) * T) * CND(d2)

        elseif CallPutFlag == "p"

                return X * exp((-r) * T) * CND((-d2)) - S * exp((bAdj - r) * T) * CND((-d1))

        else

                println(STDERR, "Falsche Eingabe! Abbruch\n")
                return 0.0 #eventuell nochmal checken ob das sinn macht
        end

end


module PoissonFourier1D

using FFTW, LinearAlgebra

function generate_solver(ρ₀, Δx)
    PF = FFTW.plan_rfft(ρ₀)
    F = FFTW.rfft(ρ₀)
    ω = collect(2π*rfftfreq(length(ρ₀)))
    ω[1] = ω[2]
    ω⁻² = -1.0./(ω.^2)
    Pφ = FFTW.plan_irfft(F, length(ρ₀))
    φ = FFTW.irfft(F, length(ρ₀))
    function solve!(e::Vector{T}, ρ::Vector{T}) where {T}
        mul!(F, PF, ρ)
        F[1] = 0.0 + 0.0im
        for i = 2:length(F)
            F[i] = ω⁻²[i] * F[i]
        end
        mul!(φ, Pφ, F)
        e[1] = 0.5*(φ[2]-φ[end])/Δx
        for i = 2:length(φ)-1
            e[i] = 0.5*(φ[i+1] - φ[i-1])/Δx
        end
        e[end] = 0.5*(φ[1]-φ[end-1])/Δx
        return e
    end
    return solve!
end

end


using Plots

using simulator.utilities
using simulator.matrixcore
using simulator.fresnelltools

mirror1_r = 0.6
mirror2_r = 0.6
distance = 3e-4

mirror1 = [
    1 - mirror1_r mirror1_r;
    mirror1_r     1 - mirror1_r
]
mirror2 = [
    1 - mirror2_r mirror2_r;
    mirror2_r     1 - mirror2_r
]

gap(ν) = FresnellSlab(1, 2*π*ν/c_0, distance, 0)

transmittance(ν) = CascadeScattering([mirror1, gap(ν), mirror2])[1, 1]
golden(ν) = (1 - mirror1_r) * (1 - mirror2_r) * ℯ^(-im * distance * 2*π*ν / c_0) / (1 - mirror1_r * mirror2_r * ℯ^(-2im * distance * 2*π*ν / c_0))

xs = 1e9:1e8:2e12
transmission = map(ν -> abs(transmittance(ν))^2, xs)
transmission_g = map(ν -> abs(golden(ν))^2, xs)

reflection = 1 .- transmission
reflection_g = 1 .- transmission_g

plot(xs, transmission, label="transmission")
plot!(xs, reflection, label="reflection")
plot!(xs, transmission_g, label="transmission_g")
plot!(xs, reflection_g, label="reflection_g")


using LinearAlgebra

module AnaliseEstrutura
using LinearAlgebra

function mrig(l::Float64, E::Float64 = 1.0, I::Float64 = 1.0)::Matrix{Float64}
    k = zeros(Float64, 4, 4)

    # I = (0.20 * 0.50^3) / 12.
    # E = 2.1e6

    # k[1, 1] = 12.0
    # k[2, 1] = 6.0 * l
    # k[2, 2] = 4.0 * l^2
    # k[3, 1] = -k[1, 1]
    # k[3, 2] = -k[2, 1]
    # k[3, 3] = k[1, 1]
    # k[4, 1] = k[2, 1]
    # k[4, 2] = 2.0 * l^2
    # k[4, 3] = -k[2, 1]
    # k[4, 4] = k[2, 2]

    k[1, 1] = 3.0 * E * I / l^3
    # k[1,2] = 3. * E * I / l ^ 2
    # k[1,4] = - 3. * E * I / l ^ 3
    k[2, 1] = 3.0 * E * I / l^2
    k[2, 2] = 3.0 * E * I / l
    # k[2,4] = - 3. * E * I / l ^ 2
    k[4, 1] = -3.0 * E * I / l^3
    k[4, 2] = -3.0 * E * I / l^2
    k[4, 4] = 3.0 * E * I / l^3

    for i in 1:4
        for j in 1:4
            k[i, j] = k[j, i]
        end
    end

    return k
    # return k * ((E * I) / l^3)
end

function pequiv(p::Float64, l::Float64)
    f = zeros(Float64, 4)
    f[1] = 0.5
    f[2] = (p * l) / 12.
    f[3] = f[1]
    f[4] = -f[2]
    return f * (p * l)
end
end

# Coordenadas
c = (1, 2, 3, 4, 5, 6)

# Coordenadas livres
w = (2, 4, 6)
# Coordenadas restringidas
r = c[setdiff(1:end, w)]

e1 = (1, 2, 3, 4)
e2 = (3, 4, 5, 6)

# Matrizes de rigidez dos elementos

k1 = AnaliseEstrutura.mrig(4.35)
k2 = AnaliseEstrutura.mrig(3.0)

# Matriz de rigidez da estrutura
K = zeros(Float64, length(w) + length(r), length(w) + length(r))
K[1:4, 1:4] = k1
K[3:6, 3:6] += k2

println("[K] Matriz de Rigidez da Estrutura")
display(K)
println("\n")

# Deslocamentos da Estrutura
U = zeros(Float64, length(w) + length(r))

#  Matriz reduzida
Kr = K[setdiff(1:end, r), setdiff(1:end, r)]

# Forças da Estrutura
P = zeros(Float64, length(w) + length(r))
P[1:4] = AnaliseEstrutura.pequiv(-1605.0, 4.35)
P[3:6] += AnaliseEstrutura.pequiv(-1444.0, 3.0)

println("[P] Forças da Estrutura")
display(P)
println("\n")

# Forças da Matriz Reduzida
Pr = P[setdiff(1:end, r)]
println("[Pr] Forças da Matriz Reduzida")
display(Pr)
println("\n")

println("[Kr] Matriz de Rigidez da Estrutura Reduzida")
display(Kr)
println("\n")

# Deslocamentos da Estrutura Reduzida
Ur = pinv(Kr) * Pr
U[setdiff(1:end, r)] = Ur

println("[Ur] Deslocamentos da Estrutura Reduzida")
display(Ur)
println("\n")

println("====================")
println("[U] Deslocamentos da Estrutura")
display(U)
println("\n")

println("[k1] Forças do Elemento 1")
_k1 = k1[setdiff(1:end, r), setdiff(1:end, r)]
display(_k1 * U[setdiff(1:end, e2)])
println("\n")

println("[k2] Forças do Elemento 2")
_k2 = k2[setdiff(1:end, r), setdiff(1:end, r)]
display(_k2 * U[setdiff(1:end, e1)])
println("\n")

println("\n")

# def k(l, rotula_inicial, rotula_final, area, inercia, elasticidade):
#     matriz_de_rigidez_local_no_sistema_de_coordenadas_locais = []
#     for _ in range(6):
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais.append([0, 0, 0, 0, 0, 0])
#     matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[0][0] = area * elasticidade / l
#     matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[0][3] = - area * elasticidade / l
#     matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[3][0] = - area * elasticidade / l
#     matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[3][3] = area * elasticidade / l
#     if not rotula_inicial and not rotula_final:
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[1][1] = 12 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[1][2] = 6 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[1][4] = - 12 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[1][5] = 6 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[2][1] = 6 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[2][2] = 4 * elasticidade * inercia / l
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[2][4] = - 6 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[2][5] = 2 * elasticidade * inercia / l
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[4][1] = - 12 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[4][2] = - 6 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[4][4] = 12 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[4][5] = - 6 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[5][1] = 6 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[5][2] = 2 * elasticidade * inercia / l
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[5][4] = - 6 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[5][5] = 4 * elasticidade * inercia / l
#     if rotula_inicial and not rotula_final:
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[1][1] = 3 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[1][4] = - 3 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[1][5] = 3 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[4][1] = - 3 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[4][4] = 3 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[4][5] = - 3 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[5][1] = 3 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[5][4] = - 3 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[5][5] = 3 * elasticidade * inercia / l
#     if not rotula_inicial and rotula_final:
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[1][1] = 3 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[1][2] = 3 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[1][4] = - 3 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[2][1] = 3 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[2][2] = 3 * elasticidade * inercia / l
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[2][4] = - 3 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[4][1] = - 3 * elasticidade * inercia / l ** 3
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[4][2] = - 3 * elasticidade * inercia / l ** 2
#         matriz_de_rigidez_local_no_sistema_de_coordenadas_locais[4][4] = 3 * elasticidade * inercia / l ** 3
#     return matriz_de_rigidez_local_no_sistema_de_coordenadas_locais


vec_proj(vec_in::SVector{3, TD}, n̂::SVector{3,TD}) where {TD} = n̂ * dot(n̂, vec_in)
function vec_sub_vec_proj(v::SVector{3,T}, n̂::SVector{3,T}) where T
	t = -dot(v, n̂)
	return SVector{3,T}(muladd(t, n̂[1], v[1]),
						muladd(t, n̂[2], v[2]),
						muladd(t, n̂[3], v[3]))
end

function a_dot_one_pad_b(a::Union{SMatrix{1,4,T1,4},SVector{4,T1}}, b::SVector{3,T2}) where {T1,T2}
	d =    muladd(a[1], b[1], a[4])
	d =    muladd(a[2], b[2], d)
	return muladd(a[3], b[3], d)
end


using Cubature
using Statistics

# Hard coded for a Bickley jet but want to generalize to other profiles including
# -> Bump and Guassian jets
# -> Vortices of different profiles
# -> In Cartesian and Spherical profiles

# Velocity is specified to be a Bickley jet
# Free-Surface is computed using numerical integration

function profile(y, params);

    Δη = params.Uj * params.Lj * params.f₀ / params.g

    U_function(x)  =  params.Uj * sech.(x / params.Lj).^2

    U =  U_function(y)
    E = zeros((params.Ny+1))
    for i in 1:params.Ny
        result = pquadrature(U_function, y[end], y[i])
        E[i] = - params.f₀ / params.g * result[1]
    end
    E = E .- mean(E)

    return U,E
end


module DVeTest
using Base.Test
using Quantum

xb = basis(:X,[1:5])
d = DiracVector([1:5], xb)
@test d==d''
@test d[:]==d.coeffs
@test d'*d == 55
@test d*d' == DiracMatrix(kron(d.coeffs, d.coeffs'), d.basis, d.basis')
@test isdual(d,d')
@test filterstates(s->label(s)%2==0, d) == DiracVector([2,4], basis(xb[2], xb[4]))
@test filtercoeffs(c->c%2==0, d) == DiracVector([2,4], basis(xb[2], xb[4]))
@test map(x->x*2, d)==d+d
@test xb[1]'*d == 1
@test d'*xb[1] == 1
@test norm(normalize(d)) < 1+1e-8 && norm(normalize(d)) > 1-1e-8
@test reduce(+,[i*xb[i]' for i=1:length(xb)]) == d'
@test (1/sqrt(2)*xb[1]) + (1/sqrt(2)*xb[2]) == dvec([1/sqrt(2), 1/sqrt(2)], basis(xb[1], xb[2]))
@test ket(:X,1)+ket(:X,1)+ket(:X,1) = dvec([3], basis(xb[1]))
@test ket(:X,1)+(bra(:S,"1")*ket(:X, 1)) * ket(:X,1)==dvec([1+1*bra(:S,"1")*ket(:X,1)], basis(ket(:X,1)))

end


##############################################################################
##
## The following function follows the command ranktest (called by ivreg2)
## RANKTEST: Stata module to test the rank of a matrix using the Kleibergen-Paap rk statistic
## Authors: Frank Kleibergen, Mark E Schaffer
## IVREG2: Stata module for extended instrumental variables/2SLS and GMM estimation
## Authors: Christopher F Baum, Mark E Schaffer, Steven Stillman
## More precisely, it corresponds to the Stata command:  ranktest  (X) (Z), wald full
##############################################################################

function ranktest!(X::Matrix{Float64},
                    Z::Matrix{Float64},
                    Pi::Matrix{Float64},
                    vcov_method_data::AbstractVcovMethodData,
                    df_small::Int,
                    df_absorb::Int)

    K = size(X, 2)
    L = size(Z, 2)

    crossz = cholfact!(At_mul_B(Z, Z), :U)
    crossx = cholfact!(At_mul_B(X, X), :U)

    Fmatrix = crossz[:U]
    Gmatrix = crossx[:U]
    theta = Fmatrix * (Gmatrix' \ Pi')'

    svddecomposition = svdfact(theta, thin = false)
    u = svddecomposition.U
    vt = svddecomposition.Vt

    # compute lambda
    if K == 1
        a_qq = sqrtm(A_mul_Bt(u, u))
        b_qq = sqrtm(A_mul_Bt(vt, vt))
    else
        u_12 = u[1:(K-1),(K:L)]
        v_12 = vt[1:(K-1),K]
        u_22 = u[(K:L),(K:L)]
        v_22 = vt[K,K]
        a_qq = vcat(u_12, u_22) * (u_22 \ sqrtm(A_mul_Bt(u_22, u_22)))
        b_qq = sqrtm(A_mul_Bt(v_22, v_22)) * (v_22' \ vcat(v_12, v_22)')
    end
    kronv = kron(b_qq, a_qq')
    lambda = kronv * vec(theta)

    # compute vhat
    if typeof(vcov_method_data) == VcovSimpleData
        vhat= eye(L * K) / size(X, 1)
    else
        temp1 = convert(Matrix{eltype(Gmatrix)}, Gmatrix)
        temp2 = convert(Matrix{eltype(Fmatrix)}, Fmatrix)
        k = kron(temp1, temp2)'
        vcovmodel = VcovData(Z, k, X, size(Z, 1) - df_small - df_absorb)
        matrix_vcov2 = shat!(vcov_method_data, vcovmodel)
        vhat = k \ (k \ matrix_vcov2)'
    end

    # return statistics
    # why do I need to add Hermitian? (since 0.5)
    if VERSION > v"0.5.0-dev+961"
        vlab = cholfact!(Hermitian(A_mul_Bt(kronv * vhat, kronv)))
    else
        vlab = cholfact!(A_mul_Bt(kronv * vhat, kronv))
    end
    r_kp = lambda' * (vlab \ lambda)
    p_kp = ccdf(Chisq((L-K+1 )), r_kp[1])
    F_kp = r_kp[1] / size(Z, 2)
    return F_kp, p_kp
end


using ForwardDiff: derivative

"""
    DerivativeMean{T∇<:MeanFunction} <: MeanFunction

"""
struct DerivativeMean{T∇<:MeanFunction} <: MeanFunction
    m::T∇
end
(μ::DerivativeMean)(x::Real) = derivative(x->μ.m(x), x)

"""
    DerivativeKernel{Tk<:Kernel} <: Kernel

"""
struct DerivativeKernel{Tk<:Kernel} <: Kernel
    k::Tk
end
function (k::DerivativeKernel)(x::Real, x′::Real)
    return derivative(x′->derivative(x->k.k(x, x′), x), x′)
end
(k::DerivativeKernel)(x::Real) = k(x, x)

"""
    DerivativeLhsCross{Tk<:CrossKernel} <: CrossKernel

"""
struct DerivativeLhsCross{Tk<:CrossKernel} <: CrossKernel
    k::Tk
end
function (k::DerivativeLhsCross)(x::Real, x′::Real)
    return derivative(x->k.k(x, x′), x)
end

"""
    DerivativeRhsCross{Tk<:CrossKernel} <: CrossKernel

"""
struct DerivativeRhsCross{Tk<:CrossKernel} <: CrossKernel
    k::Tk
end
function (k::DerivativeRhsCross)(x::Real, x′::Real)
    return derivative(x′->k.k(x, x′), x′)
end

"""
    DerivativeCross{Tk<:CrossKernel} <: CrossKernel

"""
struct DerivativeCross{Tk<:CrossKernel} <: CrossKernel
    k::Tk
end
function (k::DerivativeCross)(x::Real, x′::Real)
    return derivative(x′->derivative(x->k.k(x, x′), x), x′)
end
(k::DerivativeCross)(x::Real) = k(x, x)


function qsat(water, P, T, cn::Constants)

    Tc = T - cn.Tm
    if (Tc > 0.0 || water)
        es = cn.e0 * exp(17.5043 * Tc / (241.3 + Tc))
    else
        es = cn.e0 * exp(22.4422 * Tc / (272.186 + Tc))
    end
    Qs = cn.eps * es / P

    return Qs

end


function construct( e::EssentialMatrix, 𝐅::AbstractArray,  𝐊₁::AbstractArray, 𝐊₂::AbstractArray)
    if (size(𝐊₁) != (3,3)) || (size(𝐊₂) != (3,3))
        throw(ArgumentError("Expect 3 x 3 calibration matrices."))
    end
    if (size(𝐅) != (3,3))
        throw(ArgumentError("Expect 3 x 3 fundamental matrix."))
    end
    # Equation 9.12 Chapter 9 from Hartley & Zisserman
    𝐄 = 𝐊₂'*𝐅*𝐊₁
    SMatrix{3,3,Float64,3*3}(𝐄)
end


## Example script for constant-strength source panel method
using LinearAlgebra
using Base.Iterators
using Seaborn
using AeroMDAO

## Airfoil
airfoil = Foil(naca4((0,0,1,2), 81; sharp_trailing_edge = true))
V, α    = 1., 0.
ρ       = 1.225
uniform = Uniform2D(V, α)
num_pans = 80
panels  = paneller(airfoil, num_pans);

## Constant-strength source panel
A = @time constant_source_matrix(panels)
b = @time constant_source_boundary_condition(panels, velocity(uniform))
σs = A \ b

##
sum(σs)

## Pressure coefficient
pts = collocation_point.(panels);
panel_vels = [ velocity(uniform) .+ sum(source_velocity.(σs, panels, Ref(panel_i))) for panel_i in panels ]

qts = @. dot(panel_vels, panel_tangent(panels))
cps = @. 1 - qts^2 / uniform.mag^2

##
upper, lower = get_surface_values(panels, cps)
lower = [ upper[end]; lower; upper[1] ]

plot(first.(upper), last.(upper), label = "Upper")
plot(first.(lower), last.(lower), label = "Lower")
ylim(maximum(cps), minimum(cps))
xlabel("(x/c)")
ylabel("Cp")
# legend()
show()

## Plotting
x_domain, y_domain = (-1, 2), (-1, 1)
grid_size = 50
x_dom, y_dom = range(x_domain..., length = grid_size), range(y_domain..., length = grid_size)
grid = product(x_dom, y_dom)
pts = panel_points(panels);

source_vels = [ velocity(uniform) .+ sum(source_velocity.(σs, panels, x, y)) for (x, y) in grid ]

speeds = @. sqrt(first(source_vels)^2 + last(source_vels)^2)
cps = @. 1 - speeds^2 / uniform.mag^2

contourf(first.(grid), last.(grid), cps)
CB1 = colorbar(label = "Pressure Coefficient (Cp)")
# quiver(first.(grid), last.(grid), first.(source_vels), last.(source_vels), speeds)
streamplot(first.(grid)', last.(grid)', first.(source_vels)', last.(source_vels)', color = speeds', cmap = "coolwarm", density = 3)
CB2 = colorbar(orientation="horizontal", label = "Relative Airspeed (U/U∞)")
fill(first.(pts), last.(pts), color = "black", zorder = 3)
tight_layout()
show()


@testset "linear" begin
    # test linear
    # x dim = 4, u dim = 2
    A = 1.0 * Matrix(I, (4, 4))
    B = [1. 0.; 0. 1.;0. 0.; 0. 0.]
    C = 1.0 * Matrix(I, (3, 4))
    D = zeros(3, 2)
    W = 0.5 * Matrix(I, (4, 4))
    V = 0.1 * Matrix(I, (3, 3))
    dm = LinearDynamicsModel(A, B, W)
    om = LinearObservationModel(C, D, V)

    rng = MersenneTwister(1)

    x0 = ones(4)
    u0 = ones(2)

    x1 = predict(dm, x0, u0)
    @test x1 == [2.0, 2.0, 1.0, 1.0]
    @inferred predict(dm, x0, u0)

    x2 = predict(dm, x0, u0, rng)
    @test x2[1] > 1.0

    o1 = measure(om, x0, u0)
    @inferred measure(om, x0, u0)
    @test o1 == [1.0, 1.0, 1.0]
    o2 = measure(om, x0, u0, rng)
end

# test non linear
@testset "non linear" begin

    rng = MersenneTwister(1)

    W = 0.001*Matrix{Float64}(I,4,4)
    V = 0.3*Matrix{Float64}(I,4,4)

    dm = NonlinearDynamicsModel((x, u) -> x.^2 + u, W);
    om = NonlinearObservationModel((x, u) -> x.^4, V)

    x0 = ones(4)
    u0 = ones(4)

    x1 = predict(dm, x0, u0)
    @inferred predict(dm, x0, u0)
    @test x1 == 2*ones(4)
    x2 = predict(dm, x1, u0, rng)
    @test all(i -> i != 5.0, x2)

    o1 = measure(om, x0, u0)
    @inferred measure(om, x0, u0)
    @test o1 == x0
    o2 = measure(om, x0, u0, rng)
    @test all(x -> !isapprox(x, 1.0), o2)
end


function semi_infinite_transform(f, a, b, args)
    if b == Inf
        return (y)->f(a+y/(1-y), args...)/(1-y)^2, 0.0, 1.0
    elseif b == -Inf
        return (y)->f(a+(1-y)/y, args...)/y^2, 1.0, 0.0
    elseif a == Inf
        return (y)->f(b+y/(1-y), args...)/(1-y)^2, 1.0, 0.0
    elseif a == -Inf
        return (y)->f(b+(1-y)/y, args...)/y^2, 0.0, 1.0
    else
        @error "Invalid integration limit"
    end
end

function infinite_transformation(f, a, b, args)
    if a == b
        @error "Invalid integration limit"
    else
        lower_limit = a < 0 ? 0.0 : 1.0
        upper_limit = 1.0 - lower_limit
    end
    (y)->f((2*y-1)/(1-y)/y, args...)*(2*y^2-2*y+1)/(1-y)^2/y^2, lower_limit, upper_limit
end

function integral_limit_transfromation(f, a, b, args)
    if isfinite(a) && isfinite(b)
        return (y)->f(y, args...), a, b
    elseif !isfinite(a) && !isfinite(b)
        return infinite_transformation(f, a, b, args)
    else
        return semi_infinite_transform(f, a, b, args)
    end
end


module TestRelationalPrograms
using Test

using Catlab.CategoricalAlgebra.CSets
using Catlab.WiringDiagrams.UndirectedWiringDiagrams
using Catlab.Programs.RelationalPrograms

# Relations
###########

# Untyped relations

parsed = @relation (x,z) where (x,y,z) begin
  R(x,y)
  S(y,z)
end

d = RelationDiagram{Symbol}(2)
add_box!(d, 2, name=:R); add_box!(d, 2, name=:S)
add_junctions!(d, 3, variable=[:x,:y,:z])
set_junction!(d, [1,2,2,3])
set_junction!(d, [1,3], outer=true)
@test parsed == d

parsed = @relation ((x,z) where (x,y,z)) -> (R(x,y); S(y,z))
@test parsed == d

parsed = @relation function (x,z) where (x,y,z); R(x,y); S(y,z) end
@test parsed == d

# Typed relations

parsed = @relation (x,y,z) where (x::X, y::Y, z::Z, w::W) begin
  R(x,w)
  S(y,w)
  T(z,w)
end
d = RelationDiagram{Symbol}([:X,:Y,:Z])
add_box!(d, [:X,:W], name=:R)
add_box!(d, [:Y,:W], name=:S)
add_box!(d, [:Z,:W], name=:T)
add_junctions!(d, [:X,:Y,:Z,:W], variable=[:x,:y,:z,:w])
set_junction!(d, [1,4,2,4,3,4])
set_junction!(d, [1,2,3], outer=true)
@test parsed == d

# Tensor networks
#################

# Parsing
#--------

parsed = @tensor_network (i,j,k,ℓ) D[i,j,k] = A[i,ℓ] * B[j,ℓ] * C[k,ℓ]
d = RelationDiagram{Symbol}(3)
add_box!(d, 2, name=:A); add_box!(d, 2, name=:B); add_box!(d, 2, name=:C)
add_junctions!(d, 4, variable=[:i,:j,:k,:ℓ])
set_junction!(d, [1,4,2,4,3,4])
set_junction!(d, 1:3, outer=true)
@test parsed == d

parsed = @tensor_network D[i,j,k] = A[i,ℓ] * B[j,ℓ] * C[k,ℓ]
@test parsed == d
parsed = @tensor_network D[i,j,k] := A[i,ℓ] * B[j,ℓ] * C[k,ℓ]
@test parsed == d

# Degenerate case: single term.
parsed = @tensor_network B[i,j,k] = A[i,j,k]
d = singleton_diagram(RelationDiagram{Symbol}, 3, name=:A)
set_subpart!(d, :variable, [:i,:j,:k])
@test parsed == d

# Degenerate case: no terms.
parsed = @tensor_network A[i,j] = 1
d = RelationDiagram{Symbol}(2)
add_junctions!(d, 2, variable=[:i,:j])
set_junction!(d, 1:2, outer=true)
@test parsed == d

# Round trip
#-----------

# Parse and then compile.
macro roundtrip_tensor(tensor)
  quote
    compiled = compile_tensor_expr(@tensor_network($tensor),
                                   assign_op=:(=), assign_name=:out)
    @test compiled == $(QuoteNode(tensor))
  end
end

@roundtrip_tensor out[i,k] = A[i,j] * B[j,k]
@roundtrip_tensor out[i,j,k] = A[i,ℓ] * B[j,ℓ] * C[k,ℓ]
@roundtrip_tensor out = u[i] * A[i,j] * v[j]
@roundtrip_tensor out[j] = α * a[i] * B[i,j]

end


"""
Produce an elliptic torus with elliptic tube cross
section and overall elliptic contour
"""
module DoubleEllipticTorus

# minimum distance between a point and ellipse (X-Y plane)
# note: the original algorithm only works for ellipse with
# major axis aligned to x-axis
# This is why I need to check whether a < b,
# and make a rotation by -90 degrees.
function min_distance_2d(a, b, coordinates)
    if a < b
        return min_distance_2d(b, a, [coordinates[2], -coordinates[1]])
    end
    x = abs(coordinates[1])
    y = abs(coordinates[2])
    function aux(phi, accum)
        new_phi = atan(((a^2 - b^2)*sin(phi) + y*b)/(x*a))
        if abs(phi - new_phi) < 1e-4
            return new_phi
        else
            return aux(new_phi, accum+1)
        end
    end
    phi = aux(0, 0)
    return sqrt((x - a * cos(phi))^2 + (y - b*sin(phi))^2)
end

function isInside(a, b, xy)
    x, y = xy
    return (x^2/(a^2) + y^2/(b^2) < 1.0)
end

# check whether a point is inside the torus

function get_fn(argd::Dict)
    major_a = float(argd["major"]["a"])
    major_b = float(argd["major"]["b"])
    minor_a = float(argd["minor"]["a"])
    minor_b = float(argd["minor"]["b"])
    center = [float(x) for x in argd["center"]]
    power = convert(Int, argd["power"])
    force_constant = float(argd["k"])

    function fn_compute(raw_coordinates)
        coordinates = raw_coordinates - center
        if norm(coordinates[1:2]) < 1e-6
            d_xy = min(major_a, major_b)
        else
            d_xy = min_distance_2d(major_a, major_b, coordinates[1:2])
        end
        if isInside(major_a, major_b, coordinates[1:2])
            new_coordinates = [-d_xy, coordinates[3]]
        else
            new_coordinates = [d_xy, coordinates[3]]
        end

        # min. distance to the minor ellipse
        if norm(new_coordinates) < 1e-6
            d = min(minor_a, minor_b)
        else
            d = min_distance_2d(minor_a, minor_b, new_coordinates)
        end

        if isInside(minor_a, minor_b, new_coordinates)
            return 0.0
        else
            return (1.0/power) * force_constant * (d^power)
        end
    end
    return fn_compute
end

export get_fn

end


using LiquidSDR
using PyPlot

fs = 1000; # sampling frequency

# test signal generation
t = 0:1.0/fs:1;
s1 = sin(2*pi*t*10);  # 10Hz sinusoid
s2 = sin(2*pi*t*100); # 100Hz sinusoid
s = s1+s2; # mixed signal

# let's build butterworth filter
fc = 50.0 / fs; # normalized cut-off frequency (50Hz)
f0 = 51.0 / fs; # f0 is not considered for Low-Pass filters( so using a dummy value)
b,a = butterworth(fc, f0);

filt = createiirfilter(b,a);

# let's try to remove 100Hz component!
sig_result = applyfilter(filt, s);
plot(t, sig_result);


#Reineke SDI calculation
function sdi(tpa,qmd)
    sdi=tpa*(qmd/10.0)^1.605
end

function qmd(ba, tpa, constant)
    qmd=sqrt((ba/tpa)/constant)
end


"""
From Reynolds (2020) (Eq2-4)
Calculates the radius of the inner most stable circular orbit of the a black hole,
with spin a_star, and mass M.
"""
function r_isco(a_star, M)
    a = a_star
    r_g = M
    z1 = 1 + ∛(1 - a^2) * (∛(1 + a) + ∛(1 - a))
    z2 = √(3 * a^2 + z1^2)
    if a >= 0
        r_isco = (3 + z2 - √((3 - z1) * (3 + z1 + 2 * z2))) * r_g
    elseif a < 0
        r_isco = (3 + z2 + √((3 - z1) * (3 + z1 + 2 * z2))) * r_g
    end
end

"""
From Page & Thorne (1974) (Eq15n)
Calculates the function f at radius r, for a black hole of spin a_star, and mass M,
which is used to calculate the flux given by its accretion disk.
"""
function flux(r, a_star, M)
    R_isco = r_isco(a_star, M)
    x = √(r / M)
    x0 = √(R_isco / M)
    x1 = 2 * cos((1 / 3) * (acos(a_star)) - (π / 3))
    x2 = 2 * cos((1 / 3) * (acos(a_star)) + (π / 3))
    x3 = -2 * cos((1 / 3) * (acos(a_star)))

    flux =
        (3 / (2 * M)) *
        (1 / (x^2 * (x^3 - (3 * x) + (2 * a_star)))) *
        (
            x - x0 - ((3 / 2) * a_star * log(x / x0)) -
            (3 * (x1 - a_star)^2) / (x1 * (x1 - x2) * (x1 - x3)) *
            log((x - x1) / (x0 - x1)) -
            (3 * (x2 - a_star)^2) / (x2 * (x2 - x1) * (x2 - x3)) *
            log((x - x2) / (x0 - x2)) -
            (3 * (x3 - a_star)^2) / (x3 * (x3 - x1) * (x3 - x2)) *
            log((x - x3) / (x0 - x3))
        )
end

function mdot(M)
    # need something here to convert M into SI units, as M_☼ is in SI, althought we
    # aren't currently clear about what the units M currently is in
    # would look something like:
    # M_kg = M*c^2
    # L_edd = 3e4*L_☼*(M_kg/M_☼)

    L_edd = 3e4 * L_☼ * (M / M_☼)
    Mdot = -L_edd / (c^2 * η)
    mdot = -0.1 * Mdot
end

function diss(mdot, r, a_star, M)
    diss = ((c^6) / (G^2)) * mdot * flux(r, a_star, M) / (4 * π * r)
end

function temperature(r, a_star, M)
    m_dot = mdot(M)
    temperature = (diss(m_dot, r, a_star, M) / σ_SB)^(1 / 4)
end

"""
From Fanton et al. (1997) (Eq78)
"""
function observed_temperature(r, a_star, M, g)
    T = temperature(r, a_star, M)
    observed_temperature = g * T
end

function mass_scale_fraction(M, η, edd_ratio, r_isco, r_g, r)
    mass_scale_fraction =
        ((M * η)^(-1) * (edd_ratio * (1 - √(r_isco / r))) * (r_g / r)^3)^(1 / 4)
end

# constants
G = 6.67e-11
c = 3e8
L_☼ = 3.8e26
M_☼ = 1.99e30
σ_SB = 5.67e-8
η = 0.1

export observed_temperature, r_isco, temperature, mass_scale_fraction


# The Dual Arrhenius and Michealis-Menten (DAMM) model, Davidson et al. 2012

"""
    DAMM(x::VecOrMat{<: Real}, p::NTuple{7, Float64})
Calculate respiration as a function of soil temperature and moisture.
# Examples
```julia-repl
julia> Tₛ = [18.0, 22.0] # soil temperature [°C]
julia> θ = [0.35, 0.22] # soil moisture [m³ m⁻³]
julia> x = hcat(Tₛ, θ)
julia> p = (1e9, 64.0, 3.46e-8, 2.0e-3, 0.4, 0.0125, 1.0) # αₛₓ, Eaₛₓ, kMₛₓ, kMₒ₂, Sxₜₒₜ, Q10kM
julia> DAMM(x, p)
  1.6 # μmolCO₂ m⁻² s⁻¹
  2.8 # μmolCO₂ m⁻² s⁻¹
```
"""
function DAMM(x::VecOrMat{<: Real}, p::NTuple{7, Float64})
# Independent variables
  Tₛ = x[:, 1]°C # Soil temperature
  Tₛ = Tₛ .|> K # Tₛ in Kelvin
  θ = x[:, 2]m^3*m^-3 # Soil moisture, m³ m⁻³
# Parameters
  αₛₓ = p[1]mg*cm^-3*hr^-1 # Pre-exponential factor, mgC cm⁻³ h⁻¹
  Eaₛₓ = p[2]kJ*mol^-1 # Activation energy, kJ mol⁻¹
  kMₛₓ = p[3]g*cm^-3 # Michaelis constant, gC cm⁻³
  kMₒ₂ = p[4]L*L^-1 # Michaelis constant for O₂, L L⁻¹
  porosity = p[5] # 1 - soil buld density / soil particle density
  Sxₜₒₜ = p[6]g*cm^-3 # Total soil carbon, gC cm⁻³
  Q10Km = p[7]
  # DAMM model
  Tref = 293.15K
  Vmax = @. αₛₓ * exp(-Eaₛₓ/(R * Tₛ)) # Maximum potential rate of respiration
  Sₓ = @. pₛₓ * Sxₜₒₜ * Dₗᵢ * θ^3 # All soluble substrate, gC cm⁻³
  MMₛₓ = @. Sₓ / (kMₛₓ * Q10Km^(ustrip(Tₛ - Tref)/10.0) + Sₓ) # Availability of substrate factor, 0-1
  airfilled_porosity = porosity .- θ
  airfilled_porosity[airfilled_porosity .< 0.0] .= NaN
  O₂ = @. Dₒₐ * O₂ₐ * (airfilled_porosity^(4/3)) # Oxygen concentration
  MMₒ₂ = @. O₂ / (kMₒ₂ + O₂) # Oxygen limitation factor, 0-1
  Rₛₘ = @. Vmax * MMₛₓ * MMₒ₂ # Respiration, mg C cm⁻³ hr⁻¹
  Rₛₘₛ = @. Rₛₘ * Eₛ # Respiration, effective depth 10 cm, mg C cm⁻² hr⁻¹
  Rₛₛₘ = Rₛₘₛ .|> molC*m^-2*s^-1
  Rₛ = Rₛₛₘ .|> μmolCO₂*m^-2*s^-1
  Rₛ = Float64.(ustrip(Rₛ)) # no units
  return Rₛ
end


@doc raw"""
    ItemkNN(k::Int64, shrink::Float64, weighting::Union{Nothing,Symbol}, weighting_at_inference::Bool, normalize::Bool, normalize_similarity::Bool)

Item-based k-nearest neighborhood algorithm with cosine similarity.
The model first computes the item-to-item similarity matrix

```math
s_{ij} = \frac{\bm r_i \cdot \bm r_j}{\|\bm r_i\|\|\bm r_j\| + h}\,,
```
where ``r_{i,u}`` is rating for item ``i`` by user ``u`` and ``h`` is the shrink parameter to suppress the contributions from items with a few ratings.

# Constructor arguments
- `k`: size of the nearest neighbors. Only the k-most similar items to each item are stored, which reduces sparse similarity maxrix size, and also make better predictions.
- `shrink`: shrink paramerer explained above.
- `weighting`: if set to `:ifidf` or `:bm25`, the raw rating matrix is weighted by TF-IDF or BM25, respectively, before computing similarity. If not necessary, just set `nothing`.
- `weighting_at_inference`: to use above weighting at inference time, only relevant for BM25.
- `normalize_similarity`: if set to `true`, normalize each column of similarity matrix. See the reference for detail.

# References
M. Deshpande and G. Karypis (2004), [Item-based top-N recommendation algorithms](https://doi.org/10.1145/963770.963776).
"""
mutable struct ItemkNN <: AbstractRecommender
    k::Int64
    shrink::Float64
    weighting::Union{Nothing,Symbol}
    weighting_at_inference::Bool
    normalize::Bool
    normalize_similarity::Bool

    similarity::Any
    user_histories::Any
    user2uidx::Union{Dict,Nothing}
    item2iidx::Union{Dict,Nothing}
    iidx2item::Union{Dict,Nothing}

    ItemkNN(
        k::Int64,
        shrink::Float64,
        weighting::Union{Nothing,Symbol},
        weighting_at_inference::Bool,
        normalize::Bool,
        normalize_similarity::Bool,
    ) = new(
        k,
        shrink,
        weighting,
        weighting_at_inference,
        normalize,
        normalize_similarity,
        nothing,
        nothing,
        nothing,
        nothing,
        nothing,
    )
end

"""
    fit!(model::ItemkNN, table; col_user = :userid, col_item = :itemid, col_rating = :rating)

Fit the `ItemkNN` model. `col_rating` specifies rating column in the `table`, which will be all unity if implicit feedback data is given.
"""
function fit!(model::ItemkNN, table; kwargs...)
    col_user = get(kwargs, :col_user, :userid)
    col_item = get(kwargs, :col_item, :itemid)
    col_rating = get(kwargs, :col_rating, :rating)

    table, model.user2uidx, model.item2iidx, model.iidx2item =
        make_idmap(table, col_user = col_user, col_item = col_item)

    X = rows2sparse(
        table,
        col_user = col_user,
        col_item = col_item,
        col_rating = col_rating,
    )

    if !model.weighting_at_inference
        model.user_histories = X
    end

    if model.weighting == :tfidf
        X = tfidf(X)
    elseif model.weighting == :bm25
        X = bm25(X)
    end

    if model.weighting_at_inference
        model.user_histories = X
    end

    model.similarity = compute_similarity(
        X,
        model.k,
        model.shrink,
        model.normalize,
        model.normalize_similarity,
    )
    return model
end

@doc raw"""
    predict_u2i(model::ItemkNN, userid::Union{AbstractString,Int}, n::Int64; drop_history = false)

Recommend top-`n` items for user by `ItemkNN`. The predicted rating of item ``i`` by user ``u`` is computed by

```math

\hat{r}_{i, u} = \sum_{j} s_{ij} r_{j, u}\,,
```
where ``r_{j, u}`` is the actual user rating while ``\hat{r}_{i, u}`` is the model prediction.
"""
function predict_u2i(
    model::ItemkNN,
    userid::Union{AbstractString,Int},
    n::Int64;
    drop_history = false,
    kwargs...,
)
    if userid in keys(model.user2uidx)
        uidx = model.user2uidx[userid]
        pred = predict_u2i(
            model.similarity,
            model.user_histories[uidx, :],
            n,
            drop_history = drop_history,
        )
        return [model.iidx2item[iidx] for iidx in pred]

    else
        return []
    end
end