function incident_infections(params::InfParams, S::Int, I::Vector{Int}, R::Int)
    iszero(S) && return 0
    infSum = 0
    N = S + sum(I) + R
    for (i, inf) in enumerate(I)
        d = params.infectiousness[i]
        k,θ = Distributions.params(d)
        d′ = Gamma(k*S/N, θ) # conversion from R₀ to Rₜ
        infSum += rand(RVsum(d′, inf))
    end

    return min(S, floor(Int, infSum)) # Can't infect more people than are susceptible
end

function symptomatic_isolation(params::InfParams, I::Vector{Int})
    isolating = zero(I)
    for (i, inf) in enumerate(I)
        symptomatic_prob = params.symptom_probs[i]
        isolation_prob = symptomatic_prob*(1-params.asymptomatic_prob)*params.symptomatic_isolation_prob
        isolating[i] = rand(Binomial(inf,isolation_prob))
    end
    return isolating
end

function positive_tests(params::InfParams, I::Vector{Int}, T::Matrix{Int}, a::CovidAction)
    pos_tests = zeros(Int, length(params.pos_test_probs))

    for (i, inf) in enumerate(I)
        num_already_tested = sum(@view T[:,i])
        num_tested = floor(Int,(inf-num_already_tested)*a.testing_prop)
        pos_tests[i] = rand(Binomial(num_tested,params.pos_test_probs[i]))
    end
    return pos_tests
end

# Only record number that have taken the test, the number that return postive is
# Binomial dist, such that s' is stochastic on s.
function update_isolations(params::InfParams, I, R, T, a::CovidAction)

    sympt = symptomatic_isolation(params, I) # Number of people isolating due to symptoms
    pos_tests = positive_tests(params, I, T, a)

    sympt_prop = sympt ./ I # Symptomatic Isolation Proportion
    replace!(sympt_prop, NaN=>0.0)

    R += sum(sympt)
    I .-= sympt

    T[end,:] .= pos_tests

    @. T = floor(Int, (1 - sympt_prop)' * T)

    R += sum(@view T[1,:])
    @views I .-= T[1,:]

    @assert all(≥(0), I)

    CovidPOMDPs.shift_test!(T)

    return I, R, T, pos_tests
end

function CovidPOMDPs.sim_step(pomdp::SingleCovidPOMDP, state::SingleCovidState, a::CovidAction)
    (;S, I, R, T, params, prev_action) = state
    I = copy(I)
    T = copy(T)

    # Update symptomatic and testing-based isolations
    I, R, T, pos_tests = update_isolations(params, I, R, T, a)

    # Incident Infections
    R += I[end]
    I = circshift(I, 1) # shift_inf!(I)
    new_infections = incident_infections(params, S, I, R)
    I[1] = new_infections
    S -= new_infections
    sp = SingleCovidState(S, I, R, T, params, a)

    return sp, new_infections, pos_tests
end


# Car

function car_dynamics!(ẋ::AbstractVector,x::AbstractVector,u::AbstractVector)
    ẋ[1] = u[1]*cos(x[3])
    ẋ[2] = u[1]*sin(x[3])
    ẋ[3] = u[2]
    return nothing
end
n,m = 3,2

car = Model(car_dynamics!,n,m)


abstract type AbstractBC{T} <: AbstractDiffEqAffineOperator{T} end

abstract type AtomicBC{T} <: AbstractBC{T} end

"""
Robin, General, and in general Neumann, Dirichlet and Bridge BCs
are not necessarily linear operators.  Instead, they are affine
operators, with a constant term Q*x = Qa*x + Qb.
"""
abstract type AffineBC{T} <: AtomicBC{T} end

struct NeumannBC{N} end
struct Neumann0BC{N} end
struct DirichletBC{N} end
struct Dirichlet0BC{N} end

"""
q = PeriodicBC{T}()
Qx, Qy, ... = PeriodicBC{T}(size(u)) #When all dimensions are to be extended with a periodic boundary condition.
-------------------------------------------------------------------------------------
Creates a periodic boundary condition, where the lower index end of some u is extended with the upper index end and vice versa.
It is not reccomended to concretize this BC type in to a BandedMatrix, since the vast majority of bands will be all 0s. SpatseMatrix concretization is reccomended.
"""
struct PeriodicBC{T} <: AtomicBC{T}
    PeriodicBC(T::Type) = new{T}()
end

"""
  q = RobinBC(left_coefficients, right_coefficients, dx::T, approximation_order) where T # When this BC extends a dimension with a uniform step size
  q = RobinBC(left_coefficients, right_coefficients, dx::Vector{T}, approximation_order) where T # When this BC extends a dimension with a non uniform step size. dx should be the vector of step sizes for the whole dimension
-------------------------------------------------------------------------------------
  The variables in l are [αl, βl, γl], and correspond to a BC of the form αl*u(0) + βl*u'(0) = γl imposed on the lower index boundary.
  The variables in r are [αl, βl, γl], and correspond to an analagous boundary on the higher index end.
  Implements a robin boundary condition operator Q that acts on a vector to give an extended vector as a result
  Referring to (https://github.com/JuliaDiffEq/DiffEqOperators.jl/files/3267835/ghost_node.pdf)
  Write vector b̄₁ as a vertical concatanation with b0 and the rest of the elements of b̄ ₁, denoted b̄`₁, the same with ū into u0 and ū`. b̄`₁ = b̄`_2 = fill(β/Δx, length(stencil)-1)
  Pull out the product of u0 and b0 from the dot product. The stencil used to approximate u` is denoted s. b0 = α+(β/Δx)*s[1]
  Rearrange terms to find a general formula for u0:= -b̄`₁̇⋅ū`/b0 + γ/b0, which is dependent on ū` the robin coefficients and Δx.
  The non identity part of Qa is qa:= -b`₁/b0 = -β.*s[2:end]/(α+β*s[1]/Δx). The constant part is Qb = γ/(α+β*s[1]/Δx)
  do the same at the other boundary (amounts to a flip of s[2:end], with the other set of boundary coeffs)
"""
struct RobinBC{T, V<:AbstractVector{T}} <: AffineBC{T}
    a_l::V
    b_l::T
    a_r::V
    b_r::T
    function RobinBC(l::NTuple{3,T}, r::NTuple{3,T}, dx::T, order = 1) where {T}
        αl, βl, γl = l
        αr, βr, γr = r

        s = calculate_weights(1, one(T), Array(one(T):convert(T,order+1))) #generate derivative coefficients about the boundary of required approximation order

        a_l = -s[2:end]./(αl*dx/βl + s[1])
        a_r = s[end:-1:2]./(αr*dx/βr - s[1]) # for other boundary stencil is flippedlr with *opposite sign*

        b_l = γl/(αl+βl*s[1]/dx)
        b_r = γr/(αr-βr*s[1]/dx)

        return new{T, typeof(a_l)}(a_l, b_l, a_r, b_r)
    end
    function RobinBC(l::Union{NTuple{3,T},AbstractVector{T}}, r::Union{NTuple{3,T},AbstractVector{T}}, dx::AbstractVector{T}, order = 1) where {T}
        αl, βl, γl = l
        αr, βr, γr = r

        s_index = Array(one(T):convert(T,order+1))
        dx_l, dx_r = dx[1:length(s_index)], dx[(end-length(s_index)+1):end]

        s = calculate_weights(1, one(T), s_index) #generate derivative coefficients about the boundary of required approximation order
        denom_l = αl+βl*s[1]/dx_l[1]
        denom_r = αr-βr*s[1]/dx_r[end]

        a_l = -βl.*s[2:end]./(denom_l*dx_l[2:end])
        a_r = βr.*s[end:-1:2]./(denom_r*dx_r[1:(end-1)]) # for other boundary stencil is flippedlr with *opposite sign*

        b_l = γl/denom_l
        b_r = γr/denom_r

        return new{T, typeof(a_l)}(a_l, b_l, a_r, b_r)
    end
end

stencil(q::AffineBC{T}, N::Int) where T = ([transpose(q.a_l) transpose(zeros(T, N-length(q.a_l)))], [transpose(zeros(T, N-length(q.a_r))) transpose(q.a_r)])
affine(q::AffineBC) = (q.b_l, q.b_r)

stencil(q::PeriodicBC{T}, N::Int) where T= ([transpose(zeros(T, N-1)) one(T)], [one(T) transpose(zeros(T, N-1))])
affine(q::PeriodicBC{T}) where T = (zero(T), zero(T))
"""
q = GeneralBC(α_leftboundary, α_rightboundary, dx::T, approximation_order)

-------------------------------------------------------------------------------------

Implements a generalization of the Robin boundary condition, where α is a vector of coefficients.
Represents a condition of the form α[1] + α[2]u[0] + α[3]u'[0] + α[4]u''[0]+... = 0
Implemented in a similar way to the RobinBC (see above).
This time there are multiple stencils for multiple derivative orders - these can be written as a matrix S.
All components that multiply u(0) are factored out, turns out to only involve the first column of S, s̄0. The rest of S is denoted S`. the coeff of u(0) is s̄0⋅ᾱ[3:end] + α[2].
the remaining components turn out to be ᾱ[3:end]⋅(S`ū`) or equivalently (transpose(ᾱ[3:end])*S`)⋅ū`. Rearranging, a stencil q_a to be dotted with ū` upon extension can readily be found, along with a constant component q_b
"""
struct GeneralBC{T, L<:AbstractVector{T}, R<:AbstractVector{T}} <:AffineBC{T}
    a_l::L
    b_l::T
    a_r::R
    b_r::T
    function GeneralBC(αl::AbstractVector{T}, αr::AbstractVector{T}, dx::T, order = 1) where {T}
        nl = length(αl)
        nr = length(αr)
        S_l = zeros(T, (nl-2, order+nl-2))
        S_r = zeros(T, (nr-2, order+nr-2))

        for i in 1:(nl-2)
            S_l[i,:] = [transpose(calculate_weights(i, one(T), Array(one(T):convert(T, order+i)))) transpose(zeros(T, Int(nl-2-i)))]./(dx^i) #am unsure if the length of the dummy_x is correct here
        end

        for i in 1:(nr-2)
            S_r[i,:] = [transpose(calculate_weights(i, convert(T, order+i), Array(one(T):convert(T, order+i)))) transpose(zeros(T, Int(nr-2-i)))]./(dx^i)
        end
        s0_l = S_l[:,1] ; Sl = S_l[:,2:end]
        s0_r = S_r[:,end] ; Sr = S_r[:,(end-1):-1:1]

        denoml = αl[2] .+ αl[3:end] ⋅ s0_l
        denomr = αr[2] .+ αr[3:end] ⋅ s0_r

        a_l = -transpose(transpose(αl[3:end]) * Sl) ./denoml
        a_r = reverse(-transpose(transpose(αr[3:end]) * Sr) ./denomr)

        b_l = -αl[1]/denoml
        b_r = -αr[1]/denomr
        new{T, typeof(a_l), typeof(a_r)}(a_l,b_l,a_r,b_r)
    end

    function GeneralBC(αl::AbstractVector{T}, αr::AbstractVector{T}, dx::AbstractVector{T}, order = 1) where {T}

        nl = length(αl)
        nr = length(αr)
        dx_l, dx_r = (dx[1:(order+nl-2)], reverse(dx[(end-order-nr+3):end]))
        S_l = zeros(T, (nl-2, order+nl-2))
        S_r = zeros(T, (nr-2, order+nr-2))

        for i in 1:(nl-2)
            S_l[i,:] = [transpose(calculate_weights(i, one(T), Array(one(T):convert(T, order+i)))) transpose(zeros(T, Int(nl-2-i)))]./(dx_l.^i)
        end

        for i in 1:(nr-2)
            S_r[i,:] = [transpose(calculate_weights(i, convert(T, order+i), Array(one(T):convert(T, order+i)))) transpose(zeros(T, Int(nr-2-i)))]./(dx_r.^i)
        end
        s0_l = S_l[:,1] ; Sl = S_l[:,2:end]
        s0_r = S_r[:,end] ; Sr = S_r[:,(end-1):-1:1]

        denoml = αl[2] .+ αl[3:end] ⋅ s0_l
        denomr = αr[2] .+ αr[3:end] ⋅ s0_r

        a_l = -transpose(transpose(αl[3:end]) * Sl) ./denoml
        a_r = reverse(-transpose(transpose(αr[3:end]) * Sr) ./denomr)

        b_l = -αl[1]/denoml
        b_r = -αr[1]/denomr
        new{T,  typeof(a_l), typeof(a_r)}(a_l,b_l,a_r,b_r)
    end
end

#implement Neumann and Dirichlet as special cases of RobinBC
NeumannBC(α::NTuple{2,T}, dx::Union{AbstractVector{T}, T}, order = 1) where T = RobinBC((zero(T), one(T), α[1]), (zero(T), one(T), α[2]), dx, order)
DirichletBC(αl::T, αr::T) where T = RobinBC((one(T), zero(T), αl), (one(T), zero(T), αr), one(T), 2one(T) )
#specialized constructors for Neumann0 and Dirichlet0
Dirichlet0BC(T::Type) = DirichletBC(zero(T), zero(T))
Neumann0BC(dx::Union{AbstractVector{T}, T}, order = 1) where T = NeumannBC((zero(T), zero(T)), dx, order)

# other acceptable argument signatures
#RobinBC(al::T, bl::T, cl::T, dx_l::T, ar::T, br::T, cr::T, dx_r::T, order = 1) where T = RobinBC([al,bl, cl], [ar, br, cr], dx_l, order)

Base.:*(Q::AffineBC, u::AbstractVector) = BoundaryPaddedVector(Q.a_l ⋅ u[1:length(Q.a_l)] + Q.b_l, Q.a_r ⋅ u[(end-length(Q.a_r)+1):end] + Q.b_r, u)
Base.:*(Q::PeriodicBC, u::AbstractVector) = BoundaryPaddedVector(u[end], u[1], u)

Base.size(Q::AtomicBC) = (Inf, Inf) #Is this nessecary?

gettype(Q::AbstractBC{T}) where T = T


#Function to calculate triangle coefficients for angular momenta
#Based on Matlab code written by Amita B. Deb, Clarendon Lab. 2007.

function triangle_coeff(a,b,c)
  return tri = factorial(a+b-c)*factorial(a-b+c)*factorial(-a+b+c)/factorial(a+b+c+1)
end


@recipe function f(sol::AbstractNLOSolution;plot_analytic=false, evolution=false)
    ω = sol.prob.ω
    zend = sol.prob.prob.tspan[2]
    spectrum = abs2.(FT(sol, zend))
    layout := (2, 1)
    if evolution
        zmesh = linspace(0, zend, 100)
        intensity = 10*log10.(1e-30+abs2.(sol.(zmesh)))
        intensity = hcat(intensity...)
        spectrum = 10*log10.(1e-30+abs2.(FT.(sol, zmesh)))
        spectrum = hcat(spectrum...)
        @series begin
            title := "Intensity"
            subplot := 1
            #ylims := (minimum(sol.prob.tmesh)/25, maximum(sol.prob.tmesh)/25)
            clims := (maximum(intensity)-80, maximum(intensity))
            zmesh, sol.prob.tmesh, intensity
        end
        @series begin
            title := "Spectrum"
            subplot := 2
            clims := (maximum(spectrum)-80, maximum(spectrum))
            zmesh, ω, spectrum
        end
    else
        @series begin
            title := "Intensity"
            subplot := 1
            seriestype := :path
            sol.prob.tmesh, abs2.(sol(zend))
        end
        @series begin
            ylabel := ("(dB)")
            title := "Spectrum"
            subplot := 2
            seriestype := :path
            ω, spectrum/maximum(spectrum)
        end
    end
end

@recipe function f(optical_property::OpticalAttr)
    fs = frequency(optical_property)/1e12 #plot in THz
    λs = wavelength(optical_property)*1e9 #plot in nm
    @series begin
        xlabel := "Frequency (THz)"
        ylabel := get_label(optical_property)
        seriestype := :path
        xmirror := true
        fs, get_property(optical_property)
    end
end


using CUSPARSE
using CUDArt
using Base.Test

m = 25
n = 35
k = 10
blockdim = 5

@testset "mm" begin
    @testset for elty in [Float32,Float64,Complex64,Complex128]
        A = sparse(rand(elty,m,k))
        B = rand(elty,k,n)
        C = rand(elty,m,n)
        alpha = rand(elty)
        beta = rand(elty)
        @testset "csr" begin
            d_B = CudaArray(B)
            d_C = CudaArray(C)
            d_A = CudaSparseMatrixCSR(A)
            @test_throws DimensionMismatch CUSPARSE.mm!('T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm!('N',alpha,d_A,d_B,beta,d_B,'O')
            d_D = CUSPARSE.mm('N',alpha,d_A,d_B,beta,d_C,'O')
            h_D = to_host(d_D)
            D = alpha * A * B + beta * C
            @test D ≈ h_D
            d_D = CUSPARSE.mm('N',d_A,d_B,beta,d_C,'O')
            h_D = to_host(d_D)
            D = A * B + beta * C
            @test D ≈ h_D
            d_D = CUSPARSE.mm('N',d_A,d_B,d_C,'O')
            h_D = to_host(d_D)
            D = A * B + C
            @test D ≈ h_D
            d_D = CUSPARSE.mm('N',alpha,d_A,d_B,'O')
            h_D = to_host(d_D)
            D = alpha * A * B
            @test D ≈ h_D
            d_D = CUSPARSE.mm('N',d_A,d_B,'O')
            h_D = to_host(d_D)
            D = A * B
            @test D ≈ h_D
            d_D = d_A*d_B
            h_D = to_host(d_D)
            D = A * B
            @test D ≈ h_D
            @test_throws DimensionMismatch CUSPARSE.mm('T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm('N',alpha,d_A,d_B,beta,d_B,'O')
        end
        @testset "csc" begin
            d_B = CudaArray(B)
            d_C = CudaArray(C)
            d_A = CudaSparseMatrixCSC(A)
            @test_throws DimensionMismatch CUSPARSE.mm!('T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm!('N',alpha,d_A,d_B,beta,d_B,'O')
            d_D = CUSPARSE.mm('N',alpha,d_A,d_B,beta,d_C,'O')
            h_D = to_host(d_D)
            D = alpha * A * B + beta * C
            @test D ≈ h_D
            d_D = CUSPARSE.mm('N',d_A,d_B,beta,d_C,'O')
            h_D = to_host(d_D)
            D = A * B + beta * C
            @test D ≈ h_D
            d_D = CUSPARSE.mm('N',d_A,d_B,d_C,'O')
            h_D = to_host(d_D)
            D = A * B + C
            @test D ≈ h_D
            d_D = CUSPARSE.mm('N',alpha,d_A,d_B,'O')
            h_D = to_host(d_D)
            D = alpha * A * B
            @test D ≈ h_D
            d_D = CUSPARSE.mm('N',d_A,d_B,'O')
            h_D = to_host(d_D)
            D = A * B
            @test D ≈ h_D
            d_D = d_A*d_B
            h_D = to_host(d_D)
            D = A * B
            @test D ≈ h_D
            @test_throws DimensionMismatch CUSPARSE.mm('T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm('N',alpha,d_A,d_B,beta,d_B,'O')
        end
    end
end

@testset "mm_symm" begin
    @testset for elty in [Float32,Float64,Complex64,Complex128]
        A = sparse(rand(elty,m,m))
        A = A + A.'
        B = rand(elty,m,n)
        C = rand(elty,m,n)
        alpha = rand(elty)
        beta = rand(elty)
        @testset "csr" begin
            d_B = CudaArray(B)
            d_C = CudaArray(C)
            d_A = Symmetric(CudaSparseMatrixCSR(A))
            d_C = CUSPARSE.mm!('N',alpha,d_A,d_B,beta,d_C,'O')
            h_C = to_host(d_C)
            D = alpha * A * B + beta * C
            @test D ≈ h_C
            d_C = d_A.' * d_B
            h_C = to_host(d_C)
            D = A.' * B
            @test D ≈ h_C
            d_B = CudaArray(rand(elty,k,n))
            @test_throws DimensionMismatch CUSPARSE.mm!('T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm!('N',alpha,d_A,d_B,beta,d_B,'O')
        end
        @testset "csc" begin
            d_B = CudaArray(B)
            d_C = CudaArray(C)
            d_A = Symmetric(CudaSparseMatrixCSC(A))
            d_C = CUSPARSE.mm!('N',alpha,d_A,d_B,beta,d_C,'O')
            h_C = to_host(d_C)
            D = alpha * A * B + beta * C
            @test D ≈ h_C
            d_C = d_A.' * d_B
            h_C = to_host(d_C)
            D = A.' * B
            @test D ≈ h_C
            d_B = CudaArray(rand(elty,k,n))
            @test_throws DimensionMismatch CUSPARSE.mm!('T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm!('N',alpha,d_A,d_B,beta,d_B,'O')
        end
    end
end
@testset "mm_herm" begin
    @testset for elty in [Float32,Float64,Complex64,Complex128]
        A = sparse(rand(elty,m,m))
        A = A + A'
        B = rand(elty,m,n)
        C = rand(elty,m,n)
        alpha = rand(elty)
        beta = rand(elty)
        @testset "csr" begin
            d_B = CudaArray(B)
            d_C = CudaArray(C)
            d_A = Hermitian(CudaSparseMatrixCSR(A))
            d_C = CUSPARSE.mm!('N',alpha,d_A,d_B,beta,d_C,'O')
            h_C = to_host(d_C)
            D = alpha * A * B + beta * C
            @test D ≈ h_C
            d_B = CudaArray(rand(elty,k,n))
            @test_throws DimensionMismatch CUSPARSE.mm!('T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm!('N',alpha,d_A,d_B,beta,d_B,'O')
        end
        @testset "csc" begin
            d_B = CudaArray(B)
            d_C = CudaArray(C)
            d_A = Hermitian(CudaSparseMatrixCSC(A))
            d_C = CUSPARSE.mm!('N',alpha,d_A,d_B,beta,d_C,'O')
            h_C = to_host(d_C)
            D = alpha * A * B + beta * C
            @test D ≈ h_C
            d_B = CudaArray(rand(elty,k,n))
            @test_throws DimensionMismatch CUSPARSE.mm!('T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm!('N',alpha,d_A,d_B,beta,d_B,'O')
        end
    end
end

@testset "mm2" begin
    @testset for elty in [Float32,Float64,Complex64,Complex128]
        A = sparse(rand(elty,m,k))
        B = rand(elty,k,n)
        C = rand(elty,m,n)
        alpha = rand(elty)
        beta = rand(elty)
        @testset "csr" begin
            d_B = CudaArray(B)
            d_C = CudaArray(C)
            d_A = CudaSparseMatrixCSR(A)
            @test_throws DimensionMismatch CUSPARSE.mm2!('N','T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm2!('T','N',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm2!('T','T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm2!('N','N',alpha,d_A,d_B,beta,d_B,'O')
            d_D = CUSPARSE.mm2('N','N',alpha,d_A,d_B,beta,d_C,'O')
            h_D = to_host(d_D)
            D = alpha * A * B + beta * C
            @test D ≈ h_D
            d_D = CUSPARSE.mm2('N','N',d_A,d_B,beta,d_C,'O')
            h_D = to_host(d_D)
            D = A * B + beta * C
            @test D ≈ h_D
            d_D = CUSPARSE.mm2('N','N',d_A,d_B,d_C,'O')
            h_D = to_host(d_D)
            D = A * B + C
            @test D ≈ h_D
            d_D = CUSPARSE.mm2('N','N',alpha,d_A,d_B,'O')
            h_D = to_host(d_D)
            D = alpha * A * B
            @test D ≈ h_D
            d_D = CUSPARSE.mm2('N','N',d_A,d_B,'O')
            h_D = to_host(d_D)
            D = A * B
            @test D ≈ h_D
        end
        @testset "csc" begin
            d_B = CudaArray(B)
            d_C = CudaArray(C)
            d_A = CudaSparseMatrixCSC(A)
            @test_throws DimensionMismatch CUSPARSE.mm2!('N','T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm2!('T','N',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm2!('T','T',alpha,d_A,d_B,beta,d_C,'O')
            @test_throws DimensionMismatch CUSPARSE.mm2!('N','N',alpha,d_A,d_B,beta,d_B,'O')
            d_D = CUSPARSE.mm2('N','N',alpha,d_A,d_B,beta,d_C,'O')
            h_D = to_host(d_D)
            D = alpha * A * B + beta * C
            @test D ≈ h_D
            d_D = CUSPARSE.mm2('N','N',d_A,d_B,beta,d_C,'O')
            h_D = to_host(d_D)
            D = A * B + beta * C
            @test D ≈ h_D
            d_D = CUSPARSE.mm2('N','N',d_A,d_B,d_C,'O')
            h_D = to_host(d_D)
            D = A * B + C
            @test D ≈ h_D
            d_D = CUSPARSE.mm2('N','N',alpha,d_A,d_B,'O')
            h_D = to_host(d_D)
            D = alpha * A * B
            @test D ≈ h_D
            d_D = CUSPARSE.mm2('N','N',d_A,d_B,'O')
            h_D = to_host(d_D)
            D = A * B
            @test D ≈ h_D
        end
    end
end
@testset "bsrmm2" begin
    @testset for elty in [Float32,Float64,Complex64,Complex128]
        A = sparse(rand(elty,m,k))
        B = rand(elty,k,n)
        C = rand(elty,m,n)
        alpha = rand(elty)
        beta = rand(elty)
        d_B = CudaArray(B)
        d_C = CudaArray(C)
        d_A = CudaSparseMatrixCSR(A)
        d_A = CUSPARSE.switch2bsr(d_A,convert(Cint,blockdim))
        @test_throws DimensionMismatch CUSPARSE.mm2('N','T',alpha,d_A,d_B,beta,d_C,'O')
        @test_throws DimensionMismatch CUSPARSE.mm2('T','N',alpha,d_A,d_B,beta,d_C,'O')
        @test_throws DimensionMismatch CUSPARSE.mm2('T','T',alpha,d_A,d_B,beta,d_C,'O')
        @test_throws DimensionMismatch CUSPARSE.mm2('N','N',alpha,d_A,d_B,beta,d_B,'O')
        d_D = CUSPARSE.mm2('N','N',alpha,d_A,d_B,beta,d_C,'O')
        h_D = to_host(d_D)
        D = alpha * A * B + beta * C
        @test D ≈ h_D
        d_D = CUSPARSE.mm2('N','N',d_A,d_B,beta,d_C,'O')
        h_D = to_host(d_D)
        D = A * B + beta * C
        @test D ≈ h_D
        d_D = CUSPARSE.mm2('N','N',d_A,d_B,d_C,'O')
        h_D = to_host(d_D)
        D = A * B + C
        @test D ≈ h_D
        d_D = CUSPARSE.mm2('N','N',alpha,d_A,d_B,'O')
        h_D = to_host(d_D)
        D = alpha * A * B
        @test D ≈ h_D
        d_D = CUSPARSE.mm2('N','N',d_A,d_B,'O')
        h_D = to_host(d_D)
        D = A * B
        @test D ≈ h_D
    end
end


using LAR

datafile = readcsv("test/svg/test2.lines");
#datafile = readcsv("test/svg/provaWall.lines");
V = reshape(datafile',(size(datafile',1)÷2,size(datafile',2)*2))
len = length(datafile)
EV = collect(reshape(1:(len÷2), 2,(len÷4)))
larview(V,EV)
lineArray = (V,EV)
W,EW = lines2lar(lineArray)
viewexploded(W,EW)
#VV = vertices2vertices(W,EW)
viewLarIndices(W,EW,0.75)
#viewLarIndices(W,EW,0.025)
chains = boundary(W,EW)
operator = boundaryOp(EW,chains)
#println(full(operator))
FW = [sort(collect(Set(vcat([EW[:,abs(e)] for e in face]...)))) for face in chains]
viewLarIndices(W,EW,FW,0.75)

boxes = lar2boxes(W,FW)
parts = boxBucketing(boxes)

V,FV,EV = larFromLines(datafile)

chainAreas(V,EV,chains)


struct BasicReg{L,R} <: RegressionModel
    nobs::Int
    formula::FormulaTerm{L,R}
    coef::Vector{Float64}
    coefnames::Vector{String}
    yname::String
    tss::Float64
    rss::Float64
    residuals::Union{Vector{Float64}, Nothing}
    function BasicReg(nobs, formula::FormulaTerm{L,R}, coef, coefnames, yname, tss, rss, residuals) where {L,R}
        # @assert rss >= 0 "Residual sum of squares must be greater than 0"
        # @assert tss >= 0 "Total sum of squares must be greater than 0"
        # @assert nobs >= 0 "Observations must be greater than 0"
        # @assert length(coef) == length(coefnames) "Number of coefficients must be same as number of coefficient names"
        new{L,R}(nobs, formula, coef, coefnames, yname, tss, rss, residuals)
    end
    BasicReg(x::Int, f::FormulaTerm{L,R}) where {L, R} = new{L,R}(x, f)
end

# These are created to minimize the amount of allocations Julia does
# Julia typically allocates a vector for each loop, which when using
# so many loops, can create real garbage collection problems
# As it turns out, doing sum(abs2, resp - mean(resp)) also does
# an allocation, which could mean allocating a huge amount
# caluclating rss was even worse, so these functions are only
# meant to be used internally but do not allocate if passed a view
function calc_tss(resp)
    out = 0.0
    m = mean(resp)
    @simd for x in resp
        out += (x - m) ^ 2
    end
    out
end
function fast_pred(pred, coef, i)
    out = 0.0
    @simd for j in 1:length(coef)
        @inbounds out += pred[i, j] * coef[j]
    end
    out
end
function calc_rss(resp, pred, coef)
    out = 0.0
    @simd for i in 1:length(resp)
        @inbounds out += (resp[i] - fast_pred(pred, coef, i)) ^ 2
    end
    out
end
"""
    function BasicReg(
        resp::AbstractVector{Float64},
        pred::AbstractMatrix{Float64},
        yname::String,
        xnames::Vector{String},
        f::FormulaTerm{L,R};
        save_residuals::Bool=false,
        minobs::Int=1
    )::BasicReg{L,R} where {L,R}

## Arguments
- resp::AbstractVector{Float64}: The "Y" or response in a linear regression
- pred::AbstractMatrix{Float64}: The "X" matrix in a linear regression
- yname::String: The name of the response variable
- xnames::Vector{String}: The names of the prediction variables
- f::FormulaTerm{L,R}: A StatsModels.jl formula, saved in the resulting struct
- save_residuals::Bool=false: Whether or not to save the vector of residuals from
    the regression. Note for large numbers of regressions this can significantly slow
    down the speed
- minobs::Int=1: The minimum length of the response vector for the regression to
    run. The regression will also not run if the length of the response vector is
    less than or equal to the number of columns in the prediction matrix.

BasicReg is an intentionally simplistic linear regression. It also attempts to produce
a minimum number of allocations if views of vectors are passed.
"""
function BasicReg(
    resp::AbstractVector{Float64},
    pred::AbstractMatrix{Float64},
    yname::String,
    xnames::Vector{String},
    f::FormulaTerm{L,R};
    save_residuals::Bool=false,
    minobs=1
)::BasicReg{L,R} where {L,R}
    if length(resp) <= size(pred, 2) || length(resp) < minobs
        return BasicReg(length(resp), f)
    end

    coef = cholesky!(Symmetric(pred' * pred)) \ (pred' * resp)

    BasicReg(
        length(resp),
        f,
        coef,
        xnames,
        yname,
        calc_tss(resp),
        calc_rss(resp, pred, coef),
        save_residuals ? resp - pred * coef : nothing
    )
end

function create_pred_matrix(data::IterateTimelineTable, sch)
    id = first(data)[1]
    create_pred_matrix(
        parent(data)[id, internal_termvars(sch.rhs), AllowMissing{true}],
        sch
    )
end

function create_pred_matrix(data::TimelineTable{true}, sch)
    # This allows missing to make sure all dates are in the data

    mat = modelcols(sch.rhs, data)
    DataMatrix(
        mat,
        data_dates(data),
        calendar(data)
    )
end

"""
    quick_reg(
        data::TimelineTable{false},
        f::FormulaTerm;
        minobs::Real=0.8,
        save_residuals::Bool=false
    )

    quick_reg(
        data::IterateTimelineTable,
        f::FormulaTerm;
        minobs::Real=0.8,
        save_residuals::Bool=false
    )

Calculates a linear regression for the supplied data based on the formula (formula from StatsModels.jl).
Unless the formula explicitly excludes the intercept (i.e., `@formula(y ~ 0 + x)`), an intercept is added.

If `data` is of the type `IterateTimelineTable`, then the formula is applied to each `TimelineTable` in an
optimized way and returns a `Vector{BasicReg}`.

## Arguments
- `minobs::Real`: The minimum number of observations to return a completed regression. If less than 1,
    the value is used as a percentage relative to the total number of business days in the time period.
    Therefore, the default of 0.8 corresponds to at least 80% of the business days over the time period have values.
- `save_residuals::Bool=false`: Whether to save the residuals into `BasicReg`, This can have significant performance implications.

"""
function quick_reg(
    data::TimelineTable{false},# only works if no missing data for multiplication
    f::FormulaTerm;
    minobs::V=0.8,
    save_residuals::Bool=false
) where {V<:Real}

    if !StatsModels.omitsintercept(f) & !StatsModels.hasintercept(f)
        f = FormulaTerm(f.lhs, InterceptTerm{true}() + f.rhs)
    end
    sch = apply_schema(f, schema(f, data))
    select!(data, internal_termvars(sch))

    resp, pred = modelcols(sch, data)
    yname, xnames = coefnames(sch)
    BasicReg(
        resp,
        pred,
        yname,
        xnames,
        f;
        save_residuals,
        minobs=adjust_minobs(minobs, calendar(data), norm_dates(data))
    )
end

function fill_vector_reg!(
    out_vector::Vector{BasicReg{L,R}},
    iter_data::IterateTimelineTable{T},
    cache::DataMatrix,
    sch::FormulaTerm,
    f::FormulaTerm{L,R};
    save_residuals::Bool=false,
    minobs=0.8
) where {T, L, R}
    @assert validate_iterator(iter_data, out_vector) "Length of out_vector does not match the number of indexes in the iterator"
    cols = internal_termvars(sch)
    yname, xnames = coefnames(sch)

    Threads.@threads for (u_id, iter_indexes) in iter_data
        data = parent(iter_data)[u_id, cols, AllowMissing{true}]
        resp = datavector_modelcols(int_lhs(sch), data)
        for iter in iter_indexes
            cur_dates = dates_min_max(data_dates(data), iter_dates(iter))
            if nnz(data_missing_bdays(data)) == 0
                x = view(resp, cur_dates)
                y = view(cache, cur_dates)
            else
                new_mssngs = get_missing_bdays(data, cur_dates)
                x = view(resp, cur_dates, new_mssngs)
                y = view(cache, cur_dates, new_mssngs)
            end
            @inbounds out_vector[iter_index(iter)] = BasicReg(
                x,
                y,
                yname,
                xnames,
                f;
                save_residuals,
                minobs=adjust_minobs(minobs, calendar(parent(data)), iter_dates(iter))
            )
        end
    end
    out_vector
end

function quick_reg(
    data::IterateTimelineTable,
    f::FormulaTerm;
    minobs::V=0.8,
    save_residuals::Bool=false
) where {V<:Real}
    if !StatsModels.omitsintercept(f) & !StatsModels.hasintercept(f)
        f = FormulaTerm(f.lhs, InterceptTerm{true}() + f.rhs)
    end
    sch = apply_schema(f, schema(f, parent(data)))
    cache = create_pred_matrix(data, sch)
    out = fill(BasicReg(0, f), total_length(data))
    fill_vector_reg!(
        out,
        data,
        cache,
        sch,
        f;
        save_residuals,
        minobs
    )
end

StatsBase.predict(mod::BasicReg, x) = x * coef(mod)

StatsBase.coef(x::BasicReg) = isdefined(x, :coef) ? x.coef : missing
StatsBase.coefnames(x::BasicReg) = isdefined(x, :coef) ? x.coefnames : missing
StatsBase.responsename(x::BasicReg) = isdefined(x, :coef) ? x.yname : missing
StatsBase.nobs(x::BasicReg) = x.nobs
StatsBase.dof_residual(x::BasicReg) = isdefined(x, :coef) ? nobs(x) - length(coef(x)) : missing
StatsBase.r2(x::BasicReg) = 1 - (rss(x) / deviance(x))
StatsBase.adjr2(x::BasicReg) = 1 - rss(x) / deviance(x) * (nobs(x) - 1) / dof_residual(x)
StatsBase.islinear(x::BasicReg) = true
StatsBase.deviance(x::BasicReg) = isdefined(x, :coef) ? x.tss : missing
StatsBase.rss(x::BasicReg) = isdefined(x, :coef) ? x.rss : missing
function StatsBase.residuals(x::BasicReg)
    if !isdefined(x, :coef)
        return missing
    end
    if x.residuals === nothing
        @error("To access residuals, run `cache_reg` with the option `save_residuals=true`")
    else
        x.residuals
    end
end

function rhs_str(nms, vals; intercept = "(Intercept)")
    out = String[]
    for (nm, val) in zip(nms, vals)
        val_str = string(round(val, digits=3))
        if nm == intercept
            push!(out, val_str)
        else
            push!(out, val_str * "*" * nm)
        end
    end
    join(out, " + ")
end

function Base.show(io::IO, rr::BasicReg)
    if !isdefined(rr, :coef)
        print(io, "Obs: $(nobs(rr)), $(rr.formula)")
    else
        print(io, "Obs: $(nobs(rr)), $(responsename(rr)) ~ $(rhs_str(coefnames(rr), coef(rr))), AdjR2: ", round(adjr2(rr) * 100, digits=3), "%")
        # mat = hcat(
        #     coefnames(rr),
        #     string.(round.(coef(rr), digits=3))
        # )
        # PrettyTables.pretty_table(io, mat)
    end
end


@testset "Continuous identity system" begin
    for sd in 1:3
        s = ContinuousIdentitySystem(sd)
        @test statedim(s) == sd
        @test inputdim(s) == 0
        @test islinear(s) && isaffine(s)
    end
end

@testset "Continuous constrained identity system" begin
    for sd in 1:3
        X = Singleton(ones(sd))
        s = ConstrainedContinuousIdentitySystem(sd, X)
        @test statedim(s) == sd
        @test stateset(s) == X
        @test inputdim(s) == 0
        @test islinear(s) && isaffine(s)
    end
end

@testset "Continuous linear system" begin
    for sd in 1:3
        s = LinearContinuousSystem(zeros(sd, sd))
        @test statedim(s) == sd
        @test inputdim(s) == 0
        @test islinear(s) && isaffine(s)
    end
end

@testset "Continuous affine system" begin
    for sd in 1:3
        s = AffineContinuousSystem(zeros(sd, sd), zeros(sd))
        @test statedim(s) == sd
        @test inputdim(s) == 0
        @test !islinear(s) && isaffine(s)
    end
end

@testset "Continuous affine control system with state constraints" begin
    A = zeros(2, 2)
    B = Matrix([0.5 1.5]')
    c = [0.0, 1.0]
    X = Line([1., -1], 0.) # line x = y
    U = Interval(-1.0, 1.0)  # -1 <= u <= 1
    s = ConstrainedAffineControlContinuousSystem(A, B, c, X, U)
    @test statedim(s) == 2
    @test stateset(s) == X
    @test inputdim(s) == 1
    @test inputset(s) == U
    @test !islinear(s) && isaffine(s)
end

@testset "Continuous linear control system" begin
    for sd in 1:3
        s = LinearControlContinuousSystem(zeros(sd, sd), ones(sd, sd))
        @test statedim(s) == sd
        @test inputdim(s) == sd
        @test islinear(s) && isaffine(s)
    end
end

@testset "Continuous constrained linear system" begin
    A = [1. 1; 1 -1]
    X = Line([1., -1], 0.) # line x = y
    s = ConstrainedLinearContinuousSystem(A, X)
    @test statedim(s) == 2
    @test stateset(s) == X
    @test inputdim(s) == 0
    @test islinear(s) && isaffine(s)
end

@testset "Continuous constrained affine system" begin
    X = Line([1., -1], 0.) # line x = y
    s = ConstrainedAffineContinuousSystem(zeros(2, 2), zeros(2), X)
    @test statedim(s) == 2
    @test stateset(s) == X
    @test inputdim(s) == 0
    @test !islinear(s) && isaffine(s)
end

@testset "Continuous constrained linear control system" begin
    A = [1. 1; 1 -1]
    B = Matrix([0.5 1.5]')
    X = Line([1., -1], 0.)
    U = Interval(0.9, 1.1)
    s = ConstrainedLinearControlContinuousSystem(A, B, X, U)
    @test statedim(s) == 2
    @test stateset(s) == X
    @test inputdim(s) == 1
    @test inputset(s) == U
    @test islinear(s) && isaffine(s)

    # initial value problem composite type
    x0 = Singleton([1.5, 2.0])
    p = InitialValueProblem(s, x0)
    @test inputset(p) == U
    @test statedim(p) == 2
    @test inputdim(p) == 1

    # check that the matrices A and B need not be of the same type
    # with A dense and B a lazy adjoint
    B = [0.5 1.5]'
    s = ConstrainedLinearControlContinuousSystem(A, B, X, U)

    # check that the matrices A and B need not be of the same type
    # with A sparse and B a lazy adjoint
    A = sparse([1], [2], [1.0], 2, 2) # sparse matrix
    s = ConstrainedLinearControlContinuousSystem(A, B, X, U)
end

@testset "Continuous linear algebraic system" begin
    for sd in 1:3
        s = LinearAlgebraicContinuousSystem(zeros(sd, sd), zeros(sd, sd))
        @test islinear(s) && isaffine(s)
        @test statedim(s) == sd
        @test inputdim(s) == 0
    end
end

@testset "Continuous constrained linear algebraic system" begin
    A = [1. 1; 1 -1]
    E = [0. 1; 1 0]
    X = LinearConstraint([0, -1.], 0.) # the set y ≥ 0
    s = ConstrainedLinearAlgebraicContinuousSystem(A, E, X)
    @test islinear(s) && isaffine(s)
    @test statedim(s) == 2
    @test stateset(s) == X
    @test inputdim(s) == 0
end

@testset "Initial value problem for a continuous constrained linear algebraic system" begin
    A = [1. 1; 1 -1]
    E = [0. 1; 1 0]
    X = LinearConstraint([0, -1.], 0.) # the set y ≥ 0
    s = ConstrainedLinearAlgebraicContinuousSystem(A, E, X)
    @test islinear(s) && isaffine(s)
    x0 = Singleton([1.5, 2.0])
    p = IVP(s, x0)
    @test statedim(p) == 2
    @test inputdim(p) == 0
end

@testset "Polynomial system in continuous time" begin
    @polyvar x y
    p = 2x^2 - 3x + y

    # default constructor for scalar p and
    s = PolynomialContinuousSystem(p)
    @test !islinear(s) && !isaffine(s)
    @test statedim(s) == 2
    @test inputdim(s) == 0
    @test TypedPolynomials.nvariables(s) == 2
    @test TypedPolynomials.variables(s) == (x, y)

    # constructor for scalar p and given number of variables
    s = PolynomialContinuousSystem([p], 2)
end

@testset "Polynomial system in continuous time with state constraints" begin
    @polyvar x y
    p = 2x^2 - 3x + y
    X = BallInf(zeros(2), 0.1)

    # default constructor for scalar p and
    s = ConstrainedPolynomialContinuousSystem(p, X)
    @test !islinear(s) && !isaffine(s)
    @test statedim(s) == 2
    @test inputdim(s) == 0
    @test dim(stateset(s)) == dim(X)
    @test TypedPolynomials.nvariables(s) == 2
    @test TypedPolynomials.variables(s) == (x, y)
end

function vanderpol!(x, dx)
    dx[1] = x[2]
    dx[2] = x[2] * (1-x[1]^2) - x[1]
    return dx
end

@testset "Continuous system defined by a function" begin
    s = BlackBoxContinuousSystem(vanderpol!, 2)
    f! = (x, dx) -> s.f(x, dx)
    x = [1.0, 0.0]
    dx = similar(x)
    f!(x, dx)
    @test dx ≈ [0.0, -1.0]
end

@testset "Continuous system defined by a function with state constraints" begin
    H = HalfSpace([1.0, 0.0], 0.0) # x <= 0
    s = ConstrainedBlackBoxContinuousSystem(vanderpol!, 2, H)
    f! = (x, dx) -> s.f(x, dx)
    x = [1.0, 0.0]
    dx = similar(x)
    f!(x, dx)
    @test dx ≈ [0.0, -1.0]
    @test stateset(s) == H
end

function vanderpol_controlled!(x, u, dx)
    dx[1] = x[2]
    dx[2] = x[2] * (1-x[1]^2) - x[1] + u[1]
    return dx
end

@testset "Continuous control system defined by a function with state constraints" begin
    H = HalfSpace([1.0, 0.0], 0.0) # x <= 0
    U = Interval(-0.1, 0.1)
    s = ConstrainedBlackBoxControlContinuousSystem(vanderpol_controlled!, 2, 1, H, U)
    f! = (x, u, dx) -> s.f(x, u, dx)
    x = [1.0, 0.0]
    u = an_element(U)
    dx = similar(x)
    f!(x, u, dx)
    @test dx ≈ [0.0, -1.0 + u[1]]
    @test stateset(s) == H
    @test inputdim(s) == 1
    @test inputset(s) == U
end


using PastaQ
using ITensors
using Test
using Random

@testset "trotter circuit: real dynamics" begin
  N = 2
  B = 0.2

  sites = qubits(N)

  ampo = OpSum()
  # loop over the pauli operators
  for j in 1:N-1
    ampo += -1.0,"Z",j,"Z",j+1
    ampo += -B,"X",j
  end
  ampo += -B,"X",N
  Hmpo = MPO(ampo, sites)

  H = OpSum()
  # loop over the pauli operators
  for j in 1:N-1
    H += -1.0,"ZZ",(j,j+1)
    H += -B,"X",j
  end
  H += -B,"X",N

  t = 1.0
  ψ₀ = productstate(sites)
  ψtest = noprime(exp(-im * prod(Hmpo) * t) * prod(ψ₀))

  # 1. Specify final time and trotter step
  δt = 0.001
  t = 1.0
  circuit = trottercircuit(H; δt = δt, t = t)
  @test length(circuit) == 1000
  ψ = runcircuit(sites, circuit)
  @test prod(ψ) ≈ ψtest atol = 1e-5

  # 2. Specify  a time sequence
  ts = 0:0.001:1.0
  circuit = trottercircuit(H; ts = ts)
  @test length(circuit) == 1000
  ψ = runcircuit(sites, circuit)
  @test prod(ψ) ≈ ψtest atol = 1e-5
  ts = collect(0:0.001:1.0)
  circuit = trottercircuit(H; ts = ts)
  @test length(circuit) == 1000
  ψ = runcircuit(sites, circuit)
  @test prod(ψ) ≈ ψtest atol = 1e-5

  # 3. Specify a non-uniform time-sequence
  ts = vcat(collect(0:0.001:0.5), collect(0.502:0.002:1.0))
  circuit = trottercircuit(H; ts = ts)
  @test length(circuit) == length(ts)-1
  ψ = runcircuit(sites, circuit)
  @test prod(ψ) ≈ ψtest atol = 1e-5
end

@testset "trotter circuit: imaginary dynamics" begin
  N = 2
  B = 0.2

  sites = qubits(N)

  ampo = OpSum()
  # loop over the pauli operators
  for j in 1:N-1
    ampo += -1.0,"Z",j,"Z",j+1
    ampo += -B,"X",j
  end
  ampo += -B,"X",N
  Hmpo = MPO(ampo, sites)

  H = OpSum()
  # loop over the pauli operators
  for j in 1:N-1
    H += -1.0,"ZZ",(j,j+1)
    H += -B,"X",j
  end
  H += -B,"X",N

  τ = 1.0
  ψ₀ = productstate(sites)
  ψtest = noprime(exp(-prod(Hmpo) * τ) * prod(ψ₀))

  ## 1. Specify final time and trotter step
  δτ = 0.001
  circuit = trottercircuit(H; δτ = δτ, τ = τ)
  @test length(circuit) == 1000
  ψ = runcircuit(sites, circuit)
  @test prod(ψ) ≈ ψtest atol = 1e-5

  # 2. Specify  a time sequence
  τs = 0:0.001:1.0
  circuit = trottercircuit(H; τs = τs)
  @test length(circuit) == 1000
  ψ = runcircuit(sites, circuit)
  @test prod(ψ) ≈ ψtest atol = 1e-5
  τs = collect(0:0.001:1.0)
  circuit = trottercircuit(H; τs = τs)
  @test length(circuit) == 1000
  ψ = runcircuit(sites, circuit)
  @test prod(ψ) ≈ ψtest atol = 1e-5

  # 3. Specify a non-uniform time-sequence
  τs = vcat(collect(0:0.001:0.5), collect(0.502:0.002:1.0))
  circuit = trottercircuit(H; τs = τs)
  @test length(circuit) == length(τs)-1
  ψ = runcircuit(sites, circuit)
  @test prod(ψ) ≈ ψtest atol = 1e-5
end

#@testset "trotter layer 1" begin
#  Random.seed!(1234)
#  N = 20
#  H = OpSum()
#  nterms = 30
#  for j in 1:nterms
#    h = rand()
#    localop = rand(["X","Y","Z"])
#    site = rand(1:N)
#    H += h, localop, site
#
#    g = rand()
#    localop = rand(["CX","CZ"])
#    siteA = rand(1:N)
#    siteB = rand(deleteat!(collect(1:N),siteA))
#    H += g, localop, siteA,siteB
#  end
#
#  layer = PastaQ.trotterlayer(H, 0.1)
#  twoqubitgates = layer[1:nterms]
#  @test length(layer) == 2*nterms
#  @test all([g[2] isa Tuple for g in twoqubitgates])
#  onequbitgates = layer[nterms+1:2*nterms]
#  @test all([g[2] isa Int for g in onequbitgates])
#end
#
#@testset "trotter layer 2" begin
#  Random.seed!(1234)
#  N = 20
#  H = OpSum()
#  nterms = 30
#  for j in 1:nterms
#    h = rand()
#    localop = rand(["X","Y","Z"])
#    site = rand(1:N)
#    H += h, localop, site
#
#    g = rand()
#    localop = rand(["CX","CZ"])
#    siteA = rand(1:N)
#    siteB = rand(deleteat!(collect(1:N),siteA))
#    H += g, localop, siteA,siteB
#  end
#  layer = PastaQ.trotterlayer(H; δt = 0.1)
#  twoqubitgates = layer[1:nterms]
#  for (j,g) in enumerate(twoqubitgates)
#    @test g == layer[end+1-j]
#    @test g[2] isa Tuple
#  end
#  onequbitgates = layer[nterms+1:3*nterms]
#  for g in onequbitgates
#    @test g[2] isa Int
#  end
#end


module HenonMap
export henon_map

using Parameters: @with_kw, @unpack
using ..ExampleBase: LEDemo, DiscreteExample

@with_kw struct HenonMapParam
    a::Float64 = 1.4
    b::Float64 = 0.3
end

@inline function phase_dynamics!(u_next, u, p, t)
    @unpack a, b = p
    u_next[1] = 1 + u[2] - a * u[1]^2
    u_next[2] = b * u[1]
end

@inline @views function tangent_dynamics!(u_next, u, p, t)
    @unpack a, b = p
    phase_dynamics!(u_next[:, 1], u[:, 1], p, t)
    u_next[1, 2:end] .= u[2, 2:end] .- a * 2 * u[1, 1] .* u[1, 2:end]
    u_next[2, 2:end] .= b .* u[1, 2:end]
end

"""
Return a [`LEDemo`](@ref) for the Hénon map.

* M. Hénon, Commun. Math. Phys. Phys. 50, 69-77 (1976)
* <http://sprott.physics.wisc.edu/chaos/comchaos.htm>
* <https://en.wikipedia.org/wiki/H%C3%A9non_map>
"""
function henon_map(;
        u0=[-0.3332501594195585, 0.2743725483437717],
        t_renorm=10,
        t_attr=100000,
        atol=0, rtol=1e-2,
        terminator_options = [:atol => atol, :rtol => rtol],
        kwargs...)
    LEDemo(DiscreteExample(
        "Hénon map",
        phase_dynamics!, u0, t_renorm, HenonMapParam(),
        tangent_dynamics!,
        t_attr,
        [0.41922, -1.62319],   # known_exponents
        atol, rtol,
        terminator_options,
    ); kwargs...)
end

end


transform_coordinates(xyz, twist, section) = RotY(-twist) * xyz + section

function chop_sections(scaled_foils, twisties, leading_xyz, span_num, spacings, flip = false)
    # Reverse direction if left side
    if flip
        scaled_foils = reverse(scaled_foils, dims = 2)
        twisties     = (permutedims ∘ reverse)(twisties)
    end

    # Rotate and translate coordinates
    foil_coords  = reduce(hcat, transform_coordinates.(foil, Ref(twist), Ref(section)) for (foil, twist, section) in zip(eachcol(scaled_foils), twisties, leading_xyz))

    # Chop up spanwise sections
    permutedims(reduce(hcat, chop_coordinates(coords, span_num, spacings, flip) for coords in eachrow(foil_coords)))
end

"""
    surface_coordinates(wing :: HalfWing, n_s :: Integer, n_c :: Integer, flip = false)

Compute the coordinates of a `HalfWing` consisting of `Foil`s and relevant geometric quantities, given numbers of spanwise ``n_s`` and chordwise ``n_c`` panels, with an option to flip the signs of the ``y``-coordinates.
"""
function surface_coordinates(wing :: HalfWing, span_num :: Vector{<: Integer}, chord_num :: Integer; spacings = ["cosine"], flip = false)
    leading_xyz  = leading_edge(wing, flip)
    scaled_foils = reduce(hcat, @. wing.chords * (extend_yz ∘ cosine_foil)(wing.foils, chord_num))
    chop_sections(scaled_foils, twists(wing), leading_xyz, span_num, spacings, flip)
end

"""
    camber_coordinates(wing :: HalfWing, n_s :: Integer, n_c :: Integer, flip = false)

Compute the coordinates of a `HalfWing` consisting of camber distributions of `Foil`s and relevant geometric quantities, given numbers of spanwise ``n_s`` and chordwise ``n_c`` panels, with an option to flip the signs of the ``y``-coordinates.
"""
function camber_coordinates(wing :: HalfWing, span_num :: Vector{<: Integer}, chord_num :: Integer; spacings = ["cosine"], flip = false)
    leading_xyz  = leading_edge(wing, flip)
    scaled_foils = reduce(hcat, @. wing.chords * (camber_coordinates ∘ camber_thickness)(wing.foils, chord_num))
    chop_sections(scaled_foils, twists(wing), leading_xyz, span_num, spacings, flip)
end

"""
    mesh_horseshoes(wing :: HalfWing, n_s :: Integer, n_c :: Integer; flip = false)

Mesh a `HalfWing` into panels of ``n_s`` spanwise divisions per section and ``n_c`` chordwise divisions meant for lifting-line/vortex lattice analyses using horseshoe elements.
"""
mesh_horseshoes(wing :: HalfWing, span_num :: Vector{<: Integer}, chord_num :: Integer, spacings = ["cosine"]; flip = false) = make_panels(coordinates(wing, span_num, chord_num; span_spacing = spacings, flip = flip))

"""
    mesh_wing(wing :: HalfWing, n_s :: Integer, n_c :: Integer; flip = false)

Mesh a `HalfWing` into panels of ``n_s`` spanwise divisions per section and ``n_c`` chordwise divisions meant for 3D analyses using doublet-source elements or equivalent formulations. TODO: Tip meshing.
"""
mesh_wing(wing :: HalfWing, span_num :: Vector{<: Integer}, chord_num :: Integer; flip = false) = (make_panels ∘ surface_coordinates)(wing, span_num, chord_num, flip)

"""
    mesh_cambers(wing :: HalfWing, n_s :: Integer, n_c :: Integer; flip = false)

Mesh the camber distribution of a `HalfWing` into panels of ``n_s`` spanwise divisions per section and ``n_c`` chordwise divisions meant for vortex lattice analyses.
"""
mesh_cambers(wing :: HalfWing, span_num :: Vector{<: Integer}, chord_num :: Integer, spacings = ["cosine"]; flip = false) = make_panels(camber_coordinates(wing, span_num, chord_num; spacings = spacings, flip = flip))

chop_leading_edge(obj :: HalfWing, span_num; flip = false)  = chop_coordinates(leading_edge(obj, flip), span_num)
chop_trailing_edge(obj :: HalfWing, span_num; flip = false) = chop_coordinates(leading_edge(obj, flip), span_num)

chop_leading_edge(obj :: Wing, span_num :: Integer)  = let (lead, trail) = wing_bounds(obj); chop_spans(lead, trail, span_num)[1] end
chop_trailing_edge(obj :: Wing, span_num :: Integer) = let (lead, trail) = wing_bounds(obj); chop_spans(lead, trail, span_num)[2] end

"""
    mesh_horseshoes(wing :: Wing, n_s :: Integer, n_c :: Integer)

Mesh a `Wing` into panels of ``n_s`` spanwise divisions per section and ``n_c`` chordwise divisions meant for lifting-line/vortex lattice analyses using horseshoe elements.
"""
function mesh_horseshoes(wing :: Wing, span_num, chord_num, spacings = ["cosine"])
    left_panels  = mesh_horseshoes(left(wing), reverse(span_num), chord_num, reverse(spacings), flip = true)
    right_panels = mesh_horseshoes(right(wing), span_num, chord_num, spacings)

    [ left_panels right_panels ]
end

"""
    mesh_wing(wing :: Wing, n_s :: Integer, n_c :: Integer)

Mesh a `Wing` into panels of ``n_s`` spanwise divisions per section and ``n_c`` chordwise divisions meant for 3D analyses using doublet-source elements or equivalent formulations. TODO: Tip meshing.
"""
function mesh_wing(wing :: Wing, span_num, chord_num)
    left_panels  = mesh_wing(left(wing), reverse(span_num), chord_num, flip = true)
    right_panels = mesh_wing(right(wing), span_num, chord_num)

    [ left_panels right_panels ]
end

"""
    mesh_cambers(wing :: Wing, n_s :: Integer, n_c :: Integer)

Mesh the camber distribution of a `Wing` into panels of ``n_s`` spanwise divisions per section and ``n_c`` chordwise divisions meant for vortex lattice analyses.
"""
function mesh_cambers(wing :: Wing, span_num, chord_num, spacings = ["cosine"])
    left_panels  = mesh_cambers(left(wing), reverse(span_num), chord_num, reverse(spacings), flip = true)
    right_panels = mesh_cambers(right(wing), span_num, chord_num, spacings)

    [ left_panels right_panels ]
end

vlmesh_wing(wing :: Union{HalfWing, Wing}, span_num, chord_num, spacings = ["cosine"]) = mesh_horseshoes(wing, span_num, chord_num, spacings), mesh_cambers(wing, span_num, chord_num, spacings)

function paneller(wing :: Union{Wing, HalfWing}, span_num :: Union{Integer, Vector{<: Integer}}, chord_num :: Integer; rotation = one(RotMatrix{3, Float64}), translation = zeros(3), spacings = ["cosine"])
    horseshoes, cambers = vlmesh_wing(wing, ifelse(typeof(span_num) <: Integer, [span_num], span_num), chord_num, spacings)
    horseshoe_panels    = [ transform(panel, rotation, translation)               for panel in horseshoes ]
    panel_normals       = [ panel_normal(transform(panel, rotation, translation)) for panel in cambers 	  ]

    horseshoe_panels, panel_normals
end

panel_wing(comp :: Union{Wing, HalfWing}, span_panels :: Union{Integer, Vector{<: Integer}}, chord_panels :: Integer; position = zeros(3), angle = 0., axis = [1., 0., 0.], spacing = spanwise_spacing(comp)) = paneller(comp, span_panels, chord_panels, rotation = AngleAxis{Float64}(angle, axis...), translation = position, spacings = ifelse(typeof(spacing) <: String, [spacing], spacing))

number_of_spanwise_panels(wing :: HalfWing, span_num :: Integer) = ceil.(Int, span_num .* spans(wing) / span(wing))
number_of_spanwise_panels(wing :: Wing,     span_num :: Integer) = number_of_spanwise_panels(right(wing), ceil(Int, span_num / 2))

function number_of_spanwise_panels(wing :: HalfWing, span_num :: Vector{<: Integer})
    @assert (length ∘ spans)(wing) > 1 "Provide an integer number of spanwise panels for 1 wing section."
    span_num
end

function number_of_spanwise_panels(wing :: Wing, span_num :: Vector{<: Integer})
    @assert (length ∘ spans ∘ right)(wing) > 1 "Provide an integer number of spanwise panels for 1 wing section."
    ceil.(Int, span_num ./ 2)
end

spanwise_spacing(wing :: HalfWing) = [ "sine"; fill("cosine", (length ∘ spans)(wing)         - 1) ]
spanwise_spacing(wing :: Wing)     = [ "sine"; fill("cosine", (length ∘ spans ∘ right)(wing) - 1) ]

coordinates(wing :: Union{HalfWing, Wing}) = let (lead, trail) = wing_bounds(wing); permutedims([ lead trail ]) end

function coordinates(wing :: HalfWing, span_num, chord_num; span_spacing = ["cosine"], flip = false)
    lead, trail = wing_bounds(wing, flip)
    reduce(hcat, chop_wing(lead, trail, span_num, chord_num; span_spacing = span_spacing, flip = flip))
end

coordinates(wing :: Wing, span_num, chord_num; span_spacing = ["cosine"], rotation = one(RotMatrix{3, Float64}), translation = zeros(3)) = [ coordinates(left(wing), span_num, chord_num; span_spacing = span_spacing, flip = true)[:,1:end-1] coordinates(right(wing), span_num, chord_num; span_spacing = span_spacing) ]


# DTU02435 Decision Making under Uncertainty
# Assignment 3, Task 2
# Solve by heuristics and Benders Algorithm / Gurobi
# Edward J. Xu
# April 27th, 2019
push!(LOAD_PATH, "$(homedir())/Desktop/optim")
cd("$(homedir())/Desktop/optim")
using JuMP
using GLPKMathProgInterface
using Gurobi
using CSV
using DataFrames
using BlackBoxOptim

function processData()
    datf_numMax = CSV.read("MaxPlanes.csv", header = ["p", "num"], delim = ',')
    datf_cost = CSV.read("OperationCosts2.csv", header = true, delim = ',')
    datf_cap = CSV.read("PlaneCapacity.csv", header = ["p", "cap"], delim = ',')
    datf_route = CSV.read("DemandsRoute.csv", header = true, delim = ',')
    datf_costAdmin = CSV.read("CostAdquisition.csv", header = ["p", "cost"], delim = ',')
    datf_prob = CSV.read("Probability.csv", header = ["prob"], delim = ',')
    #
    datf_demand = CSV.read("Scenario.csv", header = true, delim = ',')
    mat4_demand = zeros(4, 5, 12, 10)
    for i = 1: 4
        for j = 1: 5
            for w = 1: 10
                mat4_demand[i, j, :, w] = datf_demand[w] * datf_route[i, j+1]
            end
        end
    end
    return (datf_numMax.num, datf_cost, datf_cap.cap, datf_costAdmin.cost, mat4_demand, datf_prob.prob)
end

function solve_sub(vec_yBar, datf_cost, mat_demand, vec_cap)
    model_sub = Model(solver = GurobiSolver(Presolve = 0, OutputFlag = 0, gurobi_env))
    # model = Model(solver = GLPKSolverMIP())
    @variable(model_sub, mat3_z[1: 4, 1: 5, 1: 10] >= 0, Int)
    @variable(model_sub, mat_m[1: 4, 1: 5] >= 0)
    @objective(model_sub, Min, sum([datf_cost.cost[.&((datf_cost[:i] .== i), (datf_cost[:j] .== j),
        (datf_cost[:p] .== p))][1] * mat3_z[i, j, p] + mat_m[i, j] * 1000] for p = 1: 10, i = 1: 4,
        j = 1: 5)[1])
    @constraint(model_sub, [i = 1: 4, j = 1: 5], mat_demand[i, j] -
        sum([mat3_z[i, j, p] * vec_cap[p]] for p = 1: 10)[1] <= mat_m[i, j])
    @constraint(model_sub, [p = 1: 10], sum([mat3_z[i, j, p]] for i = 1: 4, j = 1: 5)[1] <=
        vec_yBar[p])
    solve(model_sub)
    obj = getobjectivevalue(model_sub)
    vec_result_z = getvalue(mat3_z)
    vec_result_m = getvalue(mat_m)
    return (obj, vec_result_z, vec_result_m)
end

function solve_sub_all(vec_yBarRaw; datf_cost = datf_cost, vec_cap = vec_cap, vec_costAdmin = vec_costAdmin,
        mat4_demand = mat4_demand, vec_prob = vec_prob)
# function solve_sub_all(vec_yBarRaw)
    vec_yBar = round.(vec_yBarRaw)
    println("--------------------------------------------------------------------------------\n",
            "------------------------------ Solve Sub-Problem -------------------------------\n",
            "Input: vec_yBarRaw = $(vec_yBarRaw)\n",
            "Input: vec_yBar = $(vec_yBar)\n",
            "--------------------------------------------------------------------------------\n")
    mat_obj = zeros(10, 12)
    for w = 1: 10
        for t = 1: 12
            println("Input: w = $(w), t = $(t), step = $(12 * (w - 1) + t) / 120 ")
            (mat_obj[w, t], mat_result_z, mat_result_m) = solve_sub(
                vec_yBar, datf_cost, mat4_demand[:, :, t, w], vec_cap)
        end
    end
    obj = sum([vec_yBar[p] * vec_costAdmin[p] + vec_prob[w] * mat_obj[w, t]] for p = 1: 10, w = 1: 5, t = 1: 12)[1]
    println("--------------------------------------------------------------------------------\n",
            "Result: obj = $(obj)\n",
            "--------------------------------------------------------------------------------\n")
    return obj
end

function main()
    # Begin Optimization
    println("################################################################################\n",
            "############################ 1/2. Begin Optimization ###########################\n",
            "################################################################################\n")
    mod_master = bboptimize(solve_sub_all; SearchRange = [(0.0, 12.0), (0.0, 12.0), (0.0, 12.0), (0.0, 12.0),
        (0.0, 12.0), (0.0, 12.0), (0.0, 12.0), (0.0, 8.0), (0.0, 8.0), (0.0, 8.0)], NumDimensions = 10,
        MaxTime = false)
    println("################################################################################\n",
            "############################## 2/2. Nominal Ending #############################\n",
            "################################################################################\n")
    # Print the Result
    # println("Result: obj = $(obj).")
    # println("Result: vec_y = $(vec_result_y).")
    # println("Result: mat5_z = $(vec_result_z).")
    # println("Result: mat4_m = $(vec_result_m).")
    # return (vec_result_z, vec_result_m)
end

const gurobi_env = Gurobi.Env()
const (vec_numMax, datf_cost, vec_cap, vec_costAdmin, mat4_demand, vec_prob) = processData()
main()


using AbstractGPs
using TemporalGPs
using Distributions
using StatsFuns: logistic

# In this example we are showing how to work with non-Gaussian likelihoods,
# here a binary classification problem with a logistic link,
# by using variable augmentations as in [Galy-Fajou et al, 2020](https://arxiv.org/abs/2002.114510).
# We base our example on the "exact_time_inference" example

# Load up the separable kernel from TemporalGPs.
using TemporalGPs: RegularSpacing

# Build a GP as per usual, and wrap it inside a TemporalGPs.jl object.
f_raw = GP(Matern52Kernel());
f = to_sde(f_raw, SArrayStorage(Float64));

# Specify a collection of inputs. Must be increasing.
T = 1_000;
x = RegularSpacing(0.0, 1e-1, T);

# Generate some synthetic data from the GP and get a random binary output
σ²_noise = 0.01;
f_true = rand(f(x, σ²_noise));
y = rand.(Bernoulli.(logistic.(f_true)));
y_sign = sign.(y .- 0.5)

# We are using the augmentation trick and look for the optimal
# expected value of the variable ω

η₁_like(ω, g, β) = g .+ β .* ω # The augmented likelihood first natural parameter
Λ_like(ω, γ) = 2 * γ * ω # The augmented likelihood precision (diagonal)

# These are the values corresponding to our problem
γ = 0.5
g = 0.5 * y_sign
β = 0

function compute_optimal_expectation(f, x, g, β, γ; n_iter=5)
    T = length(x)
    ω̄ = rand(T) # preallocation
    c = zeros(T) # preallocation
    for i in 1:n_iter
        Λ = Λ_like(ω̄, γ)
        p_f = marginals(posterior(f(x, inv.(Λ)), inv.(Λ) .* η₁_like(ω̄, g, β))(x))
        @. c = sqrt(var(p_f) + abs2(mean(p_f))) / 2
        @. ω̄ = 0.5 * tanh(c) / c
    end
    return ω̄
end

ω̄ = compute_optimal_expectation(f, x, g, β, γ)
Λ = Λ_like(ω̄, γ)
f_post = posterior(f(x, inv.(Λ)), inv.(Λ) .* η₁_like(ω̄, g, β))

# Specify some locations at which to make predictions.
T_pr = 1_200_000;
x_pr = RegularSpacing(0.0, 1e-4, T_pr);

# Compute the exact posterior marginals at `x_pr`.
f_post_marginals = marginals(f_post(x_pr));
m_post_marginals = mean.(f_post_marginals);
σ_post_marginals = std.(f_post_marginals);

# Generate a few posterior samples. Not fantastically-well optimised at present.
f_post_samples = [rand(f_post(x_pr)) for _ in 1:5];

# Visualise the posterior. The if block is just to prevent it running in CI.
if get(ENV, "TESTING", "FALSE") == "FALSE"
    using Plots
    plt = plot()
    plot!(plt, f_post(x_pr); ribbon_scale=3.0, label="");
    plot!(plt, x_pr, f_post_samples; color=:red, alpha=0.3, label="");
    plot!(plt, x, f_true; label="", lw=2.0, color=:blue); # Plot the true latent GP on top
    scatter!(plt, x, y; label="", markersize=1.0, alpha=1.0); # Plot the data
    savefig(plt, "posterior.png");
end


type DoubleDescription{T<:Real}
    A::Matrix{T}
    R::Vector
    K::Set{Int}
    adj::Dict{(Int,Int),Bool}
    num_rays::Int
end

type CountedVector{T<:Real}
    v::Vector{T}
    Av::Vector{T}
    dd::DoubleDescription{T}
    id::Int

    function CountedVector(v::Vector{T},dd::DoubleDescription{T})
        dd.num_rays += 1
        canonicalize!(v)
        new(v,dd.A*v,dd,dd.num_rays)
    end
end
CountedVector{T<:Real}(v::Vector{T},dd::DoubleDescription{T}) = CountedVector{T}(v,dd)

Base.vec(c::CountedVector) = c.v

function initial_description{T<:Real}(A::Matrix{T})
    m,n = size(A)
    B = rref(A')
    # find pivots
    r = 1
    K = Set{Int}()
    for i in 1:m
        comp = zeros(n)
        comp[r] = 1
        if B[:,i] == comp
            r += 1
            push!(K, i)
        end
        r > n && break
    end
    cK = sort(collect(K))
    Ak = A[cK,:]
    R = Ak \ eye(n,n)
    dd = DoubleDescription(A,CountedVector{T}[],K,Dict{(Int,Int),Bool}(),n)
    Rk = [CountedVector{T}(R[:,i],dd) for i in 1:n]
    dd.R = kR
    for i in 1:n
        # Ar = Rk[i].Av[cK]
        for j in (i+1):n
            # As = Rk[j].Av[cK]
            id = extrema([Rk[i].id,Rk[j].id])
            cache_adjacency!(dd, n, Rk[i].Av[cK], Rk[j].Av[cK], id)
        end
    end
    return dd
end

function double_description{T<:Real}(A::Matrix{T})
    A = [zeros(T,1,size(A,2)); A]
    A[1,1] = one(T)
    m, n = size(A)
    dd = initial_description(A)
    Kᶜ = setdiff(1:m, dd.K)
    while !isempty(Kᶜ)
        i = pop!(Kᶜ)
        println("iteration $(length(dd.K)+1) of $m")
        # length(dd.K) == 31 && break
        update!(dd, i)
    end
    return dd
end

function is_approx_included(haystack, needle)
    n = length(needle)
    diff = zeros(n)
    for h in haystack
        diff = norm(vec(h)-needle)
        diff < n*ε && return true
    end
    return false
end

function canonicalize!{T<:Real}(v::Vector{T})
    n = length(v)
    val = abs(v[1])
    if val < ε
        val = abs(v[findfirst(abs(v) .> ε)])
    end
    for i in 1:n
        v[i] = v[i] / val
    end
    v
end

# use Lemma 8 from Fukuda (1996) to update the double description
function update!{T<:Real}(dd::DoubleDescription{T}, i)
    m, n = size(dd.A)
    Aᵢ = reshape(dd.A[i,:], (n,))
    Rⁿᵉʷ = CountedVector{T}[]
    R⁺, R⁰, R⁻ = partition_rays(dd.R, Aᵢ)
    for r in R⁺, s in R⁻
        if isadjacent(dd,r,s)
            w = dot(Aᵢ,vec(r))*vec(s) - dot(Aᵢ,vec(s))*vec(r)
            v = CountedVector(w,dd)
            if sum(abs(w)) > n*ε &&
               !is_approx_included(R⁰,   vec(w)) &&
               !is_approx_included(Rⁿᵉʷ, vec(w))
                dd.num_rays += 1
                push!(Rⁿᵉʷ, v)
            end
        end
    end
    dd.R = vcat(R⁺, R⁰, Rⁿᵉʷ)
    push!(dd.K, i)
    cK = sort(collect(dd.K))
    Ak = dd.A[cK,:]
    # should really add a test right about here to ensure
    # that old rays do not become adjacent...I think this
    # can only happen if both v,w ∈ R⁰
    d = rank(Ak)
    for s in Rⁿᵉʷ
        # As = s.Av[cK]#Ak*vec(s)
        for r in dd.R
            r.id == s.id && continue
            # Ar = r.Av[cK]#Ak*vec(r)
            id = extrema([r.id, s.id])
            cache_adjacency!(dd, d, r.Av[cK], s.Av[cK], id)
        end
    end
    nothing
end

function partition_rays{T<:Real}(R::Vector{CountedVector{T}}, a::Vector{T})
    R⁺, R⁰, R⁻ = CountedVector{T}[], CountedVector{T}[], CountedVector{T}[]
    n = length(a)
    for r in R
        if sum(abs(vec(r))) < n*ε
            println("have a zero vector!")
            continue
        end
        val = dot(a,vec(r))
        if val > ε
            push!(R⁺, r)
        elseif val < -ε
            push!(R⁻, r)
        else
            push!(R⁰, r)
        end
    end
    return R⁺, R⁰, R⁻
end

isadjacent(dd, r, s) = dd.adj[extrema([r.id,s.id])]

function cache_adjacency!(dd, d, Ar, As, id::(Int,Int))
    Z = active_sets(dd, Ar, As)
    if length(Z) < d - 2
        val = false
    elseif length(intersect(Z,dd.K)) ≥ d - 2
        val = true
    else
        val = (rank(dd.A[sort(collect(dd.K)),:][Z,:]) == d - 2)
    end
    dd.adj[id] = val
    return val
end

function active_sets(dd, Ar, As)
    Z = Int[]
    m = length(Ar)
    sizehint!(Z,m)
    for i in 1:m
        if abs(Ar[i]) ≤ ε && abs(As[i]) ≤ ε
            push!(Z, i)
        end
    end
    return Z
end


# rational number reconstruction implementation borrowed from CLUE
# and modified a bit to suit the 'Modern Computer Algebra' definitions
# returns a rational r // h of QQ field in a canonical form such that
#   r // h ≡ a (mod m)
#
# let n = max( λ(a), λ(m) ) , where λ(x) is a number of bits for x
# O(n^2)
"""
    rational_reconstruction(a, m)
Rational number reconstruction implementation borrowed from CLUE
and modified a bit to suit the 'Modern Computer Algebra' definitions.
Returns a rational of QQ field in a canonical form that
is congruent a modulo m
a, m are integers
"""
function rational_reconstruction(a::I, m::I) where {I<:Union{Int, BigInt}}
    # restricting to I<:Union{Int, BigInt} for compatibility reasons

    a = mod(a, m)
    if a == 0 || m == 0
        return Nemo.QQ(0, 1)
    end
    if m < 0
        m = -m
    end
    if a < 0
        a = m - a
    end
    if a == 1
        return Nemo.QQ(1, 1)
    end
    bnd = sqrt(float(m) / 2)

    U = I[1, 0, m]
    V = I[0, 1, a]
    while abs(V[3]) >= bnd
        q = div(U[3], V[3])
        T = U .- q .* V
        U = V
        V = T
    end

    t = abs(V[2])
    r = V[3] * sign(V[2])
    # changed from `<= bnd` to `<= m / bnd`
    if t <= m / bnd && gcd(r, t) == 1
        return Nemo.QQ(r, t)
    end

    throw(DomainError(
        :($a//$m), "rational reconstruction of $a (mod $m) does not exist"
    ))
end

### Several ad hoc functions for reconstruction

function rational_reconstruction(f::Nemo.gfp_elem, m)
    rational_reconstruction(BigInt(f.data), m)
end

function rational_reconstruction(f::Nemo.fmpz, m)
    rational_reconstruction(BigInt(f), m)
end

function rational_reconstruction(f::AbstractAlgebra.GFElem{T}, m) where {T}
    rational_reconstruction(AbstractAlgebra.lift(f), m)
end

function rational_reconstruction(f::Union{MPoly, gfp_mpoly, fmpz_mpoly}, m)
    map_coefficients(c -> rational_reconstruction(c, m), f)
end

function rational_reconstruction(f::Frac, m)
    q1 = rational_reconstruction(numerator(f), m)
    q2 = rational_reconstruction(denominator(f), m)
    q1 // q2
end

function rational_reconstruction(A::Array, m)
    map(c -> rational_reconstruction(c, m), A)
end

function rational_reconstruction(f::Singular.n_Zp, m)
    rational_reconstruction(BigInt(Int(f)), m)
end

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

"""
    Coerces the given rational field element into the given finite field
"""
function modular_reduction(x, field)
    n, d = field(Int(numerator(x))), field(Int(denominator(x)))
    if iszero(d)
        throw(DomainError(
            :($x), "modular reduction of $x (to $field) does not exist"
        ))
    end
    n // d
end

### Reduction ad hoc functions

function modular_reduction(f::Union{MPoly, fmpq_mpoly}, field)
    map_coefficients(c -> modular_reduction(c, field), f)
end

function modular_reduction(f::Frac, field)
    modular_reduction(numerator(f), field) //
        modular_reduction(denominator(f), field)
end

function modular_reduction(x::gfp_fmpz_elem, field)
    return field(convert(BigInt, x))
end

function modular_reduction(x::gfp_elem, field)
    return field(x)
end

function modular_reduction(arr::Array, field)
    map(f -> modular_reduction(f, field), arr)
end

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

"""
    Chinese remainder algorithm
    (Eucleadean one. Ii there a Gauss version of the algorithm btw?..)
"""
function CRT(moduli, remainders)
    """
    The function implements the CR Algorithm mentioned as Algorithm 5.4
    in  'Modern Computer Algebra', 2nd edition,
        Joachim von zur Gathen and Jurgen Gerhard
    """

    m = prod(moduli)

    ans = BigInt(0)
    for (i, (mi, vi)) in enumerate(zip(moduli, remainders))
        mi_hat = divexact(m, mi)
        _, s, t = Nemo.gcdx(mi_hat, mi)
        ans = BigInt(mod(ans + vi * s * mi_hat, m))
    end

    ans
end

"""
    Reconstructs the ultimate Groebner basis with coefficients
    obtained from applying CRT to the given bases coeffs and moduli
"""
function CRT_reconstruction(groebner_bases, moduli)
    onegb = first(groebner_bases)
    onepoly = first(onegb)

    basepolyring = base_ring(base_ring(onepoly))
    yoverx = parent(onepoly)

    ystring = string.(symbols(yoverx))
    xstring = string.(symbols(basepolyring))

    ground = Nemo.ZZ
    inner_ring, = Nemo.PolynomialRing(ground, xstring)
    outer_ring, = Nemo.PolynomialRing(FractionField(inner_ring), ystring)

    npolys = length(onegb)
    ncoeffs = map(length, onegb)

    new_basis = []

    for (polyidx, poly) in zip(1:npolys, onegb)
        outer_f = deepcopy(poly)
        outer_builder = AbstractAlgebra.MPolyBuildCtx(outer_ring)

        for (outer_i, outer_e) in zip(1:ncoeffs[polyidx], exponent_vectors(poly))
            inner_frac = coeff(poly, outer_e)
            num_and_denom = [zero(inner_ring), zero(inner_ring)]

            for (jk, locator) in enumerate([numerator, denominator])
                inner_f = locator(inner_frac)
                inner_builder = AbstractAlgebra.MPolyBuildCtx(inner_ring)

                for (inner_i, inner_e) in enumerate(exponent_vectors(inner_f))
                    oldcoeffs = [
                        coeff(locator(coeff(gb[polyidx], outer_e)), inner_e)
                        for gb in groebner_bases
                    ]

                    newcoeff = CRT(moduli, map(x -> BigInt(x.data), oldcoeffs))
                    push_term!(inner_builder, newcoeff, inner_e)
                end

                num_and_denom[jk] = finish(inner_builder)

            end

            num, denom = num_and_denom
            push_term!(outer_builder, num // denom, outer_e)
        end

        push!(new_basis, finish(outer_builder))
    end

    new_basis
end


using Luxor
using Test

function checkpoints(points, hull)
    flag  = true
    for pt in points
        if ! isinside(pt, offsetpoly(hull, 0.1), allowonedge=true)
            flag = false
        end
    end
    return flag
end

function testpolyhull1(fname)
    Drawing(800, 1200, fname)
    origin()
    setopacity(0.5)
    tiles = Tiler(600, 600, 4, 4)
    for (pos, n) in tiles
        @layer begin
            translate(pos)
            if n == 1
                pts = box(O, 100, 100, vertices=true)
            elseif n == 2
                pts = ngon(O, 60, 3, vertices=true)
            elseif n == 3
                pts = randompointarray(BoundingBox(box(O, 100, 100, vertices=true)), 25)
            elseif n == 4
                pts = star(O, 60, 5, 0.2, vertices=true)
            else
                pts = ngon(O, 60, n, vertices=true)
            end
            hull = polyhull(pts)
            @test checkpoints(pts, hull)
            sethue("black")
            setline(10)
            poly(pts, :fill, close=true)
            sethue("red")
            setline(2)
            poly(hull, :stroke, close=true)
        end
    end
    @test finish() == true
    println("...finished test: output in $(fname)")
end

function testpolyhull2(fname)
    Drawing(800, 1200, fname)
    origin()
    setopacity(0.5)
    points = randompointarray(BoundingBox(), 30)
    while length(points) > 2
        hull = polyhull(points)
        randomhue()
        prettypoly(hull, close=true, :fill)
        # discard this hull and repeat
        points = setdiff(points, hull)
    end
    @test finish() == true
    println("...finished test: output in $(fname)")
end

testpolyhull1("polyhull-test-1.png")
testpolyhull2("polyhull-test-2.png")


@testset "SimpleCodec" begin
    sc = SimpleCodec(x -> x .^ 2, x -> sqrt.(x))
    @test encode(sc, 3) == 9
    @test encode(sc, [1, 2]) == [1, 4]
    @test decode(sc, 100) == 10
    @test decode(sc, [1, 9]) == [1, 3]
end


using StructuresKit

#Verify Julia solution against Mathematica solution from
#Example 2 from Plaut, R.H., Moen, C.D.(2020). "Lateral-Torsional Deformations of Single-Span and Two-Span Thin-Walled Beams with Continuous Bracing".
#Journal of Constructional Steel Research.

#Z-section, no slope, single span, fixed-fixed
#kϕ=1500 N*mm/rad/mm, kx=0.1 N/mm^2, gravity load
#q = 1 kN/m
#Check again Figure 14 in the Plaut and Moen (2020) manuscript.
#  max ϕ

plaut_solution = 0.0164

#*********** Julia solution

#inputs

#Ix Iy Ixy J Cw
section_properties = [(3.230E6,449530,-865760, 397.09, 3.4104E9)]

#E  ν
material_properties = [(200,0.30)]

#ax ay
load_location = [(27.826,101.6)]

#kx kϕ
spring_stiffness = [(0.1/1000, 1500/1000)]

#ay_kx
spring_location = [(101.6)]

#member information
#L dL SectionProperties MaterialProperties LoadLocation SpringStiffness spring_location
member_definitions = [(7620,7620/48,1,1,1,1,1)]

#end boundary conditions
#type=1 u''=v''=ϕ''=0 (simply supported), type=2 u'=v'=ϕ'=0  (fixed), type=3 u''=v''=ϕ''=u'''=v'''=ϕ'''=0 (free end, e.g., a cantilever)
end_boundary_conditions = [2 2]

#supports
#location where u=v=ϕ=0
supports = [0.0 7620]

load = (0.0, 5.0/1000)  #kN/mm

z, u, v, ϕ, beam_properties = ThinWalledBeam.solve(member_definitions, section_properties, material_properties, spring_stiffness, spring_location, supports, load, load_location, end_boundary_conditions)

ϕmax = maximum(ϕ)

#calculate percent error as compared to Mathematica solution
error= abs.((ϕmax .- plaut_solution))./ plaut_solution

#accept 1% error from numerical solution
@test error <= 0.01


struct SARAH_basic_iterable{R<:Real,C<:RealOrComplex{R},Tx<:AbstractArray{C},Tf}
    F::Array{Tf}            # smooth term - array of f_i s
    x0::Tx                  # initial point
    N::Int                  # of data points in the finite sum problem
    L::Maybe{Union{Array{R},R}}  # Lipschitz moduli of ∇f_i
    μ::Maybe{Union{Array{R},R}}  # convexity moduli of the gradients
    γ::Maybe{R}             # stepsize (a single scalar)
    m::Maybe{Int}           # number of inner loop updates
end

mutable struct SARAH_basic_state{R<:Real,Tx}
    γ::R                    # stepsize
    m::Maybe{Int}           # number of inner loop updates
    av::Tx                  # average of ∇f_i s in outer loop
    w::Tx                   # w_{t-1} in the inner loop
    w_plus::Tx              # w_{t} in the inner loop
    ind::Array{Int}         # running idx set (1 to N)

    # some extra placeholders
    ∇f_temp::Tx             # placeholder for gradients
    temp::Tx                # placeholder for gradients
end

function SARAH_basic_state(γ::R, m, av::Tx, w::Tx, w_plus::Tx, ind) where {R,Tx}
    return SARAH_basic_state{R,Tx}(γ, m, av, w, w_plus, ind, copy(av), copy(av))
end

function Base.iterate(iter::SARAH_basic_iterable{R}) where {R} #? we doesn't need other types like C, Tx, ...?
    N = iter.N
    m = iter.m
    ind = collect(1:N) # a vector of 1 to N
    # updating the stepsize
    if iter.γ === nothing
        if iter.μ === nothing # means it is nonconvex
            if iter.L === nothing
                @warn "smoothness parameter absent"
                return nothing
            else
                L_M = maximum(iter.L) # bug prone: double-check this
                # γ = 2 / (L_M * (1+sqrt(1+4*iter.m))) # bug prone: double-check this
                γ = 0.5 / L_M
            end
        elseif maximum(iter.μ) == 0 # convex
            @warn "convex problem"
        else
            @warn "strongly convex problem"
        end
    else
        γ = iter.γ # provided γ
    end
    # initializing the vectors

    av = zero(iter.x0) # mu_tilde in the outer loop
    for i = 1:N
        ∇f, ~ = gradient(iter.F[i], iter.x0)
        ∇f ./= N
        av .+= ∇f
    end
    w = iter.x0
    w_plus = w - γ * av

    state = SARAH_basic_state(γ, m, av, w, w_plus, ind)
    return state, state
end

function Base.iterate(iter::SARAH_basic_iterable{R}, state::SARAH_basic_state{R}) where {R}
    # The inner cycle
    for i in rand(state.ind, state.m)
        gradient!(state.temp, iter.F[i], state.w_plus)
        gradient!(state.∇f_temp, iter.F[i], state.w)
        state.av .+= state.temp
        state.av .-= state.∇f_temp

        state.temp .= state.av
        state.temp .*= state.γ
        state.w .= state.w_plus
        state.w_plus .-= state.temp
    end
    # full update
    state.w .= state.w_plus
    state.av .= zero(state.w)  # for next iterate
    for i = 1:iter.N
        gradient!(state.∇f_temp, iter.F[i], state.w)
        state.∇f_temp ./= iter.N
        state.av .+= state.∇f_temp
    end
    state.temp .= state.av
    state.temp .*= state.γ
    state.w_plus .-= state.temp

    return state, state
end

solution(state::SARAH_basic_state) = state.w_plus


using SequentialMonteCarlo
using RNGPool
using SMCExamples.LinearGaussian: LGTheta, Float64Particle, kalmanlogZ,
  defaultLGModel, makeLGModel
import MonteCarloMarkovKernels: simulateChainProgress, simulateChain,
  makeAMKernel, kde, estimateBM
using StaticArrays
using StatsBase
using LinearAlgebra
using Random
using Plots
Plots.gr()

setRNGs(0)
lgModel, theta, ys, ko = defaultLGModel(100)

const truex0 = theta.x0
const truev0 = theta.v0
const trueC = theta.C

@inline function toLGTheta(v::SVector{3, Float64})
  return LGTheta(v[1], v[2], trueC, v[3], truex0, truev0)
end

t0 = SVector{3, Float64}(theta.A, theta.Q, theta.R)
const sigmaProp = SMatrix{3, 3, Float64}(Matrix{Float64}(I, 3, 3))

@inline function lglogprior(theta::LGTheta)
  if theta.A < 0 || theta.A > 10 return -Inf end
  if theta.Q < 0 || theta.Q > 10 return -Inf end
  if theta.R < 0 || theta.R > 10 return -Inf end
  return 0.0
end

function makelgsmcltd(ys::Vector{Float64}, N::Int64, nthreads::Int64)
  smcio = SMCIO{Float64Particle, Nothing}(N, length(ys), nthreads, false, 0.5)
  function ltd(in::SVector{3, Float64})
    theta::LGTheta = toLGTheta(in)
    lp::Float64 = lglogprior(theta)
    if lp == -Inf return -Inf end
    model::SMCModel = makeLGModel(theta, ys)
    smc!(model, smcio)
    return smcio.logZhats[length(ys)]
  end
end

function makelgkalmanltd(ys::Vector{Float64})
  function ltd(in::SVector{3, Float64})
    theta::LGTheta = toLGTheta(in)
    lp::Float64 = lglogprior(theta)
    if lp == -Inf return -Inf end
    return kalmanlogZ(theta, ys)
  end
end

logtargetSMC = makelgsmcltd(ys, 1024, Threads.nthreads())
logtargetKalman = makelgkalmanltd(ys)

PSMC = makeAMKernel(logtargetSMC, sigmaProp)
PKalman = makeAMKernel(logtargetKalman, sigmaProp)

Random.seed!(12345)
chainSMC = simulateChainProgress(PSMC, t0, 2^15)
sar = PSMC(:acceptanceRate)

@time chainKalman = simulateChain(PKalman, t0, 2^20)
kar = PKalman(:acceptanceRate)

savefigures = false

vsKalman = (i->(x->x[i]).(chainKalman)).(1:3)
vsSMC =  (i->(x->x[i]).(chainSMC)).(1:3)

plot(kde(vsSMC[1], sar))
plot!(kde(vsKalman[1], kar))
savefigures && savefig("pmmh_kde1.png")

plot(kde(vsSMC[2], sar))
plot!(kde(vsKalman[2], kar))
savefigures && savefig("pmmh_kde2.png")

plot(kde(vsSMC[3], sar))
plot!(kde(vsKalman[3], kar))
savefigures && savefig("pmmh_kde3.png")

contour(kde(vsSMC[1], vsSMC[2], sar))
contour!(kde(vsKalman[1], vsKalman[2], kar))
savefigures && savefig("pmmh_kde12.png")

contour(kde(vsSMC[1], vsSMC[3], sar))
contour!(kde(vsKalman[1], vsKalman[3], kar))
savefigures && savefig("pmmh_kde13.png")

contour(kde(vsSMC[2], vsSMC[3], sar))
contour!(kde(vsKalman[2], vsKalman[3], kar))
savefigures && savefig("pmmh_kde23.png")

plot(autocor(vsSMC[1]))
plot!(autocor(vsKalman[1]))
savefigures && savefig("pmmh_acf1.png")
plot(autocor(vsSMC[2]))
plot!(autocor(vsKalman[2]))
savefigures && savefig("pmmh_acf2.png")
plot(autocor(vsSMC[3]))
plot!(autocor(vsKalman[3]))
savefigures && savefig("pmmh_acf3.png")

estimateBM.(vsSMC)
estimateBM.(vsKalman)


module Bravais

using Formatting,
RecipesBase

export BravaisLattice,
bravais_coordinates_unit_cell,
bravais_coordinates_unit_cell!,
bravais_coordinates

"""
    lattice = BravaisLattice(; a=Inf, α=0, b=Inf, β=π/2, x0=0, y0=0)

bravais lattice with:

- `a` is the length of the first vector (by default aligned along x-axis).

- `b` is length of second vector.

- `α` is the angle of first primitive vector.

- `β` is the angle of second primitive vector

- `x0`, `y0` define the origin of the lattice.
"""
struct BravaisLattice
    a::Float64
    b::Float64
    α::Float64
    β::Float64
    x0::Float64
    y0::Float64

    sin²θ::Float64
    cosθ::Float64
    R::Array{Float64,2}#SArray{Tuple{2,2},Float64,2,4}#
    Rᵀ::Array{Float64,2}#SArray{Tuple{2,2},Float64,2,4}#
    v1::Array{Float64,1}#SArray{Tuple{2},Float64,1,2}#
    v2::Array{Float64,1}#SArray{Tuple{2},Float64,1,2}#

    function BravaisLattice(;a::Number=Inf, α::Number=0, b::Number=Inf, β::Number=π/2, x0::Number=0, y0::Number=0)
        θ = β-α
        if iszero(θ)
            throw(ArgumentError("lattice angle θ=$(θ) cannot be zero"))
        end
        sinθ, cosθ = sincos(θ)
        # R = @SMatrix [ cos(α) -sin(α); sin(α)  cos(α)]
        R = [ cos(α) -sin(α); sin(α)  cos(α)]
        Rᵀ = transpose(R)
        # v1 = R*SVector(1,0)
        v1 = R*[1,0]
        # v2 = R*SVector(cosθ,sinθ)
        v2 = R*[cosθ,sinθ]
        new(float(a), float(b), float(α), float(β), float(x0), float(y0),
            sinθ^2, cosθ, R, Rᵀ, v1, v2)
    end

    function Base.show(io::IO, bvl::BravaisLattice)
        if !get(io, :sub, false)
            print(io, typeof(bvl), ": \n",
            "\tprimitive vector 1: ", fmt("3.2f",bvl.a), ", ∠", fmt("3.2f",(mod2pi(bvl.α))*180/pi), "°\n",
            "\tprimitive vector 2: ", fmt("3.2f",bvl.b), ", ∠", fmt("3.2f",(mod2pi(bvl.β))*180/pi), "°\n",
            "\torigin: (", fmt("3.2f",bvl.x0), ", ", fmt("3.2f",bvl.y0), ")")
        elseif !get(io, :sub1, false)
            print(io,
            "\n\tprimitive vector 1: ", fmt("3.2f",bvl.a), ", ∠", fmt("3.2f",(mod2pi(bvl.α))*180/pi), "°\n",
            "\tprimitive vector 2: ", fmt("3.2f",bvl.b), ", ∠", fmt("3.2f",(mod2pi(bvl.β))*180/pi), "°\n",
            "\torigin: (", fmt("3.2f",bvl.x0), ", ", fmt("3.2f",bvl.y0), ")")
        else
            print(io,
            "\n\t\tprimitive vector 1: ", fmt("3.2f",bvl.a), ", ∠", fmt("3.2f",(mod2pi(bvl.α))*180/pi), "°\n",
            "\t\tprimitive vector 2: ", fmt("3.2f",bvl.b), ", ∠", fmt("3.2f",(mod2pi(bvl.β))*180/pi), "°\n",
            "\t\torigin: (", fmt("3.2f",bvl.x0), ", ", fmt("3.2f",bvl.y0), ")")
        end
    end
end # end of struct BravaisLatice

"""
    lattice = BravaisLattice(lattice; :key1 => value1, :key2 => value2, ...)

new lattice from old lattice with modified fields.
"""
BravaisLattice(lattice::BravaisLattice; a=lattice.a, b=lattice.b, α=lattice.α, β=lattice.β, x0=lattice.x0, y0=lattice.y0) = BravaisLattice(;a=a, b=b, α=α, β=β, x0=x0, y0=y0)

################################################################################
# COORDINATE TRANSFORMATIONS
################################################################################
"""
    xb, yb = bravais_coordinates_unit_cell(x, y, lattice)

maps cartesian (`x`,`y`) into cartesian (`xb`,`yb`) unit cell specified by
lattice
"""
function bravais_coordinates_unit_cell(x::Real, y::Real, lattice::BravaisLattice)
    a, b, α, β, v1, v2, x0, y0 = lattice.a, lattice.b, lattice.α, lattice.β, lattice.v1, lattice.v2, lattice.x0, lattice.y0
    if (isinf(a) && iszero(β-π/2)) || (isinf(b) && iszero(α)) || (isinf(a) && isinf(b))
        return float(x), float(y)
    end

    p1, p2 = bravais_coordinates(x, y, lattice)
    p1 = mod(p1, a)
    p2 = mod(p2, b)

    xb = v1[1]*p1 + v2[1]*p2 + x0
    yb = v1[2]*p1 + v2[2]*p2 + y0

    return xb, yb
end

function bravais_coordinates_unit_cell(x::AbstractArray, y::AbstractArray, lattice::Union{Tlat,AbstractArray{Tlat}}) where Tlat<:BravaisLattice
    xb = Array{Float64}(undef, size(x)...)
    yb = Array{Float64}(undef, size(y)...)
    bravais_coordinates_unit_cell!(xb,yb,x,y,lattice)
    return xb, yb
end

function bravais_coordinates_unit_cell!(xb::AbstractArray, yb::AbstractArray, x::AbstractArray, y::AbstractArray, lattice::BravaisLattice)
    for k ∈ eachindex(x)
        xb[k], yb[k] = bravais_coordinates_unit_cell(x[k],y[k],lattice)
    end
    return nothing
end
function bravais_coordinates_unit_cell!(xb::AbstractArray, yb::AbstractArray, x::AbstractArray, y::AbstractArray, lattices::AbstractArray{Tlat}) where Tlat<:BravaisLattice
    for k ∈ eachindex(x)
        xb[k], yb[k] = bravais_coordinates_unit_cell(x[k],y[k],lattices[k])
    end
    return nothing
end

"""
    p1, p2 = bravais_coordinates(x,y,lattice)

coordinates in bravais frame (i.e. (x,y) = p1*v1 + p2*v2)
"""
function bravais_coordinates(x::Real, y::Real, lattice::BravaisLattice)
    a, b, α, β, v1, v2, x0, y0 = lattice.a, lattice.b, lattice.α, lattice.β, lattice.v1, lattice.v2, lattice.x0, lattice.y0
    if (isinf(a) && iszero(β-π/2)) || (isinf(b) && iszero(α)) || (isinf(a) && isinf(b))
        return float(x-x0), float(y-y0)
    end

    x += -x0
    y += -y0

    rv1 = v1[1]*x + v1[2]*y
    rv2 = v2[1]*x + v2[2]*y

    p1 = (rv1 - rv2*lattice.cosθ)/lattice.sin²θ
    p2 = (rv2 - rv1*lattice.cosθ)/lattice.sin²θ
    return p1, p2
end
function bravais_coordinates(x::AbstractArray,y::AbstractArray,lattice::BravaisLattice)
    p1 = Array{Float64,2}(undef,length(x),length(y))
    p2 = Array{Float64,2}(undef,length(x),length(y))
    bravais_coordinates!(p1,p2,x,y,lattice)
    return p1, p2
end
function bravais_coordinates!(p1::AbstractArray,p2::AbstractArray,x::AbstractArray,y::AbstractArray,lattice::BravaisLattice)
    if size(x)!==size(y)
        for i ∈ eachindex(x), j ∈ eachindex(y)
            p1[i,j],p2[i,j] = bravais_coordinates(x[i],y[j],lattice)
        end
    else
        for i ∈ eachindex(x)
            p1[i],p2[i] = bravais_coordinates(x[i],y[i],lattice)
        end
    end
    return nothing
end
bravais_coordinates(x::Real, y::AbstractArray, args...) = bravais_coordinates(hcat(x),y,args...)
bravais_coordinates(x::AbstractArray, y::Real, args...) = bravais_coordinates(x,vcat(y),args...)

########################################################################
### PLOTTING
########################################################################
"""
    plot(lattice::BravaisLattice, N=[4,4])

scatter plot of lattice vectors from -`N[i]`:`+N[i]` for each dimension i.

If `N` is a scalar, uses same span for both dimensions.
"""
@recipe function f(lattice::BravaisLattice, N::Array{Int,1}=[4,4])
    seriestype --> :scatter
    legend --> false
    aspect_ratio --> 1
    xlims --> [-N[1]*(lattice.a*lattice.v1[1]+lattice.b*lattice.v2[1]), +N[1]*(lattice.a*lattice.v1[1]+lattice.b*lattice.v2[1])]/2
    ylims --> [-N[2]*(lattice.a*lattice.v1[2]+lattice.b*lattice.v2[2]), +N[2]*(lattice.a*lattice.v1[2]+lattice.b*lattice.v2[2])]/2
    @series begin
        x, y = Float64[], Float64[]
        for n ∈ -N[1]:N[1], m ∈ -N[2]:N[2]
            temp = n*lattice.a*lattice.v1 + m*lattice.b*lattice.v2
            push!(x,temp[1]); push!(y,temp[2])
        end
        (x, y)
    end
    @series begin
        x, y = Float64[], Float64[]
        for i ∈ 1:5
            n = [0,1,1,0,0][i]
            m = [0,0,1,1,0][i]
            temp = n*lattice.a*lattice.v1 + m*lattice.b*lattice.v2
            push!(x,temp[1]); push!(y,temp[2])
        end
        seriestype --> :path
        fill --> (0,.15, :red)
        (x, y)
    end
end
@recipe f(lattice::BravaisLattice, N::Int) = (lattice, [N,N])

end # module