using MultilayerPerceptron

using MLDatasets: Iris
using Random: randperm

function iris_example()
    features = begin
        features = Iris.features()
        normalize!(features)
        features
    end
    (n_features, n_samples) = size(features)
    n_classes = 0
    labels = begin
        labels = Iris.labels()
        label_dict = Dict(label => i for (i, label) in enumerate(Set(labels)))
        n_classes = length(label_dict)
        map(l -> label_dict[l], labels)
    end
    oh_labels = one_hot(labels, n_classes)

    # Prepares training and test set
    n_test = n_samples ÷ 3
    n_train = n_samples - n_test
    ind_train, ind_test = begin
        ind = randperm(n_samples)
        sort(ind[1:n_train]), sort(ind[(n_train + 1):end])
    end
    @assert length(ind_test) == n_test

    train_x = features[:, ind_train]
    train_y = labels[ind_train]
    oh_train_y = oh_labels[:, ind_train]

    test_x = features[:, ind_test]
    test_y = labels[ind_test]
    oh_test_y = oh_labels[:, ind_test]

    # The model contains a single output layer, which is equivalent to logistic regression.
    output_layer = Layer((n_classes, n_features))

    mlp = MLP([output_layer])
    fit!(mlp, train_x, oh_train_y;
        loss_fn=logit_cross_entropy,
        learning_rate=0.2, batch_size=5, n_epochs=200)

    oh_pred_y = softmax(predict(mlp, test_x))
    pred_y = inv_one_hot(oh_pred_y)

    n_correct = sum(pred_y .== test_y)
    println("Accuracy = $(n_correct / n_test)")

    confusion_matrix = oh_test_y * oh_pred_y'
    println("Confusion matrix =")
    display(Float16.(confusion_matrix))

    # The accuracy is usually above 90% (it varies from 70% - ~100%).
end

iris_example()


using StaticArrays

# Frequently-used array types
const Mat3{T} = SMatrix{3, 3, T, 9} where T
const Vec3{T} = SVector{3, T} where T
const AbstractArray3{T} = AbstractArray{T, 3}

# Represents a symmetry (S,τ)
const SymOp = Tuple{Mat3{Int}, Vec3{Float64}}
identity_symop() = (Mat3{Int}(I), Vec3(zeros(3)))


@testset "rand" begin
    ϵ = Ellipsoid(PDMat([2 0; 0 2]), [0, 0])

    Random.seed!(8128)
    U = rand(ϵ, 1000000)
    Random.seed!()

    @test size(U) == (2, 1000000)
    @test maximum(U; dims=2) ≈ [1.0, 1.0] atol = 1e-3
    @test minimum(U; dims=2) ≈ [-1.0, -1.0] atol = 1e-3
    @test mean(U; dims=2) ≈ [0.0; 0.0] atol = 1e-3

    ϵ = Ellipsoid(PDMat([2 0; 0 2]), [0.5, 0.5])

    Random.seed!(8128)
    U = rand(ϵ, 1000000)
    Random.seed!()

    @test mean(U; dims=2) ≈ [0.5, 0.5] atol = 1e-2
end


```
Declination(args...)

Estimate decliantion of a given data set. By default it is applied an Anomaly Dectection
algorithm to filter the representative data.

The next table show the list of variables allowed:

|PropertyName|Args|Default|Input|Description
|---|---|---|---|---|
|Date|Mandatory|--|Array{Date,1}| Array of dates|
|Rate|Mandatory|--|Array{Number,1}| Array of rates|
|AD|Optional|AD=true|AD=Bool| Activation of Anomaly detection algorithm|
```
function Declination(time,rate; AD=true)

        #Anomally Detection
if AD==true
    epsilon=0.05339
    mu=mean(rate)
    sigma=std(rate)
    NormRate=map(x->(x-mu)/sigma,rate)
    pval=map(x->pdf(Normal(),x),NormRate)
    ev=pval.>epsilon
    Time=time[ev]
    Rate=rate[ev]
else
    Time=time
    Rate=rate
end

        #
        Ti=Dates.days.(Time[1])
        Cum=accumulate(+,Rate)

        D=Reg(Dates.days.(Time).-Ti,Rate, TypeReg="Exponential")
        Q=Reg(Cum,Rate, TypeReg="Linear")

        S=ExponentialDec(D[1],Q[2],Ti)
        DecLine=map(X->S.Qi.*exp(S.Di.*X),Dates.days.(Time).-S.Ti)

            return S, Time, DecLine

end #End Function

function DeclinationForecast(S,DateRange)

    ForecastLine=map(X->S.Qi.*exp(S.Di.*X),Dates.days.(DateRange).-S.Ti)

end # End function


"""
Test the peakedisotropicspectrum function.
"""
function testpeakedisotropicspectrum(dev::Device=CPU())
  n, L = 128, 2π
  grid = TwoDGrid(dev, n, L)
  k0, E0 = 6, 0.5
  qi = peakedisotropicspectrum(grid, k0, E0; allones=true)
  ρ, qhρ = FourierFlows.radialspectrum(rfft(qi) .* grid.invKrsq, grid)

  ρtest = ρ[ (ρ.>15.0) .& (ρ.<=17.5)]
  qhρtest = qhρ[ (ρ.>15.0) .& (ρ.<=17.5)]

  CUDA.@allowscalar isapprox(abs.(qhρtest)/abs(qhρtest[1]), (ρtest/ρtest[1]).^(-2), rtol=5e-3)
end

function testpeakedisotropicspectrum_rectangledomain()
  nx, ny = 32, 34
  Lx, Ly = 2π, 3π
  grid = TwoDGrid(nx, Lx, ny, Ly)
  k0, E0 = 6, 0.5
  qi = peakedisotropicspectrum(grid, k0, E0; allones=true)
end


# include("Load_LTSpice_Net.jl")
# include("RunAnalysis.jl")
# include("SPICE2Matrix.jl")
# include("ToroidOptimizer.jl")

"""
This function takes in  the following parameters:

    LDrive, drive coil inductance (H)

    RDrive, Drive coil resistance (Ω)

    TargetZ, Amplifier/target impedance to match to  (Ω)

    DriveFreq; drive frequency (Hz)
The following are keyword arguments and should be entered after the first four (required) as;
DesignDriveFilter(LDrive,RDrive,Amp_Z,DriveFreq; [kwarg] = value). See below for an example

    CDrive = 1e6, Any series capacitance with drive coil

    NumDriveElements = 1, The number of series copies of the L,C,R for example if there are 6 pancakes of LDrive, RDrive, Cdrive, then this would be 6

    WireDiam = 2e-3, Wire diameter for toroids

    WireFillFac = 0.75, If litz wire, this is the ratio of copper to air

    PlotFTs = true, Plotting be default is true
    VSrc = 2, The source voltage for plotting
    DetermineFreq = false, IF you have a fixed capacitor (CDrive has been set) you may want to search for an ideal drive freq. in which case, set this to true.
    AddNotchFreq = nothing, This can be a single value of a notch frequency, or an array of length 2, the two values are the notch frequencies

    FilterZ = TargetZ, The characteristic impedance of the filtering section
    RDampVal = 100, [Ω] The damping resistor added in around the series L-C sections
    PerturbTxReactance = nothing, an optional element to add some purturbation to the reactance of the drive coil, which effectively enables the user to tune where the operating point ends up on the transfer function (V in → I drive)

    Example (Copy/paste):
    RDrive = 400e-3
    LDrive = 400e-6
    Amp_Z = 4
    DriveFreq = 25e3
    AddNotchFreqList = [DriveFreq*2, DriveFreq*3]

    DesignDriveFilter(LDrive,RDrive,Amp_Z,DriveFreq; AddNotchFreq = AddNotchFreqList)

"""
function DesignDriveFilter(
    LDrive,
    RDrive,
    TargetZ,
    DriveFreq;
    CDrive = 2,
    NumDriveElements = 1,
    WireDiam = 2e-3,
    WireFillFac = 0.75,
    PlotSrcR = TargetZ,
    PlotFTs = true,
    VSrc = 2,
    DetermineFreq = false,
    AddNotchFreq = nothing,
    FilterZ = TargetZ,
    RDampVal = 100,
    PerturbTxReactance = nothing,
    AxesArray = nothing,
    WriteFileName=nothing
)

    ZeroVal = 1e-9 #There are issues with zero-valued components
    ωDr = 2 * π * DriveFreq
    ZDrive =
        RDrive * NumDriveElements +
        im * ωDr * LDrive * NumDriveElements +
        NumDriveElements ./ (im * 2 * pi * DriveFreq * CDrive) #Equivalent complex impednace of the load

    Reactance_Load =
        ωDr * LDrive * NumDriveElements - NumDriveElements ./ (ωDr * CDrive)
    Reactance_Load = round(Reactance_Load; sigdigits = 3)
    display(println("The reactance of the load is: $(round(Reactance_Load;sigdigits=3)) Ω"))
    if ~DetermineFreq
        if (Reactance_Load > 0)

            SerCap,CParAct = ImpMatch_LLoad(TargetZ, ZDrive, Reactance_Load,ωDr)

            LTee_2 = ZeroVal
            LTee_2_ESR = ZeroVal
            LTee_1 = ZeroVal
            LTee_1_ESR = ZeroVal

        elseif (Reactance_Load < 0)

            LTee_2,LTee_2_ESR,CParAct = ImpMatch_CLoad(TargetZ, ZDrive, Reactance_Load,ωDr)

            LTee_1 = ZeroVal
            LTee_1_ESR = ZeroVal
            SerCap = 2

        elseif (Reactance_Load == 0)
            matchRatio = real(TargetZ) / real(ZDrive)
            Q = sqrt(matchRatio - 1)
            Xs = Q * real(ZDrive)
            LSer2 = Xs / (ωDr)
            LTee_2 = LSer2
            LTee_1 = ZeroVal
            LTee_1_ESR = ZeroVal
            CParAct = findResPair((1 + Q^(-2)) * LSer2, DriveFreq)
            LTee_2_Geom =
                ToroidOptimizer(WireDiam, LTee_2; CuFillFactor = WireFillFac)
            LTee_2_ESR = LTee_2_Geom.DCore.Resistance
            SerCap = 2
        end
    else

        matchRatio = real(TargetZ) / real(ZDrive)
        Q = √(matchRatio - 1)
        Xs = Q * real(ZDrive)

        DriveFreq = findFilterFreq(Q,real(ZDrive), LDrive * NumDriveElements, CDrive / NumDriveElements)
        ωDr = 2*π*DriveFreq
        # println("Determined Q to be $(round(Q))")
        # println("Determined drive freq. to be $(round(DriveFreq)) Hz")
        ZSer = 1*im * ωDr*LDrive * NumDriveElements -  1*im /(ωDr* CDrive / NumDriveElements) +RDrive* NumDriveElements
        YSer = 1/ZSer

        CParAct = abs.(imag(YSer))/ωDr
        LTee_2 = ZeroVal
        LTee_2_ESR = ZeroVal
        LTee_1 = ZeroVal
        LTee_1_ESR = ZeroVal
        SerCap = 1e6
    end

    (LFilt, CFilt) = Butterworth_2(FilterZ, DriveFreq)
    CFilt2 = findResPair(LFilt, DriveFreq)
    LFilt2 = findResPair(CFilt, DriveFreq)

    LFilt1_Geom =
            ToroidOptimizer(WireDiam, LFilt; CuFillFactor = WireFillFac)
    LFilt1_ESR = LFilt1_Geom.DCore.Resistance

    LFilt2_Geom =
            ToroidOptimizer(WireDiam, LFilt2; CuFillFactor = WireFillFac)
    LFilt2_ESR = LFilt2_Geom.DCore.Resistance

    CalcC(r,xl) = 1/(ωDr*(r^2/xl+xl))
    C_Par_2 = abs(CalcC(TargetZ, abs.(im * ωDr * LDrive * NumDriveElements + NumDriveElements ./ (im * ωDr * CDrive)+1/(im*ωDr*SerCap))))
    X_Par_2 =  1 ./ (im * ωDr * C_Par_2)
    println(C_Par_2)
    if ~(AddNotchFreq===nothing)
        if length(AddNotchFreq)==1
            LNotch, CNotch = makeNotchSection(AddNotchFreq[1], DriveFreq, X_Par_2)
        else
            LNotch, CNotch = makeNotchSection(AddNotchFreq[1], DriveFreq, X_Par_2*2)
        end
        LNotch_Geom =
                ToroidOptimizer(WireDiam, LNotch; CuFillFactor = WireFillFac)
        LNotch_ESR = LNotch_Geom.DCore.Resistance

        println("Adding notch")
        if length(AddNotchFreq)==2
            LNotch2, CNotch2 = makeNotchSection(AddNotchFreq[2], DriveFreq, X_Par_2*2)
            LNotch2_Geom =
                    ToroidOptimizer(WireDiam, LNotch2; CuFillFactor = WireFillFac)
            LNotch2_ESR = LNotch2_Geom.DCore.Resistance

            println("Adding second notch")
        else
            LNotch2 = 10
            CNotch2 = 1e-12
            LNotch2_ESR = 1e-3

        end

    else
        LNotch = 1e-9
        CNotch = C_Par_2

        LNotch2 = 10 #Set default values is no notch is added
        CNotch2 = 1e-12
        LNotch2_ESR = 1e-3
    end

    CurrentVec, Results, SPICE_DF,inputs,InputList,FreqList,AxesArray = CircModel_SPICE(DriveFreq, VSrc,
    1e-3,
    RDrive,
    NumDriveElements,
    LDrive,
    CDrive,
    SerCap,
    CParAct,
    LFilt,
    LFilt2_ESR,
    LFilt1_ESR,
    CFilt2,
    CFilt,
    LFilt2,
    LNotch,
    LNotch_ESR,
    CNotch,
    LNotch2,
    LNotch2_ESR,
    CNotch2;
    PlotOn = PlotFTs,
    RDampVal = RDampVal,
    AxesArray = AxesArray)

    if ~(WriteFileName===nothing)
    writeFilterSPICE(WriteFileName,
    RDrive,
    NumDriveElements,
    LDrive,
    CDrive,
    SerCap,
    CParAct,
    LFilt,
    LFilt2_ESR,
    LFilt1_ESR,
    LFilt2,
    CFilt2,
    CFilt,
    LNotch,
    LNotch_ESR,
    CNotch,
    LNotch2,
    LNotch2_ESR,
    CNotch2,
    RDampVal,
    NumDriveElements*LDrive,
    Par(CDrive/NumDriveElements,SerCap))
    end
    DetermineComponentsTempCoeffs(SPICE_DF,InputList,1,DriveFreq,"LDrive")

    return DriveFreq, CurrentVec, Results, SPICE_DF,inputs,InputList,FreqList,AxesArray

end

function CircModel_SPICE(DriveFreq, VSrc,
    PlotSrcR,
    RDrive,
    NumDriveElements,
    LDrive,
    CDrive,
    SerCap,
    CParAct,
    LFilt,
    LFilt2_ESR,
    LFilt1_ESR,
    CFilt2,
    CFilt,
    LFilt2,
    LNotch,
    LNotch_ESR,
    CNotch,
    LNotch2,
    LNotch2_ESR,
    CNotch2;
    PlotOn = true,
    FreqList = 1000:10:100e3,
    ArchetypeNetFileName = nothing,
    RDampVal = nothing,
    AxesArray=nothing)

    if ArchetypeNetFileName === nothing

        ArchetypeNetFileName = joinpath(dirname(pathof(DesignDriveCoilFilt)),"Filter_Archetype_Damped_3.net")

    end
    SPICE_DF,NodeList,InputList,NumVSources = SPICE2Matrix(ArchetypeNetFileName)
    UpdateElementVal!(SPICE_DF,"RSrc",PlotSrcR)

    UpdateElementVal!(SPICE_DF,"LNotch",LNotch)
    UpdateElementESR!(SPICE_DF,"LNotch",LNotch_ESR)
    UpdateElementVal!(SPICE_DF,"CNotch",CNotch)
    UpdateElementVal!(SPICE_DF,"LNotch1",LNotch2)
    UpdateElementESR!(SPICE_DF,"LNotch1",LNotch2_ESR)
    UpdateElementVal!(SPICE_DF,"CNotch1",CNotch2)

    UpdateElementVal!(SPICE_DF,"LFilt1",LFilt)
    UpdateElementVal!(SPICE_DF,"LFilt2",LFilt)
    UpdateElementVal!(SPICE_DF,"CFilt1",CFilt2)
    UpdateElementVal!(SPICE_DF,"CFilt2",CFilt2)
    UpdateElementESR!(SPICE_DF,"LFilt1",LFilt1_ESR)

    UpdateElementVal!(SPICE_DF,"CFilt3",CFilt)
    UpdateElementVal!(SPICE_DF,"LFilt3",LFilt2)
    UpdateElementESR!(SPICE_DF,"LFilt3",LFilt2_ESR)

    UpdateElementVal!(SPICE_DF,"RDrive",RDrive)
    UpdateElementVal!(SPICE_DF,"LDrive",NumDriveElements*LDrive)
    UpdateElementVal!(SPICE_DF,"CDrive",CDrive/NumDriveElements)

    UpdateElementVal!(SPICE_DF,"CSer",SerCap)
    UpdateElementVal!(SPICE_DF,"CPar",CParAct)

    UpdateElementVal!(SPICE_DF,"LDrivePair",NumDriveElements*LDrive)
    UpdateElementESR!(SPICE_DF,"LDrivePair",LFilt1_ESR)
    UpdateElementVal!(SPICE_DF,"CDrivePair",Par(CDrive/NumDriveElements,SerCap))

    if ~(RDampVal===nothing)

        UpdateElementVal!(SPICE_DF,"RDamp2",RDampVal)
        UpdateElementVal!(SPICE_DF,"RDamp1",RDampVal)

    end

    inputs = zeros(length(InputList))
    inputs[end-(NumVSources-1):end] .= 1

    ResultNodeNames = vcat("V(".*NodeList.*")", "I(".*(InputList[end-(NumVSources-1):end]).*")")

    Results = [(inv(SPICE_DF2Matrix_ω(SPICE_DF,2*π*FreqList[i],InputList))*inputs) for i in 1:length(FreqList)]
    Results = hcat(Results...)
    ResDict = Dict(ResultNodeNames[1] => Results[1,:])
    for i in 2:length(ResultNodeNames)

         merge!(ResDict,Dict(ResultNodeNames[i] => Results[i,:]))

    end

    CurrentVec = VSrc.* plotACElCurrent(SPICE_DF,FreqList,Results,"LDrive")

    if PlotOn
        if (AxesArray===nothing)
            AxMain = PyPlot.axes([0.13, 0.15, 0.85, 0.85])

            AxesNewArray = [AxMain, AxMain] #Initialize with length 2
        else
            PyPlot.axes(AxesArray[1])
        end
        pygui(true)
        plot(FreqList,(20*log10.(abs.(CurrentVec[:]))))
        xlabel("Frequency, Hz")
        ylabel("Current in LDrive [dBAmps per Volt input]")

        StartIndex = findfirst(x-> x>(DriveFreq-1e3),FreqList)
        StopIndex = findfirst(x-> x>(DriveFreq+1e3),FreqList)
        if (AxesArray===nothing)
            AxInset = PyPlot.axes([0.68, 0.65, 0.25, 0.25]) #[left, bottom, width, height]
            AxesNewArray[2] = AxInset
        else
            PyPlot.axes(AxesArray[2])
        end

        plot(FreqList[StartIndex:StopIndex],(20*log10.(abs.(CurrentVec[StartIndex:StopIndex]))))
        plotDriveFreqDot(FreqList,CurrentVec,DriveFreq)

    end
    if (AxesArray===nothing)
        AxesArray = AxesNewArray
    end
    getElementCurrents(SPICE_DF,Results,DriveFreq)

    return CurrentVec, Results, SPICE_DF,inputs,InputList,FreqList,AxesArray
end

function ImpMatch_LLoad(TargetZ, ZDrive, Reactance_Load,ωDr)
    println("Load is inductive")
    EquivSerL = Reactance_Load / ωDr
    println(
        "Load appears to be a inductor with a value of $(round(EquivSerL*1e6;sigdigits=3))μH and a ESR of $(real(ZDrive))Ω ",
    )

    matchRatio = real(TargetZ) / real(ZDrive)
    Q = sqrt(matchRatio - 1)
    Xs = Q * real(ZDrive) #Target reactance

    X_SerCap = Reactance_Load - Xs #Reactance of series capacitor
    SerCap = 1 / (ωDr * X_SerCap) #Series Capacitor value
    CParAct = Q / (ωDr * TargetZ)

    return SerCap,CParAct

end

function ImpMatch_CLoad(TargetZ, ZDrive, Reactance_Load,ωDr; WireDiam = 2e-3,
    WireFillFac = 0.75)
    println("Load is capacitive")
        EquivSerC = abs.(1 / (Reactance_Load * ωDr))
        println(
            "Load appears to be a capacitor with a value of $(round(EquivSerC*1e6;sigdigits=3))μF and a ESR of $(real(ZDrive))Ω ",
        )
        LSer = abs.(Reactance_Load) / (ωDr)
        matchRatio = real(TargetZ) / real(ZDrive)
        Q = sqrt(matchRatio - 1)
        Xs = Q * real(ZDrive)
        LSer2 = Xs / (ωDr)
        LTee_2 = LSer2 + LSer

        CParAct = findResPair((1 + Q^(-2)) * LSer2, ωDr*2*π)
        LTee_2_Geom =
            ToroidOptimizer(WireDiam, LTee_2; CuFillFactor = WireFillFac)
        LTee_2_ESR = LTee_2_Geom.DCore.Resistance
        println("Q = $Q")

        return LTee_2,LTee_2_ESR,CParAct

end

"""
This function determines the ideal frequency of a resonant RLC such that it can be matched to a different impedance. It assumes there is already an L, C, and R, so at some frequency it the real part of the admittance will be the matched admittance. This admittance is made fully real with a shunt reactive element

"""
function findFilterFreq(Q,RLoad,L,C)
        f = ((Q*RLoad + √(Q^2*RLoad^2+4*L/C)) / (4*π*L) , (Q*RLoad - √(Q^2*RLoad^2+4*L/C)) / (4*π*L) )

        return maximum(f)
end

"""
This function makes a filter section that notches out a frequency (NotchFreq), but is apparently real at another (DriveFreq)
    the inputs are:
        * NotchFreq: the freq that will be shunted out
        * DriveFreq: The freq that should remain real
        * RMatch: The resistance the drive coil is tuned to
        * LVal is a kwarg because either the inductor or capacitor value should be pre-defined
The outputs are:
    LVal(Henries)
    CVal(Farads)
    LTune(Henries)
"""
function makeNotchSection(NotchFreq, DriveFreq, XEquiv)
    println(XEquiv)
    LVal,CVal = findEquivLC(XEquiv,DriveFreq,NotchFreq)
    return abs(LVal),abs(CVal)
end

"""

Calculates parallel impedances.
If given two inputs, it assumes both are inputs
If given an array, it takes the parallel combo of it all

Eg.
ZList = [1 + 1*im, 1]
Par(ZList[1], ZList[2])
Par(ZList)
"""
function Par(Z1, Z2)
    return 1 / (1 / Z1 + 1 / Z2)
end
function Par(Z::Array)
    Y = 1 ./Z
    YTotal = sum(Y)
    Zeff = 1/YTotal
    return Zeff
end

"""
Parallel component lists
Input is Array{Tuple{Number,String},1}
e.g.
Freq = 25e3
CompList = [(3, "R"),(30e-6, "L"),(200e-9,"C")]
Par(CompList,Freq)

"""
function Par(C::Array{Tuple{Real,String},1},freq)

    Z = Complex.(zeros(length(C)))
    for i in 1:length(C)
        if (lowercase(C[i][2])=="r") | (lowercase(C[i][2])=="resistor")
            Z[i] = C[i][1]
        elseif (lowercase(C[i][2])=="l") | (lowercase(C[i][2])=="inductor" )
            Z[i] = Z_Ind(C[i][1],freq)
        elseif (lowercase(C[i][2])=="c") | (lowercase(C[i][2])=="capacitor" )
            Z[i] = Z_Cap(C[i][1],freq)
        elseif lowercase(C[i][2])=="z"
            Z[i] = C[i][1]
        else
            error("Unknown component")
        end

    end

    Y = 1 ./Z
    YTotal = sum(Y)
    Zeff = 1/YTotal
    return Zeff
end

"""
Converts from a polar coordinate (Mag, phase) to cartesian
if you enter a tuple or array it assumes first is mag.

"""
function Polar2Cart(Mag, Phase)
    Real = real.(Mag * exp(-im * Phase / 360 * 2 * pi))
    Imag = imag.(Mag * exp(-im * Phase / 360 * 2 * pi))
    return (Real, Imag)
end

function Polar2Cart(MagPhaseTup::Union{Tuple, Array})
    Polar2Cart(MagPhaseTup[1], MagPhaseTup[2])
end

function Z_Cap(C, f)
    return (1 ./ (im * 2 * pi .* f .* C))
end
function Z_Ind(L, f)
    return im * 2 * pi .* f .* L
end

"""
This function takes in some reactance element (E) and a frequency, and finds the pair

"""
function findResPair(E, f)
    ##Finds the resonant pair for a reactive element, E
    ## ωL = 1/(ωC) → L = 1/(ω^2C),and vice versa
    return 1 ./ ((2 * pi * f) .^ 2 * E)
end

function Butterworth_2(Z, f)
    #Sqrt(L/C) = Z ; f = 1/(2π*sqrt(LC))
    #L = Z^2*C
    #f = 1/(2π*Z C) → C = 1/(2*pi*f*Z)
    #Butterworth 2nd order = [LX = sqrt(2)Z, CX = sqrt(2)Z]
    C = 1 / (2 * pi * f) / Z * sqrt(2)
    L = 1 / (2 * pi * f) * Z * sqrt(2)
    return (L, C)
end

function PlotPhasor(
    Impedance::Tuple,
    PrevImpedance;
    Color = nothing,
    NumSteps = 1,
    Admittance = false,
    HeadWidth = 0.01,
)

    ## This function takes in an impedance, and plots it using 'phasor()'
    #optionally, it takes in a previous impedance which can be added in series or parallel
    #It then plots the line using PrevImpedance as a base
    #If Admittance is true, it means the inputs are Admittance not impedance, so parallel and series are switched
    NewImp = 0 + 0im
    dx = 0
    dy = 0
    for I = 1:NumSteps
        if I > 1
            #    Color = "gray"
        end
        if lowercase(Impedance[2]) == "series"
            if !Admittance
                NewImp = PrevImpedance + Impedance[1] / NumSteps
            else
                NewImp = Par(PrevImpedance, Impedance[1] * NumSteps)
            end
        elseif lowercase(Impedance[2]) == "parallel"
            if !Admittance
                NewImp = Par(PrevImpedance, Impedance[1] * NumSteps)
            else
                NewImp = PrevImpedance + Impedance[1] / NumSteps
            end
        else
            error("must be series or parallel")
        end

        RealParts = [real(NewImp), real(PrevImpedance)]
        ImagParts = [imag(NewImp), imag(PrevImpedance)]

        dx = -(RealParts[2] - RealParts[1])
        dy = -(ImagParts[2] - ImagParts[1])
        if Color !== nothing

            PyPlot.plot(RealParts, ImagParts, color = "gray")

            if I == 1
                phasor(PrevImpedance, color = Color)
            end
        else
            PyPlot.plot(RealParts, ImagParts)
            if I == 1
                phasor(PrevImpedance)
            end
        end
        PrevImpedance = NewImp
    end
    # arrow(real(NewImp)-dx,imag(NewImp)-dy,dx,dy,head_width=HeadWidth,fc="gray",ec="k",alpha=1.,width = 0.00001,head_length= 2*HeadWidth)

    xlabel("Real")
    ylabel("Imag.")
    return NewImp
end

"""
This function takes in an array of tuples structures such as:

    [(<Complex impedance #1> , <"series" or "Parallel">),
    (<Complex impedance #2> , <"series" or "Parallel">)
    .
    .
    .
    (<Complex impedance #n> , <"series" or "Parallel">)]

    e.g.

    ZList = [(1+1*im, "series"),
             (1+5*im, "series"),
             (5+1*im, "parallel"),]
    PlotImpedanceTransformList(ZList)
"""
function PlotImpedanceTransformList(ImpList; InitialImp = nothing, ArrHeadWidth =1)
    ColorList = ["r", "g", "b", "magenta", "teal"]
    L = zeros(length(ImpList))
    for I = 1:length(ImpList)
        L[I] = abs(ImpList[I][1])
    end
    LongestVector = maximum(L)
    HeadWidth = LongestVector / ArrHeadWidth
    ##Impedance Plotting
    if InitialImp !== nothing
        PrevImp = InitialImp
        StartVal = 1
    else
        PrevImp = ImpList[1][1]
        StartVal = 2
    end
    for I = StartVal:length(ImpList)
        PrevImp = PlotPhasor(
            ImpList[I],
            PrevImp;
            NumSteps = 500,
            Color = ColorList[I],
            HeadWidth = HeadWidth,
        )
    end
    phasor(PrevImp, color = ColorList[length(ImpList)+1])
    title("Impedance")
    ##Admittance Plotting
    figure()

    L = zeros(length(ImpList))
    for I = 1:length(ImpList)
        L[I] = abs(1 / ImpList[I][1])
    end
    LongestVector = maximum(L)
    HeadWidth = LongestVector / ArrHeadWidth
    if InitialImp !=0 nothing
        PrevImp = 1 / InitialImp
        StartVal = 1
    else
        PrevImp = 1 / ImpList[1][1]
        StartVal = 2
    end
    for I = StartVal:length(ImpList)
        PrevImp = PlotPhasor(
            (1 / ImpList[I][1], ImpList[I][2]),
            PrevImp;
            NumSteps = 500,
            Color = ColorList[I],
            Admittance = true,
            HeadWidth = HeadWidth,
        )
    end
    phasor(PrevImp, color = ColorList[length(ImpList)+1])
    title("Admittance")
end

function findMinima_Bounds(x,y;StartVal=nothing,StopVal=nothing)

    if !(StartVal===nothing)
        StartIndex = minimum(findall(x .>= StartVal))
    else
        StartIndex = 1
    end

    if !(StopVal===nothing)
        StopIndex = maximum(findall(x .<= StopVal))
    else
        StopIndex = length(x)
    end

    xSubset = x[StartIndex:StopIndex]
    ySubset = y[StartIndex:StopIndex]
    FirstDiff = diff(ySubset)
    SecondDiff = diff(FirstDiff)
    Tmp = SecondDiff .< 0
    push!(Tmp,1)
    FirstDiff[Tmp] .= 1000 #Only look at values where the second derivative is positive

    minIndex = findfirst(isequal(minimum(abs.(FirstDiff))),abs.(FirstDiff))

    Min_x = xSubset[minIndex]
    Min_y = ySubset[minIndex]
    return Min_x, Min_y

end

function findDriveDistFromTrough(fVec,CurVec,DriveFreq)
    TroughFreq,val = findMinima_Bounds(fVec,abs.(CurVec);StartVal=(DriveFreq-1e3),StopVal=(DriveFreq+1e3))
    return abs(TroughFreq-DriveFreq)
end

function plotDriveFreqDot(fVec,CurVec,DriveFreq)
    DriveFreqCurrentIndex = findfirst(fVec.>=DriveFreq)
    plot(fVec[DriveFreqCurrentIndex],20*log10.(abs.(CurVec[DriveFreqCurrentIndex])),"r*")
end

# """
# This function takes in similar inputs to DesignDriveFilter. It will iterate that function with varying matching impedances to minimize the expected drift in the current.
# """
# function DesignDriveFilter_OptimDrift(
#     LDrive,
#     RDrive,
#     TargetZ,
#     DriveFreq;
#     CDrive = 2,
#     NumDriveElements = 1,
#     WireDiam = 2e-3,
#     WireFillFac = 0.75,
#     PlotSrcR = TargetZ,
#     PlotFTs = true,
#     VSrc = 2,
#     DetermineFreq = false,
#     AddNotchFreq = nothing,
#     FilterZ = TargetZ,
#     RDampVal = FilterZ,
#     PerturbTxReactance = nothing,
#     BruteForceOpt = false,
#     MinimizeDistToTrough = true,
#     OptimIters = 10,
#     WriteFileName=nothing
# )

# AxMain = PyPlot.axes([0.13, 0.15, 0.85, 0.85])
# AxInset = PyPlot.axes([0.68, 0.65, 0.25, 0.25]) #[left, bottom, width, height]

# AxesArray = [AxMain,AxInset]

#         if MinimizeDistToTrough
#             println("minimizing distance to trough")
#         else
#             println("Minimizing drift coeff")
#         end
#         function WrapperFunkZin(Zin)
#             DriveFreq, CurrentVec, Results, SPICE_DF,inputs,InputList,FreqList = DesignDriveFilter(
#             LDrive,RDrive, Zin,DriveFreq;
#             CDrive = CDrive,
#             NumDriveElements = NumDriveElements,
#             WireDiam = WireDiam,
#             DetermineFreq=DetermineFreq,
#             AddNotchFreq = AddNotchFreq,
#             FilterZ = FilterZ,
#             AxesArray=AxesArray)

#             DetermineComponentsTempCoeffs(SPICE_DF,InputList,1,DriveFreq,"LDrive")
#             if CDrive>1 #Check the drift in the drive capacitor if it is used, otherwise use the CSer
#                 Drift = SPICE_DF.DriftCoeff[findfirst(isequal("CSer"),SPICE_DF.Name)]
#                 PhaseDrift = SPICE_DF.DriftCoeffPhase[findfirst(isequal("CSer"),SPICE_DF.Name)]
#             else
#                 Drift = SPICE_DF.DriftCoeff[findfirst(isequal("CDrive"),SPICE_DF.Name)]
#                 PhaseDrift = SPICE_DF.DriftCoeffPhase[findfirst(isequal("CDrive"),SPICE_DF.Name)]
#             end
#             TroughDist = findDriveDistFromTrough(FreqList,CurrentVec,DriveFreq)
#             println("$TroughDist is the distance from the drive frequency to the trough, $(round(Drift;sigdigits=3)) is the drift in magnitude, $(round(PhaseDrift;sigdigits=3)) is the drift in phase")
#             if MinimizeDistToTrough
#                 return TroughDist
#             else
#                 return abs(Drift)
#             end
#         end

#         function WrapperFunk_Per(PerturbationX)
#             DriveFreq, CurrentVec, Results, SPICE_DF,inputs,InputList,FreqList = DesignDriveFilter(
#             LDrive,RDrive, TargetZ,DriveFreq;
#             CDrive = CDrive,
#             NumDriveElements = NumDriveElements,
#             WireDiam = WireDiam,
#             DetermineFreq=DetermineFreq,
#             AddNotchFreq = AddNotchFreq,
#             FilterZ = TargetZ,
#             PerturbTxReactance = PerturbationX,AxesArray=AxesArray)

#             DetermineComponentsTempCoeffs(SPICE_DF,InputList,1,DriveFreq,"LDrive")
#             if CDrive>1 #Check the drift in the drive capacitor if it is used, otherwise use the CSer
#                 Drift = SPICE_DF.DriftCoeff[findfirst(isequal("CSer"),SPICE_DF.Name)]
#                 PhaseDrift = SPICE_DF.DriftCoeffPhase[findfirst(isequal("CSer"),SPICE_DF.Name)]
#             else
#                 Drift = SPICE_DF.DriftCoeff[findfirst(isequal("CDrive"),SPICE_DF.Name)]
#                 PhaseDrift = SPICE_DF.DriftCoeffPhase[findfirst(isequal("CDrive"),SPICE_DF.Name)]
#             end
#             TroughDist = findDriveDistFromTrough(FreqList,CurrentVec,DriveFreq)
#             println("$TroughDist is the distance from the drive frequency to the trough, $(round(Drift;sigdigits=3)) is the drift in magnitude, $(round(PhaseDrift;sigdigits=3)) is the drift in phase")
#             if MinimizeDistToTrough
#                 return TroughDist
#             else
#                 return abs(Drift)
#             end
#         end

#     if  (PerturbTxReactance === nothing)
#         MinZ = optimize((WrapperFunkZin),5,25,iterations = OptimIters)
#         DriveFreq, CurrentVec, Results, SPICE_DF,inputs,InputList,FreqList = DesignDriveFilter(
#         LDrive,RDrive, MinZ.minimizer,DriveFreq;
#         CDrive = CDrive,
#         WireDiam = WireDiam,
#         DetermineFreq=DetermineFreq,
#         AddNotchFreq = AddNotchFreq,
#         FilterZ = FilterZ,
#         WriteFileName=WriteFileName)

#         MinZVal = MinZ.minimizer
#         DetermineComponentsTempCoeffs(SPICE_DF,InputList,1,DriveFreq,"LDrive")
#         return MinZVal, DriveFreq, CurrentVec, Results, SPICE_DF,inputs,InputList,FreqList,AxesArray
#     else
#         println("Perturbation given")
#         if BruteForceOpt

#             PerVec = -2:.1:2
#             DriftVec = zeros(length(PerVec),1)
#             for i in 1:length(PerVec)
#                 println(i)
#                 DriftVec[i] =WrapperFunk_Per(PerVec[i])
#             end
#             MinIndex = findfirst(x-> x==minimum(DriftVec),DriftVec)
#             MinXVal = PerVec[MinIndex]
#         else

#             MinXValOptimRes = optimize((WrapperFunk_Per),-2,2,iterations = OptimIters)
#             MinXVal = MinXValOptimRes.minimizer
#         end
#         DriveFreq, CurrentVec, Results, SPICE_DF,inputs,InputList,FreqList = DesignDriveFilter(
#         LDrive,RDrive, TargetZ,DriveFreq;
#         CDrive = CDrive,
#         WireDiam = WireDiam,
#         DetermineFreq=DetermineFreq,
#         AddNotchFreq = AddNotchFreq,
#         FilterZ = TargetZ,
#         PerturbTxReactance = MinXVal,
#         WriteFileName=WriteFileName)

#         DetermineComponentsTempCoeffs(SPICE_DF,InputList,1,DriveFreq,"LDrive")
#         return MinXVal, DriveFreq, CurrentVec, Results, SPICE_DF,inputs,InputList,FreqList,AxesArray
#     end

# end


#=
ml.jl

    Maximum Likelihood (ML) algorithm to estimate the hyper parameters of
    a linear Gaussian State Space model.

@author: Quint Wiersma <q.wiersma@vu.nl>

@date: 2022/03/14
=#

function f(ψ::AbstractVector, model::StateSpaceModel, filter::KalmanFilter, sys::StateSpaceSystem)
    # Get dims
    (n,T)= size(model.y)

    # Store parameters
    store_params!(model, ψ)

    # Get system
    init_system!(sys, model)
    get_system!(sys, model)

    # Run Kalman filter
    kalman_filter!(filter, model.y, sys)

    # Average negative log-likelihood
    ll= -inv(n * T) * loglik(filter)

    return ll
end

"""
    maximum_likelihood!(model; init=NamedTuple(), ϵ_abs=1e-7, ϵ_rel=1e-3, max_iter=1000)

Maximum Likelihood (ML) algorithm to estimate the hyper parameters of
a linear Gaussian State Space model as defined by `model`, results are stored in
`model`.

#### Arguments
  - `model::StateSpaceModel`: state space model
  - `init::NamedTuple`      : initial model parameters
  - `ϵ_abs::Real`           : absolute tolerance
  - `ϵ_rel::Real`           : relative tolerance
  - `max_iter::Integer`     : max number of iterations

#### Returns
  - `ll::Real`          : log-likelihood value
"""
function maximum_likelihood!(model::StateSpaceModel;
            init::NamedTuple=NamedTuple(), ϵ_abs::Real=1e-7, ϵ_rel::Real=1e-3, max_iter::Integer=1_000)
    # Initialize state space model and system
    sys= init!(model, init)

    # Get dims
    (n,T)= size(model.y)
    p= length(sys.a1)
    n_params= nparams(model)

    # Initialize parameters
    ψ0= similar(model.y, n_params)
    get_parameters!(ψ0, model)

    # Initialize filter and smoother
    filter= MultivariateFilter(similar(model.y, p, T), similar(model.y, p, p, T),
                            similar(model.y, n, T), similar(model.y, n, n, T),
                            similar(model.y, p, n, T))

    # Closure of objective function
    f_cl(x::AbstractVector)= f(x, model, filter, sys)

    res= optimize(f_cl, ψ0, LBFGS())

    return minimum(res)
end


function solveValueFunction()

#This function obtains the value function for each time period and
#the policy function (i.e. optimal next-period asset choice) for each time
#period. From there we can work the optimal consumption level.

#The approach taken is by backwards recursion. The optimisation each period
#is carried out using 'fminbnd'. This is an in-built optimiser in Matlab.
#The optimisation routine it uses is known as the 'golden search method'

#global T r tol minCons
#global numPointsA numPointsY Agrid Ygrid incTransitionMrx
#global Agrid1 EV1

#% ------------------------------------------------------------------------
# GENERATE MATRICES TO STORE NUMERICAL APPROXIMATIONS AND INITIATE AS NAN

# Matrices to hold the policy, value and marginal utility functions
V        = Array(Float64,T+1, numPointsA, numPointsY);
policyA1 = Array(Float64,T,   numPointsA, numPointsY);
policyC  = Array(Float64,T,   numPointsA, numPointsY);

#Matrices to hold expected value and marginal utility functions
EV  = Array(Float64,T+1, numPointsA, numPointsY);

#% ------------------------------------------------------------------------
#Set the terminal value function and expected value function to 0

EV[T + 1, :,:]  = .0;          # continuation value at T-1
V[T + 1,:,:]    = .0;
#% ------------------------------------------------------------------------
# SOLVE RECURSIVELY THE CONSUMER'S PROBLEM, STARTING AT TIME T-1 AND MOVING
# BACKWARDS TO ZERO, ONE PERIOD AT A TIME

for ixt=T:-1:1                                # Loop from time T-1 to 1
    Agrid1 = vec(Agrid[ixt + 1, :]);          # The grid on assets tomorrow, vector

    for ixA = 1:1:numPointsA                  # points on asset grid

        # STEP 1. solve problem at grid points in assets and income
        # ---------------------------------------------------------
        A    = Agrid[ixt, ixA];            # assets today
        lbA1 = Agrid[ixt + 1, 1];          # lower bound: assets tomorrow

        for ixY = 1:1:numPointsY               # points on income grid

            # Value of income and information for optimisation
            Y    = Ygrid[ixt, ixY];            # income today
            ubA1 = (A + Y - minCons)*(1+r);    # upper bound: assets tomorrow
            EV1  = vec(EV[ixt + 1,:, ixY]);    # relevant section of EV matrix (in assets tomorrow), vector

            intfV  = InterpIrregular(Agrid1,EV1, BCnil, InterpLinear); # Define the interpolation

            # Compute solution
            if (ubA1 - lbA1 < minCons)                       # if liquidity constrained
                negV = objectivefunc(lbA1, A, Y,Agrid1,EV1,intfV);
                policyA1[ixt,ixA,ixY] = lbA1;
            else                                             # if interior solution

                function obj(A1)
                    return objectivefunc(A1, A, Y, Agrid1,EV1,intfV)
                end

                Res = optimize(obj,lbA1,ubA1);          #Find the solution...

                policyA1[ixt,ixA,ixY]=Res.minimum;
                negV=Res.f_minimum;

                if isnan(negV)
                    println("Sendo error en ixt=$ixt, ixA=$ixA, ixY=$ixY")
                end

            end # if (ubA1 - lbA1 < minCons)

            # Store solution and its value
            policyC[ixt, ixA, ixY] = A + Y - policyA1[ixt, ixA, ixY]/(1+r);
            V[ixt, ixA, ixY]       = -negV;
        end #ixY

        # STEP 2. integrate out income today conditional on income
        # yesterday to get EV and EdU
        # --------------------------------------------------------
        realisedV = squeeze(V[ixt, ixA, :],1);
        for ixY = 1:1:numPointsY
            d=incTransitionMrx[ixY,:]*realisedV'
            EV[ixt, ixA, ixY]  = d[1,1];
        end #ixY
    end #ixA

    println("Passed period $ixt of $T")

end #ixt

return policyA1, policyC, V, EV

end #function


using FromFile
using SymbolicUtils
@from "Core.jl" import CONST_TYPE, Node, Options
@from "Utils.jl" import isgood, isbad, @return_on_false

const SYMBOLIC_UTILS_TYPES = Union{<:Number,SymbolicUtils.Symbolic{<:Number}}

const SUPPORTED_OPS = (cos, sin, exp, cot, tan, csc, sec, +, -, *, /)

isgood(x::SymbolicUtils.Symbolic) = SymbolicUtils.istree(x) ? all(isgood.([SymbolicUtils.operation(x);SymbolicUtils.arguments(x)])) : true
subs_bad(x) = isgood(x) ? x : Inf

function parse_tree_to_eqs(tree::Node, options::Options, index_functions::Bool=false, evaluate_functions::Bool=false)
    if tree.degree == 0
        # Return constant if needed
        tree.constant && return subs_bad(tree.val)
        return SymbolicUtils.Sym{Number}(Symbol("x$(tree.feature)"))
    end
    # Collect the next children
    children = tree.degree >= 2 ? (tree.l, tree.r) : (tree.l,)
    # Get the operation
    op = tree.degree > 1 ? options.binops[tree.op] : options.unaops[tree.op]
    # Create an N tuple of Numbers for each argument
    dtypes = map(x->Number, 1:tree.degree)
    #
    if !(op ∈ SUPPORTED_OPS) && index_functions
        op = SymbolicUtils.Sym{(SymbolicUtils.FnType){Tuple{dtypes...}, Number}}(Symbol(op))
    end

    return subs_bad(op(map(x->parse_tree_to_eqs(x, options, index_functions, evaluate_functions), children)...))
end

## Convert symbolic function back
convert_to_function(x, args...) = x

function convert_to_function(x::SymbolicUtils.Sym{SymbolicUtils.FnType{T, Number}}, options::Options) where {T <: Tuple}
    degree = length(T.types)
    if degree == 1
        ind = findoperation(x.name, options.unaops)
        return options.unaops[ind]
    elseif degree == 2
        ind = findoperation(x.name, options.binops)
        return options.binops[ind]
    else
        throw(AssertionError("Function $(String(x.name)) has degree > 2 !"))
    end
end

# Split equation
function split_eq(op, args, options::Options; varMap::Union{Array{String, 1}, Nothing}=nothing)
    !(op ∈ (sum, prod, +, *)) && throw(error("Unsupported operation $op in expression!"))
    if Symbol(op) == Symbol(sum)
        ind = findoperation(+, options.binops)
    elseif Symbol(op) === Symbol(prod)
        ind = findoperation(*, options.binops)
    else
        ind = findoperation(op, options.binops)
    end
    return Node(ind, convert(Node, args[1], options; varMap=varMap), convert(Node, op(args[2:end]...), options; varMap=varMap))
end

function findoperation(op, ops)
    for (i,oi) in enumerate(ops)
        Symbol(oi) == Symbol(op) && return i
    end
    throw(error("Operation $op in expression not found in operationrs $ops!"))
end

function Base.convert(::typeof(Node), x::Number, options::Options; varMap::Union{Array{String, 1}, Nothing}=nothing)
    return Node(CONST_TYPE(x))
end

function Base.convert(::typeof(Node), x::Symbol, options::Options; varMap::Union{Array{String, 1}, Nothing}=nothing)
    varMap === nothing && return Node(String(x))
    return Node(String(x), varMap)
end

function Base.convert(::typeof(Node), x::SymbolicUtils.Symbolic, options::Options; varMap::Union{Array{String, 1}, Nothing}=nothing)
    if !SymbolicUtils.istree(x)
        varMap === nothing && return Node(String(x.name))
        return Node(String(x.name), varMap)
    end

    op = convert_to_function(SymbolicUtils.operation(x))
    args = SymbolicUtils.arguments(x)

    length(args) > 2 && return split_eq(op, args, options; varMap=varMap)
    ind = length(args) == 2 ? findoperation(op, options.binops) : findoperation(op, options.unaops)

    return Node(ind, map(x->convert(Node, x, options, varMap=varMap), args)...)
end

"""
    node_to_symbolic(tree::Node, options::Options;
                varMap::Union{Array{String, 1}, Nothing}=nothing,
                evaluate_functions::Bool=false,
                index_functions::Bool=false)

The interface to SymbolicUtils.jl. Passing a tree to this function
will generate a symbolic equation in SymbolicUtils.jl format.

## Arguments

- `tree::Node`: The equation to convert.
- `options::Options`: Options, which contains the operators used in the equation.
- `varMap::Union{Array{String, 1}, Nothing}=nothing`: What variable names to use for
    each feature. Default is [x1, x2, x3, ...].
- `evaluate_functions::Bool=false`: Whether to evaluate the operators, or
    leave them as symbolic.
- `index_functions::Bool=false`: Whether to generate special names for the
    operators, which then allows one to convert back to a `Node` format
    using `symbolic_to_node`.
"""
function node_to_symbolic(tree::Node, options::Options;
                     varMap::Union{Array{String, 1}, Nothing}=nothing,
                     evaluate_functions::Bool=false,
                     index_functions::Bool=true
                     )
    expr = subs_bad(parse_tree_to_eqs(tree, options, index_functions, evaluate_functions))
    # Check for NaN and Inf
    @assert isgood(expr) "The recovered equation contains NaN or Inf."
    # Return if no varMap is given
    varMap === nothing && return expr
    # Create a substitution tuple
    subs = Dict(
        [SymbolicUtils.Sym{Number}(Symbol("x$(i)")) => SymbolicUtils.Sym{Number}(Symbol(varMap[i])) for i in 1:length(varMap)]...
    )
    return substitute(expr, subs)
end

function symbolic_to_node(eqn::T, options::Options;
                          varMap::Union{Array{String, 1}, Nothing}=nothing)::Node where {T<:SymbolicUtils.Symbolic}

    convert(Node, eqn, options; varMap=varMap)
end


# This file is a part of AstroLib.jl. License is MIT "Expat".

function _euler(ai::T, bi::T, select::Integer, FK4::Bool, radians::Bool) where {T<:AbstractFloat}

    if select>6 || select<1
        error("Input for coordinate transformation should be an integer in the range 1:6")
    end

    if FK4
        psi = (0.57595865315, 4.9261918136, 0.0, 0.0, 0.11129056012, 4.7005372834)
        stheta = (0.88781538514, -0.88781538514,0.39788119938, -0.39788119938,
                  0.86766174755, -0.86766174755)
        ctheta = (0.46019978478, 0.46019978478, 0.91743694670, 0.9174369467,
                  0.49715499774, 0.49715499774)
        phi = (4.9261918136,  0.57595865315, 0.0, 0.0, 4.7005372834, 0.11129056012)
    else
        psi = (0.574770433, 4.9368292465, 0.0, 0.0, 0.11142137093, 4.71279419371)
        stheta = (0.88998808748, -0.88998808748, 0.39777715593, -0.39777715593,
                  0.86766622025, -0.86766622025)
        ctheta = (0.45598377618, 0.45598377618, 0.91748206207, 0.91748206207,
                  0.49714719172, 0.49714719172)
        phi = (4.9368292465, 0.574770433, 0.0, 0.0, 4.71279419371, 0.11142137093)
    end

    if !radians
        ai = deg2rad(ai)
        bi = deg2rad(bi)
    end
    sa, ca = sincos(ai - phi[select])
    sb, cb = sincos(bi)
    x = (cb * sa, cb * ca, sb)
    bo = ctheta[select]*x[3] - stheta[select]*x[1]
    ao = mod2pi(atan(ctheta[select]*x[1] + stheta[select]*x[3], x[2]) + psi[select])
    bo = asin(bo)

    if radians
        return (ao, bo)
    end
    return (rad2deg(ao), rad2deg(bo))
end

"""
    euler(ai, bi, select[, FK4=true, radians=true])

### Purpose ###

Transform between Galactic, celestial, and ecliptic coordinates.

### Explanation ###

The function is used by the astro procedure.

### Arguments ###

* `ai`: input longitude, scalar or vector.
* `bi`: input latitude, scalar or vector.
* `select` : integer input specifying type of coordinate
  transformation.
  SELECT   From          To     | SELECT   From       To
     1   RA-Dec (2000) Galactic |   4    Ecliptic   RA-Dec
     2   Galactic      RA-DEC   |   5    Ecliptic   Galactic
     3   RA-Dec        Ecliptic |   6    Galactic   Ecliptic

* `FK4` (optional boolean keyword) : if this keyword is set to `true`,
  then input and output celestial and ecliptic coordinates should be
  given in equinox B1950. When `false`, by default, they should be given in
  equinox J2000.
* `radians` (optional boolean keyword) : if this keyword is set to
  `true`, all input and output angles are in radians rather than degrees.

### Output ###

a 2-tuple `(ao, bo)`:

* `ao`: output longitude in degrees.
* `bo`: output latitude in degrees.

### Example ###

Find the Galactic coordinates of Cyg X-1 (ra=299.590315, dec=35.201604)

```jldoctest
julia> using AstroLib

julia> euler(299.590315, 35.201604, 1)
(71.33498957116959, 3.0668335310640984)
```

### Notes ###

Code of this function is based on IDL Astronomy User's Library.
"""
euler(ai::Real, bi::Real, select::Integer; FK4::Bool=false, radians::Bool=false) =
    _euler(promote(float(ai), float(bi))..., select, FK4, radians)

euler(aibi::Tuple{Real, Real}, select::Integer; FK4::Bool=false, radians::Bool=false) =
    euler(aibi[1], aibi[2], select, FK4=FK4, radians=radians)

function euler(ai::AbstractVector{R}, bi::AbstractVector{<:Real}, select::Integer;
               FK4::Bool=false, radians::Bool=false) where {R<:Real}
    @assert length(ai) == length(bi) "ai and bi arrays should be of the same length"
    typeai = float(R)
    ai_out  = similar(ai,  typeai)
    bi_out = similar(bi, typeai)
    for i in eachindex(ai)
        ai_out[i], bi_out[i] = euler(ai[i], bi[i], select,
                                        FK4=FK4, radians=radians)
    end
    return ai_out, bi_out
end


"""
$(TYPEDEF)

Implements the Block Urn Design (BUD) of [Zhao and Wang (2011)](https://doi.org/10.1016/j.cct.2011.08.004).

The BUD has conditional allocation probability

```math
P_{im} = \\frac{w_m\\lambda + w_mk_{i-1}-n_{i-1,m}}{W\\lambda + Wk_{i-1} - (i - 1)},
\\quad k_{i-1} = \\min_{m=1,...,M} \\left\\lfloor \\frac{n_{i-1,k}}{w_k} \\right\\rfloor.
```

# Fields
$(TYPEDFIELDS)
"""
struct BlockUrn <: MultiArmRandomisationModel
    target::AbstractVector{<:AbstractFloat}
    blocksize::Integer
    weights::AbstractVector{<:Integer}
    W::Integer
    sequence::AbstractVector{<:Integer}
    dist::AbstractVector{<:Integer}
    BlockUrn(
        target::AbstractVector{<:AbstractFloat},
        blocksize::Integer,
        weights::AbstractVector{<:Integer},
        W::Integer,
        sequence::AbstractVector{<:Integer},
        dist::AbstractVector{<:Integer},
    ) = new(target, blocksize, weights, W, sequence, dist)
end

"""
$(TYPEDSIGNATURES)
"""
function BlockUrn(weights::AbstractVector{<:Integer}, blocksize::Int)
    if !all(weights .≥ zero(eltype(weights)))
        throw(DomainError(weights, "All values must be ≥ 0"))
    end
    target = normalize(weights, 1)
    W = sum(weights)
    if mod(blocksize, W) != 0
        throw(DomainError(blocksize, "blocksize not integer multiple of sum(weights)."))
    end
    BlockUrn(target, blocksize, weights, W, zeros(Int, 0), zeros(Int, length(target)))
end

"""
$(TYPEDSIGNATURES)
"""
function BlockUrn(target::AbstractVector{<:AbstractFloat}, blocksize::Int)
    if !isposvec(target)
        throw(DomainError(target, "All values must be ≥ 0"))
    end
    if !isnormvec(target)
        target = target ./ sum(target)
    end
    weights = round.(Int, target .* blocksize)
    W = sum(weights)
    if mod(blocksize, W) != 0
        errstr = "blocksize not integer multiple of sum(target*blocksize)."
        throw(DomainError(blocksize, errstr))
    end
    BlockUrn(target, blocksize, weights, W, zeros(Int, 0), zeros(Int, length(target)))
end

Base.deepcopy(m::BlockUrn) = BlockUrn(deepcopy(m.weights), deepcopy(m.blocksize))

function show(io::IO, BU::BlockUrn)
    k = length(BU.target)
    n = length(BU.sequence)
    print(io, "BlockUrn(arms = $k, blocksize = $(blocksize(BU)), n = $n)")
end

blocksize(BU::BlockUrn) = BU.blocksize
weights(BU::BlockUrn) = BU.weights
target(BU::BlockUrn) = BU.target

"""
$(TYPEDSIGNATURES)
"""
function prob(BU::BlockUrn)
    b = blocksize(BU)
    w = weights(BU)
    W = sum(w)
    λ = b ÷ W
    n = dist(BU)
    i = sum(n) + 1
    k = minimum(floor.(Int, n ./ w)) # minimal balanced sets in previous assignments
    return (λ * w + w * k - n) / (W * λ + W * k - (i - 1))
end

"""
$(TYPEDSIGNATURES)
"""
function update!(BU::BlockUrn, y::Int)
    push!(BU.sequence, y)
    BU.dist[y] += 1
    return nothing
end


using DReal
using Base.Test

import DReal:  opensmt_init, opensmt_mk_context,
               opensmt_mk_real_var, opensmt_mk_eq, opensmt_assert, opensmt_check,
               opensmt_mk_sin, opensmt_mk_cos, opensmt_get_ub, opensmt_get_lb,
               opensmt_set_precision

begin
# Blank slate
reset_ctx!()

# ## Example
# ## =======
ctx = opensmt_mk_context(@compat UInt32(1))
opensmt_set_precision(ctx, 0.00001)
x = opensmt_mk_real_var( ctx, "x" , -3.141592, 3.141592)
y = opensmt_mk_real_var( ctx, "y" , -3.141592, 3.141592)
eq = opensmt_mk_eq( ctx, opensmt_mk_sin(ctx, x), opensmt_mk_cos(ctx, y));
opensmt_assert( ctx, eq );
res = opensmt_check( ctx );
println("res is ", res == -1 ? "unsat" : "sat")
@test res == 1
if res == 1
  x_ub = opensmt_get_ub(ctx, x)
  x_lb = opensmt_get_lb(ctx, x)
  println("x = [$x_lb, $x_ub]")
  y_ub = opensmt_get_ub(ctx, y)
  y_lb = opensmt_get_lb(ctx, y)
  println("y = [$y_lb, $y_ub]")
end
end


using QInterpolations, Test

@testset "QInterpoations" begin
x = range(0,stop=10,length=100)
y1 = Array(x) + 1.0im*Array(x)
y2 = (10.0+10.0im) .- Array(x)
inter_y1 = construct_interpolations(x,y1)
test_x = range(0,stop=10,length=50)
exp_y1 = Array(test_x) + 1.0im*Array(test_x)
exp_y2 = (10.0+10.0im) .- Array(test_x)
res_y1 = [inter_y1(i) for i in test_x]
@test isapprox(exp_y1, res_y1)
# Test for complex vector interpolation
y_vec_test = [[y1[i],y2[i]] for i in eachindex(y1)]
inter_yvec = construct_interpolations(x, y_vec_test)
res_y_vec = [inter_yvec(i) for i in test_x]
exp_y_vec = [[exp_y1[i],exp_y2[i]] for i in eachindex(exp_y1)]
@test isapprox(res_y_vec, exp_y_vec)
# Test for complex matrix interolation
y_mat_test = [[y1[i] y2[i]; y2[i] y1[i]] for i in eachindex(y1)]
inter_ymat = construct_interpolations(x, y_mat_test)
res_y_mat = [inter_ymat(i) for i in test_x]
exp_y_mat = [[exp_y1[i] exp_y2[i]; exp_y2[i] exp_y1[i]] for i in eachindex(exp_y1)]
@test isapprox(res_y_mat, exp_y_mat)
# Test for complex vector gradient
res_g_yvec = gradient(inter_yvec, 1.0)
@test isapprox(res_g_yvec, [1.0+1.0im,-1], atol=1e-6)
# Test for real number extrapolation
x = range(1.0,stop=10.0)
eitp = construct_interpolations(x, Array(x) , extrapolation="line")
@test isapprox(eitp(0.0),0.0, atol=1e-8)
end


using CSV, StateSpaceModels, Plots, Dates

# Load the Nile annual flow dataset
nile = CSV.read("Nile.csv")

# Convert data to an array of Float64
flow = Float64.(nile.Flow)

# Plot the data
p1 = plot(nile.Year, flow, label = "Annual flow", legend = :topright, color = :black)

# Specify the state-space model
model = local_level(flow)

# Estimate the state-space model
ss = statespace(model)

# Innovations
p2 = plot(nile.Year, ss.filter.v, label = "Innovations (v_t)", legend = :topright, color = :black)

# Plot filtered state and 90% confidence interval
std_ptt = sqrt.(ss.filter.Ptt[1, 1, :])
att = vec(ss.filter.att)
lb = vec(att + 1.64 * std_ptt)
ub = vec(att - 1.64 * std_ptt)
p3 = plot!(p1, nile.Year, att, ribbon = [ub - att, lb + att],
           label = "Filtered state (att)", color = :red, fillalpha = 0.1)


## Transformations
probit(x) = quantile(Normal(), x)

function transform!(trans_samples::AbstractMatrix, samples::AbstractMatrix, lb::AbstractVector, ub::AbstractVector)
    nd, ns = size(samples)
    for j = 1:ns, i = 1:nd
        trans_samples[i,j] = transform(samples[i,j], lb[i], ub[i])
    end
    return trans_samples
end
transform(samples::AbstractMatrix, lb::AbstractVector, ub::AbstractVector) = transform!(similar(samples), samples, lb, ub)
transform(x, lb::Real, ub::Real) = probit((x - lb)/(ub - lb))
transform(x, ::Missing, ::Missing) = x
transform(x, lb::Real, ::Missing) = log(x - lb)
transform(x, ::Missing, ub::Real) = log(ub - x)

# With bijectors
transform(x, dist::Distribution, trans::Bool) = trans ? Bijectors.link(dist,x) : x

# Inverse transformations
function invtransform!(samples::AbstractMatrix, trans_samples::AbstractMatrix, lb::AbstractVector, ub::AbstractVector)
    nd, ns = size(samples)
    for j = 1:ns, i = 1:nd
        samples[i,j] = invtransform(trans_samples[i,j], lb[i], ub[i])
    end
    return samples
end
invtransform(trans_samples::AbstractMatrix, lb::AbstractVector, ub::AbstractVector) = invtransform!(similar(trans_samples), trans_samples, lb, ub)
invtransform(x, lb::Real, ub::Real) = lb + (ub - lb) * cdf(Normal(), x)
invtransform(x, ::Missing, ::Missing) = x
invtransform(x, lb::Real, ::Missing) = exp(x) + lb
invtransform(x, ::Missing, ub::Real) = ub - exp(x)

# With bijectors
invtransform(x, dist::Distribution, trans::Bool) = trans ? Bijectors.invlink(dist,x) : x

# Log jacobian contribution : x is the transformed variable
logjacobian_contribution(x, lb, ub) = log(ub-lb) + logpdf(Normal(), x)
logjacobian_contribution(x, ::Missing, ::Missing) = 0.0
logjacobian_contribution(x, lb, ::Missing) = x
logjacobian_contribution(x, ::Missing, ub) = x

# With bijectors
function logjacobian_contribution(x, dist::Distribution, trans::Bool)
    b =  Bijectors.bijector(dist)
    return trans ? Bijectors.logabsdetjac(b, Bijectors.invlink(dist, x)) : 0.0
end


# rotate2d.jl
# Copyright 2019-03-05, Jeff Fessler, University of Michigan

using Test: @test

"""
`(xr,yr) = rotate2d(x, y, theta)`
2D rotation
"""
function rotate2d(x, y, theta)
	xr = cos(theta) * x + sin(theta) * y
	yr = -sin(theta) * x + cos(theta) * y
	return (xr, yr)
end

"""
`rotate2d(:test)`
self test
"""
function rotate2d(test::Symbol)
	test != :test && throw(ArgumentError("test $test"))
	@test isapprox([rotate2d(1, 1, pi/2)...], [(1.,-1.)...])
	true
end

rotate2d(:test)


using OscillatoryIntegralsODE
using SpecialFunctions
using StaticArrays
using BenchmarkTools
using PyPlot
using OrdinaryDiffEq

ω = 100.
f(x) = exp(-x^2/16)

clf()
xs = 0:0.001:5.0
plot(xs, real.([f(x) * exp(1im * ω * x) for x in xs]), "-", label=raw"$e^{-x^2/16} e^{-i 100 x}$")
legend()
gcf()

##
hi = HarmonicIntegral{Complex{Float64}}(ω)
result = levintegrate(hi, f, 1.0, 5.0, Vern9(); abstol=1e-6, reltol=1e-6)

ref = 0.003798145404262353+0.009953606082753418im
print("relative error = ", (result - ref) / abs(ref), "\n")

## timing
@btime levintegrate($si, $f, 1.0, 5.0, Vern9(); abstol=1e-6, reltol=1e-6)


#========================================================================

	Steady state solution for an Aiyagari model with a diffusion process

	Translated julia code based on code from Ben Moll's Website:
		http://www.princeton.edu/%7Emoll/HACTproject.htm

		For Julia 1.0.0
========================================================================#
using LinearAlgebra, SparseArrays, Plots

# Model parameters
γ = 2 # CRRA utility
α = 1/3 # parameter for cobb-douglas production function
δ = 0.05 # captal depreciation
ρ = 0.05 # discount rate
A_prod =0.1 # term in production function

# Parameters for productivity and the poisson process q
z_1 = 1
z_2 = 2*z_1
z = [z_1 z_2]
λ_1 = 1/3 # prob of transitioning from state 1 to 2
λ_2 = 1/3  # state 2 to 1
λ = [λ_1 λ_2]
z_ave = (z_1*λ_2 + z_2*λ_1)/(λ_1 + λ_2) # Average value of z

H = 1000 # Number of elements in grid space for a
a_min = 0.0
a_max = 20.0

# Grid spaces for variables
a = LinRange(a_min, a_max, H)
a = convert(Array,a)
da = (a_max-a_min)/(H-1)

aa = [a a]
zz = ones(H,1)*z #check later

# Paramters for the simulation
maxit = 100
Ir = 40
crit = 10^(-6)
critS = 1e-5
Δ = 1000 #step size for HJB

# Spaces for the finite difference terms
Vaf, Vab, Vzf, Vzb, Vzz, c= [zeros(H,2) for i in 1:6]
V_r = zeros(H,2,H)

# Spaces for other terms
r_r =[]
r_min_r =[]
r_max_r =[]
KD=[]
KS=[]
S=[]
adot=[]
V_n=[]
g_r=[]

# Construct the matrix to summarize the evolution of z over time
# this matrix will not change over the finite differences and is based on the poisson process

Aswitch = [-sparse(I,H,H)*λ[1] sparse(I,H,H)*λ[2] ; sparse(I,H,H)*λ[1] -sparse(I,H,H)*λ[2]]

# Prices
r =0.04 #initial r
global w = 0.05

r0=0.03
global r_min=0.01
global r_max=.99*ρ

dist = zeros(1,maxit)
v0=zeros(H,2)

# The main loop, this makes sure markets clear

for ir in 1:Ir
	global r =r
	global r_min = r_min
	global r_max = r_max
	global v0 = v0

	println("Main Loop iteration $(ir)")

	push!(r_r, r)
	push!(r_min_r, r_min)
	push!(r_max_r, r_max )

	push!(KD, (α*A_prod/(r+δ))^(1/(1-α))*z_ave)
	global w = (1-α)*A_prod*KD[ir].^α * z_ave^(-α)

	v0[:,1] = (w*z[1] .+ r.*a).^(1-γ)/(1-γ)/ρ
	v0[:,2] = (w*z[2] .+ r.*a).^(1-γ)/(1-γ)/ρ

	if ir >1
		v0 = V_r[:,:,ir-1]
	end

	global v=v0

# Inner loop, value function iteration

	for n in 1:maxit
		V=v
		push!(V_n, V)
		# Forward difference
		Vaf[1:H-1,:] = (V[2:H,:]-V[1:H-1,:])/da
		Vaf[H,:] = (w*z .+ r.*a_max).^(-γ)

		# Backward difference
		Vab[2:H,:] = (V[2:H,:]-V[1:H-1,:])/da
		Vab[1,:] = (w*z .+ r.*a_min).^(-γ)

		# caculate consumption and savings
		# First the forward difference case
		cf = Vaf.^(-1/γ)
		sf = w*zz + r.*aa -cf

		# consumption and savings for the backward difference
		cb = Vab.^(-1/γ)
		sb = w*zz + r.*aa -cb

		#Consumption and V' at the steady state
		c0 = w*zz + r.*aa
		Va0 = c0.^(-γ)

		# Setup to the Upwind scheme
		If = sf .> 0
		Ib = sb .< 0
		I0 = (1 .- If - Ib)

		#Now implement the upwind scheme
		Va_Upwind = Vaf.*If + Vab.*Ib + Va0.*I0

		global c= Va_Upwind.^(-1/γ)
		u = c.^(1-γ)/(1-γ)

		X = -min.(sb,0)/da
		Y = -max.(sf,0)/da + min.(sb,0)/da
		Z = max.(sf,0)/da

		A1 = spdiagm(0=>Y[:,1], -1 => X[2:H,1], 1=> Z[1:H-1,1])
		A2 = spdiagm(0=>Y[:,2], -1 => X[2:H,2], 1=> Z[1:H-1,2])
		AA = [A1 spzeros(H,H); spzeros(H,H) A2]

		global A= AA + Aswitch

		B = (1/Δ + ρ)*sparse(I,2*H,2*H)-A

		u_stacked = [u[:,1]; u[:,2]]
		V_stacked = [V[:,1]; V[:,2]]

		b = u_stacked + V_stacked/Δ

		V_stacked = B\b

		global V = [V_stacked[1:H] V_stacked[H+1:2*H]]

		V_change = V-v

		v = V

		dist[n] = findmax(V_change)[1]

		if dist[n] < crit
			println("Value Function Converge Iteration = $(n)")
			break
		end
	end
	# Now solve the Fokker-Planck equation
	AT = A'
	b = zeros(2*H,1)

	# Dirty fix to avoid matrix singularities
	i_fix=1
	b[i_fix] =.1
	row = [zeros(1,i_fix-1) 1 zeros(1,H*2-i_fix)]
	AT[i_fix,:] = row

	# Now solve the linear system
	gg = AT\b
	g_sum = gg'*ones(2*H,1)*da
	gg = gg./g_sum

	global g = [gg[1:H] gg[H+1:2*H]]

	push!(g_r, g)
	push!(adot, w*zz+r.*aa-c)
	V_r[:,:,ir] = V

	push!(KS, g[:,1]'*a*da + g[:,2]'*a*da)
	push!(S, KS[ir]-KD[ir])

	# Update aggregate capital
	if S[ir]>critS
		println("Excess Supply")
		r_max =r
		r = 0.5*(r+r_min)
	elseif S[ir] < -critS
		println("Excess Demand")
		r_min =r
		r = 0.5*(r+r_max)
	elseif abs(S[ir]) < critS
		println("Equilibrium Found!!!")
		break
	end
end

# Graphs

amax1=5
amin1=a_min-.1
plot(a, adot[end][:,1], label="\$s_{1}(a)\$", legend=:topright,
	xlims=(amin1,amax1), ylims=(-0.03,0.05),color=:blue,
	ylabel="Savings", xlabel="Wealth")
plot!(a,adot[end][:,2], label="\$s_{2}(a)\$", color=:red)
plot!(LinRange(amin1,amax1,H),zeros(H,1), label="",line=:dash, color=:black)
plot!(zeros(H,1),LinRange(-0.03,0.05,H), label="",line=:dash, color=:black)
png("savings_plot")


function TableauHSPARKSymmetricProjection(name, q::Tableau{T}, p::Tableau{T}, d=[]; R∞=1) where {T}

    @assert q.s == p.s

    o = min(q.o, p.o)

    a_q = q.a
    a_p = p.a

    α_q = zeros(T, q.s, 2)
    α_q[:,1] .= 0.5

    α_p = zeros(T, p.s, 2)
    α_p[:,1] .= 0.5

    a_q̃ = vcat(zero(q.b)', q.b')
    a_p̃ = vcat(zero(p.b)', p.b')

    α_q̃ = [[0.0  0.0]
           [0.5  0.5]]
    α_p̃ = [[0.0  0.0]
           [0.5  0.5]]

    b_q = q.b
    b_p = p.b
    β_q = [0.5, 0.5]
    β_p = [0.5, 0.5]

    c_q = q.c
    c_p = p.c
    c_λ = [ 0.0, 1.0]
    d_λ = [ 0.5, 0.5*R∞]

    ω_λ = reshape(T[1  1  0], (1,3))
    δ_λ = reshape(T[-1   R∞], (1,2))

    if length(d) == 0
        return TableauHSPARKprimary(name, o,
                            a_q, a_p, α_q, α_p,
                            a_q̃, a_p̃, α_q̃, α_p̃,
                            b_q, b_p, β_q, β_p,
                            c_q, c_p, c_λ, d_λ,
                            ω_λ, δ_λ)
    else
        @assert length(d) == q.s == p.s

        return TableauHSPARKprimary(name, o,
                            a_q, a_p, α_q, α_p,
                            a_q̃, a_p̃, α_q̃, α_p̃,
                            b_q, b_p, β_q, β_p,
                            c_q, c_p, c_λ, d_λ,
                            ω_λ, δ_λ, d)
    end

end

"Tableau for Gauss-Lobatto IIIA-IIIB method with s stages and symmetric projection."
function TableauHSPARKLobattoIIIAIIIBpSymmetric(s)
    TableauHSPARKSymmetricProjection(Symbol("HSPARKLobattoIIIAIIIB($s)pSymmetric"), TableauLobattoIIIA(s), TableauLobattoIIIB(s), get_lobatto_nullvector(s))
end

"Tableau for Gauss-Lobatto IIIB-IIIA method with s stages and symmetric projection."
function TableauHSPARKLobattoIIIBIIIApSymmetric(s)
    TableauHSPARKSymmetricProjection(Symbol("HSPARKLobattoIIIBIIIA($s)pSymmetric"), TableauLobattoIIIB(s), TableauLobattoIIIA(s), get_lobatto_nullvector(s))
end

"Tableau for Gauss-Legendre method with s stages and symplectic projection."
function TableauHSPARKGLRKpSymmetric(s)
    glrk = TableauGauss(s)
    TableauHSPARKSymmetricProjection(Symbol("HSPARKGLRK($s)"), glrk, glrk)
end


using ParamLevelSet
using jInv.Mesh

println("test for SPD regularizer");

plotting = false;
if plotting
	using jInvVis
	using PyPlot
end

n = [64,64,64];
Mesh = getRegularMesh([0.0;5.0;0.0;5.0;0.0;5.0],n);
midmesh = [(Mesh.domain[2]+Mesh.domain[1])/2 ; (Mesh.domain[4]+Mesh.domain[3])/2; (Mesh.domain[5]+Mesh.domain[6])/2];
lenmesh = [(Mesh.domain[2]-Mesh.domain[1]) ; (Mesh.domain[4]-Mesh.domain[3]); (Mesh.domain[6]-Mesh.domain[5])];
numParamOfRBF = 10;
nRBF = 5
m = zeros(nRBF*numParamOfRBF);
dm = zeros(length(m));

for k=1:nRBF
	offset = (k-1)*numParamOfRBF + 1;
	m[offset] = randn();
	A = randn(3,3); A = 0.3*A'*A + 0.5*Matrix(I,3,3);
	m[(offset+1):(offset+6)] = A[idx_trilA()];
	m[(offset+7):(offset+9)] = midmesh + 0.05*lenmesh.*randn(size(lenmesh));
	dm[offset] = 0.01*randn();
	dm[(offset+1):(offset+6)] = 0.01*randn(6);# 0.01*[randn(); randn(); 0.0; randn(); 0.0; randn()];#
	dm[(offset+7):(offset+9)] = 0.01*randn(3)
end

regfun = (m)-> RBF_SPD_regularization(m,0,nRBF);
hh = 1.0;
R0,dR,d2R = regfun(m);
for k=0:8
	hhh = (0.5^k)*hh;
	Rt,dRt = regfun(m+ hhh*dm);
	println("norm(Rt-R0): ",norm(Rt-R0),", norm(Rt - R0 - dR*dm): ",norm(Rt - R0 - dot(dR,hhh*dm)));
end

println("Testing reg Hessian")
for k=0:8
	hhh = (0.5^k)*hh;
	Rt,dRt = regfun(m+ hhh*dm);
	println("norm(dRt-dR0): ",norm(dRt-dR),", norm(dRt - dR - d2R*dm): ",norm(dRt - dR - d2R*(hhh*dm)));
end

## Below are some checks that we had to do along the way. May be ignored.

# regfun = (A)->(-log(det(A)),-inv(A))
# m = randn(3,3); m = 0.3*m'*m + 0.5*eye(3);
# dm = 0.01*randn(3,3); dm = dm + dm';
# R0,dR = regfun(m);
# for k=0:8
	# hhh = (0.5^k);
	# Rt, = regfun(m+ hhh*dm);
	# println("norm(Rt-R0): ",norm(Rt-R0),", norm(Rt - R0 - dR*dm): ",norm(Rt - R0 - dot(dR[:],hhh*dm[:])));
# end

# regfun = (A)->(-inv(A),kron(inv(A),inv(A)))
# m = randn(3,3); m = 0.3*m'*m + 0.5*eye(3);
# dm = 0.01*randn(3,3); dm = dm + dm';
# R0,dR = regfun(m);
# for k=0:8
	# hhh = (0.5^k);
	# Rt, = regfun(m+ hhh*dm);
	# println("norm(Rt-R0): ",vecnorm(Rt-R0),", vecnorm(Rt - R0 - dR*dm): ",vecnorm(Rt[:] - R0[:] - dR*(hhh*dm[:])));
# end

# itrilA = idx_trilA();
# P = zeros(9,6);
# P[itrilA,1:6] = eye(6);
# P[4,2] = 1.0; P[7,3] = 1.0; P[8,5] = 1.0;
# regfun = (a)->(A = [a[1] a[2] a[3] ; a[2] a[4] a[5] ; a[3] a[5] a[6]]; invA = inv(A); return -invA[itrilA],(kron(invA,invA)*P)[itrilA,:]);
# m = randn(3,3); m = 0.3*m'*m + 0.5*eye(3);
# m = m[itrilA];
# dm = 0.01*randn(6);
# R0,dR = regfun(m);
# for k=0:8
	# hhh = (0.5^k);
	# Rt, = regfun(m+ hhh*dm);
	# println("norm(Rt-R0): ",vecnorm(Rt-R0),", vecnorm(Rt - R0 - dR*dm): ",vecnorm(Rt[:] - R0[:] - dR*(hhh*dm[:])));
# end


# This file includes baseband modulator

export Modulator, scheme, alphabet, modulate, constellation, basis, iswaveform

"""
    $TYPEDEF

Baseband digital modulator.

!!! note `tsample` determines the type of modulation. If `tsample` is `NaN`, the modulator transmits the vectors whose
    elements are the coefficients of the expansion of the symbol waveform over the orthogonal basis. Otherwise, the
    modulator trasmits the symbol waveform sampled by `tsample`.

# Fields

    $TYPEDFIELDS
"""
struct Modulator{T<:AbstractScheme, S<:AbstractPulse}
    "Modulation scheme"
    scheme::T
    "Modulation pulse energy"
    pulse::S
    "Sampling time."
    tsample::Float64
end
Modulator(scheme) = Modulator(scheme, RectangularPulse(), NaN)

show(io::IO, modulator::Modulator) = print(io,
    "Modulator(scheme:$(modulator.scheme), pulse:$(modulator.pulse))")

"""
    $SIGNATURES

Returns true if modulator a waveform modulator
"""
iswaveform(modulator::Modulator) = modulator.tsample !== NaN

# Modulation
(modulator::Modulator)(ms) = iswaveform(modulator) ? wavemodulate(modulator, ms) : vecmodulate(modulator, ms)

function vecmodulate(modulator::Modulator{T,S}, stream) where {T, S}
    # Note: In this case, `modulator` maps each element of `stream` to a vector whose elements(possibly complex) are the
    # coefficients of the expansion of symbol waveform over the basis of the modulator.
    scheme = modulator.scheme
    pulse = modulator.pulse
    Eg = energy(pulse)
    T <: FSK ? alphabet(scheme)[stream] * sqrt(2 * Eg) : alphabet(scheme)[stream] * sqrt(Eg)
end

function wavemodulate(modulator::Modulator{T, S}, symbols) where {T, S}
    # Note: In this case, `modulator` maps each element of `stream` to a waveform that is spanned by the basis of the
    # modulator.
    pulse = modulator.pulse
    ts = modulator.tsample
    tp = pulse.duration
    t = 0 : ts : tp - ts
    basisvals = map(base -> base.(t), basis(modulator)) # Precomputation of the values of the basis functions
    si = map(symbol -> sum(alphabet(modulator.scheme)[symbol] .* basisvals), symbols)
    Eg = energy(pulse)
    T <: FSK ? si * sqrt(2 * Eg)  : si * sqrt(Eg)
end

"""
    $SIGNATURES

Returns basis `modulator`
"""
function basis(modulator::Modulator{T, S}) where {T, S}
    iswaveform(modulator) || error("Expected waveform modulator, got a vector modulator")
    pulse = modulator.pulse
    Eg = energy(pulse)
    M = constelsize(modulator.scheme)
    if T <: FSK
        # Multidimensional signalling
        Δf = 1 / pulse.duration  # Minimum frequency sepeartion for orthogonality of the basis.
        map(m -> (t -> pulse(t) * exp(1im * 2π * m * Δf * t)), 1 : M)
    else
        # Two dimensional signalling
        [t -> pulse(t) / sqrt(Eg)]
    end
end

"""
    $SIGNATURES

Returns scheme of the modulation `modulator`.
"""
scheme(modulator::Modulator) = modulator.scheme

"""
    $SIGNATURES

Returns the constellation size of `modulator`
"""
constelsize(modulator::Modulator) = constelsize(modulator.scheme)


@with_kw struct PoleResidueDispersionModel
    inf
    activation_tensor
    poles
    residues
    wavelength_range
end

function PoleResidueDispersionModel(nktable::NKTable; activation_tensor = I(3), npoles = 5)
    @unpack wavelength, n, k = nktable
    s = im * wavelength
    Ns = length(s)
    f = reshape(epsilon(nktable), 1, Ns)
    poles = Array{ComplexF64}(-2 * pi * exp10.(range(0, stop = wavelength[end], length = npoles)))
    weight = ones(1 , Ns)
    ϵ_inf, a, c = vectfit3(f, s, poles, weight, asymp = 2, stable = true)
    PoleResidueDispersionModel(inf = ϵ_inf, poles = a, residues = c, activation_tensor = activation_tensor, wavelength_range = nktable.wavelength)
end
function epsilon(model::PoleResidueDispersionModel, wavelength)
    @unpack inf, activation_tensor, poles, residues, wavelength_range = model
    inf + sum([residues[i] / (im * wavelength - poles[i]) for i in 1:length(poles)])
end

function mu(model::PoleResidueDispersionModel, wavelength)
    @unpack inf, activation_tensor, poles, residues, wavelength_range = model
    inf + sum([residues[i] / (im * wavelength - poles[i]) for i in 1:length(poles)])
end

@recipe function plot(model::PoleResidueDispersionModel;
    xlabel = "Wavelength μm", color = [:blue :red], label = ["n" "k"], linewidth = [3, 3])
    xlabel --> xlabel
    color --> color
    n = map(λ -> n_from_epsilon(epsilon(model,λ)),model.wavelength_range)
    k = map(λ -> k_from_epsilon(epsilon(model,λ)),model.wavelength_range)
    model.wavelength_range , [n, k]
end


export sinkhorn, ot_dist, empirical_sinkhorn, dtw, emd, ot_plot1D, empirical_emd
# roadmap: implement all related functions from https://github.com/rflamary/POT/blob/master/ot/bregman.py

@doc raw"""
    sinkhorn(a::Union{PyObject, Array{Float64}}, b::Union{PyObject, Array{Float64}}, M::Union{PyObject, Array{Float64}};
    reg::Float64 = 1.0, iter::Int64 = 1000, tol::Float64 = 1e-9, method::String="sinkhorn")

Computes the optimal transport with Sinkhorn algorithm. The mathematical formulation is

```math
\begin{aligned}
\arg\min_P &\ \left(P, M\right) + \lambda \Omega(\Gamma)\\
\text{s.t.} &\ \Gamma 1 = a\\
&\ \Gamma^T 1 = b\\
& \Gamma \geq 0
\end{aligned}
```
Here $\Omega$ is the entropic regularization. Note if $\lambda$ is very small, the algorithm may encounter numerical instabilities.

The implementation are adapted from https://github.com/rflamary/POT.
"""
function sinkhorn(a::Union{PyObject, Array{Float64}}, b::Union{PyObject, Array{Float64}}, M::Union{PyObject, Array{Float64, 2}};
    reg::Union{PyObject,Float64} = 1.0, iter::Int64 = 1000, tol::Float64 = 1e-9, method::String="sinkhorn", returnall::Bool=false)
    if isa(a, Array)
        @assert sum(a)≈1.0
        @assert all(a .>= 0)
    end
    if isa(b, Array)
        @assert sum(b)≈1.0
        @assert all(b .>= 0)
    end
    a = convert_to_tensor(a)
    b = convert_to_tensor(b)
    M = convert_to_tensor(M)
    reg = convert_to_tensor(reg)
    iter = convert_to_tensor(iter)
    tol = convert_to_tensor(tol)
    if method=="lp"
        pth = install("OTNetwork")
        lp = load_op_and_grad(pth, "ot_network"; multiple=true)
        if returnall
            P, loss = lp(a, b, M, iter)
            P = set_shape(P, (length(a), length(b)))
            P, loss
        end
        return lp(a, b, M, iter)[2]
    else
        METHODIDS = Dict(
            "sinkhorn"=>0,
            "greenkhorn"=>1
        )

        if !haskey(METHODIDS, method)
            error("$method not implemented")
        end
        METHODID = METHODIDS[method]

        sk = load_system_op("sinkhorn_knopp"; multiple=true)
        if returnall
            return sk(a,b,M,reg,iter,tol, constant(METHODID))
        end
        return sk(a,b,M,reg,iter,tol, constant(METHODID))[2]
    end
end

@doc raw"""
    emd(a::Union{PyObject, Array{Float64}}, b::Union{PyObject, Array{Float64}}, M::Union{PyObject, Array{Float64}};
    iter::Int64 = 1000, tol::Float64 = 1e-9, returnall::Bool=false)

Computes the Earth Mover's Distance, which is defined as

$$D(M) = \sum_{i=1}^m \sum_{j=1}^n M_{ij} d_{ij}$$

Here $M \in \mathbb{R}^{m\times n}$ is the ground distance matrix. The algorithm solves the following optimization problem

$$\begin{aligned}\min_{M} &\ D(M)\\\text{s.t.} & \ \sum_{i=1}^m M_{ij} = b_j\\ &\ \sum_{j=1}^n M_{ij} = a_i \end{aligned}$$

The internal solver for the optimization problem is a netflow solver. The algorithm requires $\sum_i a_i = \sum_j b_j = 1$.
"""
function emd(a::Union{PyObject, Array{Float64}}, b::Union{PyObject, Array{Float64}}, M::Union{PyObject, Array{Float64}};
    iter::Int64 = 1000, tol::Float64 = 1e-9, returnall::Bool=false)
    return sinkhorn(a, b, M, reg = 0.0, iter = iter, tol = tol, method = "lp",
            returnall = returnall)
end

"""
    ot_plot1D(a::Array{Float64, 1}, b::Array{Float64, 1}, M::Array{Float64, 2})

Plots the optimal transport matrix for 1D distributions.
"""
function ot_plot1D(a::Array{Float64, 1}, b::Array{Float64, 1}, M::Array{Float64, 2})
    @assert length(a) == size(M, 1)
    @assert length(b) == size(M, 2)
    pl = require_import(:PyPlot)
    pl.figure(4, figsize = (5, 5))

    xa = 0:size(M, 1)-1
    xb = 0:size(M, 2)-1

    ax1 = pl.subplot(222)
    pl.plot(xb, b, "r")
    pl.yticks(())
    pl.title("Target")

    ax2 = pl.subplot(223)
    pl.plot(a, xa, "b")
    pl.gca().invert_xaxis()
    pl.gca().invert_yaxis()
    pl.xticks(())
    pl.title("Source")

    pl.subplot(224, sharex=ax1, sharey=ax2)
    pl.imshow(M, interpolation="nearest")
    pl.axis("off")

    pl.xlim((0, size(M, 2)-1))
    pl.tight_layout()
    pl.subplots_adjust(wspace=0., hspace=0.2)

end

@doc raw"""
    empirical_sinkhorn(x::Union{PyObject, Array{Float64}}, y::Union{PyObject, Array{Float64}};
        reg::Union{PyObject,Float64} = 1.0, iter::Int64 = 1000, tol::Float64 = 1e-9, method::String="sinkhorn", dist::Function=dist, returnall::Bool=false)

Computes the empirical Sinkhorn distance with sinkhorn algorithm. Here $x$ and $y$ are samples from two distributions.

- `reg` (default = 1.0): entropy regularization parameter
- `tol` (default = 1e-9), `iter` (default = 1000): tolerance and max iterations for the Sinkhorn algorithm
- `dist` (default = 2): Integer or Function, the distance function between two samples; if `dist` is integer, $L-dist$ norm is used.
- `returnall`: returns (`TransportMatrix`, `Loss`) if true; otherwise, only `Loss` is returned.

The implementation are adapted from https://github.com/rflamary/POT.
"""
function empirical_sinkhorn(x::Union{PyObject, Array{Float64}}, y::Union{PyObject, Array{Float64}};
    reg::Union{PyObject,Float64} = 1.0, iter::Int64 = 1000, tol::Float64 = 1e-9,
    method::String="sinkhorn", dist::Union{Integer,Function}=2, returnall::Bool=false, normalized::Bool = false)
    x, y = convert_to_tensor([x,y], [Float64, Float64])
    length(size(x))==1 && (x = reshape(x, :, 1))
    length(size(y))==1 && (y = reshape(y, :, 1))
    if isa(dist, Integer)
        d = copy(dist)
        dist = (x,y) -> ot_dist(x, y, d)
    end
    M = dist(x, y)
    if normalized
        M = M/maximum(M)
    end
    a = tf.ones(tf.shape(x)[1], dtype=tf.float64)/cast(Float64, tf.shape(x)[1])
    b = tf.ones(tf.shape(y)[1], dtype=tf.float64)/cast(Float64, tf.shape(y)[1])
    sinkhorn(a, b, M; reg=reg, iter=iter, tol=tol, method=method, returnall=returnall)
end

"""
    empirical_emd(x::Union{PyObject, Array{Float64}}, y::Union{PyObject, Array{Float64}};
        iter::Int64 = 1000, tol::Float64 = 1e-9, dist::Union{Integer,Function}=2, returnall::Bool=false)

Same as [`empirical_sinkhorn`](@ref), except that the Earth Mover Distance is computed.
"""
function empirical_emd(x::Union{PyObject, Array{Float64}}, y::Union{PyObject, Array{Float64}};
     iter::Int64 = 1000, tol::Float64 = 1e-9, dist::Union{Integer,Function}=2, returnall::Bool=false, normalized::Bool = false)
     empirical_sinkhorn(x, y; iter = iter, tol = tol, method = "lp",  returnall = returnall)
end

"""
    ot_dist(x::Union{PyObject, Array{Float64}}, y::Union{PyObject, Array{Float64}}, order::Union{Int64, PyObject}=2)

Computes the distance function with norm `order`. `dist` returns a ``n\\times m`` matrix, where ``x\\in \\mathbb{R}^{n\\times d}`` and
``y\\in \\mathbb{R}^{m\\times d}``, and the return ``M\\in \\mathbb{R}^{n\\times m}``
```math
M_{ij} = ||x_i - y_j||_{o}
```
"""
function ot_dist(x::Union{PyObject, Array{Float64}}, y::Union{PyObject, Array{Float64}}, order::Union{Int64, PyObject}=2)
    if !(isa(x, PyObject) || isa(y, PyObject) || isa(order, PyObject))
        x = reshape(x, size(x, 1), :)
        y = reshape(y, size(y, 1), :)
        M = zeros(size(x, 1), size(y, 1))
        for i = 1:size(x, 1)
            for j = 1:size(y, 1)
                M[i, j] = norm(x[i,:]-y[j,:], order)
            end
        end
        return M
    end
    x = convert_to_tensor(x)
    y = convert_to_tensor(y)
    order = convert_to_tensor(order, dtype=Float64)
    x = tf.expand_dims(x, axis=1)
    y = tf.expand_dims(y, axis=0)
    sum(abs(x-y)^order, dims=3)^(1.0/order)
end

"""
    dtw(s::Union{PyObject, Array{Float64}}, t::Union{PyObject, Array{Float64}},
        use_fast::Bool = false)

Computes the dynamic time wrapping (DTW) distance between two time series `s` and `t`.
Returns the distance and path. `use_fast` specifies whether fast algorithm is used. Note
fast algorithm may not be accurate.
"""
function dtw(s::Union{PyObject, Array{Float64}}, t::Union{PyObject, Array{Float64}},
            use_fast::Bool = false)
    use_fast = Int32(use_fast)
    pth = install("FastDTW")
    dtw_ = load_op_and_grad(pth, "dtw"; multiple=true)
    s,t, use_fast = convert_to_tensor([s,t,use_fast], [Float64,Float64, Int32])
    cost, path = dtw_(s,t,use_fast)
    return cost, path + 1
end


using ModelTES
using Test

Tc=0.107; Tbath = 0.065
R0 = 1.55e-3; Rl = 0.35e-3; Rn = 10.3e-3; Rpara=0.0
n=3.3; k = 3.50e-9; C = 0.72e-12
alpha = 92.0;β = 1.4; L=300e-9
model1 = ModelTES.ShankRIT(alpha, β, n, Tc, Tbath, k, R0, Rn);
tes_param = TESParams(n,Tc,Tbath,k,C,L,Rl,Rpara,Rn,model1)
bt = BiasedTES(tes_param, R0)
ihtes = IrwinHiltonTES(bt)

@test Tbath == ihtes.Tbath
@test isapprox(R0, ihtes.R0; rtol=1e-5)
@test Rl == ihtes.Rl
@test C == ihtes.C0
@test isapprox(alpha, ihtes.alpha; rtol=1e-5)
@test isapprox(β, ihtes.beta; rtol=1e-5)
@test L == ihtes.L

f = 10.0 .^ range(0, stop=6, length=100);
n,n1,n2,n3,n4 = ModelTES.noisePSD(ihtes, f);
z = ModelTES.Z(ihtes,f)
zcircuit = ModelTES.Zcircuit(ihtes,f)

zr, zi = real.(z), imag.(z)

# Check issue #4: is the normalization correct on PSD and autocorrelation?
# Estimate the total power by a weird log-space trapezoid rule.
amplifier_noise=1e-35
frequencies = collect(10 .^ range(0, stop=6, length=1501));
psdRef = ModelTES.noisePSD(bt, frequencies, amplifier_noise)[1];
totalpower = psdRef[1]*frequencies[1];
for i=2:length(frequencies)
    global totalpower += (frequencies[i]-frequencies[i-1])*0.5*(psdRef[i]+psdRef[i-1]);
end

sampleTimeA = 1e-8
modelRef = NoiseModel(bt, sampleTimeA, amplifier_noise)
for sampleTimeB in 10 .^ range(-7.5, stop=-6, length=4)
    model = NoiseModel(bt, sampleTimeB, amplifier_noise)
    @test isapprox(model.covarIV[1], modelRef.covarIV[1], rtol=0.01)
    @test isapprox(model.covarIV[1], totalpower, rtol=0.2)
end


using EconPDEs, Distributions

struct AchdouHanLasryLionsMollModel
    # income process parameters
    κy::Float64
    ybar::Float64
    σy::Float64

    r::Float64

    # utility parameters
    ρ::Float64
    γ::Float64

    amin::Float64
    amax::Float64
end

function AchdouHanLasryLionsMollModel(;κy = 0.1, ybar = 1.0, σy = 0.07, r = 0.03, ρ = 0.05, γ = 2.0, amin = 0.0, amax = 500.0)
    AchdouHanLasryLionsMollModel(κy, ybar, σy, r, ρ, γ, amin, amax)
end

function initialize_stategrid(m::AchdouHanLasryLionsMollModel; yn = 5, an = 50)
    κy = m.κy ; ybar = m.ybar ; σy = m.σy  ; ρ = m.ρ ; γ = m.γ ; amin = m.amin ; amax = m.amax

    distribution = Gamma(2 * κy * ybar / σy^2, σy^2 / (2 * κy))
    ymin = quantile(distribution, 0.001)
    ymax = quantile(distribution, 0.999)
    ys = range(ymin, stop = ymax, length = yn)
    as = range(amin, stop = amax, length = an)
    OrderedDict(:y => ys, :a => as)
end

function initialize_y(m::AchdouHanLasryLionsMollModel, stategrid)
    OrderedDict(:v => [log(y + a) for y in stategrid[:y], a in stategrid[:a]])
end

function (m::AchdouHanLasryLionsMollModel)(state::NamedTuple, value::NamedTuple)
    κy = m.κy ; σy = m.σy ; ybar = m.ybar ; r = m.r ; ρ = m.ρ ; γ = m.γ ; amin = m.amin ; amax = m.amax
    y, a = state.y, state.a
    v, vy, va, vyy, vya, vaa = value.v, value.vy, value.va, value.vyy, value.vya, value.vaa
    μy = κy * (ybar - y)
    va = max(va, eps())

    # There is no second derivative at 0 so just specify first order derivative
    c = va^(-1 / γ)
    μa = y + r * a - c
    if (a ≈ amin) && (μa <= 0.0)
        va = (y + r * amin)^(-γ)
        c = y + r * amin
        μa = 0.0
    end

    vt = c^(1 - γ) / (1 - γ) + μa * va + μy * vy + 0.5 * vyy * σy^2 - ρ * v
    return (vt,), (μy, μa), (v = v, c = c, va = va, vy = vy, y = y, a = a, μa = μa, vaa = vaa)
end

m = AchdouHanLasryLionsMollModel()
stategrid = initialize_stategrid(m)
y0 = initialize_y(m, stategrid)
y, result, distance = pdesolve(m, stategrid, y0)

# Check marginal value of wealth converges to 1.0 at infinity
#b = ((m.r + (m.ρ - m.r)/m.γ))^(1/(1 - 1/m.γ))
#pw = (result[:v] * (1-m.γ)).^(1/(1-m.γ)-1) .* result[:va] ./ b


# This file is part of IntegerSequences.
# Copyright Peter Luschny. License is MIT.

(@__DIR__) ∉ LOAD_PATH && push!(LOAD_PATH, (@__DIR__))

module BooleanOperations
using NumberTheory

export Not, And, Dif, Cnimp, Xor, Or, Nor, Eqv, Cimp, Imp, Nand
export Bits, BoolOps, BinDigits
export V035327, V003817, V129760, V142151, V080079, V086799, V163617
export V038712, V006257, V048724, V003188, V038554, V048735, V213370
export V080940, V135521, V051933, V135481, V327987
export is327988, L327988, L280172

"""

The 16 boolean operations

* FALSE, AND, DIF, PRJ1, CNIMP, PRJ2, XOR, OR, NOR, EQV, NEG2, CIMP, NEG1, IMP, NAND, TRUE

evaluated bitwise on the binary expansions of integers.

The main function is

* Bits(op::String, n::Int, k::Int, algo=max)

where op are the above acronyms. If the lengths of the binary expansions of ``n`` and ``k`` are different, the parameter 'algo=min' offers the choice to reduce the range of comparison to the smaller range or to the larger range, 'algo=max', which will first pad the representation of the smaller operand with 0's up to the length of the larger operand.

* Not, And, Dif, Cnimp, Xor, Or, Nor, Eqv, Cimp, Imp, Nand, BinDigits, BoolOps, Bits, V035327, V003817, V129760, V142151, V080079, V086799, V038712, V163617, V006257, V048724, V003188, V038554, V048735, V213370, V080940, V135521,  , V280172.
"""
const ModuleBooleanOperations = ""

"""
Return the binary expansions of nonnegative ``n`` and ``k``. If algo=max then pad the resulting int arrays with 0 to make them equally long. For example

    BinDigits``(12, 17, max) = ([0, 0, 1, 1, 0], [1, 0, 0, 0, 1])``.

If algo=min then the arrays are truncated to the length of the smaller operand.

    BinDigits``(12, 17, min) = ([0, 0, 1, 1], [1, 0, 0, 0])``.

These are the lists which are itemwise compared by the logical operators.
"""
function BinDigits(n::Int, k::Int, algo=max)
    N = n == 0 ? [0] : digits(n, base = 2)
    K = k == 0 ? [0] : digits(k, base = 2)
    lenN = length(N)
    lenK = length(K)

    if algo == max
        maxlen = max(lenN, lenK)
        if maxlen > lenN
            N = vcat(N, [0 for i ∈ 1:maxlen-lenN])
        else
            K = vcat(K, [0 for i ∈ 1:maxlen-lenK])
        end
    else
        minlen = min(lenN, lenK)
        if lenN > minlen
            N = N[1:minlen]
        else
            K = K[1:minlen]
        end
    end
    return (N, K)
end

"""

Return the bitwise negation of ``N``.
"""
Not(N) = [1 - n for n ∈ N]

"""

Return the bitwise conjunction of ``N`` and ``K``.
"""
And(N, K) = [N[i] & k for (i, k) ∈ enumerate(K)]

"""

Return the bitwise difference of ``N`` and ``K``.
"""
Dif(N, K) = [N[i] & (1 - k) for (i, k) ∈ enumerate(K)]

"""

Return the bitwise converse nonimplication of ``N`` and ``K``.
"""
Cnimp(N, K) = [(1 - N[i]) & k for (i, k) ∈ enumerate(K)]

"""

Return the bitwise exclusive disjunction of ``N`` and ``K``.
"""
Xor(N, K) = [xor(N[i], k) for (i, k) ∈ enumerate(K)]

"""

Return the bitwise disjunction of ``N`` and ``K``.
"""
Or(N, K) = [N[i] | k for (i, k) ∈ enumerate(K)]

"""

Return the bitwise joint denial of ``N`` and ``K``.
"""
Nor(N, K) = [1 - (N[i] | k) for (i, k) ∈ enumerate(K)]

"""

Return the bitwise equivalence of ``N`` and ``K``.
"""
Eqv(N, K) = [xor(1 - N[i], k) for (i, k) ∈ enumerate(K)]

"""

Return the bitwise converse implication of ``N`` and ``K``.
"""
Cimp(N, K) = [N[i] | (1 - k) for (i, k) ∈ enumerate(K)]

"""

Return the bitwise implication of ``N`` and ``K``.
"""
Imp(N, K) = [(1 - N[i]) | k for (i, k) ∈ enumerate(K)]

"""

Return the bitwise nonconjunction of ``N`` and ``K``.
"""
Nand(N, K) = [1 - (N[i] & k) for (i, k) ∈ enumerate(K)]

"""

Return the bitwise boolean operation represented by ``op`` applied to the binary expansions of the integers ``n`` and ``k``. ``op`` is an integer ``0 <= op < 16`` encoding the result of the operation in terms of the truth table.
"""
function Bits(op::Int, n::Int, k::Int, algo=max)
    if op < 0 || op > 15 || n < 0 || k < 0
        @error("op must be 0 <= op <= 15 and n,k >= 0. op=$op, n=$n, k=$k.")
        throw(DomainError(-1))
    end

    op == 0  && return 0
    op == 15 && return 1

    N, K = BinDigits(n, k, algo)

    if     op == 1   R = And(N, K)
    elseif op == 2   R = Dif(N, K)
    elseif op == 3   R = N
    elseif op == 4   R = Cnimp(N, K)
    elseif op == 5   R = K
    elseif op == 6   R = Xor(N, K)
    elseif op == 7   R = Or(N, K)
    elseif op == 8   R = Nor(N, K)
    elseif op == 9   R = Eqv(N, K)
    elseif op == 10  R = Not(K)
    elseif op == 11  R = Cimp(N, K)
    elseif op == 12  R = Not(N)
    elseif op == 13  R = Imp(N, K)
    elseif op == 14  R = Nand(N, K)
    end

    sum(r << (i-1)  for (i, r) ∈ enumerate(R))
end

"""

The list of acronyms for the 16 binary boolean operations.
"""
const BoolOps = ["FALSE", "AND", "DIF", "PRJ1", "CNIMP", "PRJ2", "XOR", "OR", "NOR", "EQV", "NEG2", "CIMP", "NEG1", "IMP", "NAND", "TRUE"]

"""

Return the bitwise boolean operation represented by ``op`` applied to the binary expansions of the integers ``n`` and ``k``. ``op`` is an acronym as given in the list 'BoolOps'.
"""
function Bits(op::String, n::Int, k::Int, algo=max)
    opnum = findfirst(isequal(op), BoolOps)
    Bits(opnum - 1, n, k, algo)
end

"""

Return ``n`` NAND ``n``.
"""
V035327(n) = Bits("NAND", n, n)
# also : V035327(n) = Bits("NEG1", n, n+1, min)

"""

Return ``n`` EQV ``n``.
"""
V003817(n) = Bits("EQV", n, n)
# sets a(0) = 1

"""

Return ``n`` AND ``n+1``, using max length.
"""
V129760(n) = Bits("AND", n, n+1)
# with offset 0

"""

Return ``n`` CIMP ``n+1``, using max length.
"""
V142151(n) = Bits("CIMP", n, n+1)

#"""
# Return n CNIMP n+1, using max length.
# Offset is 0, shift n left if you want the OEIS offset 1.
# Is exported from Module LandauConstants
#"""
#V006519(n) = Bits("CNIMP", n, n+1)

"""

Return ``n`` NEG1 ``n+1``, using max length.
"""
V080079(n) = Bits("NEG1", n, n+1)
# with offset 0

"""

Return ``n`` OR ``n+1``, using max length.
"""
V086799(n) = Bits("OR", n, n+1)
# with offset 0

"""

Return ``n`` OR ``2n``, using max length.
"""
V163617(n) = Bits("OR", n, n<<1)

"""

Return ``n`` XOR ``n+1``, using max length.
"""
V038712(n) = Bits("XOR", n, n+1)
# with offset 0

"""

Return max``(1, 2n) - (n`` EQV ``n)``, using max length.
"""
V006257(n) = max(1, 2n) - Bits("EQV", n, n)

"""

Return ``n`` XOR ``2n``, using max length.
"""
V048724(n) = Bits("XOR", n, n<<1)

"""

Return ``n`` XOR ``n>>1``, using max length.
"""
V003188(n) = Bits("XOR", n, n>>1)

"""

Return ``n`` XOR ``n>>1``, using min length.
"""
V038554(n) = Bits("XOR", n, n>>1, min)
# with a(1) = 1

"""

Return ``n`` AND ``n>>1``, using min length.
"""
V048735(n) = Bits("AND", n, n>>1, min)

"""

Return ``n`` AND ``n<<1``, using min length.
"""
V213370(n) = Bits("AND", n, n<<1, min)

"""

Return ``n`` CNIMP ``n+1``, using min length.
"""
V080940(n) = Bits("CNIMP", n, n+1, min)
# with a(0) = 1

"""

Return ``n`` XOR ``n+1``, using min length.
"""
V135521(n) = Bits("XOR", n, n+1, min)
# prepend a(0) = 1

"""

Return ``n`` XOR ``k``, using max length.
"""
V051933(n, k) = Bits("XOR", n, k)

"""

Return ``(k-1`` XOR ``n-k) + 1``, using max length.
"""
L280172(n) = [Bits("XOR", k-1, n-k) + 1 for k in 1:n]

"""

Return ``n+1`` CNIMP ``n``, using max length.
"""
V135481(n) = Bits("CNIMP", n+1, n)

"""

Return ∑``_{d|n} d & (n/d)``, where & is the bitwise AND operator.
"""
V327987(n) = sum([Bits("AND", d, div(n, d)) for d ∈ divisors(n)])

"""

Is V327987(n) = ∑``_{d|n} d & (n/d) = 0`` ?
"""
is327988(n) = V327987(n) == 0

"""

Return a list of the zeros of V327987 below (inc.)  ``max``.
"""
L327988(max) = [n for n ∈ 0:max if is327988(n)]

#START-TEST-########################################################

using Test

function test()
    @testset "BoolOps" begin
        A = [1, 1, 0, 0]
        B = [1, 0, 1, 0]
        S(R) = sum(r << (i-1)  for (i, r) ∈ enumerate(R))

        for n ∈ 0:15
            @test n == S([Bits(n, A[k], B[k]) for k ∈ 1:4])
        end
        for n ∈ 0:15
            @test n == S([Bits(BoolOps[n+1], A[k], B[k]) for k ∈ 1:4])
        end

        @test BinDigits(12, 17) == ([0, 0, 1, 1, 0], [1, 0, 0, 0, 1])
        @test [V163617(n) for n ∈ 0:8] == [0, 3, 6, 7, 12, 15, 14, 15, 24]
        @test [V035327(n) for n ∈ 0:8] == [1, 0, 1, 0, 3, 2, 1, 0, 7]
        @test [V129760(n) for n ∈ 0:8] == [0, 0, 2, 0, 4, 4, 6, 0, 8]
        @test [V142151(n) for n ∈ 0:8] == [0, 1, 2, 3, 6, 5, 6, 7, 14]
        @test [V003817(n) for n ∈ 0:8] == [1, 1, 3, 3, 7, 7, 7, 7, 15]
        @test [V080079(n) for n ∈ 0:8] == [1, 2, 1, 4, 3, 2, 1, 8, 7]
        @test [V086799(n) for n ∈ 0:8] == [1, 3, 3, 7, 5, 7, 7, 15, 9]
        @test [V038712(n) for n ∈ 0:8] == [1, 3, 1, 7, 1, 3, 1, 15, 1]
        @test [V006257(n) for n ∈ 0:8] == [0, 1, 1, 3, 1, 3, 5, 7, 1]

        @test [V080940(n) for n ∈ 0:8] == [1, 0, 1, 0, 1, 2, 1, 0, 1]
        @test [V135521(n) for n ∈ 0:8] == [1, 1, 1, 3, 1, 3, 1, 7, 1]

        @test [V048724(n) for n ∈ 0:8] == [0, 3, 6, 5, 12, 15, 10, 9, 24]
        @test [V003188(n) for n ∈ 0:8] == [0, 1, 3, 2, 6, 7, 5, 4, 12]
        @test [V038554(n) for n ∈ 0:8] == [0, 1, 1, 0, 2, 3, 1, 0, 4]
        @test [V048735(n) for n ∈ 0:8] == [0, 0, 0, 1, 0, 0, 2, 3, 0]
        @test [V213370(n) for n ∈ 0:8] == [0, 0, 0, 2, 0, 0, 4, 6, 0]

        @test [V135481(n) for n ∈ 0:8] == [0, 1, 0, 3, 0, 1, 0, 7, 0]
        @test [V327987(n) for n ∈ 0:8] == [0, 1, 0, 2, 2, 2, 4, 2, 0]
        @test L327988(99) == [0, 2, 8, 10, 26, 32, 34, 40, 50, 58, 74, 82]

        @test is327988(121) == false
        @test is327988(122) == true
    end
end

function demo()
    println()
    for n ∈ 0:15
        seq = [Bits(n, k, k+1) for k ∈ 0:12]
        println(BoolOps[n+1], " ", seq)
    end

    println()
    for n ∈ 1:8
        L280172(n) |> println
    end
end

function perf()
end

function main()
    test()
    demo()
    perf()
end

main()

end # module

#=
A327489, A327488, A327490, A001511,
A062383, A265705, A261693, A265716
=#


check(bound::Vector) = cell -> all([1, 1] .≤ cell .≤ bound)
neighbors(cell::Vector, bound::Vector, step::Int=2) =
    filter(check(bound), map(dir -> cell + step * dir, [[0, 1], [-1, 0], [0, -1], [1, 0]]))

function walk(maze::Matrix, nxtcell::Vector, visited::Vector=[])
    push!(visited, nxtcell)
    for neigh in shuffle(neighbors(nxtcell, size(maze)))
        if neigh ∉ visited
            maze[round.(Int, (nxtcell + neigh) / 2)...] = 0
            walk(maze, neigh, visited)
        end
    end
    maze
end
function maze(w::Int, h::Int)
    maze = collect(i % 2 | j % 2 for i in 1:2w+1, j in 1:2h+1)
    firstcell = 2 * [rand(1:w), rand(1:h)]
    return walk(maze, firstcell)
end


function create_grids()

    function xgrid(theta,xval)
        N      = length(xval)
        xub    = vcat(xval[2:N],Inf)
        xtran1  = zeros(N,N)
        xtran1c = zeros(N,N)
        lcdf   = zeros(N)
        for i=1:length(xval)
            xtran1[:,i]   = (xub[i] .>= xval).*(1 .- exp.(-theta*(xub[i] .- xval)) .- lcdf)
            lcdf        .+= xtran1[:,i]
            xtran1c[:,i] .+= lcdf
        end
        return xtran1, xtran1c
    end

    zval   = collect([.25:.01:1.25]...)
    zbin   = length(zval)
    xval   = collect([0:.125:25]...)
    xbin   = length(xval)
    tbin   = xbin*zbin
    xtran  = zeros(tbin,xbin)
    xtranc = zeros(xbin,xbin,xbin)
    for z=1:zbin
        xtran[(z-1)*xbin+1:z*xbin,:],xtranc[:,:,z] = xgrid(zval[z],xval)
    end

    return zval,zbin,xval,xbin,xtran
end


function b2c!(wc::AbstractVector, c::AbstractVector, α,
              prev=Array(eltype(wc),length(wc)))
    T = eltype(wc)

    fill!(wc, zero(T))
    desired_order = length(wc) - 1

    m1 = length(c)-1
    for i=-m1:0
        copy!(prev, wc)
        wc[1] = c[-i+1]

        if desired_order >= 1
            wc[2] = (1.0-α*α)*prev[1] + α*prev[2]
        end
        for m=3:desired_order+1
            @inbounds wc[m] = prev[m-1] + α*(prev[m] - wc[m-1])
        end
    end

    wc
end

b2c(c::AbstractVector, order, α) = b2c!(Array(eltype(c), order+1), c, α)


module TestMINRES

using IterativeSolvers
using Test
using Random
using SparseArrays
using LinearAlgebra
using LinearMaps

@testset "MINRES" begin

function hermitian_problem(T, n)
    B = rand(T, n, n) + n * I
    A = B + B'
    x = ones(T, n)
    b = B * x
    A, x, b
end

function skew_hermitian_problem(T, n)
    B = rand(T, n, n) + n * I
    A = B - B'
    x = ones(T, n)
    b = A * x
    A, x, b
end

Random.seed!(123)
n = 15

@testset "Hermitian Matrix{$T}" for T in (Float32, Float64, ComplexF32, ComplexF64)
    A, x, b = hermitian_problem(T, n)
    reltol = sqrt(eps(real(T)))
    x0 = rand(T, n)

    x1, hist1 = minres(A, b, maxiter = 10n, reltol = reltol, log = true)
    x2, hist2 = minres!(x0, A, b, maxiter = 10n, reltol = reltol, log = true)

    @test isa(hist1, ConvergenceHistory)
    @test norm(b - A * x1) / norm(b) ≤ reltol
    @test hist1.isconverged
    @test norm(b - A * x2) / norm(b) ≤ reltol
    @test x2 == x0
end

@testset "Skew-Hermitian Matrix{$T}" for T in (Float32, Float64, ComplexF32, ComplexF64)
    A, x, b = skew_hermitian_problem(T, n)
    reltol = sqrt(eps(real(T)))
    x_approx, hist = minres(A, b, skew_hermitian = true, maxiter = 10n, reltol = reltol, log = true)

    @test norm(b - A * x_approx) / norm(b) ≤ reltol
    @test hist.isconverged
end

@testset "SparseMatrixCSC{$T, $Ti}" for T in (Float32, Float64, ComplexF32, ComplexF64), Ti in (Int64, Int32)
    A = let
        B = sprand(n, n, 2 / n)
        B + B' + I
    end

    x = ones(T, n)
    b = A * x
    reltol = sqrt(eps(real(T)))

    x_approx, hist = minres(A, b, maxiter = 10n, reltol = reltol, log = true)

    @test norm(b - A * x_approx) / norm(b) ≤ reltol
    @test hist.isconverged
end

@testset "Termination criterion" begin
    for T in (Float32, Float64, ComplexF32, ComplexF64)
        A = T[ 2 -1  0
              -1  2 -1
               0 -1  2]
        n = size(A, 2)
        b = ones(T, n)
        x0 = A \ b
        perturbation = 10 * sqrt(eps(real(T))) * T[(-1)^i for i in 1:n]

        # If the initial residual is small and a small relative tolerance is used,
        # many iterations are necessary
        x = x0 + perturbation
        initial_residual = norm(A * x - b)
        x, ch = minres!(x, A, b, log=true)
        @test 2 ≤ niters(ch) ≤ n

        # If the initial residual is small and a large absolute tolerance is used,
        # no iterations are necessary
        x = x0 + perturbation
        initial_residual = norm(A * x - b)
        x, ch = minres!(x, A, b, abstol=2*initial_residual, reltol=zero(real(T)), log=true)
        @test niters(ch) == 0
    end
end

end

end # module


# Author: Lukasz Bratos
include("methods.jl")

# Różnica naszych funkcji
f(x) = exp(x) - 3 * x
g(x) = 3 * x - exp(x)
delta = 10^-4
epsilon = 10^-4

println("Przedział [0.0, 1.0: ", Methods.mbisekcji(f, 0.0, 1.0, delta, epsilon))
println("Przedział [1.0, 2.0: ", Methods.mbisekcji(f, 1.0, 2.0, delta, epsilon))

println("Przedział [0.0, 1.0: ", Methods.mbisekcji(g, 0.0, 1.0, delta, epsilon))
println("Przedział [1.0, 2.0: ", Methods.mbisekcji(g, 1.0, 2.0, delta, epsilon))