import StatsBase: StatsBase,fit, fit!

function _fit!{A<:Any}(
  mchain::DTFiniteMarkovChainWithFit,runs::Vector{Vector{A}})
  alphbt = mchain.mc.alphabet
  sz = length(alphbt)
  I = eltype(mchain.fitparams.counts_initial)
  fill!(mchain.fitparams.counts_initial,zero(I))
  fill!(mchain.fitparams.counts_transition,zero(I))
  initial = mchain.fitparams.counts_initial
  transition = mchain.fitparams.counts_transition
  @inbounds for r in runs
    k = _retrieveindex(r[1],alphbt)
    initial[k] = initial[k] + 1
    for n in 1:endof(r)-1
      k1 = _retrieveindex(r[n],alphbt)
      k2 = _retrieveindex(r[n+1],alphbt)
      transition[k1,k2] = transition[k1,k2] + 1
    end
  end
  initialcount = sum(initial)
  mchain.mc.initial[:] = initial ./ initialcount
  rowcounts = sum(transition,2)[:,1]
  mchain.mc.transition[:] = broadcast(/,transition,rowcounts)
  return mchain
end

StatsBase.fit!{A<:Any}(
  mchain::DTFiniteMarkovChainWithFit,runs::Vector{Vector{A}}) = _fit!(
    mchain,runs)

function StatsBase.fit{A<:Any}(::Type{DTFiniteMarkovChain},
    runs::Vector{Vector{A}},states::Vector{A})
  sz = length(states)
  fitparams = DTFiniteMarkovChainMleFitParams(
    Vector{UInt64}(sz),Matrix{UInt64}(sz,sz),Dict{A,Int}())
  mc = DTFiniteMarkovChain(Vector{Float64}(sz),
    Matrix{Float64}(sz,sz),states)
  mchain = DTFiniteMarkovChainWithFit(mc,fitparams)
  return fit!(mchain,runs)
end

function _retrieveindex(v,alphabet)
  k = 0
  for i in eachindex(alphabet)
    if alphabet[i]==v
      k = i
      break
    end
  end
  return k
end


using Test
using LinearAlgebra
using DiffEqProblemLibrary.ODEProblemLibrary: importodeproblems; importodeproblems()
import DiffEqProblemLibrary.ODEProblemLibrary: prob_ode_linear, prob_ode_2Dlinear, prob_ode_lotkavoltera, prob_ode_fitzhughnagumo

prob = prob_ode_lotkavoltera
prob = remake(prob, tspan=(0.0, 10.0))
prob = ODEFilters.remake_prob_with_jac(prob)

@testset "Condition numbers of A,Q" begin
    h = 0.1*rand()
    σ = rand()

    d, q = 2, 3

    A!, Q! = ODEFilters.vanilla_ibm(d, q)
    Ah = diagm(0 => ones(d*(q+1)))
    Qh = zeros(d*(q+1), d*(q+1))
    A!(Ah, h)
    Q!(Qh, h, σ^2)

    A_p, Q_p = ODEFilters.ibm(d, q)
    Ah_p = A_p
    Qh_p = Q_p * σ^2

    # First test that they're both equivalent
    P = ODEFilters.preconditioner(d, q)
    P, PI = P(h), inv(P(h))
    @test Qh_p ≈ P * Qh * P'
    @test Ah_p ≈ P * Ah * PI

    # Check that the preconditioning actually helps
    @info "Condition numbers" cond(Qh) cond(Matrix(Qh_p)) cond(Ah) cond(Ah_p)
    @test cond(Qh) > cond(Matrix(Qh_p))
    @test cond(Qh) > cond(Matrix(Qh_p))^2
end


using InfiniteLinearAlgebra, BlockArrays, ArrayLayouts, Test
import InfiniteLinearAlgebra: BlockTridiagonalToeplitzLayout, ul, adaptiveqr

@testset "∞-UL" begin
    @testset "Toeplitz" begin
        Δ = SymTridiagonal(Fill(-2,∞), Fill(1,∞))
        h = 0.01
        A = I - h*Δ
        U,L = ul(A)
        N = 10
        @test U[1:N,1:N+1]*L[1:N+1,1:N] ≈ A[1:N,1:N]

        u =  adaptiveqr(A) \ [1; zeros(∞)]
        v = L \ (U \ [1;zeros(∞)])
        @test u ≈ v
    end

    @testset "Symmetric" begin
        B = randn(2,2)
        A = randn(2,2) - 10I #; A = A + A'
        C = Matrix(B')

        J = mortar(Tridiagonal(Fill(C,∞), Fill(A,∞), Fill(B,∞)))

        @test MemoryLayout(J) isa BlockTridiagonalToeplitzLayout
        U,L = ul(J, Val(false))
        N = 10
        @test U[Block.(1:N),Block.(1:N+1)] * L[Block.(1:N+1),Block.(1:N)] ≈ J[Block.(1:N),Block.(1:N)]

        @test (J \ [1; zeros(∞)])[Block(1)] ≈ inv(L[Block(1,1)])[:,1]
    end

    @testset "Periodic Jacobi" begin
        e = 0.1
        B = [e 0; 1 e]
        A = [1 1; 1 -1.0]
        C = Matrix(B')
        J = mortar(Tridiagonal(Fill(C,∞), Fill(A,∞), Fill(B,∞))) - 10I
        U,L = ul(J, Val(false))
        N = 10;
        @test U[Block.(1:N),Block.(1:N+1)] * L[Block.(1:N+1),Block.(1:N)] ≈ J[Block.(1:N),Block.(1:N)]
    end
end


import FinanceLib
import Dates

@testset "FinanceLib                                                  " begin

  @testset "tv" begin
    @test FinanceLib.yearFrac(Dates.Date(2027,2,12), Dates.Date(2018,2,12)) ≈ -8.999315537303216
    @test FinanceLib.invYearFrac(Dates.Date(2027,2,12), -8.999315537303216) == Dates.Date(2018,2,12)

    @test FinanceLib.disFactAnnual(0.07) == 0.9345794392523364
    @test FinanceLib.disFact(0.09, 3) == 0.7721834800610642
    @test FinanceLib.fwdDisFact((0.07, 1), (0.09, 3)) == 0.8262363236653387

    @test FinanceLib.xdisFact(0.09, Dates.Date(2015,3,15), Dates.Date(2018,10,8)) == 0.7353328680759499

    @test FinanceLib.tMul(0.06/12, -120.0) == 0.5496327333641637
    @test FinanceLib.tMul(0.06, -10.0, 12.0) == 0.5496327333641637
    @test FinanceLib.rateGwth(7.35, 8.52, 5.0) == -0.029111071029244595
    @test FinanceLib.periodGwth(100.0,50.0,0.07) == 10.244768351058712

  end

  @testset "pv" begin
    @test FinanceLib.pv(10_000_000., 0.09, 5.0) == 6_499_313.862983453
    @test FinanceLib.pv(12_704_891.6109538, 0.06, 4.0, 12.0) ≈ 10_000_000.
    @test FinanceLib.pvr(10_000_000., 1.09, 5.0) == 6_499_313.862983453
    @test FinanceLib.pvc(11_735.108709918102, 0.08, 2.0) == 10_000
  end

  @testset "fv" begin
    @test FinanceLib.fv(6_499_313.862983453, 0.09, 5.0) == 10_000_000.0
    @test FinanceLib.fv(10_000_000.0, 0.06, 4.0, 12.0) ≈ 12_704_891.6109538
    @test FinanceLib.fvc(10_000., 0.08, 2.0) == 11_735.108709918102
  end

  @testset "annuity" begin
    @test FinanceLib.pvAnnuity(1000.0, 0.12, 5.0) == 3_604.776202345007
    @test FinanceLib.pvAnnuity(7.33764573879378, 0.08, 30.0, 12.0) == 1000

    @test FinanceLib.pvAnnuity(100.0, 0.05) == 2000.0

    @test FinanceLib.fvAnnuity(1000.0, 0.05, 5.0) == 5_525.631250000007
    @test FinanceLib.fvAnnuity(2000.0, 0.24, 5.0, 3.0) == 54_304.2278549568

    @test FinanceLib.pmt(3_604.776202345007, 0.12, 5.0) == 1000.0
    @test FinanceLib.pmt(1000.0, 0.08, 30.0, 12.0) == 7.33764573879378

    @test FinanceLib.fmt(5_525.631250000007, 0.05, 5.0) == 1000
    @test FinanceLib.fmt(54_304.2278549568, 0.24, 5.0, 3.0) == 2000

    @test FinanceLib.pv(FinanceLib.pvAnnuity(10.0^6,.05,30.0),0.05,9.0) == 9_909_218.99605011
  end

  @testset "effective rates" begin
    @test FinanceLib.effR(0.08, 2.0) ≈ 0.0816
    @test FinanceLib.expR(0.08, 2.0) == 0.07844142630656266
    @test FinanceLib.expR(0.08) == 0.0769610411361284
    @test FinanceLib.nomR(FinanceLib.effR(0.08, 4), 4) ≈ 0.08

    FinanceLib.pvc(20,FinanceLib.expR(0.07,4),4.25) == FinanceLib.pvr(20,1+FinanceLib.effR(0.07,4),4.25)

    eT = FinanceLib.RateCurve{FinanceLib.NomRate}([0.0016, 0.0021, 0.0027, 0.0033, 0.0037, 0.0041], 2)

    eR = FinanceLib.effR(eT)

    @test eR.rate[1] ≈ 0.0016006400
    @test eR.rate[2] ≈ 0.0021011025
    @test eR.rate[3] ≈ 0.0027018225
    @test eR.rate[4] ≈ 0.0033027225
    @test eR.rate[5] ≈ 0.0037034225
    @test eR.rate[6] ≈ 0.0041042025

    eN = FinanceLib.nomR(eR)

    @test eN.rate[1] ≈ 0.0016
    @test eN.rate[2] ≈ 0.0021
    @test eN.rate[3] ≈ 0.0027
    @test eN.rate[4] ≈ 0.0033
    @test eN.rate[5] ≈ 0.0037
    @test eN.rate[6] ≈ 0.0041

    eZ = FinanceLib.nomR(FinanceLib.effR(FinanceLib.expR(eT))) # N - X - E - N
    eY = FinanceLib.nomR(FinanceLib.expR(FinanceLib.effR(eT))) # N - E - X - N
    eW = FinanceLib.nomR(FinanceLib.expR(eT)) # N - X - N

    @test eZ.rate[3] == eW.rate[3]
    @test eY.rate[4] == eW.rate[4]
    @test eZ.rate[2] == eY.rate[2]
    @test eZ.rate[5] == eY.rate[5]
    @test eZ.rate[1] == eW.rate[1]

  end

  @testset "npv" begin
    @test FinanceLib.npv(0.05, [0.0:1.0:4.0;],[1000.,2000.0,4000.0,5000.0,6000.0],-1.45)==14709.923338335731
    @test FinanceLib.npv(0.08, [0.25,6.25,3.5,4.5,1.25], [-6.25,1.2,1.25,3.6,2.5], -0.45) == 0.36962283798505946
    @test FinanceLib.npv(0.08, zip([0.25,6.25,3.5,4.5,1.25],[-6.25,1.2,1.25,3.6,2.5]), -0.45) == 0.36962283798505946
    @test FinanceLib.npv(0.08, [0.25,6.25,3.5,4.5,1.25], [-6.25,1.2,1.25,3.6,2.5], 6.25) == 0.619010419015909

    @test FinanceLib.irr([0.125,0.29760274,0.49760274,0.55239726,0.812671233], [-10.25,-2.5,3.5,9.5,1.25]) ≈ 0.31813386476788824

    ts = collect(zip([0.125,0.29760274,0.49760274,0.55239726,0.812671233], [-10.25,-2.5,3.5,9.5,1.25])) :: FinanceLib.PeriodSeries

    @test FinanceLib.irr(ts) ≈ 0.31813386476788824

    @test FinanceLib.irr(zip([0.125,0.29760274,0.49760274,0.55239726,0.812671233], [-10.25,-2.5,3.5,9.5,1.25])) ≈ 0.31813386476788824

    @test FinanceLib.xnpv(0.08, [Dates.Date(2012,2,25), Dates.Date(2012,6,28),
      Dates.Date(2013,2,15), Dates.Date(2014,9,18), Dates.Date(2015,2,20)],
      [-15, 5, 25, -10, 50], Dates.Date(2012,1,10) ) ==  44.15557928534869

    @test FinanceLib.xnpv(0.08, zip([Dates.Date(2012,2,25), Dates.Date(2012,6,28),
      Dates.Date(2013,2,15), Dates.Date(2014,9,18), Dates.Date(2015,2,20)],
      [-15, 5, 25, -10, 50.]), Dates.Date(2012,1,10) ) ==  44.15557928534869

    @test FinanceLib.xirr([Dates.Date(2012,2,25), Dates.Date(2012,6,28),
      Dates.Date(2013,2,15), Dates.Date(2014,9,18), Dates.Date(2015,2,20)],
      [-115, 5, 25, -10, 200] ) ==  0.2783166029306355

    td = collect(zip([Dates.Date(2012,2,25), Dates.Date(2012,6,28),
    Dates.Date(2013,2,15), Dates.Date(2014,9,18), Dates.Date(2015,2,20)],
    [-115, 5, 25, -10, 200]))

    td1 = FinanceLib.dateToPeriodSeries(Dates.Date(2010,05,12), td)
    @test td1[3][1] == 2.7652292950034223

    @test FinanceLib.irr(td1) ==  0.2783166029306353
    @test FinanceLib.xirr(td) ==  0.2783166029306355

    @test FinanceLib.xirr(zip([Dates.Date(2012,2,25), Dates.Date(2012,6,28),
      Dates.Date(2013,2,15), Dates.Date(2014,9,18), Dates.Date(2015,2,20)],
      [-115, 5, 25, -10, 200]) ) ==  0.2783166029306355

    @test FinanceLib.npv(FinanceLib.PeriodSeries([(0.5,0.05), (1.25, 0.0575), (2, 0.0485), (3.5, 0.0625), (4.25, 0.055)]), [-150, 20, 15, 80, 100], 0.3) == 31.530253870718543

    @test FinanceLib.xnpv(FinanceLib.DateSeries([(Dates.Date(2014,9,20),0.05), (Dates.Date(2015,2,1), 0.0575), (Dates.Date(2016,10,5), 0.0485), (Dates.Date(2017,12,5), 0.0625), (Dates.Date(2019,1,5), 0.055)]), [-150, 20, 15, 80, 100], Dates.Date(2014,2,15)) ==  29.323165765999597

  end

  @testset "Sharpe" begin
    @test FinanceLib.sharpe(1.58,9.26,22.36) ≈ 0.3434704830053667
  end

  @testset "Rates" begin

    @test FinanceLib.discFactorToNominalRate(FinanceLib.DiscountFactor([0.9524, 0.89, 0.8163, 0.735],1)).rate[3] == 0.0699990723472752

    dsc = FinanceLib.discFactorToNominalRate(FinanceLib.DiscountFactor([ 0.99920063949, 0.99790330288, 0.99596091045, 0.99342713542, 0.99080111671, 0.98778777227 ],2)).rate

    @test dsc[1] ≈ 0.0016
    @test dsc[2] ≈ 0.0021
    @test dsc[3] ≈ 0.0027
    @test dsc[4] ≈ 0.0033
    @test dsc[5] ≈ 0.0037
    @test dsc[6] ≈ 0.0041

    @test FinanceLib.estimR(FinanceLib.RateCurve{FinanceLib.NomRate}([0.05, 0.06, 0.07, 0.08], 2), 1.5) == 0.07
    @test FinanceLib.estimR(FinanceLib.RateCurve{FinanceLib.NomRate}([0.05, 0.06, 0.07, 0.08], 2), 1.2) == 0.064

  end

end

include("FixedIncomes/runtests.jl")
include("Derivatives/runtests.jl")
include("Statements/runtests.jl")


using ForwardDiff

abstract ExtendedKalmanFilter <: KalmanFilter

type NonlinearModel <: Model
    f::Function
    j::Function
    g::Function
    q::Matrix
    function NonlinearModel(f::Function,g::Function,q::Matrix)
        j = forwarddiff_jacobian(f,Float64,fadtype=:typed)
        new(f,j,g,q)
    end
end

function ap(f::NonlinearModel,x::State)
    x1 = f.f(x.x)
    F = f.j(x.x)
    G = f.g(x.x)
    p1 = F*x.p*F' + G*f.q*G'
    State(x1,p1)
end

type NonlinearObservationModel <: ObservationModel
    h::Function
    j::Function
    r::Matrix
    function NonlinearObservationModel(h::Function,r::Matrix)
        j = forwarddiff_jacobian(h,Float64,fadtype=:typed)
        new(h,j,r)
    end
end

type BasicExtendedKalmanFilter <: ExtendedKalmanFilter
    x::State
    f::NonlinearModel
    z::NonlinearObservationModel
end

function covs(kf::BasicExtendedKalmanFilter,y::Observation)
    res = y.y - kf.z.h(kf.x.x)
    H = kf.z.j(kf.x.x)'
    ph = kf.x.p * H'
    s = H * kf.x.p * H' + kf.z.r
    (res,ph,s)
end


using Test
using LinearAlgebra
using KernelFunctions
using SpecialFunctions

x = rand()*2; v1 = rand(3); v2 = rand(3); id = IdentityTransform()
@testset "Kappa functions of kernels" begin
    @testset "Constant" begin
        @testset "ZeroKernel" begin
            k = ZeroKernel()
            @test eltype(k) == Any
            @test kappa(k,2.0) == 0.0
        end
        @testset "WhiteKernel" begin
            k = WhiteKernel()
            @test eltype(k) == Any
            @test kappa(k,1.0) == 1.0
            @test kappa(k,0.0) == 0.0
        end
        @testset "ConstantKernel" begin
            c = 2.0
            k = ConstantKernel(c)
            k₂ = ConstantKernel(IdentityTransform(),c)
            @test eltype(k) == Any
            @test kappa(k,1.5)== kappa(k₂,1.5)
            @test kappa(k,1.0) == c
            @test kappa(k,0.5) == c
        end
    end
    @testset "Exponential" begin
        @testset "SqExponentialKernel" begin
            k = SqExponentialKernel()
            @test kappa(k,x) ≈ exp(-x)
            @test k(v1,v2) ≈ exp(-norm(v1-v2)^2)
            @test kappa(SqExponentialKernel(id),x) == kappa(k,x)
            l = 0.5
            k = SqExponentialKernel(l)
            @test k(v1,v2) ≈ exp(-l^2*norm(v1-v2)^2)
            v = rand(3)
            k = SqExponentialKernel(v)
            @test k(v1,v2) ≈ exp(-norm(v.*(v1-v2))^2)
        end
        @testset "ExponentialKernel" begin
            k = ExponentialKernel()
            @test kappa(k,x) ≈ exp(-x)
            @test k(v1,v2) ≈ exp(-norm(v1-v2))
            @test kappa(ExponentialKernel(id),x) == kappa(k,x)
            l = 0.5
            k = ExponentialKernel(l)
            @test k(v1,v2) ≈ exp(-l*norm(v1-v2))
            v = rand(3)
            k = ExponentialKernel(v)
            @test k(v1,v2) ≈ exp(-norm(v.*(v1-v2)))
        end
        @testset "GammaExponentialKernel" begin
            k = GammaExponentialKernel(1.0,2.0)
            @test kappa(k,x) ≈ exp(-(x)^(k.γ))
            @test k(v1,v2) ≈ exp(-norm(v1-v2)^(2k.γ))
            @test kappa(GammaExponentialKernel(id),x) == kappa(k,x)
            l = 0.5
            k = GammaExponentialKernel(l,1.5)
            @test k(v1,v2) ≈ exp(-l^(3.0)*norm(v1-v2)^(3.0))
            v = rand(3)
            k = GammaExponentialKernel(v,3.0)
            @test k(v1,v2) ≈ exp(-norm(v.*(v1-v2)).^6.0)
            #Coherence :
            @test KernelFunctions._kernel(GammaExponentialKernel(1.0,1.0),v1,v2) ≈ KernelFunctions._kernel(SqExponentialKernel(),v1,v2)
            @test KernelFunctions._kernel(GammaExponentialKernel(1.0,0.5),v1,v2) ≈ KernelFunctions._kernel(ExponentialKernel(),v1,v2)
        end
    end
    @testset "Exponentiated" begin
        @testset "ExponentiatedKernel" begin
            k = ExponentiatedKernel()
            @test kappa(k,x) ≈ exp(x)
            @test kappa(k,-x) ≈ exp(-x)
            @test k(v1,v2) ≈ exp(dot(v1,v2))
            l = 0.5
            k = ExponentiatedKernel(l)
            @test k(v1,v2) ≈ exp(l^2*dot(v1,v2))
            v = rand(3)
            k = ExponentiatedKernel(v)
            @test k(v1,v2) ≈ exp(dot(v.*v1,v.*v2))
        end
    end
    @testset "Matern" begin
        @testset "MaternKernel" begin
            ν = 2.0
            k = MaternKernel(1.0,ν)
            matern(x,ν) = 2^(1-ν)/gamma(ν)*(sqrt(2ν)*x)^ν*besselk(ν,sqrt(2ν)*x)
            @test kappa(k,x) ≈ matern(x,ν)
            @test kappa(k,0.0) == 1.0
            @test kappa(MaternKernel(id,ν),x) == kappa(k,x)
            l = 0.5; ν = 3.0
            k = MaternKernel(l,ν)
            @test k(v1,v2) ≈ matern(l*norm(v1-v2),ν)
            v = rand(3); ν = 2.1
            k = MaternKernel(v,ν)
            @test k(v1,v2) ≈ matern(norm(v.*(v1-v2)),ν)
        end
        @testset "Matern32Kernel" begin
            k = Matern32Kernel()
            @test kappa(k,x) ≈ (1+sqrt(3)*x)exp(-sqrt(3)*x)
            @test k(v1,v2) ≈ (1+sqrt(3)*norm(v1-v2))exp(-sqrt(3)*norm(v1-v2))
            @test kappa(Matern32Kernel(id),x) == kappa(k,x)
            l = 0.5
            k = Matern32Kernel(l)
            @test k(v1,v2) ≈ (1+l*sqrt(3)*norm(v1-v2))exp(-l*sqrt(3)*norm(v1-v2))
            v = rand(3)
            k = Matern32Kernel(v)
            @test k(v1,v2) ≈ (1+sqrt(3)*norm(v.*(v1-v2)))exp(-sqrt(3)*norm(v.*(v1-v2)))
        end
        @testset "Matern52Kernel" begin
            k = Matern52Kernel()
            @test kappa(k,x) ≈ (1+sqrt(5)*x+5/3*x^2)exp(-sqrt(5)*x)
            @test k(v1,v2) ≈ (1+sqrt(5)*norm(v1-v2)+5/3*norm(v1-v2)^2)exp(-sqrt(5)*norm(v1-v2))
            @test kappa(Matern52Kernel(id),x) == kappa(k,x)
            l = 0.5
            k = Matern52Kernel(l)
            @test k(v1,v2) ≈ (1+l*sqrt(5)*norm(v1-v2)+l^2*5/3*norm(v1-v2)^2)exp(-l*sqrt(5)*norm(v1-v2))
            v = rand(3)
            k = Matern52Kernel(v)
            @test k(v1,v2) ≈ (1+sqrt(5)*norm(v.*(v1-v2))+5/3*norm(v.*(v1-v2))^2)exp(-sqrt(5)*norm(v.*(v1-v2)))
        end
        @testset "Coherence Materns" begin
            @test kappa(MaternKernel(1.0,0.5),x) ≈ kappa(ExponentialKernel(),x)
            @test kappa(MaternKernel(1.0,1.5),x) ≈ kappa(Matern32Kernel(),x)
            @test kappa(MaternKernel(1.0,2.5),x) ≈ kappa(Matern52Kernel(),x)
        end
    end
    @testset "Polynomial" begin
        c = randn();
        @testset "LinearKernel" begin
            k = LinearKernel()
            @test kappa(k,x) ≈ x
            @test k(v1,v2) ≈ dot(v1,v2)
            @test kappa(LinearKernel(id),x) == kappa(k,x)
            l = 0.5
            k = LinearKernel(l,c)
            @test k(v1,v2) ≈ l^2*dot(v1,v2) + c
            v = rand(3)
            k = LinearKernel(v,c)
            @test k(v1,v2) ≈ dot(v.*v1,v.*v2) + c
        end
        @testset "PolynomialKernel" begin
            k = PolynomialKernel()
            @test kappa(k,x) ≈ x^2
            @test k(v1,v2) ≈ dot(v1,v2)^2
            @test kappa(PolynomialKernel(id),x) == kappa(k,x)
            d = 3.0
            l = 0.5
            k = PolynomialKernel(l,d,c)
            @test k(v1,v2) ≈ (l^2*dot(v1,v2) + c)^d
            v = rand(3)
            k = PolynomialKernel(v,d,c)
            @test k(v1,v2) ≈ (dot(v.*v1,v.*v2) + c)^d
            #Coherence test
            @test kappa(PolynomialKernel(1.0,1.0,c),x) ≈ kappa(LinearKernel(1.0,c),x)
        end
    end
    @testset "RationalQuadratic" begin
        @testset "RationalQuadraticKernel" begin
            k = RationalQuadraticKernel()
            @test kappa(k,x) ≈ (1.0+x/2.0)^-2
            @test k(v1,v2) ≈ (1.0+norm(v1-v2)^2/2.0)^-2
            @test kappa(RationalQuadraticKernel(id),x) == kappa(k,x)
            l = 0.5
            a = 1.0 + rand()
            k = RationalQuadraticKernel(l,a)
            @test k(v1,v2) ≈ (1.0+l^2*norm(v1-v2)^2/a)^-a
            v = rand(3)
            k = RationalQuadraticKernel(v,a)
            @test k(v1,v2) ≈ (1.0+norm(v.*(v1-v2))^2/a)^-a
        end
        @testset "GammaRationalQuadraticKernel" begin
            k = GammaRationalQuadraticKernel()
            @test kappa(k,x) ≈ (1.0+x^2.0/2.0)^-2
            @test k(v1,v2) ≈ (1.0+norm(v1-v2)^4.0/2.0)^-2
            @test kappa(GammaRationalQuadraticKernel(id),x) == kappa(k,x)
            l = 0.5
            a = 1.0 + rand()
            g = 4.0
            k = GammaRationalQuadraticKernel(l,a,g)
            @test k(v1,v2) ≈ (1.0+(l^2g)*norm(v1-v2)^(2g)/a)^-a
            v = rand(3)
            k = GammaRationalQuadraticKernel(v,a,g)
            @test k(v1,v2) ≈ (1.0+(norm(v.*(v1-v2))^(2g))/a)^-a
            #Coherence test
            @test kappa(GammaRationalQuadraticKernel(1.0,a,1.0),x) ≈ kappa(RationalQuadraticKernel(1.0,a),x)
        end
    end
    @testset "KernelCombinations" begin
        k1 = LinearKernel()
        k2 = SqExponentialKernel()
        X = rand(2,2)
        @testset "KernelSum" begin
            k = k1 + k2
            @test KernelFunctions.metric(k) == [KernelFunctions.DotProduct(),KernelFunctions.SqEuclidean()]
            @test length(k) == 2
            @test transform(k) == [transform(k1),transform(k2)]
            @test transform(k,X) == [transform(k1,X),transform(k2,X)]
            @test transform(k,X,1) == [transform(k1,X,1),transform(k2,X,1)]
        end
        @testset "KernelProduct" begin

        end
    end
end


"""
    Multiply(input=inputDimension, output=outputDimension, winit=xavier, atype=KnetLayers.arrtype)

Creates a matrix multiplication layer based on `inputDimension` and `outputDimension`.
    (m::Multiply) = m.w * x

By default parameters initialized with xavier, you may change it with `winit` argument.

# Keywords
* `input=inputDimension`: input dimension
* `output=outputDimension`: output dimension
* `winit=xavier`: weight initialization distribution
* `atype=KnetLayers.arrtype` : array type for parameters.
   Default value is KnetArray{Float32} if you have gpu device. Otherwise it is Array{Float32}
"""
mutable struct Multiply <: Layer
    weight
end

Multiply(;input::Int, output::Int, winit=xavier, atype=arrtype) =  Multiply(param(output, input; init=winit, atype=atype))

(m::Multiply)(x::Array{<:Integer}) = m.weight[:,x] # Lookup (EmbedLayer)

# TODO: Find a faster (or compound) way for tensor-product
function (m::Multiply)(x; keepsize=true)
    if ndims(x) > 2
        s = size(x)
        y = m.weight * reshape(x, s[1], prod(s[2:end]))
        return (keepsize ? reshape(y, size(y, 1), s[2:end]...) : y)
    else
        return m.weight * x
    end
end

"""
    Embed(input=inputSize, output=embedSize, winit=xavier, atype=KnetLayers.arrtype)
Creates an embedding layer according to given `inputSize` and `embedSize` where `inputSize` is your number of unique items you want to embed, and `embedSize` is the size of output vectors.
By default parameters initialized with xavier, you yam change it with `winit` argument.

    (m::Embed)(x::Array{T}) where T<:Integer
    (m::Embed)(x; keepsize=true)

Embed objects are callable with an input which is either and integer array
(one hot encoding) or an N-dimensional matrix. For N-dimensional matrix,
`size(x,1)==inputSize`

# Keywords

* `input=inputDimension`: input dimension
* `output=embeddingDimension`: output dimension
* `winit=xavier`: weight initialization distribution
* `atype=KnetLayers.arrtype` : array type for parameters.
   Default value is KnetArray{Float32} if you have gpu device. Otherwise it is Array{Float32}

"""
Embed = Multiply

# TODO: find a better documentation style e.g put input : and output etc.
# Input: Type of its input is an ::Array{T} where T<: Integer

"""
Gemm
"""

mutable struct Gemm
    alpha
    beta
    transA
    transB
end

# inputs: matrices to be multiplied, handle C somehow (maybe zeros() with the same size as A*B)
function (g::Gemm)(A, B, C)
    if g.transA != 0; A = transpose(A); end
    if g.transB != 0; B = transpose(B); end
    g.alpha * A * B + (g.beta * C)
end

"""
    Linear(input=inputSize, output=outputSize, winit=xavier, binit=zeros, atype=KnetLayers.arrtype)

Creates and linear layer according to given `inputSize` and `outputSize`.

# Keywords
* `input=inputSize`   input dimension
* `output=outputSize` output dimension
* `winit=xavier`: weight initialization distribution
* `bias=zeros`: bias initialization distribution
* `atype=KnetLayers.arrtype` : array type for parameters.
   Default value is KnetArray{Float32} if you have gpu device. Otherwise it is Array{Float32}
"""
mutable struct Linear <: Layer
    mult::Multiply
    bias
end

function Linear(;input::Int, output::Int, winit=xavier, binit=zeros, atype=arrtype)
    Linear(Multiply(input=input, output=output, winit=winit, atype=atype),param(output, init=binit, atype=atype))
end

(m::Linear)(x) = m.mult(x) .+ m.bias

"""
    Dense(input=inputSize, output=outputSize, activation=ReLU(), winit=xavier, binit=zeros, atype=KnetLayers.arrtype)

Creates and deense layer according to given `input=inputSize` and `output=outputSize`.
If activation is `nothing`, it acts like a `Linear` Layer.
# Keywords
* `input=inputSize`   input dimension
* `output=outputSize` output dimension
* `winit=xaiver`: weight initialization distribution
* `bias=zeros`:   bias initialization distribution
* `activation=ReLU()`  activation function(it does not broadcast) or an  activation layer
* `atype=KnetLayers.arrtype` : array type for parameters.
   Default value is KnetArray{Float32} if you have gpu device. Otherwise it is Array{Float32}
"""
mutable struct Dense <: Layer
    linear::Linear
    activation
end

function Dense(;input::Int, output::Int, activation=ReLU(), winit=xavier, binit=zeros, atype=arrtype)
    Dense(Linear(input=input, output=output, winit=winit, binit=binit, atype=atype), activation)
end

(m::Dense)(x) = m.activation===nothing ? m.linear(x) : m.activation(m.linear(x))

#TO-DO: Remove after the issue is resolved:
#https://github.com/denizyuret/Knet.jl/issues/418
"""
    BatchNorm(channels:Int;options...)
    (m::BatchNorm)(x;training=false) #forward run
# Options
* `momentum=0.1`: A real number between 0 and 1 to be used as the scale of
 last mean and variance. The existing running mean or variance is multiplied by
 (1-momentum).
* `mean=nothing': The running mean.
* `var=nothing`: The running variance.
* `meaninit=zeros`: The function used for initialize the running mean. Should either be `nothing` or
of the form `(eltype, dims...)->data`. `zeros` is a good option.
* `varinit=ones`: The function used for initialize the run
* `dataType=eltype(KnetLayers.arrtype)` : element type ∈ {Float32,Float64} for parameters. Default value is `eltype(KnetLayers.arrtype)`
* `usegpu=KnetLayers.arrtype <: KnetArray` :
# Keywords
* `training`=nothing: When training is true, the mean and variance of x are used and moments
 argument is modified if it is provided. When training is false, mean and variance
 stored in the moments argument are used. Default value is true when at least one
 of x and params is AutoGrad.Value, false otherwise.
"""
mutable struct BatchNorm <: Layer
    params
    moments::Knet.BNMoments
end

function BatchNorm(channels::Int; usegpu = arrtype <: KnetArray, dataType=eltype(arrtype), o...)
    w = bnparams(dataType,channels)
    m = bnmoments(;o...)
    BatchNorm(usegpu ? Param(KnetArray(w)) : Param(w),m)
end
(m::BatchNorm)(x;o...) = batchnorm(x,m.moments,m.params;o...)

#3D -> 2D
struct Flatten <: Layer
end

function (f::Flatten)(x)
    s = size(x)
    # batch mode (..., 1) at the end
    if s[end] == 1
        #reshape(x, :, size(x)[end-1], 1)
        reshape(x, :, size(x)[end])

    # no batch
    else
        reshape(x, :, size(x)[end])
    end
end

"""
Adds any number of inputs together.
Doesn't check size or type.
"""
struct Add; end
(a::Add)(args...) = +(args...)

"""
Mul
Performs element-wise binary multiplication (with Numpy-style broadcasting support).
"""
struct Mul; end
(m::Mul)(A, B) = A .* B

"""
Div
Performs element-wise binary division (with Numpy-style broadcasting support).
"""
struct Div; end
(d::Div)(A, B) = A ./ B

"""
Sub
Performs element-wise binary subtraction (with Numpy-style broadcasting support).
"""
struct Sub; end
(s::Sub)(A, B) = A .- B

struct Exp; end
(e::Exp)(A) = A.^2

struct Transpose
    perm
end
(t::Transpose)(A) = permutedims(A, t.perm)

struct Reshape
    shape
end
(r::Reshape)(A) = reshape(A, r.shape)

struct Cast
    to
end
(c::Cast)(A) = convert(c.to, A)

# Not working now!
struct Slice
    starts
    ends
    axes
    steps
end

function (s::Slice)(A)
    A[s.axes][s.starts:s.steps:s.ends]
end

struct MatMul
end
(m::MatMul)(A,B) = A * B


using FractionalDiffEq, Plots
α = 0.95; ϕ(x) = 0.5
τ = [2, 2.6]
fun(t, y, ϕ1, ϕ2) = 2*ϕ1/(1+ϕ2^9.65)-y
prob = FDDEProblem(fun, ϕ, α, τ, 100)
delayed, y = solve(prob, 0.01, DelayPECE())

p1=plot(delayed[1, :], y)
p2=plot(delayed[2, :], y)
plot(p1, p2, layout=(1, 2))


mutable struct CovProblem{T <: Real, S <: Int} <: NUTSGibbs.AbstractProblem
    "Observations."
    X::Matrix{T}
    "Degrees of freedom for half-T prior for standard deviations"
    v::T
    "Degrees of freedom for LKJ cholesky distribution"
    eta::T
    "Number of classes"
    G::S
    "Class indicators"
    cind::Vector{S}
end

function nuts_cov_transformation(p::CovProblem)
    d = size(p.X, 2)
    as((S = as(Matrix, asℝ₊, p.G, d), y = as(Matrix, p.G, Int(d*(d-1)/2))))
end

is_corr_cholfac(L) = isapprox(sum(L.^2, dims=2), ones(eltype(L), size(L,1))) && all(diag(L) .> 0.0)
invquad(L, x) = dot(x, L' \ (L \ x))
mvn_logpdf(x, mu, L) = -length(mu)/2 * log(2*pi) - log(det(L)) - invquad(L, x-mu)/2

function log_target(S, y, X, v, eta)
    n, d = size(X)
    L = transform_free_to_chol(y, d)
    distr_L = LKJcorrChol(d, eta)
    SL = Diagonal(S)*L

    (is_corr_cholfac(L) && all(isfinite.(S)) && all(S .> 0.00001)) || return -Inf

    res = -n*log(prod(diag(SL)))
    for i in 1:n
        res -= invquad(SL, X[i,:])/2
    end

    res += sum(logpdf.(Cauchy(0, 2.5), S)) + logpdf(distr_L, L) + log_jacobian_det_free_to_chol(y, d, L)
    isfinite(res) || return -Inf
    res
end

function (problem::CovProblem)(theta)
    @unpack cind, X, v, eta, G = problem
    @unpack S, y = theta

    d = size(X,2)

    res = 0.0
    for g in 1:G
        res += log_target(S[g,:], y[g,:], X[cind .== g,:], v, eta)
    end

    isfinite(res) || return -Inf
    res
end

function cov_model_init(theta, data, prior, warmup)
    G, q = size(theta.S)
    X = theta.b .- theta.mu[theta.cind,:]
    problem = CovProblem(X, 1.0, 1.0, G, theta.cind)
    NUTSGibbs.init(problem, nuts_cov_transformation, warmup)
end

function cov_model_step!(x::NUTSGibbsSampler, theta::Tp) where Tp <: AbstractParam
    G, q = size(theta.S)
    x.problem.X = theta.b .- theta.mu[theta.cind,:]
    x.problem.cind = theta.cind

    y = Matrix{eltype(theta.L)}(undef, G, Int(q*(q-1)/2))
    for g in 1:G
        y[g,:] = transform_chol_to_free(LowerTriangular(theta.L[g,:,:]))
    end

    res, ac = NUTSGibbs.step(x, (S=theta.S, y=y))

    theta.S = res.S

    for g in 1:G
        theta.L[g,:,:] = transform_free_to_chol(res.y[g,:], q)
    end

    return theta, ac
end


using DynamicPolynomials
using MultivariateMoments
using SemialgebraicSets
using BeurlingLasso
using Test

const MM = MultivariateMoments

@testset "poly" begin
    @polyvar x[1:2]

    # only valid for odd numbers
    function doublefactorial(n::Integer)
        if n <= 0
            1
        else
            prod(2*k+1 for k=0:div(n-1, 2))
        end
    end

    nmoments = 15

    φ = [sum(binomial(n, 2*k) * x[1]^(n-2*k) * x[2]^(2*k) * doublefactorial(2*k-1)
         for k=0:div(n, 2)) for n=0:nmoments-1]

    params = [[-0.5, 0.5], [0.6, 0.3]]
    weights = [0.7, 0.3]

    natoms = length(params)

    μ0 = AtomicMeasure(x, [WeightedDiracMeasure(params[i], weights[i]) for i=1:natoms])
    y = [MM.expectation(μ0, φ[n]) for n=1:nmoments]

    λ = 1e-5

    φ_func(u, n) = φ[n](x=>u)
    bounds = [[-1 1]; [-1 1]]

    prob = blasso(y, φ_func, λ, bounds)
    solver = sfwsolver(2, 1000)
    solve!(prob, solver)
end


function basis_m(n::Int64,m::Int64)
    len = binomial(n+m-1,m)
    base = spzeros(len,n)
    counter = 1
    index = Int64[]
    for i in 1:((m+1)^n-1)
        bin_vec = splitter(string(i,base=(m+1)),n)
        if sum(bin_vec) == m
            base[counter,:] = bin_vec
            append!(index,i)
            counter += 1
        end
    end
    return base, index
end

function splitter(x,n)
    vectorized = zeros(Int64,0)
    while length(x) < n
        x = "0"*x
    end
    for i in x
        str_to_int = parse(Int, i)
        append!(vectorized, str_to_int)
    end
    return vectorized
end

function myfind(c,j)
    a = similar(c, Int)
    count = 1
    @inbounds for i in eachindex(c)
        a[count] = i
        count += (c[i] == j)
    end
    return a[1:(count-1)]
end

function bdb(n::Int64, m::Int64, i::Int64, j::Int64)
    base, ind = basis_m(n,m)
    l = binomial(n+m-1,m)
    op = spzeros(l,l)
    if i==j
        for k in 1:l
            if base[k,:][j] != 0
                op[k,k] = base[k,:][j]
            end
        end
    else
        for k in 1:l
           if base[k,:][j] != 0
                ill = Int(ind[k]-(m+1)^(n-j)+(m+1)^(n-i))
                ill2 = myfind(ind, ill)[1]
                op[ill2,k] = 1
            end
        end
    end
    return op
end

function bbd(n::Int64, m::Int64, i::Int64, j::Int64)
    return bdb(n, m, j, i)
end

function bdb(base::SparseMatrixCSC{Float64,Int64}, ind::Array{Int64,1}, m::Int64, i::Int64, j::Int64)
    l = size(base)[1]
    n = size(base)[2]
    op = spzeros(l,l)
    if i==j
        for k in 1:l
            if base[k,:][j] != 0
                op[k,k] = base[k,:][j]
            end
        end
    else
        for k in 1:l
           if base[k,:][j] != 0
                ill = Int(ind[k]-(m+1)^(n-j)+(m+1)^(n-i))
                ill2 = myfind(ind, ill)[1]
                op[ill2,k] = 1
            end
        end
    end
    return op
end

function bbd(base::SparseMatrixCSC{Float64,Int64}, ind::Array{Int64,1}, m::Int64, i::Int64, j::Int64)
    return bdb(base, ind, m, j, i)
end

struct Op_fixed
    dim::Int
    nume::Int
    bdbtot::SparseMatrixCSC{Float64,Int64}
    le::Int
    basis::SparseMatrixCSC{Float64,Int64}
    Op_fixed(dim,nume) = new(dim, nume, operators_fixed(dim,nume)...)
end

dim(o::Op_fixed) = o.dim
nume(o::Op_fixed) = o.nume
bdbtot(o::Op_fixed) = o.bdbtot
le(o::Op_fixed) = o.le
basis(o::Op_fixed) = o.basis

bdb(o::Op_fixed,i,j) = bdbtot(o)[(1+(i-1)*le(o)):i*le(o),(1+(j-1)*le(o)):j*le(o)]
bbd(o::Op_fixed,i,j) = bdb(o,j,i)

# Here we create the full matrix with all
# fixed particle operators definitions
function operators_fixed(n::Int,m::Int)
    l1 = binomial(n+m-1,m)
    l2 = n*l1
    bas,ind = basis_m(n,m)
    z = spzeros(l2,l2)
    for i in 1:n
        for j in i:n
            if i==j
                z[(1+(i-1)*l1):i*l1,(1+(j-1)*l1):j*l1] = 1/2*bdb(bas,ind, m, i,j)
            else
                z[(1+(i-1)*l1):i*l1,(1+(j-1)*l1):j*l1] = bdb(bas,ind, m, i,j)
            end
        end
    end
    op_general = z+z'
    return op_general, l1, bas
end

function bd(n::Int64, m::Int64, i::Int64)
    base1, ind1 = basis_m(n,m)
    l2 = binomial(n+m-1,m)
    base2, ind2 = basis_m(n,m+1)
    l1 = binomial(n+m,m+1)
    op = spzeros(l1,l2)
    s2 = sparse([(m+2)^l for l in 0:(n-1)])
    for k in 1:l2
        j = myfind(ind2, (s2[end:-1:1]'*base1[k,:])+(m+2)^(n-i))[1]
        op[j,k] = 1
    end
    if m==0
        return op
    end
    return op
end

function b(n::Int64, m::Int64, i::Int64)
    op = bd(n, m-1, i)
    return op'
end

# This serves for applying several creations op
# at once. Modes will be a vector with the chosen
# modes
function bd(n::Int64, m::Int64, modes::Array{Int64,1})
    l = length(modes)
    op  = bd(n,m,modes[1])
    for k in 2:l
        op = bd(n,m+k-1,modes[k])*op
    end
    return op
end

function b(n::Int64, m::Int64, modes::Array{Int64,1})
    l = length(modes)
    op  = b(n,m,modes[1])
    for k in 2:l
        op = b(n,m-k+1,modes[k])*op
    end
    return op
end

function state_form(v::SparseMatrixCSC{Float64, Int64})
  i, j, w = findnz(v)
  l = size(v)[1]
  return sparsevec(i,w,l)
end


@diffusion_process Sine begin
    :parameters
    (a, b, c, σ) --> Float64

    :additional
    constdiff --> true
end

b(t, x, P::Sine) = ℝ{1}(P.a + P.b*sin.(P.c * x))

σ(t, x, P::Sine) = ℝ{1}(P.σ)


A = rand(3, 3)
B = ones(4, 4)
C = randn(5, 6)
K = A ⊗ B
X = collect(K)
v = rand(12)

K3 = A ⊗ B ⊗ C

@testset "multiply" begin
    @testset "Compatibility with other matrix ops" begin
        K = rand(3, 3) ⊗ randn(4, 4)
        K3 = rand(3, 3) ⊗ randn(4, 4) ⊗ randn(5, 6)
        KPow = kronecker(randn(3, 3), 3)

        for op in [adjoint, transpose, inv, pinv, conj]
            @test collect(op(K)) ≈ op(collect(K))
            if op != inv
                @test collect(op(K3)) ≈ op(collect(K3))
            else
                @test_throws DimensionMismatch op(K3)
            end
        end
    end

    @testset "Compatibility with Matrix Product" begin
        A_f = [randn(3, 4), randn(5, 6)]
        B_f = [randn(4, 2), randn(6, 7)]

        A = kronecker(A_f...)
        B = kronecker(B_f...)

        @test collect(A * B) ≈ collect(kronecker(map(*, A_f, B_f)...))

        C = randn(3, 3)
        D = randn(3, 2)
        C_pow = kronecker(C, 3)
        D_pow = kronecker(D, 3)
        @test collect(C_pow * D_pow) ≈ collect(kronecker(C*D, 3))

        @test_throws DimensionMismatch (D_pow * C_pow)
    end

    @testset "Vec trick" begin

        @test K * v ≈ X * v

        V = rand(4, 3)
        @test K * vec(V) ≈ X * vec(V)
        @test_throws DimensionMismatch K * V
        @test_throws DimensionMismatch K * reshape(V, 2, 6)

        v3 = randn(size(K3, 2))
        @test K3 * v3 ≈ collect(K3) * v3
        u = similar(v)
        @test mul!(u, K, v) ≈ X * v
    end

    @testset "Reshaped vec trick" begin
        @test K * v ≈ X * v

        V = sprand(4, 3, 1.0)
        @test K * vec(V) ≈ X * vec(V)
        @test_throws DimensionMismatch K * V
        @test_throws DimensionMismatch K * reshape(V, 2, 6)
        K3 = A ⊗ B ⊗ C
        v3 = sprand(size(K3, 2),1.0)
        @test K3 * vec(v3) ≈ collect(K3) * v3
        u = similar(v)
        @test mul!(u, K, v) ≈ X * v

    end

    @testset "AbstractKroneckerProduct * AbstractMatrix" begin
        rng = MersenneTwister(123456)
        a, b, x = randn(rng, 30, 20), randn(rng, 40, 50), randn(rng, 1000, 1100)

        @test kron(a, b) * x ≈ (a ⊗ b) * x
        @test kron(a, Eye(50)) * x ≈ (a ⊗ Eye(50)) * x
        @test kron(Eye(20), b) * x ≈ (Eye(20) ⊗ b) * x
    end

    @testset "sum" begin
        @test sum(K) ≈ sum(X)
        @test sum(K3) ≈ sum(collect(K3))

        @test sum(K, dims=1) ≈ sum(X, dims=1)
        @test sum(K3, dims=2) ≈ sum(collect(K3), dims=2)
        @test sum(K3, dims=2) isa AbstractKroneckerProduct

        @test sum(kronecker(A, 3)) ≈ sum(kron(A, A, A))
    end

    @inline function compare_against_kron(K, k, x, X, y, Y)
        @test collect(K) ≈ k

        res1 = (K * x)
        res2 = (K * X)
        @test res1 ≈ (k * x)
        @test res2 ≈ (k * X)

        if K isa AbstractKroneckerProduct
            res3 = (K \ y)
            res4 = (K \ Y)
            @test res3 ≈ (k \ y)
            @test res4 ≈ (k \ Y)
        end

        if size(K, 1) == size(x, 1)
            res1a = (x' * K)
            res1t = (transpose(x) * K)

            @test res1a ≈ (x' * k)
            @test res1t ≈ (transpose(x) * k)
            @test (K' * x) ≈ (k' * x) ≈ res1a'
            @test (transpose(K) * x) ≈ (transpose(k) * x) ≈ transpose(res1t)
        else
            @test_throws DimensionMismatch (x' * K)
            @test_throws DimensionMismatch (transpose(x) * K)
        end

        if size(K, 1) == size(X, 1)
            res2a = (X' * K)
            res2t = (transpose(X) * K)

            @test res2a ≈ (X' * k)
            @test res2t ≈ (transpose(X) * k)
            @test (K' * X) ≈ (k' * X) ≈ res2a'
            @test (transpose(K) * X) ≈ (transpose(k) * X) ≈ transpose(res2t)
        else
            @test_throws DimensionMismatch (X' * K)
            @test_throws DimensionMismatch (transpose(X) * K)
        end

    end

    @testset "2-factor square KroneckerProduct" begin
        A = randn(10, 10)
        B = randn(7, 7)
        x = randn(size(A, 2) * size(B, 2))
        X = randn(size(A, 2) * size(B, 2), 4)
        y = randn(size(A, 1) * size(B, 1))
        Y = randn(size(A, 1) * size(B, 1), 4)

        K1 = kronecker(A, B)
        K2 = kronecker(B, A)
        KS = kroneckersum(A, B)

        k1 = kron(A, B)
        k2 = kron(B, A)
        ks = kron(A, Diagonal(I, size(B, 1))) + kron(Diagonal(I, size(A, 1)), B)

        for (K, k) in [(K1, k1), (K2, k2), (KS, ks)]
            compare_against_kron(K, k, x, X, y, Y)
        end
    end

    @testset "2-factor rectangular KroneckerProduct" begin
        A = randn(10, 8)
        B = randn(7, 9)
        x = randn(size(A, 2) * size(B, 2))
        X = randn(size(A, 2) * size(B, 2), 4)
        y = randn(size(A, 1) * size(B, 1))
        Y = randn(size(A, 1) * size(B, 1), 4)

        K1 = kronecker(A, B)
        K2 = kronecker(B, A)
        k1 = kron(A, B)
        k2 = kron(B, A)

        for (K, k) in [(K1, k1), (K2, k2)]
            compare_against_kron(K, k, x, X, y, Y)
        end
    end

    @testset "3-factor rectangular KroneckerProduct" begin
        A = randn(10, 8)
        B = randn(7, 9)
        C = randn(6, 4)
        x = randn(size(A, 2) * size(B, 2) * size(C, 2))
        X = randn(size(A, 2) * size(B, 2) * size(C, 2), 4)
        y = randn(size(A, 1) * size(B, 1) * size(C, 1))
        Y = randn(size(A, 1) * size(B, 1) * size(C, 1), 4)

        K1 = kronecker(A, B, C)
        K2 = kronecker(B, A, C)
        k1 = kron(A, B, C)
        k2 = kron(B, A, C)

        for (K, k) in [(K1, k1), (K2, k2)]
            compare_against_kron(K, k, x, X, y, Y)
        end
    end

    @testset "10-factor square KroneckerProduct" begin
        matrices = [randn(2,2) for i in 1:10]
        v = randn(2^10)
        V = randn(2^10, 5)

        KP = kronecker(matrices...)
        KS = kroneckersum(matrices...)
        kp = kron(matrices...)
        ks = collect(KS)

        for (K, k) in [(KP, kp), (KS, ks)]
            compare_against_kron(K, k, v, V, v, V)
        end
    end

    @testset "10-factor square KroneckerPower" begin
        # reduce condition number of the matrix to avoid accidentally triggering a
        #  test failure
        A = randn(2,2) + 20I
        A /= opnorm(A)

        v = randn(2^10)
        V = randn(2^10, 5)

        K = kronecker(ntuple(_ -> A, 10)...)
        k = kron(ntuple(_ -> A, 10)...)

        compare_against_kron(K, k, v, V, v, V)
    end

    @testset "10-factor rectangular KroneckerProduct" begin
        matrices = [randn(3,2) for i in 1:10]
        x = randn(2^10)
        X = randn(2^10, 5)
        y = randn(3^10)
        Y = randn(3^10, 5)

        K = kronecker(matrices...)
        k = kron(matrices...)

        compare_against_kron(K, k, x, X, y, Y)
    end

    @testset "10-factor mixed KroneckerProduct" begin
        matrices1 = [randn(2,2) for i in 1:5]
        matrices2 = [randn(3,2) for i in 1:5]
        x = randn(2^10)
        X = randn(2^10, 5)
        y = randn(2^5 * 3^5)
        Y = randn(2^5 * 3^5, 5)

        K = kronecker(matrices1..., matrices2...)
        k = kron(matrices1..., matrices2...)

        @test collect(K) ≈ k

        compare_against_kron(K, k, x, X, y, Y)
    end

end


@testset "FBM" begin
    rng = MersenneTwister(42)
    h = 0.3
    k = FBMKernel(; h=h)
    v1 = rand(rng, 3)
    v2 = rand(rng, 3)
    @test k(v1, v2) ≈
          (
        sqeuclidean(v1, zero(v1))^h + sqeuclidean(v2, zero(v2))^h -
        sqeuclidean(v1 - v2, zero(v1 - v2))^h
    ) / 2 atol = 1e-5
    @test repr(k) == "Fractional Brownian Motion Kernel (h = $(h))"

    test_interface(k)
    @test repr(k) == "Fractional Brownian Motion Kernel (h = $(h))"
    test_ADs(FBMKernel; ADs=[:ReverseDiff])

    # Tests failing for ForwardDiff and Zygote@0.6.
    # Related to: https://github.com/FluxML/Zygote.jl/issues/1036
    f(x, y) = x^y
    @test_broken !isinf(
        Zygote.gradient((x, y) -> sum(f.(x, y)), zeros(1), fill(0.9, 1))[1][1]
    )

    test_params(k, ([h],))
end


# Separable sum, using arrays of arrays as variables

immutable SeparableSum{S <: AbstractArray} <: ProximableFunction
	fs::S
end

function (f::SeparableSum{S}){S <: AbstractArray}(x::AbstractArray)
	sum = 0.0
  for k in eachindex(f.fs)
		sum += f.fs[k](x[k])
	end
	return sum
end

function prox!{S <: AbstractArray}(f::SeparableSum{S}, x::AbstractArray, y::AbstractArray, gamma::Real=1.0)
  sum = 0.0
  for k in eachindex(f.fs)
	  sum += prox!(f.fs[k], x[k], y[k], gamma)
  end
  return sum
end

function prox(f::SeparableSum, x::AbstractArray, gamma::Real=1.0)
	y = map(similar, x)
	fy = prox!(f, x, y, gamma)
	return y, fy
end

fun_name(f::SeparableSum) = "separable sum"
function fun_dom(f::SeparableSum)
	# s = ""
	# for k in eachindex(f.fs)
	# 	s = string(s, fun_dom(f.fs[k]), " × ")
	# end
	# return s
	return "n/a" # for now
end
fun_expr(f::SeparableSum) = "(x₁,…,xᵣ) ↦ f[1](x₁) + … + f[r](xᵣ)"
function fun_params(f::SeparableSum)
	# s = "r = $(size(f.fs)), "
	# for k in eachindex(f.fs)
	# 	s = string(s, "f[$(k)] = $(typeof(f.fs[k])), ")
	# end
	# return s
	return "n/a" # for now
end


using DataStructures

function main()
    K = parse(Int, readline())
    INF = 10^9

    dists = INF * ones(Int, K)
    dists[2] = 1
    que = Deque{Int}()
    pushfirst!(que, 1)

    while !isempty(que)
        v = popfirst!(que)

        v′ = 10v % K
        if dists[v′+1] > dists[v+1]
            dists[v′+1] = dists[v+1]
            pushfirst!(que, v′)
        end

        v′ = (v + 1) % K
        if dists[v′+1] > dists[v+1] + 1
            dists[v′+1] = dists[v+1] + 1
            push!(que, v′)
        end
    end

    println(dists[1])
end

main()


include("godunov.jl")
using .Godunov
using Plots
fig = Plots.plot()

"""
NN = 100 # no of cells
Nt = 64000000 # no of time steps
Np = 6400000 # how often to plot
Ns = 5000 # when to start plotting
"""
NN = 40 # no of cells
#Nt = 8000 # no of time steps
#Np = 1000 # how often to plot
#Ns = 1#5000 # when to start plotting
tmax = 1.0e3 # max time
dt = 1.0e2 # how often to plot
tp = 0.0e0#5000 # when to start plotting

r=Godunov.ring(NN)#,xi=0.4,xf=3.0)
Godunov.getAlphas!(r)
#for i in 1:10
for tt in 0.0:dt:tmax
    tx = r.t
    if tx ≥ tp
        Plots.plot!(r.X_[1:NN÷1],r.Σ_[1:NN÷1],label="t: $tx",legend=false,xlim=(0,3))#:bottomright)#,title="Σ")#;legend=false)
        display(fig)
        print("time: $tx\n")
        Plots.plot!(r.X_[1:NN÷1],r.Φ_[1:NN÷1],label=false,linestyle=:dash)#,label="t: $t",legend=:topleft)#,title="J")#;legend=false)
        display(fig)
    end
#    for j in 1:800
        Godunov.step!(r)#,dt)
#    end
    Godunov.tstep!(r,dt)
end
tf=r.t
print("Done at t= $tf\n")


# -------------------------------------------------------------------------------------------
# X. Gandibleux - Metaheuristiques : TSP - Septembre 2019
# Base de depart « etudiant » pour exercice "VNS applique au TSP"

@static if VERSION < v"1.0-"
    error("NOT COMPLIANT WITH JULIA < v1.0.0")
end

using Random, Printf

# -------------------------------------------------------------------------------------------
# construit un distancier symetrique aux valeurs de distances uniformément aléatoires dans 1..vSup
function tsp_elaboreDistancierAleatoire(
           n::Int64,    # in : nombre de villes
           vSup::Int64  # in : plus grande distance entre deux villes
         )
    distancier = rand(1:vSup,n,n)
    for i=1:n
        for j=1:n
            distancier[i,j] = distancier[j,i]
        end
        distancier[i,i] = 0
    end
    return distancier # out : matrice carree symetrique de distances entieres
end

# -------------------------------------------------------------------------------------------
# calcule la longueur d’un circuit hamiltonien correspondant a une solution TSP admissible
function tsp_calculeLongueurCircuit(
           d::Matrix{Int64},  # in : distancier symetrique de ville a ville
           x::Array{Int64},   # in : solution admissible représentée par codage de permutation des villes
         )
    n::Int64 = length(x) # Nombre de villes
    longueur::Int64 = d[ x[n] , x[1] ]
    for i = 1:n-1
        longueur += d[ x[i] , x[i+1] ]
    end
    return longueur # out : longueur du circuit correspondant a la solution admissible
end

# -------------------------------------------------------------------------------------------
# construit une solution aléatoire TSP admissible représentée dans un codage de permutation
function tsp_elaboreSolutionAleatoire(
           n::Int64  # in : nombre de villes
         )
    return shuffle(collect(1:n))  # out : une permutation aleatoire des villes
end

# -------------------------------------------------------------------------------------------
# echange deux valeurs dans un vecteur sur des positions tirees aleatoirement
function swap!(
           x::Array{Int64}   # inout : vecteur d’entiers (codage binaire ou codage de permutation)
         )
    # swap entre informations aux positions i1 et i2
    i1 = rand(1:length(x))
    i2 = rand(1:length(x))
    x[i1], x[i2] = x[i2], x[i1]
end

# -------------------------------------------------------------------------------------------
function tsp_AppliqueLocalSearch2opt(
           d::Matrix{Int64}, # in : distancier symetrique de ville a ville
           x::Array{Int64},  # in : solution TSP admissible représentée pas un codage de permutation des villes
           z::Int64          # in : valeur de la solution TSP admissible
         )
    # A mettre en place ...
    return xSol, zSol    # out : meilleure solution TSP admissible trouvee et sa valeur
end

# -------------------------------------------------------------------------------------------
function tsp_solveVNS(
           d::Matrix{Int64},     # in : distancier symetrique de ville a ville
           xBest::Array{Int64},  # in : solution TSP admissible représentée pas un codage de permutation des villes
           zBest::Int64          # in : valeur de la solution TSP admissible
         )
    # A mettre en place ...
    return xBest, zBest, pBest    # out : meilleure solution TSP admissible trouvee, sa valeur, la courbe
end

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

function tsp_VNS(
           n::Int64    = 20,  # in : nombre de villes
           vSup::Int64 = 25,  # in : plus grande distance entre deux villes
           nRun::Int64 = 1,  # in : nombre de run VNS a realiser
         )

    d = tsp_elaboreDistancierAleatoire(n,vSup) # distancier aleatoire

    for run =1:nRun
        xInit = tsp_elaboreSolutionAleatoire(n)
        zInit = tsp_calculeLongueurCircuit(d,xInit)

        println("Init    : z(xInit) = ", zInit)
        #@show xInit
        println("  ")

        xBest, zBest, pBest = tsp_solveVNS(d,xInit,zInit)
        println("VNS     : z(xVNS)  = ", zBest)
        #@show xBest
        println("  ")
    end
end


#LogitDistLoss

using LaTeXStrings, LossFunctions, Measures, Latexify

logitfn = "L(r)=−ln*((4e^r)/(1+e^r)^2)"
logitdist = latexify(logitfn)
print(logitdist)
Plots.plot(LogitDistLoss(), c=:olive, linewidth=2, annotation=(0.5, .75, L"\left( r \right) = \left(  - ln \right) \cdot \frac{4 \ e^{r}}{\left( 1 + e^{r} \right)^{2}}", 18))


print("Testing ABCDQR_simula: ")
###################################
nt = 1000
m = ABCDQR_simula(nt)

y = m["y"]
A = m["A"]
C = m["C"]
Q = m["Q"]
R = m["R"]
x10 = 0*A[:,1]
P10 = 0*A

@test sum(y.^2) > 1e-6
####################################
println("OK")


abstract type AbstractCCARule <: AbstractODRule end

struct CCA{T} <: AbstractCCARule
    rule::T
    radius::Int
end

"""
    TCA(code; radius=1)

Returns a ```CCA``` object given a specific code and radius.
"""
function CCA(rule; radius=1)
    CCA(rule, radius)
end

function (cca::CCA)(starting_array)

    nextgen = evolution(starting_array, cca.rule, cca.radius)
end

function c_state_reader(neighborhood, radius)

    sum(neighborhood)/length(neighborhood)
end

function evolution(cell, rule, radius::Number)

    neighborhood_size = radius*2+1
    output = zeros(length(cell))
    cell = vcat(cell[end-neighborhood_size÷2+1:end], cell, cell[1:neighborhood_size÷2])

    for i=1:length(cell)-neighborhood_size+1
        output[i] = modf(c_state_reader(cell[i:i+neighborhood_size-1], radius)+rule)[1]
    end

    output
end

function evolution(cell, rule, radius::Tuple)

    neighborhood_size = sum(radius)+1
    output = zeros(length(cell))
    cell = vcat(cell[end-neighborhood_size÷2+1:end], cell, cell[1:neighborhood_size÷2])

    for i=1:length(cell)-neighborhood_size+1
        output[i] = modf(c_state_reader(cell[i:i+neighborhood_size-1], radius)+rule)[1]
    end

    output
end


export genAmpl2dDataFEM
function
genAmpl2dDataFEM(M::RegularMesh,SM::SparseMatrixCSC{Float64},MC::SparseMatrixCSC{Float64},b::Vector,P::SparseMatrixCSC,dobs::Vector,nbx::Vector,nby::Vector,pNoise::Int,ns::Int)

#-------------------------------------------------------------------------------
# Generating lookup table for variables u and w
#-------------------------------------------------------------------------------
  nn1  = M.n.+1
  nn2 = nn1-1
  np1 = prod(nn1)
  np2 = prod(nn2)
  wx  = zeros(Int64,np2,1)
  wy  = zeros(Int64,np2,1)
  ux  = zeros(Int64,np1,1)
  uy  = zeros(Int64,np1,1)
  #bc=(:dir,:neu,:neu,:neu,:neu,:neu)
  #iddir = getBoundaryIndicesFEM(M, bc)  # Dirichlet nodes
  #(dirX, dirY) = ind2sub(zeros(nn1[1],nn1[2]),iddir)
  for ii=1:nn2[2]
  	for jj=1:nn2[1]
  	wx[(ii-1)*nn2[1]+jj] = jj;
  	wy[(ii-1)*nn2[1]+jj] = ii;
  	nodes =	[jj+(nn2[1]+1)*(ii-1);jj+(nn2[1]+1)*(ii-1)+1;jj+(nn2[1]+1)*(ii)+1;jj+(nn2[1]+1)*ii];
  	ux[nodes[1]] = jj;  uy[nodes[1]] = ii;
  	ux[nodes[2]] = jj+1; uy[nodes[2]] = ii;
  	ux[nodes[3]] = jj+1; uy[nodes[3]] = ii+1;
  	ux[nodes[4]] = jj;   uy[nodes[4]] = ii+1;
    end
  end

#-------------------------------------------------------------------------------
# Generating data file for AMPL
#-------------------------------------------------------------------------------

	printAMPL = true
	fname = string(M.n[1], "x",	M.n[2],"S",ns,"P",length(dobs),"N",pNoise,".dat");
	fname1 = string(M.n[1], "x",M.n[2],"S",ns,"P",length(dobs),"N",pNoise);
	if printAMPL
		open(fname,"w") do f
			println(f,"data;")
			println(f, "param outfile:=",fname1, ";")
			println(f,"param Ly  := $(M.domain[2]-M.domain[1]) ;")
			println(f,"param Lx  := $(M.domain[4]-M.domain[3]) ;")
			println(f,"param Ny  := $((M.n[1])+1) ;")
			println(f,"param Nx  := $((M.n[2])+1) ;")
			println(f,"param NBH := $(length(dobs)) ;")
			@printf(f,"param nbx := ")
			for k=1:length(dobs)
				@printf(f, " %d %.8f",k, nbx[k])
			end
			@printf(f, ";\n")
			@printf(f,"param nby := ")
			for k=1:length(dobs)
				@printf(f, " %d %.8f",k, nby[k])
			end
			@printf(f, ";\n")
			for k=1:length(dobs)
				irow = find(P[k,:])
				for i=1:length(irow)
					@printf(f, "let Uhat[%d,%d]:=%.8f;\n",ux[irow[i]],uy[irow[i]],dobs[k])
				end
			end
			#for k=1:length(dirX)
			  #@printf(f, "fix U[%d,%d]:=0;\n",dirX[k], dirY[k])
			#end
			for k=1:length(dobs)
				irow = find(P[k,:])
				for i=1:length(irow)
					@printf(f, "let P[%d,%d]:=%d;\n",ux[irow[i]],uy[irow[i]],1)
				end
			end

			println(f, "model;")
			println(f, "subject to")
			for k=1:nn1[1]*nn1[2]
			  #if mod(k,nn1[1]) != 1
			    irows = find(SM[k,:])
			    print(f, "FEM_",k,": ")
			    for i=1:length(irows)
				  #@printf(f,"+ U[%d,%d]*SM[%d,%d]",ux[irows[i]],uy[irows[i]],k,irows[i])
				  if i==1
					@printf(f,"%.8f* U[%d,%d]", SM[k,irows[i]],ux[irows[i]],uy[irows[i]])

				  else
					@printf(f,"+ %.8f* U[%d,%d]", SM[k,irows[i]],ux[irows[i]],uy[irows[i]])

				  end
			    end
			    print(f,"=")
			    irows = find(MC[k,:])
			    for i=1:length(irows)
				  #@printf(f,"+ W[%d,%d]*MC[%d,%d]",wx[irows[i]],wy[irows[i]],k,irows[i])
				  if i==1
					@printf(f,"%.8f* W[%d,%d] + %.8f", MC[k,irows[i]],wx[irows[i]],wy[irows[i]], b[k])
				  else
					@printf(f,"+ %.8f* W[%d,%d] + %.8f", MC[k,irows[i]],wx[irows[i]],wy[irows[i]], b[k])
				  end
			    end
				if length(irows) == 0
			      @printf(f,"0")
				end
			    println(f,";")
			  #end
			end
		end
	end
	return
end


cases = ["B-splines","Cheb","Lin"]

# construct basis
holder = (BasisMatrices.Basis(BasisMatrices.SplineParams(15,-1,1,1),BasisMatrices.SplineParams(20,-5,2,3)),
    BasisMatrices.Basis(BasisMatrices.ChebParams(15,-1,1),BasisMatrices.ChebParams(20,-5,2)),
    BasisMatrices.Basis(BasisMatrices.LinParams(15,-1,1),BasisMatrices.LinParams(20,-5,2)))

@testset for (i, case) in enumerate(cases)
    basis = holder[i]

    # get nodes
    X, x12 = BasisMatrices.nodes(basis)

    # function to interpolate
    f(x1, x2) = cos(x1) ./ exp(x2)
    f(X::Matrix) = f(X[:, 1], X[:, 2])

    # function at nodes
    y = f(X)

    # benchmark coefficients
    c_direct, bs_direct = BasisMatrices.funfitxy(basis, X, y)

    @testset "test funfitxy for tensor and direct agree on coefs" begin
        c_tensor, bs_tensor = BasisMatrices.funfitxy(basis, x12, y)
        @test maxabs(c_tensor -  c_direct) <=  1e-12
    end

    @testset "test funfitf" begin
        c = BasisMatrices.funfitf(basis,f)
        @test maxabs(c -  c_direct) <=  1e-12
    end

    @testset "test funeval methods" begin
        # single point
        sp = BasisMatrices.funeval(c_direct,basis,X[5:5,:])[1]
        @test maxabs(sp -  y[5]) <= 1e-12

        # multiple points using tensor directly
        mp = BasisMatrices.funeval(c_direct,basis,x12)
        @test maxabs(mp -  y) <=  1e-12

        # multiple points using direct
        mp = BasisMatrices.funeval(c_direct,basis,X)
        @test maxabs(mp -  y) <=  1e-12

        # multiple points giving basis in direct form
        mpd = BasisMatrices.funeval(c_direct,bs_direct)
        @test maxabs(mpd -  y) <=  1e-12

        # multiple points giving basis in expanded form
        Phiexp = Base.convert(BasisMatrices.Expanded,bs_direct)
        mpe = BasisMatrices.funeval(c_direct,Phiexp)
        @test maxabs(mpe -  y) <=  1e-12

    end

    @testset "test interpoland methods" begin
        # (Basis,BasisMatrix,..)
        intp1 = BasisMatrices.Interpoland(basis,bs_direct,y)
        @test maxabs(BasisMatrices.evaluate(intp1,X) - y) <= 1e-12
        # (Basis,Array,..)
        intp2 = BasisMatrices.Interpoland(basis,X,y)
        @test maxabs(BasisMatrices.evaluate(intp2,X) - y) <= 1e-12
        # (BasisParams,Function)
        intp3 = BasisMatrices.Interpoland(basis,f)
        @test maxabs(BasisMatrices.evaluate(intp3,X) - y) <= 1e-12
    end

    @testset "Printing" begin
        iob = IOBuffer()
        show(iob, BasisMatrices.Interpoland(basis, bs_direct, y))
    end

    # TODO: call show on an interpoland instance to get coverage for writemime

end # testset


# This file is a part of JuliaFEM.
# License is MIT: see https://github.com/JuliaFEM/Mortar2D.jl/blob/master/LICENSE

using Test
using Mortar2D: calculate_normals

elements = Dict(
    1 => [1, 2],
    2 => [2, 3])
element_types = Dict(
    1 => :Seg2,
    2 => :Seg2)
X = Dict(
    1 => [7.0, 7.0],
    2 => [4.0, 3.0],
    3 => [0.0, 0.0])
n = calculate_normals(elements, element_types, X)
@test isapprox(n[1], 1/5*[4, -3])
@test isapprox(n[2], sqrt(2)/2*[1, -1])
@test isapprox(n[3], 1/5*[3, -4])


function diffeq(du,u,p,t)
    du[V.M] = p[C.kM]*p[C.S]/(p[C.KS]+p[C.S]) - p[C.dM]*u[V.M]
    du[V.E] = p[C.kE]*(u[V.M]/(p[C.KM]+u[V.M]))*(u[V.E]/(p[C.KE]+u[V.E])) + p[C.kb]*u[V.M]/(p[C.KM]+u[V.M]) +
                p[C.kP1]*u[V.CD]*u[V.RE]/(p[C.KCD]+u[V.RE]) + p[C.kP2]*u[V.CE]*u[V.RE]/(p[C.KCE]+u[V.RE]) - p[C.dE]*u[V.E]-p[C.kRE]*u[V.R]*u[V.E]
    du[V.CD] = p[C.kCD]*u[V.M]/(p[C.KM]+u[V.M]) + p[C.kCDS]*p[C.S]/(p[C.KS]+p[C.S]) - p[C.dCD]*u[V.CD]
    du[V.CE] = p[C.kCE]*u[V.E]/(p[C.KE]+u[V.E]) - p[C.dCE]*u[V.CE]
    du[V.R] = p[C.kR] + p[C.kDP]*u[V.RP]/(p[C.KRP]+u[V.RP]) - p[C.kRE]*u[V.R]*u[V.E] - p[C.kP1]*u[V.CD]*u[V.R]/(p[C.KCD]+u[V.R]) -
                p[C.kP2]*u[V.CE]*u[V.R]/(p[C.KCE]+u[V.R]) - p[C.dR]*u[V.R]
    du[V.RP] = p[C.kP1]*u[V.CD]*u[V.R]/(p[C.KCD]+u[V.R]) + p[C.kP2]*u[V.CE]*u[V.R]/(p[C.KCE]+u[V.R]) + p[C.kP1]*u[V.CD]*u[V.RE]/(p[C.KCD]+u[V.RE]) +
                p[C.kP2]*u[V.CE]*u[V.RE]/(p[C.KCE]+u[V.RE]) - p[C.kDP]*u[V.RP]/(p[C.KRP]+u[V.RP]) - p[C.dRP]*u[V.RP]
    du[V.RE] = p[C.kRE]*u[V.R]*u[V.E]-p[C.kP1]*u[V.CD]*u[V.RE]/(p[C.KCD]+u[V.RE]) - p[C.kP2]*u[V.CE]*u[V.RE]/(p[C.KCE]+u[V.RE]) - p[C.dRE]*u[V.RE]

end

function get_steady_state(p::Vector{Float64})
    tspan::Tuple{Float64,Float64} = (0.0,Inf)
    u0::Vector{Float64} = zeros(V.NUM)

    prob = ODEProblem(diffeq,u0,tspan,p)
    prob = SteadyStateProblem(prob)
    sol = solve(prob,DynamicSS(CVODE_BDF()),dt=1.0)

    return sol.u
end


using Example2018
using Test

# Polynomials form a ring

# 1. addition is associative
# 2. additive identity
# 3. additive inverse
# 4. addition commutes
# 5. multipilcation is ... (not code this part on the code)

#@testset "Polynomials form a ring" begin
#
#    z = Polynomial([0])
#
#    for n in 1:100
#
#        deg = rand(0:100)
#        p = Polynomial(rand(-10000000:10000000,deg+1))
#
#        @test p + z == p
#        @test z + p == p
#        @test p + (-p) == z
#    end
#
#end


import OrdinaryDiffEq
import QuantumOpticsBase

"""
    liouvilleVonNeumann(rho0, tspan, H;
    \treltol=1.0e-12, abstol=1.0e-12, fout=nothing, alg=OrdinaryDiffEq.Tsit5())

Integrate Liouville-von Neumann equation to evolve states or compute propagators

``\\frac{d}{d t} \\rho(t) = - \\frac{i}{\\hbar} [ \\hat H, \\rho(t) ], \\quad \\hbar = 1``.

# Arguments
* `rho0`: Initial state vector (can be a bra or a ket) or initial propagator.
* `tspan`: Vector specifying the points of time for which output should be displayed.
* `H`: Arbitrary operator specifying the Hamiltonian.
* `reltol`: Relative tolerance for OrdinaryDiffEq solver and its inner states.
* `abstol`: Absolute tolerance for OrdinaryDiffEq solver and its inner states.
* `fout=nothing`: If given, this function `fout(t, rho)` is called every time
        an output should be displayed. ATTENTION: The state `rho` is neither
        normalized nor permanent! It is still in use by the ode solver and
        therefore must not be changed.
* `alg`: Algorithm with which OrdinaryDiffEq will solve LvN equation.
"""
function liouvilleVonNeumann(
    rho0::T,
    tspan::Array,
    H::AbstractOperator{B,B};
    reltol::Float64 = 1.0e-12,
    abstol::Float64 = 1.0e-12,
    alg::OrdinaryDiffEq.OrdinaryDiffEqAlgorithm = OrdinaryDiffEq.DP5(),
    fout::Union{Function,Nothing} = nothing,
    kwargs...,
) where {B<:Basis,T<:Operator{B,B}}
    # tmp = copy(rho0)
    dliouvilleVonNeumann_(t, rho::T, drho::T) = dliouvilleVonNeumann(rho, H, drho)
    tspan_ = convert(Vector{float(eltype(tspan))}, tspan)
    x0 = rho0.data
    state = T(rho0.basis_l, rho0.basis_r, rho0.data)
    dstate = T(rho0.basis_l, rho0.basis_r, rho0.data)
    OpenQuantumSystems.integrate(
        tspan_,
        dliouvilleVonNeumann_,
        x0,
        state,
        dstate,
        fout;
        reltol = reltol,
        abstol = abstol,
        alg = alg,
        kwargs...,
    )
end

function dliouvilleVonNeumann(
    rho::T,
    H::AbstractOperator{B,B},
    drho::T,
) where {B<:Basis,T<:Operator{B,B}}
    QuantumOpticsBase.mul!(drho, H, rho, -eltype(rho)(im), zero(eltype(rho)))
    QuantumOpticsBase.mul!(drho, rho, H, eltype(rho)(im), one(eltype(rho)))
    return drho
end


module Day11
export power_level, part_1, part_2

function power_level(x::Int, y::Int, serial::Int)
    rack_id = x + 10
    power = rack_id * (rack_id * y + serial)
    power = div(power, 100) % 10
    return power - 5
end

function part_1(serial::Int)::Tuple{Int, Int, Int}
    power_grid = [power_level(x, y, serial) for x in 1:300, y in 1:300]

    max_power = 0
    best_x = -1
    best_y = -1
    for x in 1:298
        for y in 1:298
            power = 0
            for dx in 0:2
                for dy in 0:2
                    power += power_grid[x + dx, y + dy]
                end
            end
            if power > max_power
                best_x = x
                best_y = y
                max_power = power
            end
        end
    end

    return best_x, best_y, max_power
end

function part_2(serial::Int)::Tuple{Int, Int, Int, Int}
    power_grid = [power_level(x, y, serial) for x in 1:300, y in 1:300]

    max_power = 0
    best_x = -1
    best_y = -1
    best_n = -1
    for n in 1:300
        for x in 1:(300 - n - 1)
            for y in 1:(300 - n - 1)
                power = 0
                for dx in 0:(n - 1)
                    for dy in 0:(n - 1)
                        power += power_grid[x + dx, y + dy]
                    end
                end
                if power > max_power
                    best_x = x
                    best_y = y
                    best_n = n
                    max_power = power
                end
            end
        end
    end

    return best_x, best_y, best_n, max_power
end

end


using DelimitedFiles

content = readdlm("input.txt", ' ')

mutable struct SubmarineState
    x::Int
    y::Int
    aim::Int
end

p = SubmarineState(0, 0, 0)

function modifyState(instruction)
    command, value = instruction
    if command == "forward"
        p.x += value
        p.y += p.aim * value
    elseif command == "down"
        p.aim += value
    else
        p.aim -= value
    end
end

for i in 1:length(content[:, 1])
    modifyState(content[i, :])
end

p.x * p.y


"""
    abstract type AbstractLine{T, N, S} <: AbstractGeometry{T, N} end

T: eltype
N: Dimension
S: Coordinate System: `:cartesian` or `:cylindrical`
"""
abstract type AbstractLine{T, N, S} <: AbstractGeometry{T, N} end

"""
    struct Line{T, N, S} <: AbstractLine{T, N, S}

<----A----------B---->
"""
struct Line{T, N, S} <: AbstractLine{T, N, S}
    org::AbstractCoordinatePoint{T, N, S}
    dir::AbstractCoordinateVector{T, N, S}
end

function Line(p1::CartesianPoint{T}, p2::CartesianPoint{T})::Line{T, 3, :cartesian} where {T}
    return Line{T, 3, :cartesian}(p1, p2 - p1)
end
function in(pt::CartesianPoint{T}, l::Line{T, 3, :cartesian}; atol::Real = T(0))::Bool where {T}
    norm((pt - l.org) × l.dir) <= atol
end

"""
    struct Ray{T, N, S} <: AbstractLine{T, N, S}

    [A----------B---->
"""
struct Ray{T, N, S} <: AbstractLine{T, N, S}
    org::AbstractCoordinatePoint{T, N, S}
    dir::AbstractCoordinateVector{T, N, S}
end
function Ray(p1::CartesianPoint{T}, p2::CartesianPoint{T})::Ray{T, 3, :cartesian} where {T}
    return Ray{T, 3, :cartesian}(p1, p2 - p1)
end
function in(pt::CartesianPoint{T}, l::Ray{T, 3, :cartesian}; atol::Real = T(0))::Bool where {T}
    d::CartesianVector{T} = pt - l.org
    if norm(d × l.dir) <= atol
        return d ⋅ l.dir >= 0
    end
    return false
end

"""
    struct LineSegment{T, N, S} <: AbstractLine{T, N, S}

    [A----------B]
"""
struct LineSegment{T, N, S} <: AbstractLine{T, N, S}
    org::AbstractCoordinatePoint{T, N, S}
    dir::AbstractCoordinateVector{T, N, S}
end

function LineSegment(p1::CartesianPoint{T}, p2::CartesianPoint{T})::LineSegment{T, 3, :cartesian} where {T}
    return LineSegment{T, 3, :cartesian}(p1, p2 - p1)
end
function in(pt::CartesianPoint{T}, l::LineSegment{T, 3, :cartesian}; atol::T = T(0))::Bool where {T}
    d::CartesianVector{T} = pt - l.org
    if norm(d × l.dir) <= atol
        d2::CartesianVector{T} = d - l.dir
        return (d ⋅ l.dir >= 0) && (d2 ⋅ l.dir <= 0)
    end
    return false
end

function iscolinear(l1::AbstractLine{T, 3, :cartesian}, l2::AbstractLine{T, 3, :cartesian}, atol::Real = T(0))::Bool where {T}
    return norm(l1.dir × l2.dir) <= atol # l1.dir == l2.dir || norm(l1.dir × l2.dir) == 0
end

# Plot Recipes:
@recipe function f(l::AbstractLine{T, 3, :cartesian}) where {T}
    x::Vector{T} = [l.org.x, l.org.x + l.dir.x]
    y::Vector{T} = [l.org.y, l.org.y + l.dir.y]
    z::Vector{T} = [l.org.z, l.org.z + l.dir.z]
    x, y, z
end

@recipe function f(l::AbstractLine{T, 2, :cartesian}) where {T}
    x::Vector{T} = [l.org.x, l.org.x + l.dir.x]
    y::Vector{T} = [l.org.y, l.org.y + l.dir.y]
    x, y
end

@recipe function f(ls::Array{<:AbstractLine{T, 3, :cartesian}, 1}) where {T}
    linecolor --> 1
    for l in ls
        @series begin
            label := ""
            l
        end
    end
    @series begin
        label --> ""
        [], []
    end
end


using Mea.Green
using Base.Test

 @testset "Testing Green.jl" begin

    # setup
    fin_gf_to = "./data/self_short_moy.dat"
    (zn, gf_c) = Green.readgreen_c(fin_gf_to, zn_col=1)
    gf_t = [(7.29148945e+00 -1.25688297e+00im) (4.31791692e+00 -7.37366863e-01im) (-4.37618486e+00 +7.44382438e-01im) (6.02348575e+00 -9.90968651e-01im) (4.05606418e+00 -4.07146712e-01im);
            (6.37534240e+00 -3.08656815e+00im) (3.92112500e+00 -1.86394356e+00im) (-3.80134014e+00 +1.78496589e+00im) (5.15813767e+00 -2.33078630e+00im) (3.66679027e+00 -9.42835945e-01im)]

    @testset "test_readgreen_c" begin:
        """ """
        zn_test = [5.235989e-002, 1.570800e-001]
        @test isapprox(zn, zn_test, atol=1e-6)

        gf_c_test = Green.t_to_c(zn, gf_t)
        gf_t_test = Green.c_to_t(zn, gf_c_test)

        @test isapprox(gf_c, gf_c_test)
        @test isapprox(gf_t, gf_t_test)

    end

    @testset "test_c" begin

        gf_c_test = zeros(Complex{Float64}, (size(zn)[1], 4, 4))
        gf_c_test[:, 1, 1] = gf_t[:, 1] ; gf_c_test[:, 1, 2] = gf_t[:, 3] ; gf_c_test[:, 1, 3] = gf_t[:, 4] ; gf_c_test[:, 1, 4] = gf_t[:, 3]
        gf_c_test[:, 2, 1] = gf_c_test[:, 1, 2]; gf_c_test[:, 2, 2] = gf_t[:, 2] ; gf_c_test[:, 2, 3] = gf_c_test[:, 1, 2] ; gf_c_test[:, 2, 4] = gf_t[:, 5]
        gf_c_test[:, 3, 1] = gf_c_test[:, 1, 3] ; gf_c_test[:, 3, 2] = gf_c_test[:, 2, 3] ; gf_c_test[:, 3, 3] = gf_c_test[:, 1, 1] ; gf_c_test[:, 3, 4] = gf_c_test[:, 1, 2]
        gf_c_test[:, 4, 1] = gf_c_test[:, 1, 4] ; gf_c_test[:, 4, 2] = gf_c_test[:, 2, 4] ; gf_c_test[:, 4, 3] = gf_c_test[:, 3, 4] ; gf_c_test[:, 4, 4] = gf_c_test[:, 2, 2]

        gf_c_test_02 = [(6.02348575e+00 -9.90968651e-01im),  (5.15813767e+00 -2.33078630e+00im)]#, (4.28316747e+00 -2.88633370e+00im), (6.68961495e-02 +1.31238862e-02im)]
        gf_c_test_00 = [(7.29148945e+00 -1.25688297e+00im), (6.37534240e+00 -3.08656815e+00im)]#,  (5.42233260e+00 -4.06248226e+00im), (5.55747767e+00 -2.25696764e+00im)]

        @test isapprox(gf_c, gf_c_test, atol=1e-6)
        @test isapprox(gf_c[:, 1, 3], gf_c_test_02, atol=1e-3)
        @test isapprox(gf_c[:, 1, 1], gf_c_test_00,  atol=1e-7)

   end

    @testset "test_t_to_c" begin

        gf_to_test = Green.c_to_to(zn, gf_c)
        gf_t_test = Green.c_to_t(zn, gf_c)

        gf_test1_c = Green.to_to_c(zn, gf_to_test)
        gf_test2_c = Green.t_to_c(zn, gf_t_test)

        @test isapprox(gf_c, gf_test1_c, rtol=1e-7, atol=1e-7)
        @test isapprox(gf_c, gf_test2_c, rtol=1e-7, atol=1e-7)

    end

 end  # testset


together_rules = [
    # TODO: This rule needs to be defined in three different ways to account for
    # the possible orderings of the numerators and denominators (the middle rule
    # covers two cases because it is associative). It would be nice to replace
    # them all with a single rule.
    @acrule(~a * (~b)^-1 + ~c * (~d)^-1 => (~a*~d + ~b*~c) / (~b*~d))
    @acrule((~b)^-1 * ~a + ~c * (~d)^-1 => (~a*~d + ~b*~c) / (~b*~d))
    @acrule((~b)^-1 * ~a + (~d)^-1 * ~c => (~a*~d + ~b*~c) / (~b*~d))
]
together_rewriter =
    SymbolicUtils.Postwalk(
        SymbolicUtils.PassThrough(
            SymbolicUtils.Chain(together_rules)
        )
    )

together(ex; kwargs...) = simplify(ex; rewriter=together_rewriter, kwargs...)


# differentiated version of IR_stab.jl

function getIRA0_diff(params::ParamType{2},
                      q::AbstractArray{Tsol,1},
                      q_dot::AbstractMatrix,
                      A0::AbstractMatrix,
                      A0_dot::Abstract3DArray) where Tsol

  nd = size(q_dot, 2)  # number of dot vectors
  numDofPerNode = size(q, 1)

  @debug1 begin
    @assert size(q, 1) == size(q_dot, 1)
    @assert size(A0_dot, 3) >= nd
    @assert size(A0, 1) == size(A0_dot, 1)
    @assert size(A0, 2) == size(A0_dot, 2)
  end

  p_dot = params.get_ira0data.p_dot
  gamma = params.gamma
  gamma_1 = params.gamma_1

  # p_dot is needed for i=1:nd, so compute the derivative with respect to
  # q[1:4] and then multiply with q_dot[:, i] later
  p = calcPressure_diff(params, q, p_dot)

  rho = q[1]
  rhou = q[2]
  rhov = q[3]
  rhoe = q[4]

  rhoinv = 1/rho

  h = (rhoe + p)*rhoinv  # this isn't really h, but including the factor of
                         # 1/rho is convenient
  a2 = gamma*p*rhoinv  # speed of sound

  A0[1,1] = rho
  A0[2,1] = rhou
  A0[3,1] = rhov
  A0[4,1] = rhoe

  A0[1,2] = rhou
  A0[2,2] = rhou*rhou*rhoinv + p
  A0[3,2] = rhou*rhov*rhoinv
  A0[4,2] = rhou*h

  A0[1,3] = rhov
  A0[2,3] = rhou*rhov*rhoinv
  A0[3,3] = rhov*rhov*rhoinv + p
  A0[4,3] = rhov*h

  A0[1,4] = rhoe
  A0[2,4] = h*rhou
  A0[3,4] = h*rhov
  A0[4,4] = rho*h*h - a2*p/gamma_1

  for i=1:nd

    # compute p_dot
    p_dot_i = zero(Tsol)
    for j=1:numDofPerNode
      p_dot_i += p_dot[j]*q_dot[j, i]
    end

    rho_dot  = q_dot[1, i]
    rhou_dot = q_dot[2, i]
    rhov_dot = q_dot[3, i]
    rhoe_dot = q_dot[4, i]

    rhoinv = 1/rho
    rhoinv_dot = -rhoinv*rhoinv*rho_dot

    h = (rhoe + p)*rhoinv
    h_dot = (rhoe + p)*rhoinv_dot + (rhoe_dot + p_dot_i)*rhoinv

    a2 = gamma*p*rhoinv
    a2_dot = gamma*(p_dot_i*rhoinv + p*rhoinv_dot)

    A0_dot[1,1,i] = rho_dot
    A0_dot[2,1,i] = rhou_dot
    A0_dot[3,1,i] = rhov_dot
    A0_dot[4,1,i] = rhoe_dot

    A0_dot[1,2,i] = rhou_dot
    A0_dot[2,2,i] = 2*rhou*rhoinv*rhou_dot + rhou*rhou*rhoinv_dot + p_dot_i
    A0_dot[3,2,i] = rhov*rhoinv*rhou_dot   + rhou*rhoinv*rhov_dot + rhou*rhov*rhoinv_dot
    A0_dot[4,2,i] = h*rhou_dot + rhou*h_dot

    A0_dot[1,3,i] = rhov_dot
    A0_dot[2,3,i] = rhov*rhoinv*rhou_dot   + rhou*rhoinv*rhov_dot + rhou*rhov*rhoinv_dot
    A0_dot[3,3,i] = 2*rhov*rhoinv*rhov_dot + rhov*rhov*rhoinv_dot + p_dot_i
    A0_dot[4,3,i] = h*rhov_dot + rhov*h_dot

    A0_dot[1,4,i] = rhoe_dot
    A0_dot[2,4,i] = rhou*h_dot  + h*rhou_dot
    A0_dot[3,4,i] = rhov*h_dot  + h*rhov_dot
    A0_dot[4,4,i] = h*h*rho_dot + rho*2*h*h_dot - (p*a2_dot + p_dot_i*a2)/gamma_1
  end

  return nothing
end

function getIRA0_diff(params::ParamType{3},
                      q::AbstractArray{Tsol,1},
                      q_dot::AbstractMatrix,
                      A0::AbstractMatrix,
                      A0_dot::Abstract3DArray) where Tsol

  nd = size(q_dot, 2)  # number of dot vectors
  numDofPerNode = size(q, 1)

  @debug1 begin
    @assert size(q, 1) == size(q_dot, 1)
    @assert size(A0_dot, 3) == nd
    @assert size(A0, 1) == size(A0_dot, 1)
    @assert size(A0, 2) == size(A0_dot, 2)
  end

  p_dot = zeros(Tsol, numDofPerNode)
  gamma = params.gamma
  gamma_1 = params.gamma_1

  # p_dot is needed for i=1:nd, so compute the derivative with respect to
  # q[1:4] and then multiply with q_dot[:, i] later
  p = calcPressure_diff(params, q, p_dot)

  rho = q[1]
  rhou = q[2]
  rhov = q[3]
  rhow = q[4]
  rhoe = q[5]

  rhoinv = 1/rho

  h = (rhoe + p)*rhoinv
  a2 = gamma*p*rhoinv  # speed of sound

  A0[1,1] = rho
  A0[2,1] = rhou
  A0[3,1] = rhov
  A0[4,1] = rhow
  A0[5,1] = rhoe

  A0[1,2] = rhou
  A0[2,2] = rhou*rhou*rhoinv + p
  A0[3,2] = rhou*rhov*rhoinv
  A0[4,2] = rhou*rhow*rhoinv
  A0[5,2] = rhou*h

  A0[1,3] = rhov
  A0[2,3] = rhou*rhov/rho
  A0[3,3] = rhov*rhov*rhoinv + p
  A0[4,3] = rhov*rhow*rhoinv
  A0[5,3] = rhov*h

  A0[1,4] = rhow
  A0[2,4] = rhow*rhou*rhoinv
  A0[3,4] = rhow*rhov*rhoinv
  A0[4,4] = rhow*rhow*rhoinv + p
  A0[5,4] = rhow*h

  A0[1,5] = rhoe
  A0[2,5] = h*rhou
  A0[3,5] = h*rhov
  A0[4,5] = h*rhow
  A0[5,5] = rho*h*h - a2*p/gamma_1

  for i=1:nd

    # compute p_dot
    p_dot_i = zero(Tsol)
    for j=1:numDofPerNode
      p_dot_i += p_dot[j]*q_dot[j, i]
    end

    rho_dot  = q_dot[1, i]
    rhou_dot = q_dot[2, i]
    rhov_dot = q_dot[3, i]
    rhow_dot = q_dot[4, i]
    rhoe_dot = q_dot[5, i]

    rhoinv = 1/rho
    rhoinv_dot = -1*rhoinv*rhoinv*rho_dot

    h = (rhoe + p)*rhoinv
    h_dot = (rhoe + p)*rhoinv_dot + (rhoe_dot + p_dot_i)*rhoinv

    a2 = gamma*p*rhoinv
    a2_dot = gamma*(p_dot_i*rhoinv + p*rhoinv_dot)

    A0_dot[1,1,i] = rho_dot
    A0_dot[2,1,i] = rhou_dot
    A0_dot[3,1,i] = rhov_dot
    A0_dot[4,1,i] = rhow_dot
    A0_dot[5,1,i] = rhoe_dot

    A0_dot[1,2,i] = rhou_dot
    A0_dot[2,2,i] = 2*rhou*rhoinv*rhou_dot + rhou*rhou*rhoinv_dot + p_dot_i
    A0_dot[3,2,i] = rhov*rhoinv*rhou_dot   + rhou*rhoinv*rhov_dot + rhou*rhov*rhoinv_dot
    A0_dot[4,2,i] = rhow*rhoinv*rhou_dot   + rhou*rhoinv*rhow_dot + rhou*rhow*rhoinv_dot
    A0_dot[5,2,i] = h*rhou_dot + rhou*h_dot

    A0_dot[1,3,i] = rhov_dot
    A0_dot[2,3,i] = rhov*rhoinv*rhou_dot   + rhou*rhoinv*rhov_dot + rhou*rhov*rhoinv_dot
    A0_dot[3,3,i] = 2*rhov*rhoinv*rhov_dot + rhov*rhov*rhoinv_dot + p_dot_i
    A0_dot[4,3,i] = rhov*rhoinv*rhow_dot   + rhow*rhoinv*rhov_dot + rhov*rhow*rhoinv_dot
    A0_dot[5,3,i] = h*rhov_dot + rhov*h_dot

    A0_dot[1,4,i] = rhow_dot
    A0_dot[2,4,i] = rhow*rhoinv*rhou_dot   + rhou*rhoinv*rhow_dot + rhou*rhow*rhoinv_dot
    A0_dot[3,4,i] = rhow*rhoinv*rhov_dot   + rhov*rhoinv*rhow_dot + rhow*rhov*rhoinv_dot
    A0_dot[4,4,i] = 2*rhow*rhoinv*rhow_dot + rhow*rhow*rhoinv_dot + p_dot_i
    A0_dot[5,4,i] = h*rhow_dot + rhow*h_dot

    A0_dot[1,5,i] = rhoe_dot
    A0_dot[2,5,i] = rhou*h_dot  + h*rhou_dot
    A0_dot[3,5,i] = rhov*h_dot  + h*rhov_dot
    A0_dot[4,5,i] = rhow*h_dot  + h*rhow_dot
    A0_dot[5,5,i] = h*h*rho_dot + rho*2*h*h_dot - (p*a2_dot + p_dot_i*a2)/gamma_1
  end

  return nothing
end

"""
  Reverse mode wrt q of [`getIRA0`](@ref).

  **Inputs**

   * params
   * q: solution vector
   * A0_bar: seed matrix for reverse mode

  **Inputs/Outputs**

   * q_bar: dual part of `q`, to be updated (not overwritten), length `numDofPerNode`
   * A0: matrix, `numDofPerNode` to be overwritten
"""
function getIRA0_revq(params::ParamType{2},
                     q::AbstractArray{Tsol,1},
                     q_bar::AbstractArray{Tsol, 1},
                     A0::AbstractMatrix{Tsol},
                     A0_bar::AbstractMatrix{Tsol}) where Tsol

  gamma = params.gamma
  gamma_1 = params.gamma_1
  p = calcPressure(params, q)

  rho = q[1]
  rhou = q[2]
  rhov = q[3]
  rhoe = q[4]

  rhoinv = 1/rho

  h = (rhoe + p)*rhoinv  # this isn't really h, but including the factor of
                         # 1/rho is convenient
  a2 = gamma*p*rhoinv  # speed of sound

  A0[1,1] = rho
  A0[2,1] = rhou
  A0[3,1] = rhov
  A0[4,1] = rhoe

  A0[1,2] = rhou
  A0[2,2] = rhou*rhou*rhoinv + p
  A0[3,2] = rhou*rhov*rhoinv
  A0[4,2] = rhou*h

  A0[1,3] = rhov
  A0[2,3] = rhou*rhov*rhoinv
  A0[3,3] = rhov*rhov*rhoinv + p
  A0[4,3] = rhov*h

  A0[1,4] = rhoe
  A0[2,4] = h*rhou
  A0[3,4] = h*rhov
  A0[4,4] = rho*h*h - a2*p/gamma_1

  # reverse sweep
  rho_bar = zero(Tsol)
  rhou_bar = zero(Tsol)
  rhov_bar = zero(Tsol)
  rhoe_bar = zero(Tsol)
  h_bar = zero(Tsol)
  rhoinv_bar = zero(Tsol)
  p_bar = zero(Tsol)
  a2_bar = zero(Tsol)

  # first column
  rho_bar  += A0_bar[1,1]
  rhou_bar += A0_bar[2,1]
  rhov_bar += A0_bar[3,1]
  rhoe_bar += A0_bar[4,1]

  # second column
  rhou_bar += A0_bar[1,2]

  rhou_bar   += 2*rhou*rhoinv*A0_bar[2,2]
  rhoinv_bar += rhou*rhou*A0_bar[2,2]
  p_bar      += A0_bar[2,2]

  rhou_bar   += rhov*rhoinv*A0_bar[3,2]
  rhov_bar   += rhou*rhoinv*A0_bar[3,2]
  rhoinv_bar += rhou*rhov*A0_bar[3,2]

  rhou_bar += h*A0_bar[4,2]
  h_bar    += rhou*A0_bar[4,2]

  # 3rd column
  rhov_bar += A0_bar[1,3]

  rhou_bar   += rhov*rhoinv*A0_bar[2,3]
  rhov_bar   += rhou*rhoinv*A0_bar[2,3]
  rhoinv_bar += rhou*rhov*A0_bar[2,3]

  rhov_bar   += 2*rhov*rhoinv*A0_bar[3,3]
  rhoinv_bar += rhov*rhov*A0_bar[3,3]
  p_bar      += A0_bar[3,3]

  rhov_bar += h*A0_bar[4,3]
  h_bar    += rhov*A0_bar[4,3]

  # 4th column
  rhoe_bar += A0_bar[1,4]

  h_bar     += rhou*A0_bar[2,4]
  rhou_bar  +=  h*A0_bar[2,4]

  h_bar    += rhov*A0_bar[3,4]
  rhov_bar += h*A0_bar[3,4]

  rho_bar += h*h*A0_bar[4,4]
  h_bar   += 2*rho*h*A0_bar[4,4]
  a2_bar  += -p*A0_bar[4,4]/gamma_1
  p_bar   += -a2*A0_bar[4,4]/gamma_1

  # a2
  p_bar      += gamma*rhoinv*a2_bar
  rhoinv_bar += gamma*p*a2_bar

  # h
  rhoe_bar   += rhoinv*h_bar
  p_bar      += rhoinv*h_bar
  rhoinv_bar += (rhoe + p)*h_bar

  # rhoinv
  rho_bar += -rhoinv*rhoinv*rhoinv_bar

  calcPressure_revq(params, q, q_bar, p_bar)

  q_bar[1] += rho_bar
  q_bar[2] += rhou_bar
  q_bar[3] += rhov_bar
  q_bar[4] += rhoe_bar

  return nothing
end

function getIRA0_revq(params::ParamType{3},
                     q::AbstractArray{Tsol,1},
                     q_bar::AbstractArray{Tsol, 1},
                     A0::AbstractMatrix{Tsol},
                     A0_bar::AbstractMatrix{Tsol}) where Tsol

  gamma = params.gamma
  gamma_1 = params.gamma_1
  p = calcPressure(params, q)

  rho = q[1]
  rhou = q[2]
  rhov = q[3]
  rhow = q[4]
  rhoe = q[5]

  rhoinv = 1/rho

  h = (rhoe + p)*rhoinv  # this isn't really h, but including the factor of
                         # 1/rho is convenient
  a2 = gamma*p*rhoinv  # speed of sound

  A0[1,1] = rho
  A0[2,1] = rhou
  A0[3,1] = rhov
  A0[4,1] = rhow
  A0[5,1] = rhoe

  A0[1,2] = rhou
  A0[2,2] = rhou*rhou*rhoinv + p
  A0[3,2] = rhou*rhov*rhoinv
  A0[4,2] = rhou*rhow*rhoinv
  A0[5,2] = rhou*h

  A0[1,3] = rhov
  A0[2,3] = rhou*rhov*rhoinv
  A0[3,3] = rhov*rhov*rhoinv + p
  A0[4,3] = rhov*rhow*rhoinv
  A0[5,3] = rhov*h

  A0[1,4] = rhow
  A0[2,4] = rhow*rhou*rhoinv
  A0[3,4] = rhow*rhov*rhoinv
  A0[4,4] = rhow*rhow*rhoinv + p
  A0[5,4] = rhow*h

  A0[1,5] = rhoe
  A0[2,5] = h*rhou
  A0[3,5] = h*rhov
  A0[4,5] = h*rhow
  A0[5,5] = rho*h*h - a2*p/gamma_1

  # reverse sweep
  rho_bar = zero(Tsol)
  rhou_bar = zero(Tsol)
  rhov_bar = zero(Tsol)
  rhow_bar = zero(Tsol)
  rhoe_bar = zero(Tsol)
  h_bar = zero(Tsol)
  rhoinv_bar = zero(Tsol)
  p_bar = zero(Tsol)
  a2_bar = zero(Tsol)

  # first column
  rho_bar  += A0_bar[1,1]
  rhou_bar += A0_bar[2,1]
  rhov_bar += A0_bar[3,1]
  rhow_bar += A0_bar[4,1]
  rhoe_bar += A0_bar[5,1]

  # second column
  rhou_bar += A0_bar[1,2]

  rhou_bar   += 2*rhou*rhoinv*A0_bar[2,2]
  rhoinv_bar += rhou*rhou*A0_bar[2,2]
  p_bar      += A0_bar[2,2]

  rhou_bar   += rhov*rhoinv*A0_bar[3,2]
  rhov_bar   += rhou*rhoinv*A0_bar[3,2]
  rhoinv_bar += rhou*rhov*A0_bar[3,2]

  rhou_bar   += rhow*rhoinv*A0_bar[4,2]
  rhow_bar   += rhou*rhoinv*A0_bar[4,2]
  rhoinv_bar += rhou*rhow*A0_bar[4,2]

  rhou_bar += h*A0_bar[5,2]
  h_bar    += rhou*A0_bar[5,2]

  # 3rd column
  rhov_bar += A0_bar[1,3]

  rhou_bar   += rhov*rhoinv*A0_bar[2,3]
  rhov_bar   += rhou*rhoinv*A0_bar[2,3]
  rhoinv_bar += rhou*rhov*A0_bar[2,3]

  rhov_bar   += 2*rhov*rhoinv*A0_bar[3,3]
  rhoinv_bar += rhov*rhov*A0_bar[3,3]
  p_bar      += A0_bar[3,3]

  rhov_bar   += rhow*rhoinv*A0_bar[4,3]
  rhow_bar   += rhov*rhoinv*A0_bar[4,3]
  rhoinv_bar += rhov*rhow*A0_bar[4,3]

  rhov_bar += h*A0_bar[5,3]
  h_bar    += rhov*A0_bar[5,3]

  # 4th column
  rhow_bar += A0_bar[1,4]

  rhow_bar   += rhou*rhoinv*A0_bar[2,4]
  rhou_bar   += rhow*rhoinv*A0_bar[2,4]
  rhoinv_bar += rhow*rhou*A0_bar[2,4]

  rhow_bar   += rhov*rhoinv*A0_bar[3,4]
  rhov_bar   += rhow*rhoinv*A0_bar[3,4]
  rhoinv_bar += rhow*rhov*A0_bar[3,4]

  rhow_bar   += 2*rhow*rhoinv*A0_bar[4,4]
  rhoinv_bar += rhow*rhow*A0_bar[4,4]
  p_bar      += A0_bar[4,4]

  rhow_bar += h*A0_bar[5,4]
  h_bar    += rhow*A0_bar[5,4]

  # 5th column
  rhoe_bar += A0_bar[1,5]

  h_bar     += rhou*A0_bar[2,5]
  rhou_bar  +=  h*A0_bar[2,5]

  h_bar    += rhov*A0_bar[3,5]
  rhov_bar += h*A0_bar[3,5]

  h_bar    += rhow*A0_bar[4,5]
  rhow_bar += h*A0_bar[4,5]

  rho_bar += h*h*A0_bar[5,5]
  h_bar   += 2*rho*h*A0_bar[5,5]
  a2_bar  += -p*A0_bar[5,5]/gamma_1
  p_bar   += -a2*A0_bar[5,5]/gamma_1

  # a2
  p_bar      += gamma*rhoinv*a2_bar
  rhoinv_bar += gamma*p*a2_bar

  # h
  rhoe_bar   += rhoinv*h_bar
  p_bar      += rhoinv*h_bar
  rhoinv_bar += (rhoe + p)*h_bar

  # rhoinv
  rho_bar += -rhoinv*rhoinv*rhoinv_bar

  calcPressure_revq(params, q, q_bar, p_bar)

  q_bar[1] += rho_bar
  q_bar[2] += rhou_bar
  q_bar[3] += rhov_bar
  q_bar[4] += rhow_bar
  q_bar[5] += rhoe_bar

  return nothing
end


struct Staged{T}
    x::T
    k::Int
end

Base.:+(a::Staged, b::Staged) = Staged(a.x+b.x, a.k)
Base.:*(a::Staged, b::Number) = Staged(a.x*b, a.k)
Base.:/(a::Staged, b::Number) = Staged(a.x/b, a.k)

struct LQG1D <: POMDP{Staged{Float64}, Float64, Float64}
    a::Float64 # state transition
    b::Float64 # control
    c::Float64 # observation
    q::Float64 # state cost
    r::Float64 # control cost
    m::Float64 # process noise standard deviation
    n::Float64 # process noise standard deviation
    p::Float64 # initial distribution standard deviation
    k::Int     # number of stages
end

discount(::LQG1D) = 1.0
isterminal(m::LQG1D, s::Staged{Float64}) = s.k == m.k

rand(rng::AbstractRNG, d::Staged{D}) where D = Staged(rand(rng, d.x), d.k)
mean(d::Staged) = Staged(mean(d.x), d.k)

transition(m::LQG1D, s::Staged{Float64}, u::Float64) = Staged(Normal(m.a*s.x + m.b*u, m.m), s.k+1)
observation(m::LQG1D, s::Staged{Float64}) = Normal(m.c*s.x, m.n)
function reward(m::LQG1D, s::Staged{Float64}, u::Float64, sp::Staged{Float64})
    c = m.q*s.x^2 + m.r*u^2
    if isterminal(m, sp)
        c += m.q*sp.x^2
    end
    return -c
end
initial_state_distribution(m::LQG1D) = Staged(Normal(0.0, m.p), 1)

# function MCTS.next_action(gen::RandomActionGenerator, m::LQG1D, b, snode::MCTS.AbstractStateNode)
#     rs = rand(gen.rng, b)
#     x = rs.x
#     return rand(gen.rng, Normal(-1/2*x, 1.0))
# end

function MCTS.next_action(gen::RandomActionGenerator, m::LQG1D, b, snode::MCTS.AbstractStateNode)
    ms = mean(b)
    if ms.k == 1
        return 0.0
    else
        x = ms.x
        return rand(gen.rng, Normal(-1/2*x, 0.01))
    end
end

struct LinearFeedback <: Policy
    k::Float64
end

function action(p::LinearFeedback, b::Staged)
    return -p.k*mean(b.x)
end

struct Kalman1D <: Updater
    m::LQG1D
end

function update(kf::Kalman1D, b::Staged{D}, a::Float64, o::Float64) where D <: Normal
    m = kf.m
    propped = m.a*mean(b.x) + m.b*a
    propped_var = m.a^2*var(b.x) + m.m^2
    variance = propped_var - propped_var^2*m.c^2/(m.c^2*propped_var + m.n)
    xhat = propped + propped + variance*m.c/m.n*(o - m.c*propped)
    return Staged(Normal(xhat, sqrt(variance)), b.k+1)
end


__precompile__()

module Plexifications

import Base: +, -, *, /, \, real, isreal, inv, abs2, norm, show, zero, one, iszero, ==, exp
# import Unreal: unreal

"Error when finding the inverse of a zero divisor."
const ZeroDivisorInverse = "inverse of zero divisor"

"Error when the denominator in a quotient is zero."
const ZeroDenominator = "denominator is zero"

include("Plexification.jl")
include("Complexification.jl")
include("Perplexification.jl")
include("Nilplexification.jl")
include("Cmplex.jl")
include("Prplex.jl")
include("Nlplex.jl")
include("BiComplex.jl")
include("BiPerplex.jl")
include("BiNilplex.jl")
include("DualComplex.jl")
include("DualPerplex.jl")
include("TriComplex.jl")
include("TriPerplex.jl")
include("TriNilplex.jl")
include("HyperComplex.jl")
include("HyperPerplex.jl")
include("random.jl")

# Types
export Plexification, Complexification, Perplexification, Nilplexification,
       Cmplex, Prplex, Nlplex,
       BiComplex, BiPerplex, BiNilplex, DualComplex, DualPerplex,
       TriComplex, TriPerplex, TriNilplex, HyperComplex, HyperPerplex

# Methods
export unreal, array, iszerodivisor, random, quadrance,
       barH, barR, barA,
       barG, barQ, barB,
       barF, barP, barC,
       commutator, associator, jacobiator, alternatorL, alternatorR, flexator,
       lozenge, anti_lozenge

end


"""
A collection of various stable way to do a linear regression

#### References

Sections 3 and 4 of "Numerically stable and accurate stochastic simulation
approaches for solving dynamic economic models."

Kenneth L Judd, Serguei Maliar, and Lilia Maliar.

"""

using Parameters
using MathProgBase

abstract type AbstractLinReg end
abstract type AbstractLSMethod <: AbstractLinReg end
abstract type AbstractLADMethod <: AbstractLinReg end

# --------- #
# Utilities #
# --------- #

function standardize(x::AbstractMatrix, intercept::Bool=false)
    _x = intercept ? view(x, :, 2:size(x, 2)) : view(x, :, :)
    μ = mean(_x, dims=1)
    σ = std(_x, dims=1)
    xn = (_x .- μ) ./ σ
    xn, vec(μ), vec(σ)
end

function standardize(y::AbstractVector, intercet=false)
    μ = mean(y)
    σ = std(y)
    yn = (y .- μ) ./ σ
    yn, μ, σ
end

function add_intercept(x::AbstractArray{T}) where T
    [ones(T, size(x, 1)) x]
end

# --- #
# OLS #
# --- #

@with_kw struct OLS <: AbstractLSMethod
    standardize::Bool = true
    intercept::Bool = true
    @assert !(standardize && !(intercept)) "must have intercept if standardizing"
end

# API methods
should_standardize(m::OLS) = m.standardize
should_add_intercept(m::OLS) = m.intercept
slopes(::OLS, x, y) = pinv(x'x)*x'y

# ------ #
# LSLdiv #
# ------ #

@with_kw struct LSLdiv <: AbstractLSMethod
    standardize::Bool = true
    intercept::Bool = true
    @assert !(standardize && !(intercept)) "must have intercept if standardizing"
end

# API methods
should_standardize(m::LSLdiv) = m.standardize
should_add_intercept(m::LSLdiv) = m.intercept
slopes(::LSLdiv, x, y) = x\y

# ----- #
# LSSVD #
# ----- #

@with_kw struct LSSVD <: AbstractLSMethod
    standardize::Bool = true
    intercept::Bool = true
    @assert !(standardize && !(intercept)) "must have intercept if standardizing"
end

# API methods
should_standardize(m::LSSVD) = m.standardize
should_add_intercept(m::LSSVD) = m.intercept
function slopes(m::LSSVD, x, y)
    U, S, V = svd(x, full=false)
    S_inv = diagm(0 => 1.0 ./ S)
    V*S_inv*U'y
end

# ----------- #
# RLSTikhonov #
# ----------- #

@with_kw struct RLSTikhonov <: AbstractLSMethod
    η::Float64 = -5.0
    standardize::Bool = true
    intercept::Bool = true
    @assert !(standardize && !(intercept)) "must have intercept if standardizing"
    @assert η < 0.0 "penalty parameter for RLSTikhonov must be negative"
end

const RLST = RLSTikhonov

# API methods
should_standardize(m::RLSTikhonov) = m.standardize
should_add_intercept(m::RLSTikhonov) = m.intercept
function slopes(m::RLSTikhonov, x, y)
    nobs = size(x, 1); nx = size(x, 2)
    pinv(x'x + (nobs/nx*10^(m.η))*I)*x'y
end

# ----- #
# LADPP #
# ----- #

@with_kw struct LADPP <: AbstractLADMethod
    standardize::Bool = true
    intercept::Bool = true
    @assert (standardize && intercept) "standardize and intercept must both be true"
end

"""
Solves the linear program

``min_{v⁺; v⁻; β} 1v⁺ + 1v⁻``

such that

``\\begin{cases}v⁺ - v⁻ + X \\beta &= y \\\\ v⁺ \\ge 0, \\quad v⁻ \\ge 0``

"""
function slopes(m::LADPP, x, y::AbstractMatrix)
    nobs = size(x, 1)
    nx = size(x, 2)
    ny = size(y, 2)

    # The linear programming coefficient vector, x, is ordered [v⁺; v⁻; β]
    # we will now construct the matrices for the upper and lower bounds. Note
    # that we artificailly say that β must be between -300 and 300
    lb = [fill(0.0, 2*nobs); fill(-300, nx)]
    ub = [fill(Inf, 2*nobs); fill(300, nx)]

    # objective
    c = [ones(2*nobs); zeros(nx)]

    # equality constraints
    A = [I -I x]

    # we need to do linprog equation by equation (1 column of y at a time),
    # so we will pre-allocate a coefficient matrix and fill it in
    β = Array(Float64, nx, ny)

    for i in 1:ny
        # equality constraint
        b = y[:, i]
        out = linprog(c, A, '=', b, lb, ub).sol
        β[:, i] = out[end-nx+1:end]

    end
    β
end

# ----- #
# LADDP #
# ----- #

@with_kw struct LADDP <: AbstractLADMethod
    standardize::Bool = true
    intercept::Bool = true
    @assert (standardize && intercept) "standardize and intercept must both be true"
end

"""
Solves the linear program

``min_{q} -y'q``

such that

``X'q = 0 \\quad -1 \\le q \\le 1``

"""
function slopes(m::LADDP, x, y::AbstractMatrix)
    nobs = size(x, 1)
    nx = size(x, 2)
    ny = size(y, 2)

    # The linear programming coefficient vector, x, is ordered [q]
    # we will now construct the matrices for the upper and lower bounds. Note
    # that we artificailly say that β must be between -300 and 300
    lb = fill(-1.0, nobs)
    ub = fill(1.0, nobs)

    # equality constraint
    b = zeros(nx)

    # equality constraints
    A = x'

    # we need to do linprog equation by equation (1 column of y at a time),
    # so we will pre-allocate a coefficient matrix and fill it in
    β = Array(Float64, nx, ny)

    for i in 1:ny
        # equality constraint
        c = -y[:, i]
        out = linprog(c, A, '=', b, lb, ub)
        β[:, i] = -out.attrs[:lambda][1:nx]

    end
    β
end

# ------ #
# RLADPP #
# ------ #

@with_kw struct RLADPP <: AbstractLADMethod
    η::Float64 = -5.0
    standardize::Bool = true
    intercept::Bool = true
    @assert η < 0.0 "penalty parameter for RLSTikhonov must be negative"
    @assert (standardize && intercept) "standardize and intercept must both be true"
end

"""
Solves the linear program

``min_{c:=[v⁺; v⁻; ψ⁺; ψ⁻]} 1v⁺ + 1v⁻ + 1ψ⁺+ 1ψ⁻``

such that

``\\begin{cases}v⁺ - v⁻ + Xψ⁺ - Xψ⁻ &= y \\\\ c &\\ge 0``

"""
function slopes(m::RLADPP, x, y::AbstractMatrix)
    nobs = size(x, 1)
    nx = size(x, 2)
    ny = size(y, 2)

    # The linear programming coefficient vector, x, is ordered [v⁺; v⁻; ψ⁺; ψ⁻]
    lb = fill(0.0, 2*nobs + 2*nx)
    ub = fill(Inf, 2*nobs + 2*nx)

    # objective
    c = [ones(2*nobs); fill(10^(m.η)*nobs/nx, 2*nx)]

    # equality constraints
    A = [I -I x -x]

    # we need to do linprog equation by equation (1 column of y at a time),
    # so we will pre-allocate a coefficient matrix and fill it in
    β = Array(Float64, nx, ny)

    for i in 1:ny
        # equality constraint
        b = y[:, i]
        out = linprog(c, A, '=', b, lb, ub).sol
        ψ⁺ = out[end-(2*nx)+1:end-nx]
        ψ⁻ = out[end-nx+1:end]
        β[:, i] = ψ⁺ - ψ⁻
    end
    β
end

# ------ #
# RLADDP #
# ------ #

@with_kw struct RLADDP <: AbstractLADMethod
    η::Float64 = -5.0
    standardize::Bool = true
    intercept::Bool = true
    @assert η < 0.0 "penalty parameter for RLSTikhonov must be negative"
    @assert (standardize && intercept) "standardize and intercept must both be true"
end

"""
Solves the linear program

``min_{q} -y'q``

such that

``X'q \\le η1 \\quad -X'q \\le η1, \\quad -1 \\le q \\le 1``

"""
function slopes(m::RLADDP, x, y::AbstractMatrix)
    nobs = size(x, 1)
    nx = size(x, 2)
    ny = size(y, 2)

    # The linear programming coefficient vector, x, is ordered [q]
    # we will now construct the matrices for the upper and lower bounds. Note
    # that we artificailly say that β must be between -300 and 300
    lb = fill(-1.0, nobs)
    ub = fill(1.0, nobs)

    # constraints equations
    A = [x'; -x']
    b = fill(10^(m.η)*nobs/nx, 2*nx)

    # we need to do linprog equation by equation (1 column of y at a time),
    # so we will pre-allocate a coefficient matrix and fill it in
    β = Array(Float64, nx, ny)

    for i in 1:ny
        # equality constraint
        c = -y[:, i]
        out = linprog(c, A, '<', b, lb, ub)
        ψ⁺ = -out.attrs[:lambda][1:nx]
        ψ⁻ = -out.attrs[:lambda][nx+1:2*nx]
        β[:, i] = ψ⁺ - ψ⁻
    end
    β

end

# ------ #
# RLSSVD #
# ------ #

@with_kw struct RLSSVD <: AbstractLSMethod
    κ::Float64=100_000.0
    standardize::Bool = true
    intercept::Bool = true
    @assert κ > 0 "κ must be positive"
    @assert !(standardize && !(intercept)) "must have intercept if standardizing"
end

# API methods
should_standardize(m::RLSSVD) = m.standardize
should_add_intercept(m::RLSSVD) = m.intercept
function slopes(m::RLSSVD, x, y)
    U, S, V = svd(x, full=false)

    # find number of sufficiently independent principal components
    condition_numbers = S[1] ./ S[2:end]
    r = findfirst(_1 -> _1 > m.κ, condition_numbers)
    r = r == 0 ? length(S) : r

    # extract these columns of V, U, S
    Vr = V[:, 1:r]
    Ur = U[:, 1:r]
    Sr_inv = diagm(0 => 1.0 ./ S[1:r])

    # compute estimate
    Vr*Sr_inv*Ur'y
end

# -------- #
# Generics #
# -------- #

function slopes(m::AbstractLADMethod, x, y::AbstractVector)
    βmat = slopes(m, x, y[:, :])
    βmat[:, 1]
end

should_standardize(::AbstractLinReg) = true
should_add_intercept(::AbstractLinReg) = true

_reg_nonorm_noint(m::AbstractLinReg, x, y) = slopes(m, x, y)
_reg_nonorm_int(m::AbstractLinReg, x, y) = slopes(m, add_intercept(x), y)

function _reg_norm_noint(m::AbstractLinReg, x, y)
    xn, μx, σx = standardize(x)
    yn, μy, σy = standardize(y)

    # Compute standardized coefficients, then undo normalization
    β = slopes(m, xn, yn)
    # update slopes
    for j in 1:size(y, 2)
        for i in 1:size(β, 1)
            β[i, j] *= σy[j]/σx[i]
        end
    end
    β
end

function _reg_norm_int(m::AbstractLinReg, x, y::AbstractVector)
    xn, μx, σx = standardize(x)
    yn, μy, σy = standardize(y)

    # Compute standardized coefficients, then undo normalization
    β1 = slopes(m, xn, yn)
    for i in eachindex(β1)
        β1[i] *= σy/σx[i]
    end
    β0 = μy - dot(μx, β1)

    β = similar(β1, length(β1) + 1)
    β[1] = β0
    β[2:end] = β1
    β
end

function _reg_norm_int(m::AbstractLinReg, x, y::AbstractMatrix)
    xn, μx, σx = standardize(x)
    yn, μy, σy = standardize(y)

    # Compute standardized coefficients, then undo normalization
    β1 = slopes(m, xn, yn)
    β0 = similar(β1, 1, size(y, 2))

    # update slopes
    for j in 1:size(y, 2)
        for i in 1:size(β1, 1)
            β1[i, j] *= σy[j]/σx[i]
        end
    end

    # compute intercept
    for j in 1:size(y, 2)
        β0[j] = μy[j] - dot(β1[:, j], μx)
    end

    Float64[β0; β1]
end

regress(m::AbstractLinReg, x, y) =
    (should_standardize(m)  && should_add_intercept(m))  ? _reg_norm_int(m, x, y) :
    (!should_standardize(m) && should_add_intercept(m))  ? _reg_nonorm_int(m, x, y) :
    (should_standardize(m)  && !should_add_intercept(m)) ? _reg_norm_noint(m, x, y) :
                                                         _reg_nonorm_noint(m, x, y)


function first_order_rollout(traj::Algames.Traj, dt::T) where {T}
    N = length(traj)
    h = sum([traj[k].dt for k=1:N])
    @assert dt < h

    s = 0
    x = Algames.state(traj[1])
    while dt > 0
        z = traj[s+1]
        δ = z.dt
        if δ > dt
            x_1 = Algames.state(z)
            x1  = Algames.state(traj[s+2])
            x = x_1 + dt/δ*(x1 - x_1)
            break
        else
            dt -= δ
            s += 1
        end
    end
    return x, s
end

function simulate_MPC!(vis::Visualizer, prob::Algames.GameProblem, x0::SVx, sce::Scenario,
    mpc_stats::MPCStatistics, mpc_opts::MPCOptions) where {SVx}
    # Initialization
    reset!(mpc_stats)
    prob.x0 = x0
    newton_solve!(prob)
    prob.opts.shift = 0

    for j = 1:mpc_opts.M
        # Solve
        set_state!(vis, prob.model, sce, prob.x0)
        dt = @elapsed newton_solve!(prob)
        z0 = prob.pdtraj.pr[1]
        z = Algames.KnotPoint(Algames.state(z0), Algames.control(z0), dt)
        record!(mpc_stats, z)
        if mpc_stats.t > mpc_opts.h
            break
        end
        # Update solver
        x,s = first_order_rollout(prob.pdtraj.pr, dt)
        prob.x0 = x
        prob.opts.shift = s
    end
    return nothing
end