@doc raw"""
    K, γ, info = glover_mcfarlane(G::AbstractStateSpace, γ = 1.1; W1=1, W2=1)

Design a controller for `G` that maximizes the stability margin ϵ = 1/γ with normalized coprime factor uncertainty using the method of Glover and McFarlane
```
γ = 1/ϵ = ||[K;I] inv(I-G*K)*inv(M)||∞
G = inv(M + ΔM)*(N + ΔN)
```
γ is given as a relative factor above γmin and must be greater than 1, i.e., if γ = 1.1, the controller will be designed for γ = 1.1*γmin.

We want γmin (which is always ≥ 1) as small as possible, and we usually require that γmin is less than 4, corresponding to 25% allowed coprime uncertainty.

Performance modeling is incorporated in the design by calling `glover_mcfarlane` on the shaped system `Gs = W2*G*W1` and then forming the controller as `K = W1*Ks*W2`. Using this formulation, traditional loop shaping can be done on `Gs = W2*G*W1`. The plant shaping is handled internally if keyword arguments `W1, W2` are used and the returned controller is already scaled. In this case, `Gs` and `Ks` are included in the `info` named tuple for inspection.

See also [`glover_mcfarlane_2dof`](@ref) to design a feedforward filter as well.

# Example:
Example 9.3 from the reference below.
```julia
using RobustAndOptimalControl, ControlSystems, Plots, Test
G = tf(200, [10, 1])*tf(1, [0.05, 1])^2     |> ss
Gd = tf(100, [10, 1])                       |> ss
W1 = tf([1, 2], [1, 1e-6])                  |> ss
K, γ, info = glover_mcfarlane(G, 1.1; W1)
@test info.γmin ≈ 2.34 atol=0.005
Gcl = extended_gangoffour(G, K) # Form closed-loop system

bodeplot([G, info.Gs, G*K], lab=["G" "" "G scaled" "" "Loop transfer"]) |> display
bodeplot(Gcl, lab=["S" "KS" "PS" "T"], plotphase=false) |> display # Plot gang of four

plot( step(Gd*feedback(1, info.Gs), 3), lab="Initial controller")
plot!(step(Gd*feedback(1, G*K), 3), lab="Robustified") |> display

nyquistplot([info.Gs, G*K], ylims=(-2,1), xlims=(-2, 1),
    Ms_circles = 1.5,
    lab = ["Initial controller" "Robustified"],
    title = "Loop transfers with and without robustified controller"
    ) |> display
```

Ref: Sec 9.4.1 of Skogestad, "Multivariable Feedback Control: Analysis and Design"

# Extended help

Skogestad gives the following general advice:
1. Scale the plant outputs and inputs. This is very important for most design
    procedures and is sometimes forgotten. In general, scaling improves the
    conditioning of the design problem, it enables meaningful analysis to be made
    of the robustness properties of the feedback system in the frequency domain,
    and for loop-shaping it can simplify the selection of weights. There are a variety
    of methods available including normalization with respect to the magnitude of
    the maximum or average value of the signal in question. If one is to go straight to a design the following variation has
    proved useful in practice:
    (a) The outputs are scaled such that equal magnitudes of cross-coupling into each
        of the outputs is equally undesirable.
    (b) Each input is scaled by a given percentage (say 10%) of its expected range
        of operation. That is, the inputs are scaled to reflect the relative actuator
        capabilities. An example of this type of scaling is given in the aero-engine
        case study of Chapter 12.
2. Order the inputs and outputs so that the plant is as diagonal as possible. The
    relative gain array [`rga`](@ref) can be useful here. The purpose of this pseudo-diagonalization
    is to ease the design of the pre- and post-compensators which, for simplicity, will
    be chosen to be diagonal.

    Next, we discuss the selection of weights to obtain the shaped plant $G_s = W_2 G W_1$
    where $W_1 = W_p W_a W_g$
3. Select the elements of diagonal pre- and post-compensators $W_p$ and $W_2$ so that
    the singular values of $W_2 G W_p$ are desirable. This would normally mean high
    gain at low frequencies, roll-off rates of approximately 20 dB/decade (a slope of
    about 1) at the desired bandwidth(s), with higher rates at high frequencies. Some
    trial and error is involved here. $W_2$ is usually chosen as a constant, reflecting the
    relative importance of the outputs to be controlled and the other measurements
    being fed back to the controller. For example, if there are feedback measurements
    of two outputs to be controlled and a velocity signal, then $W_2$ might be chosen
    to be `diag([1, 1, 0.1])`, where 0.1 is in the velocity signal channel. $W_p$ contains the
    dynamic shaping. Integral action, for low frequency performance; phase-advance
    for reducing the roll-off rates at crossover, and phase-lag to increase the roll-off
    rates at high frequencies should all be placed in $W_p$ if desired. The weights should
    be chosen so that no unstable hidden modes are created in $G_s$.
5. Optional: Introduce an additional gain matrix $W_g$ cascaded with $W_a$ to provide
    control over actuator usage. $W_g$ is diagonal and is adjusted so that actuator rate
    limits are not exceeded for reference demands and typical disturbances on the
    scaled plant outputs. This requires some trial and error.

6. Robustly stabilize the shaped plant $G_s = W_2 G W_1$ , where $W_1 = W_p W_a W_g$,
    using `glover_mcfarlane`. First, the maximum stability
    margin $ϵ_{max} = 1/γ_{min}$ is calculated. If the margin is too small, $ϵmax < 0.25$, then go back and modify the weights. Otherwise, a γ-suboptimal controller is synthesized. There is usually no advantage to be gained by using the optimal controller. When $ϵ_{max}$ > 0.25
    (respectively $γ_{min}$ < 4) the design is usually successful. In this case, at least
    25% coprime factor uncertainty is allowed, and we also find that the shape of the
    open-loop singular values will not have changed much after robust stabilization.
    A small value of ϵmax indicates that the chosen singular value loop-shapes are
    incompatible with robust stability requirements. That the loop-shapes do not
    change much following robust stabilization if γ is small (ϵ large), is justified
    theoretically in McFarlane and Glover (1990).

7. Analyze the design and if all the specifications are not met make further
    modifications to the weights.
8. Implement the controller. The configuration shown in below has been found
    useful when compared with the conventional set up. This is because
    the references do not directly excite the dynamics of $K$, which can result in large amounts of overshoot (classical derivative kick). The constant prefilter ensures a steady-state gain of 1 between r and y, assuming integral action in $W_1$ or $G$ (note, the K returned by this function has opposite sign compared to that of Skogestad, so we use negative feedback here).

Anti-windup can be added to $W_1$ but putting $W_1$ on Hanus form after the synthesis, see [`hanus`](@ref).

```
       ┌─────────┐      ┌────────┐      ┌────────┐
    r  │         │    us│        │  u   │        │  y
   ───►│(K*W2)(0)├──+──►│   W1   ├─────►│   G    ├────┬──►
       │         │  │-  │        │      │        │    │
       └─────────┘  │   └────────┘      └────────┘    │
                    │                                 │
                    │                                 │
                    │   ┌────────┐      ┌────────┐    │
                    │   │        │  ys  │        │    │
                    └───┤   K    │◄─────┤   W2   │◄───┘
                        │        │      │        │
                        └────────┘      └────────┘
```
Keywords: nfcsyn, coprimeunc
"""
function glover_mcfarlane(G::AbstractStateSpace{Continuous}, γ = 1.1; W1=1, W2=1)
    γ > 1 || throw(ArgumentError("γ must be greater than 1"))
    Gs = W2*G*W1
    A,B,C,D = ssdata(Gs)

    R = I + D*D'
    S = I + D'D
    Sl = lu(S)
    # arec(A, B, R, Q, S) solves A'X + XA - (XB+S)R^(-1)(B'X+S') + Q = 0

    Ā = A - B*(S\D'C)
    Z,_ = MatrixEquations.arec(Ā', C', R, B*(Sl\B'))
    X,_ = MatrixEquations.arec(Ā, B, S, C'*(R\C))

    γmin = sqrt(1 + ρ(X*Z))

    γ *= γmin

    L = (1-γ^2)*I + X*Z
    F = -(Sl\(D'C + B'X))
    BK = γ^2*(L'\Z)*C'
    AK = A + B*F + BK*(C + D*F)
    CK = B'X
    DK = -D'
    Ks = -ss(AK, BK, CK, DK)
    Gcl = extended_gangoffour(Gs, Ks)
    imargin, ω = hinfnorm2(Gcl)
    K = W1*Ks*W2
    Gcl = extended_gangoffour(G, K)
    K, γ, (; Gcl, margin = inv(imargin), ω, γmin, Ks, Gs, Z, X)
end

"""
    K, γ, info = glover_mcfarlane(G::AbstractStateSpace{<:Discrete}, γ = 1.1; W1=1, W2=1, strictly_proper=false)

For discrete systems, the `info` tuple contains also feedback gains `F, L` and observer gain `Hkf` such that the controller on observer form is given by
```math
x′ = Ax + Bu + H_{kf}*(Cx - y)\\\\
u = Fx + L*(Cx - y)
```
Note, this controller is *not* strictly proper, i.e., it has a non-zero D matrix.
The controller can be transformed to observer form for the scaled plant (`info.Gs`)
by `Ko = observer_controller(info)`, in which case the following holds `G*K == info.Gs*Ko` (realizations are different).

If `strictly_proper = true`, the returned controller `K` will have `D == 0`.
This can be advantageous in implementations where computational delays are present. In this case, `info.L == 0` as well.

Ref discrete version: Iglesias, "The Strictly Proper Discrete-Time Controller for the Normalized Left-Coprime Factorization Robust Stabilization Problem"
"""
function glover_mcfarlane(G::AbstractStateSpace{<:Discrete}, γ = 1.1; W1=1, W2=1, strictly_proper=false)
    γ > 1 || throw(ArgumentError("γ must be greater than 1"))
    Gs = W2*G*W1
    A,B,C,D = ssdata(Gs)
    iszero(D) || throw(ArgumentError("System must be strictly proper (D must be 0)"))

    X,_,Flq = MatrixEquations.ared(A, B, I, C'C)
    Z,_ = MatrixEquations.ared(A', C', I, B*B')
    Q1 = I + C*Z*C'

    if strictly_proper
        T = Q1 # They appear to be the same
        T12 = sqrt(T)
        X12 = sqrt(X)
        M1 = T12\C*Z*A'X12
        Q2 = I + X12*Z*X12

        QM = [
            sqrt(Q1 + 1/4 * M1*M1') -1/2*M1
            -1/2*M1'       sqrt(Q2 + 1/4 * M1'*M1)
        ]
        γmin = ρ(QM)
        γ *= γmin

        W = (γ^2 - 1)*I - Z*X
        XW = X/W
        Hkf = -A*Z*C'/Q1

        # Flq is the feedback gain from the ARE above, ref Rowe and Maciejowski, "Tuning MPC using H∞ Loop Shaping" between eq 36-37.
        # Flq = -B'X*((I + B*B'X)\A) # NOTE: Flq might be required for inverse optimal control
        F = -γ^2*Flq/W # Original paper
        # F = -γ^2*B'XW*((I + γ^2*B*B'XW)\A) # Merl paper
        # @show [F; Flq; F0]
        Ak = A + B*F + Hkf*C
        # NOTE: It's unclear what the best realization is. Both papers talk about a "dual" realization, but they appear to disagree on the formulation, and none of the expressions for the Q-matrices, in the Merl paper or the Maciejowski paper, do not lead to the same caluclated feedback gain. I think they might have left out a similarity transform on some matrices. The equations used here are thus not any of the dual forms, even if those appear to be recommended.

        L = 0
        Ck = F
        Bk = -Hkf
        Dk = 0
    else

        γmin = sqrt(1 + ρ(X*Z))
        γ *= γmin

        W = (γ^2 - 1)*I - Z*X
        Hkf = -A*Z*C'/Q1
        Akf = A+Hkf*C

        XW = X/W
        hest = lu(I + γ^2*B*B'XW)
        γ2B = γ^2*B'
        arne = γ2B*XW
        Ak = hest\Akf
        Ck = arne*Ak
        Bk = hest\Hkf
        Dk = arne*Bk

        F0 = -γ2B*XW/hest
        F = F0*A
        L = F0*Hkf
    end

    Ks = -ss(Ak, Bk, Ck, Dk, G.timeevol)
    Gcl = extended_gangoffour(Gs, Ks)
    imargin, ω = hinfnorm2(Gcl)
    K = W1*Ks*W2
    Gcl = extended_gangoffour(G, K)
    K, γ, (; Gcl, margin = inv(imargin), ω, γmin, Ks, Gs, Z, X, F, L, Hkf, W)
end

"""
    observer_controller(glover_mcfarlane_info::NamedTuple)

Return a controller on observer form (observer of the scaled plant `info.Gs`).
The observer controller has input vector `y`.

Ref: eq (2.5)-(2.6) of Iglesias, "The Strictly Proper Discrete-Time Controller for the Normalized Left-Coprime Factorization Robust Stabilization Problem"
"""
function ControlSystems.observer_controller(info::NamedTuple)
    isdiscrete(info.Gs) || throw(ArgumentError("Observer controller can only be generated for Glover McFarlane designs on discrete systems."))
    A,B,C,D = ssdata(info.Gs)
    iszero(D) || throw(ArgumentError("observer_controller does not support non-zero D matrix")) # not sure if this is a strict limitation or the paper author simplified.
    H, L, F = info.Hkf, info.L, info.F
    Ao = A + B*(F + L*C) + H*C
    Bo = (B*L+H)
    Co = F + L*C
    Do = L
    ss(Ao, Bo, Co, Do, info.Gs.timeevol)
end

"""
    observer_predictor(glover_mcfarlane_info::NamedTuple)

Return a predictor for the scaled plant `info.Gs`.
The observer predictor has input vector `[u; y]`.

Ref: eq (2.5)-(2.6) of Iglesias, "The Strictly Proper Discrete-Time Controller for the Normalized Left-Coprime Factorization Robust Stabilization Problem"
"""
function ControlSystems.observer_predictor(info::NamedTuple)
    isdiscrete(info.Gs) || throw(ArgumentError("Observer predictor can only be generated for Glover McFarlane designs on discrete systems."))
    A,B,C,D = ssdata(info.Gs)
    H, L, F = info.Hkf, info.L, info.F
    Ao = A + H*C
    Bo = [B-H*D -H]
    Co = C
    Do = [D zeros(size(D,1), size(H, 2))]
    ss(Ao, Bo, Co, Do, info.Gs.timeevol)
end

"""
    K, γ, info = glover_mcfarlane_2dof(G::AbstractStateSpace{Continuous}, Tref::AbstractStateSpace{Continuous}, γ = 1.1, ρ = 1.1;
    W1 = 1, Wo = I, match_dc = true, kwargs...)

Joint design of feedback and feedforward compensators
```math
K = \\left\\[K_1 & K_2\\right\\]
```
```
   ┌──────┐   ┌──────┐        ┌──────┐    ┌─────┐
r  │      │   │      │        │      │    │     │
──►│  Wi  ├──►│  K1  ├───+───►│  W1  ├───►│  G  ├─┐y
   │      │   │      │   │    │      │    │     │ │
   └──────┘   └──────┘   │    └──────┘    └─────┘ │
                         │                        │
                         │    ┌──────┐            │
                         │    │      │            │
                         └────┤  K2  ◄────────────┘
                              │      │
                              └──────┘
```
Where the returned controller `K` takes the measurement vector `[r; y]` (positive feedback),
i.e., it includes all blocks `Wi, K1, K2, W1`.
If `match_dc = true`, `Wi` is automatically computed to make sure the static gain matches `Tref` exactly, otherwise `Wi` is set to `I`.
The `info` named tuple contains the feedforward filter for inspection (`info.K1 = K1*Wi`).

# Arguments:
- `G`: Plant model
- `Tref`: Reference model
- `γ`: Relative γ
- `ρ`: Design parameter, typically 1 < ρ < 3. Increase to emphasize model matching at the expense of robustness.
- `W1`: Pre-compensator for loop shaping.
- `Wo`: Output selction matrix. If there are more measurements than controlled variables, this matrix let's you select which measurements are to be controlled.
- `kwargs`: Are sent to [`hinfsynthesize`](@ref).

Ref: Sec. 9.4.3 of Skogestad, "Multivariable Feedback Control: Analysis and Design".
The reference contains valuable pointers regarding gain-scheduling implementation of the designed controller as an observer with feedback from estimated states.
In order to get anti-windup protection when `W1` contains an integrator,
transform `W1` to self-conditioned Hanus form (using [`hanus`](@ref)) and implement the controller like this
```julia
W1h = hanus(W1)             # Perform outside loop

# Each iteration
us = filter(Ks, [r; y])     # filter inputs through info.Ks (filter is a fictive function that applies the transfer function)
u  = filter(W1h, [us; ua])  # filter us and u-actual (after input saturation) through W1h
ua = clamp(u, lower, upper) # Calculate ua for next iteration as the saturated value of u
```

# Example:
```julia
P = tf([1, 5], [1, 2, 10]) # Plant
W1 = tf(1,[1, 0]) |> ss    # Loop shaping controller

Tref = tf(1, [1, 1]) |> ss # Reference model

K1dof, γ1, info1 = glover_mcfarlane(ss(P), 1.1; W1)
K2dof, γ2, info2 = glover_mcfarlane_2dof(ss(P), Tref, 1.1, 1.1; W1)

G1 = feedback(P*K1dof)
G2 = info2.Gcl

bodeplot(info2.K1, w, lab="Feedforward filter")
plot([step(G1, 15), step(G2, 15), step(Tref, 15)], lab=["1-DOF" "2-DOF" "Tref"])
```
"""
function glover_mcfarlane_2dof(G::AbstractStateSpace{Continuous}, Tref::AbstractStateSpace{Continuous}, γ = 1.1, ρ = 1.1; W1=1, Wo = I, match_dc = true, kwargs...)
    γ > 1 || throw(ArgumentError("γ must be greater than 1"))
    ρ > 1 || throw(ArgumentError("ρ must be greater than 1"))
    Gs = G*W1
    As,Bs,Cs,Ds = ssdata(Gs)
    Ar,Br,Cr,Dr = ssdata(Tref)
    nr,nr = size(Ar)
    lr,mr = size(Dr)
    ns,ns = size(As)
    ls,ms = size(Ds)
    Rs = I + Ds*Ds'
    sRs = sqrt(Rs) # Example in Skogestad uses matlab sqrt which is elementwise
    Ss = lu!(I + Ds'Ds)
    A1 = (As - Bs*(Ss\Ds'Cs))
    R1 = Cs'*(Rs\Cs)
    Q1 = Bs*(Ss\Bs')
    Zs, _ = MatrixEquations.arec(A1', R1, Q1)

    A = cat(As, Ar, dims=(1,2))
    B1 = [
        zeros(ns,mr) ((Bs*Ds')+(Zs*Cs'))/sRs
        Br zeros(nr,ls)
    ]
    B2 = [Bs; zeros(nr,ms)]
    C1 = [zeros(ms,ns+nr); Cs zeros(ls,nr); ρ*Wo*Cs -ρ^2*Cr]
    C2 = [zeros(mr,ns+nr); Cs zeros(ls,nr)]
    D11 = [zeros(ms,mr+ls); zeros(ls,mr) sRs;-ρ^2*Dr ρ*Wo*sRs]
    D12 = [I(ms);Ds;ρ*Ds]
    D21 = [ρ*I(mr) zeros(mr,ls);zeros(ls,mr) sRs]
    D22 = [zeros(mr,ms); Ds]
    P = ss(A, B1, B2, C1, C2, D11, D12, D21, D22)
    _, Ks, γopt = hinfsynthesize(P, γrel = γ; kwargs...)

    u1 = 1:ms
    K1 = Ks[:, u1]
    if match_dc
        K2 = Ks[:, ms+1:end]
        sens = output_sensitivity(Gs, -K2)*Gs*K1 # eq. 9.89
        Wi = (Wo*dcgain(sens, 1e-6))\dcgain(Tref)
        K1 = K1*Wi
        Ks.B[:,u1] .= K1.B
        Ks.D[:,u1] .= K2.D
    else
        Wi = I
    end
    K = W1*Ks
    Gcl = feedback(G*K, ss(1), W1=1:ms, U1=ms+1:2ms, pos_feedback=true)
    info = (; Gcl, Ks, Gs, Wi, K1)
    K, γopt, info
end

"Spectral radius"
function ρ(X)
    e = eigvals(X)
    abs(e[end])
end

"""
    Wh = hanus(W)

Return `Wh` on Hanus form. `Wh` has twice the number of inputs, where the second half of the inputs are "actual inputs", e.g., potentially saturated. This is used to endow `W` with anti-windup protection.
`W` must have an invertable `D` matrix and be minimum phase.

Ref: Sec 9.4.5 of Skogestad, "Multivariable Feedback Control: Analysis and Design"
"""
function hanus(W)
    A,B,C,D = ssdata(W)
    nu = W.nu
    BD = B/D
    A2 = A - BD*C
    B2 = [0*I(nu) BD]
    D2 = [D 0*I(nu)]
    ss(A2, B2, C, D2)
end

"""
    extended_gangoffour(P, C)

Returns a single statespace system that maps
- `w1` reference or measurement noise
- `w2` load disturbance
to
- `z1` control error
- `z2` control input
```
      z1          z2
      ▲  ┌─────┐  ▲      ┌─────┐
      │  │     │  │      │     │
w1──+─┴─►│  C  ├──┴───+─►│  P  ├─┐
    │    │     │      │  │     │ │
    │    └─────┘      │  └─────┘ │
    │                 w2         │
    └────────────────────────────┘
```

The returned system has the transfer-function matrix
```math
\\begin{bmatrix}
I \\\\ C
\\end{bmatrix} (I + PC)^{-1} \\begin{bmatrix}
I & P
\\end{bmatrix}
```

The gang of four can be plotted like so
```julia
Gcl = extended_gangoffour(G, C) # Form closed-loop system
bodeplot(Gcl, lab=["S" "CS" "PS" "T"], plotphase=false) |> display # Plot gang of four
```
Note, the last output of Gcl is the negative of the `CS` and `PS` transfer functions from `gangoffour2`.
See [`glover_mcfarlane`](@ref) for an extended example. See also [`ncfmargin`](@ref).
"""
function extended_gangoffour(P, C)
    ny,nu = size(P)
    S = feedback(ss(I(ny+nu), P.timeevol), [0*I(ny) P; -C 0*I(nu)], pos_feedback=true)
    Gcl = S + cat(0*I(ny), -I(nu), dims=(1,2))
    Gcl
end

"""
    m, ω = ncfmargin(P, K)

Normalized coprime factor margin, defined has the inverse H∞ norm of
```math
\\begin{bmatrix}
I \\\\ K
\\end{bmatrix} (I + PK)^{-1} \\begin{bmatrix}
I & P
\\end{bmatrix}
```
A margin ≥ 0.25-0.3 is a reasonable for robustness.

If controller `K` stabilizes `P` with margin `m`, then `K` will also stabilize `P̃` if `nugap(P, P̃) < m`.

See also [`extended_gangoffour`](@ref), [`diskmargin`](@ref).
"""
function ncfmargin(P, K)
    Gcl = extended_gangoffour(P, K)
    im, w = hinfnorm2(Gcl)
    inv(im), w
end


#function blkd(A :: Array,B :: Array)
#	# block diagonal matrix of A and B
#	al = size(A,1)
#	ac = size(A,2)
#	bl = size(B,1)
#	bc = size(B,2)
#	C = zeros(al+bl, ac + bc)
#	C[1:al, 1:ac] = A
#	C[al+1:end, ac+1:end] = B
#	C
#end
function eig(p :: Phs)
	# computes the eigenvalues and eigenvectors of a linear PHS
	#  (which haves a Q matrix!)
	#
	#	constrained (G != 0) cases can be addressed using
	#  generalized eigenvalues
	#
	#
	#  TODO: eigenvalues around a linearized condition:
	#       just need to compute the Hamiltonian Hessian at a given point!
	#       For e.g.: hess = ForwardDiff.hessian(p.Hamiltonian)
	#                 Q = hess(x)
	#               --> this can be useful for validating our nonlinear system!
	#

	if isdefined(p, :Q)
		if isdefined(p,:G)
			nconst = size(p.G,2)
			I = eye(size(p.J,1))
			z = zeros(nconst,nconst)
			E = blkdiag(I,z)
			A = [p.J*p.Q p.G;
			     p.G'*p.Q p.G_D]
			e = eigfact(A,E)
			ind = sortperm(imag(e.values))
			return e.values[ind], e.vectors[ind,ind]
		else
			a,v = eig(p.J*p.Q)
			ind = sortperm(imag(a))
			return a[ind], v[:,ind]
		end
	else
		fprintln("Undefined Q matrix (nonlinear system?)!")
		return
	end
end

function eigdamp(p :: Phs)
	# computes the frequency and damping of PHS with damping
	#  (which haves a Q matrix!)
	#
	#	constrained (G != 0) cases can be addressed using
	#  generalized eigenvalues
	#

	if isdefined(p, :Q)
		if isdefined(p,:G)
			nconst = size(p.G,2)
			I = eye(size(p.J,1))
			z = zeros(nconst,nconst)
			E = blkdiag(I,z)
			A = [(p.J-p.R)*p.Q p.G;
			     p.G'*p.Q p.G_D]
			e = eigfact(A,E)
			ind = sortperm(imag(e.values))
			return e.values[ind], e.vectors[ind,ind]
		else
			a,v = eig((p.J-p.R)*p.Q)
			ind = sortperm(imag(a))
			return a[ind], v[:,ind]
		end
	else
		fprintln("Undefined Q matrix (nonlinear system?)!")
		return
	end
end

function damp(p :: Phs)
	a,v = eigdamp(p)
	return [abs(a) -cos(angle(a))]
end
function frequencies(a :: Array)
		imag(a[imag(a).>=0])
end

function frequencies(ph :: Phs)
	a,v = eig(ph)
	return frequencies(a)
end


# ------------ #
# Original API #
# ------------ #

# splidef.m -- DONE
function splidef(breaks, evennum=0, k::Int=3)
    p = SplineParams(breaks, evennum, k)
    return (
        length(p.breaks),
        minimum(p.breaks),
        maximum(p.breaks),
        Any[p.breaks, 0, k],
        p
    )
end

# splinode.m  -- DONE
splinode(breaks::AbstractVector, evennum::Int, k::Int=3) =
    nodes(SplineParams(breaks, evennum, k))

function splidop(breaks, evennum=0, k=3, order=1)
    D, p = derivative_op(SplineParams(breaks, evennum, k), order)
    n = length(breaks) + k - 1

    D, n-order, breaks[1], breaks[end], Any[breaks, evennum, k-order]
end

# splibas.m -- DONE
function splibase(breaks::AbstractVector, evennum, k=3, x=splinode(breaks, evennum, k),
                  order=0)
    B = evalbase(SplineParams(breaks, evennum, k), x, order)
    B, x
end


using Revise
using Random
using SpecialFunctions
using PyCall
using PyPlot
using StatsPlots

function mySliceSampler(pdf_log, x_0, w, m, N, burnIn = 1000)
    # REQUIRES MODULES:
    # Random
    #
    # INPUT:
    # pdf_log --> log-pdf of the target distribution (a function)
    # x_init --> inital vector of x (dimension 1xD), where D dimension of pdf
    # w --> typical slice size, for a window (dimension 1xD)
    # m --> integer limiting the slice size to m*w
    # N --> number of sample points
    # burnIn --> number of burn-in samples (optional, results will be discarded)
    #
    # OUTPUT:
    # x_s --> sampled x values (NxD), only keep values after full permutations along dimensions
    # pdflog_x_s --> log-pdf of sampled values

    D = length(x_0)  # the dimension of the distribution (nr of coordinates)
    x_s = Array{Float64}(undef,N,D)   # Samples (NxD), preallocate array, with undefined values
    pdflog_x_s = Array{Float64}(undef,N)   # (unnormalised) log-prob of sampled points (Nx1)
                                           # for reducing evaluations
    pdflog_x_1 = 0.0;

    # to be able to input scalars:
    if typeof(x_0) == Float64
        x_0 = [x_0]
    end

    for ii in 1:(N+burnIn)
        # Update the new x progressively for each dimension:
        L   = 1*x_0  # R and L need to be of same dimension, because we apply the pdf upon those!
        R   = 1*x_0
        x_1 = 1*x_0  # "1*" needed, otherwise x_0 suddendly takes mysterious values
        for d in randperm(D)
            ### 1) Make the Slice
            # random "vertical" position
            if (ii == 1)
                #vertical = rand() * pdf_log(x_0) # in normal space
                z = pdf_log(x_0) + log(rand()) # in log-space
            else
                # saving evaluations, pdflog_x_1 was updated for current permutation in the end
                z = pdflog_x_1 + log(rand())
            end

            ### 2) Make the window, "Stepping-out" alogrithm
            # Randomly place a window, containing x_0:
            L[d] = x_0[d] - rand() * w[d]
            R[d] = L[d] + w[d]

            # Randomly share the max window size among left/right:
            J = floor(m*rand());
            K = (m-1) - J;

            # Extend window to the left, until outside slice or allowance seizes:
            while ((J > 0) && (z < pdf_log(L)))
                #println("Lefting")
                L[d] -= w[d]
                J -= 1
            end

            # Extend window to the right, until outside slice or allowance seizes:
            while ((K > 0) && (z < pdf_log(R)))
                #println("Righting")
                R[d] += w[d]
                K -= 1
            end

            ### 3) Sample from window
            # finding an allowable point:
            while true
                x_1[d] = L[d] + rand() * (R[d] - L[d])

                # this + breaking out of loop reduces the amount of log-pdf evaluations:
                pdflog_x_1 = pdf_log(x_1)

                if (pdflog_x_1 >= z)     # new value found
                    x_0[d] = x_1[d]     # update value for this dimension
                    break
                # Value was not within slice, shrink the interval:
                elseif (x_1[d] < x_0[d])
                    L[d] = x_1[d]
                elseif (x_1[d] > x_0[d])
                    R[d] = x_1[d]
                else
                    throw(ErrorException("Error during shrinking the interval"))
                end
            end

        end # end of permutating through dimensions

        ### 4) Update the chain:
        # Just overwrite the burnIn by using modular arithmetic:
        i = 1 + (ii-1)%N    # gives 1,2,3,...N,1,2,3,...,(N+burnIn)
        x_s[i,:] = x_1    # sample point, after one round of permutations
        pdflog_x_s[i] = pdflog_x_1 # now contains log_pdf of all new coordinates of new point, since has been updated at each permutation
        # note that x_0 has been updated during the permutations

    end

    return x_s, pdflog_x_s
end

#%%
################################################################

function log_beta(θ,a::Any,b::Any)
    #using SpecialFunctions
    θ = θ[1]
    if 0 < θ < 1
        # Numerator
        num = log(θ)*(a-1) + log(1-θ)*(b-1)
        # The denominator (Beta-function) can be integrated or made up of Gamma-functions
        # Since we only treat integer values here, Γ(a) = (a-1)!
        #den = log(factorial(a-1) * factorial(b-1) / factorial(a+b-1))
        den = log(SpecialFunctions.beta(a,b))
        return num - den
    else
        return -Inf
    end
end

# Jespers HDI:
function hdi(theta_samp,alpha=0.05)
    cred_mass = 1.0-alpha
    ci = zeros(2)
    if length(size(theta_samp))>1   # multidimensional theta
        K,N = size(theta_samp)
        cis = zeros(2,K)
        for k in 1:K    # for each dimension

            ts = theta_samp[k,:]
            sind = sortperm(ts)     # get series of indices that sorts theta values
            sts = ts[sind]  # sorted theta values

            # Shifting a 95% index block from far left to far right and use the
            # samllest interval that can be found during the shift
            N = length(sind)    # number of sample points
            length_ci = Inf     # start with inf HDI interval and shrink
            for i in 1:Int(floor(N*alpha))  # iterate over the 5% lowest values (low to high)
                i2 = Int(floor(N*cred_mass)+i)  # the 5% highest values (low to high), one 95% block away on the other side
                prop_ci = [sts[i],sts[i2]]      # proposed interval
                length_prop_ci = prop_ci[2]-prop_ci[1]
                if length_prop_ci < length_ci
                    ci = prop_ci
                    length_ci = ci[2]-ci[1]
                end
            end
            cis[:,k] = ci

        end
        return cis
    else
        N = length(theta_samp)

        ts = theta_samp
        sind = sortperm(ts)
        sts = ts[sind]

        N = length(sind)
        length_ci = Inf
        for i in 1:Int(floor(N*alpha))
            i2 = Int(floor(N*cred_mass)+i)
            prop_ci = [sts[i],sts[i2]]
            length_prop_ci = prop_ci[2]-prop_ci[1]
            if length_prop_ci < length_ci
                ci = prop_ci
                length_ci = ci[2]-ci[1]
            end
        end
        return ci
    end
end

################################################################

# Log-Bernoulli PDF:
function log_bernoulli_pdf(θ,y)
    if 0 < θ < 1 && (y==0 || y==1)    # Only admissible values
        return y * log(θ) + (1-y) * log(1-θ)
    else
        return -Inf
    end
end

# Log-Likelihood fct of a (series of) coin tosses with bias:
function log_likelihood_fct(θ, y)
    # Make array s.t. multiple theta values can be input:
    #likelihood = Array{Float64}(undef,length(θ))
    # Do that for every theta value requested:
    #for ii in 1:length(θ)
    #    # Elementwise application of the log_beronoulli_pdf over outcome vector y, then SUM together:
    #    likelihood[ii] = sum(log_bernoulli_pdf.(θ[ii],y))
    #end

    # Un array-ise the output:
    #if length(θ) == 1
    #    likelihood = likelihood[1]
    #end

    θ = θ[1]
    likelihood = sum(log_bernoulli_pdf.(θ,y))

    return likelihood
end

#%%
################################################################

### Part A
# Chain inputs:
N = 10^6
x_start = 0.5;
w = .5;  # typical window width
m = 100; # maximum number of window widths

#%%
#First sampling
a = 1   # Flat Prior
b = 1
y = [ones(Int64,11); zeros(Int64,3)];   # Given observaitons

log_prior(θ) = log_beta(θ,a,b)
log_lklhd(θ) = log_likelihood_fct(θ,y)
log_posterior(θ) = log_lklhd(θ) + log_prior(θ)

# Need to use "StatsPlots" or "Plots" histogram to add the density curve, PyPlot won't work
chain, pdf = mySliceSampler(log_posterior,x_start,w,m,N);
chain = reshape(chain,:)
#%%
histogram(chain, bins=100, normalize=:pdf, label="posterior")   # Comes from StatsPlots now
density!(chain,linewidth=3,label="posterior")

#%%
# Sample Mean
μ = sum(chain)/N

# Mode
ω = chain[argmax(pdf)]

# Equal Tail Interval:
# Find the 2.5% highest and lowest (equal tails)
# get their index and receive an interval
α = 0.05;
idcesForOrder = sortperm(chain) # Indices of the chain which make it sorted
chain_sorted = chain[idcesForOrder]

leftTailIdx = Int(floor(N*(α/2)))
rightTailIdx = Int(ceil(N*(1- α/2)))

ETI = [chain_sorted[leftTailIdx],chain_sorted[rightTailIdx]]

HDI = hdi(chain)

#%%
p_gthalf = count(i->(i>0.5), chain)/N

#%%

### Part B

# Chain inputs:
N = 10^6
x_start = 0.5;
w = .5;  # typical window width
m = 100; # maximum number of window widths

#%%
#Second dataset
a = 1   # Flat Prior
b = 1
y = [ones(Int64,3); zeros(Int64,7)];   # Given observtions

log_prior(θ) = log_beta(θ,a,b)
log_lklhd(θ) = log_likelihood_fct(θ,y)
log_posterior(θ) = log_lklhd(θ) + log_prior(θ)

# Need to use "StatsPlots" or "Plots" histogram to add the density curve, PyPlot won't work
chain2, pdf2 = mySliceSampler(log_posterior,x_start,w,m,N);
chain2 = reshape(chain2,:);
#%%
histogram(chain2, bins=100, normalize=:pdf, label="posterior")   # Comes from StatsPlots now
density!(chain2,linewidth=3,label="posterior")

#%%  Method 1
gt = (chain .> chain2)*1
sum(gt)/N

#%% Method 1.1, questionable...
#gt = Array{Float64,1}(undef,N)
#for i in 1:length(chain)
    # for each entry in chain 1, compare it to ALL entries in chain2
#    gt[i] = sum((chain[i] .> chain2) * 1)/length(chain2)
#end

#sum(gt)/length(chain)

#%% Method 1.1, questionable...
#gt = (μ .> chain2) * 1
#sum(gt)/N

#%% Method 1.2, questionable...
#μ2 = sum(chain2)/N
#ω2 = chain[argmax(pdf)]

#idcesForOrder = sortperm(chain2) # Indices of the chain which make it sorted
#chain2_sorted = chain[idcesForOrder]

#gt = (chain_sorted .> maximum(chain2_sorted))*1
#sum(gt)

#%% Method 2
dθ = (chain - chain2)
gtZero = (dθ .> 0)*1
sum(gtZero)/N
hdi(dθ)

#%% Method 3
dθ = (chain - chain2)
dθ = reshape(dθ,:)      # to get an Array{Float64,1}
histogram(dθ, bins=100, normalize=:pdf, label="d_theta")
density!(dθ,linewidth=3,label="d_theta")
#%%
diff_hdi = hdi(dθ)
#%%
using Plots
Plots.savefig("/home/johhub/Desktop/ABDA/A4/A4-B-hist.pdf")


# A semi-separable matrix is defined by
#
# S = triu(uvᵀ, bu+1) + tril(pqᵀ, -bl-1),
#
# where bu, bl >= 0. u and v are n × ru matrices and p and q are n × rl matrices.
# See [1] for the definition.

# [1] Chandrasekaran and Gu, Fast and stable algorithms for banded plus
# Semiseparable systems of linear equations, SIMAX, 25 (2003), pp. 373-384.

## Constructors

struct SemiSeparableMatrix{T} <: AbstractMatrix{T}
    L::LowRankMatrix{T}
    U::LowRankMatrix{T}
    bl::Int
    bu::Int

    function SemiSeparableMatrix(L::LowRankMatrix{T}, U::LowRankMatrix{T}, bl, bu) where T
        Lm, Ln = size(L)
        Um, Un = size(U)
        @assert Um == Un == Lm == Ln && Un >= bu+1 && Lm >= bl+1
        new{T}(L, U, bl, bu)
    end
end

function Base.convert(::Type{Matrix}, S::SemiSeparableMatrix)
    return triu(Matrix(S.U), S.bu+1) + tril(Matrix(S.L), -S.bl-1)
end

size(S::SemiSeparableMatrix) = size(S.L)

function getindex(S::SemiSeparableMatrix{T}, k::Int, j::Int)  where T
    k-j ≥ S.bl && return S.L[k,j]
    j-k ≥ S.bu && return S.U[k,j]
    return zero(T)
end


# A version of Stiefel’s Conjugate Residual method for linesearch.

"""A version of Stiefel’s Conjugate Residual method for linesearch.
CR(A, b, ϵa, ϵr, itmax; quad) solves the symmetric linear system 'A * x = b'
or the least-squares problem : 'min ‖b - A * x‖²'
If quad = true, the values of the quadratic model are computed
A can be positive definite or not.
"""
function CRlin(A, b, ϵa::Float64=1e-8, ϵr::Float64=1e-6, itmax::Int=0, ε::Float64=1e-6; quad::Bool=false)
    n = size(b, 1) # size of the problem
    (size(A, 1) == n & size(A, 2) == n) || error("Inconsistent problem size")
    @info(loggerCRlin, @sprintf("CRlin: system of %d equations in %d variables", n, n))

    x = zeros(n) # initial estimation x = 0
    xNorm = 0.0
    xNorms = [xNorm] # Values of ‖x‖
    r = b # initial residual r = b - Ax = b
    rNorm = norm(r, 2) # ‖r‖
    rNorm² = rNorm * rNorm
    pr = rNorm²
    s = A * r
    ρ = dot(r, s)
    p = r
    pNorm² = rNorm²
    q = s
    ϵ = ϵa + ϵr * rNorm
    pAp = ρ # = dot(p, q) = dot(r, s)

    iter = 0
    itmax == 0 && (itmax = 2 * n)

    if quad
        m = 0.0
        mvalues = [m] # values of the quadratic model
        @info(loggerCRlin, @sprintf("%5s %7s %8s", "Iter", "‖r‖", "q"))
        @info(loggerCRlin, @sprintf("%5d %7.1e %8.1e", iter, rNorm, m))
    end

    solved = rNorm ≤ ϵ
    tired = iter ≥ itmax

    while ! (solved || tired)
        iter += 1

        if (pAp ≤ ε * pNorm²) || (ρ ≤ ε * rNorm²)
            @debug(loggerCRlin, @sprintf("nonpositive curvature detected: pAp = %8.1e and rAr = %8.1e", pAp, ρ))
            iter == 1 && return b
            return x
        end

        α = ρ / dot(q, q) # step
        x = x + α * p
        xNorm = norm(x, 2)
        push!(xNorms, xNorm)
        r = r - α * q  # residual
        rNorm² = abs(rNorm² - α * ρ)
        rNorm = sqrt(rNorm²)

        if quad
            m = -dot(b, x) + 0.5 * dot(x, A * x)
            push!(mvalues, m)
            @info(loggerCRlin, @sprintf("%5d %7.1e %8.1e", iter, rNorm, m))
        end

        solved = rNorm <= ϵ
        tired = iter >= itmax
        (solved || tired) && continue

        s = A * r
        ρbar = ρ
        ρ = dot(r, s)
        β = ρ / ρbar # step for the direction calculus
        p = r + β * p # descent direction
        pNorm² = rNorm² + 2 * β * pr - 2 * β * α * pAp + β^2 * pNorm²
        pr = rNorm² + β * pr - β * α * pAp # pᵀr
        q = s + β * q
        pAp = ρ + β^2 * pAp # dot(p, q)

    end

    return x
end


function integrate(
    f::Function,
    a::Real,
    b::Real;
    method::Symbol = :quadrature,
)
    if method == :quadrature
        res, err = quadgk(f, a, b)
        return res
    elseif method == :simpsons
        return adaptivesimpsons(f, a, b)
    elseif method == :monte_carlo
        return montecarlo(f, a, b)
    else
        throw(
            ArgumentError(
                "method must be :quadrature, :simpsons or :monte_carlo"
            )
        )
    end
end


# julia --project=. plotscript.jl
#
# This will take a long time to start so won't
# recommend doing this.
using Plots
gr

# x = 1:10
# y = rand(10)
# p = plot(x,y)
# gui()
# display(p)
# println("Hello World!")
# readline()

n = 50 # points
x_vec = fill(0.0, n)
y_vec = fill(0.0, n)
xmin = -10.0
xmax = 10.0
x_range = LinRange(xmin, xmax, n)
# Sigmoid function
# Generalized logistic function
function fx(x, w, b, alpha)
    #y = 1/(1 + exp(-w*x + b))
    y = (1+ exp(-w*x)+b)^(-alpha)
    #y = exp(-w*x)
    return y
end

"""
    glf(x, A, K, B, mu, Q, C, M)

Generalized logistic function
"""
function glf(x, A, K, B, mu, Q, C, M)
    Y = A + (K-A)/(C + Q*exp(-B*(x-M)))^(1/mu)
end
# w = 1
# b = 20# 0 - 10
# alp = 0.3
A = 0.01
K = 1.0
Q = 1.0 #0.5
C = 1.0

# B = 1.8# 2 # 5 # 0 to 10 # top angle
# mu = 1.8 # Close rate  # bottom angle
# M = 5.8 # shift left and right
B = 1.8 # 2 # 5 # 0 to 10 # top angle
mu = 1.7 # Close rate  # bottom angle
M = 5.8 # shift left and right
for (ind,x) in enumerate(x_range)
    y_vec[ind] = glf(x, A, K, B, mu, Q, C, M)
end

# scale to 14.7 to 9200 psi
pmin = 14.7
pmax = 9200
pdist = pmax - pmin
dist = xmax - xmin
scale = pdist/dist
for (ind,x) in enumerate(x_range)
    x_vec[ind] = (x+10)*scale + pmin
end

pt = plot(x_vec,y_vec, legend=(0.1,0.9))

# B = 2 # 2 # 5 # 0 to 10 # top angle
# mu = 2 # Close rate  # bottom angle
# M = 5 # shift left and right
# for (ind,x) in enumerate(x_range)
#     y_vec[ind] = glf(x, A, K, B, mu, Q, C, M)
# end

# scale to 14.7 to 9200 psi
# pmin = 14.7
# pmax = 9200
# pdist = pmax - pmin
# dist = xmax - xmin
# scale = pdist/dist
# for (ind,x) in enumerate(x_range)
#     x_vec[ind] = (x+10)*scale + pmin
# end
#
# plot!(pt, x_vec, y_vec)

ps = [  14.7,   3000,    5000,    5316,    5800,   6284,   6500,       6700, 7167,  7601, 7900, 8200, 9204]
ks = [0.01, 0.01,    0.01,    0.015,    0.04,      0.08,     0.16,   0.30,   0.65,  0.85, 0.95, 1, 1]

plot!(pt, ps, ks)


module bavarian_wood

using JuMP
using Cbc
using CSV

function solve_bavarian_wood()

    n = 10;

    Q = 25;
    h = 5;

    K = 8;
    o = [ 1  1  2  2  3  3  4  4  ];
    d = [ 9  10 9  10 9  10 9  10 ];
    L = [ 28 14 42 35 39 18 19 31 ];

    R = [
        (1,5) (1,7) (1,8) (1,9) (1,10) #=	why this #= =# stuff? because otherwise
    =#  (2,5) (2,7) (2,8) (2,9) (2,10) #=	Julia interprets it as 8 different rows
    =#  (3,5) (3,7) (3,8) (3,9) (3,10) #=	but it is just one row of pairs. I want
    =#  (4,6) (4,7) (4,8) (4,9) (4,10) #=	to display it in 8 rows in the code to
    =#  (5,7) (5,8) (5,9) (5,10)       #=	increase readability. See further dis-
    =#  (6,7) (6,8) (6,9) (6,10)       #=    cussion here: https://github.com/JuliaLang/julia/issues/27533
    =#  (7,8) (7,9) (7,10)             #=
    =#  (8,10)
    ];

    N = [5 6 7 8];

    N_out = [
    #=  1 =# (5, 7, 8, 9, 10)
    #=  2 =# (5, 7, 8, 9, 10)
    #=  3 =# (5, 7, 8, 9, 10)
    #=  4 =# (6, 7, 8, 9, 10)
    #=  5 =# (7, 8, 9, 10)
    #=  6 =# (7, 8, 9, 10)
    #=  7 =# (8, 9, 10)
    #=  8 =# (10)
    #=  9 =# ()
    #= 10 =# ()
    ];

    N_in = [
    #=  1 =# ()
    #=  2 =# ()
    #=  3 =# ()
    #=  4 =# ()
    #=  5 =# (1, 2, 3)
    #=  6 =# (4)
    #=  7 =# (1, 2, 3, 4, 5, 6)
    #=  8 =# (1, 2, 3, 4, 5, 6, 7)
    #=  9 =# (1, 2, 3, 4, 5, 6, 7)
    #= 10 =# (1, 2, 3, 4, 5, 6, 7, 8)
    ];

    c = [
        Inf	Inf	Inf	Inf	280	Inf	530	830	780	1000;
        Inf	Inf	Inf	Inf	245	Inf	495	795	745	965;
        Inf	Inf	Inf	Inf	175	Inf	425	725	675	895;
        Inf	Inf	Inf	Inf	Inf	220	670	680	920	850;
        Inf	Inf	Inf	Inf	Inf	Inf	250	550	500	720;
        Inf	Inf	Inf	Inf	Inf	Inf	450	460	700	630;
        Inf	Inf	Inf	Inf	Inf	Inf	Inf	300	250	470;
        Inf	Inf	Inf	Inf	Inf	Inf	Inf	Inf	Inf	170;
        Inf	Inf	Inf	Inf	Inf	Inf	Inf	Inf	Inf	Inf;
        Inf	Inf	Inf	Inf	Inf	Inf	Inf	Inf	Inf	Inf;
    ]

    model = Model(with_optimizer(Cbc.Optimizer));

    @variable(model, x[1:n,1:n,1:K] >= 0)
    @variable(model, y[1:n,1:n] >= 0, Int)

    @objective(model, Min, sum( c[i,j]*y[i,j] for (i,j) in R ) +
        sum(
            sum(
                sum(
                    h*x[j,i,k]
                for j in N_in[i] )
            for i in N )
        for k in 1:K ));

    @constraint(model,[k in 1:K,i = o[k]], sum(x[i,j,k] for j in N_out[i]) == L[k]);
    @constraint(model,[k in 1:K,j = d[k]], sum(x[i,j,k] for i in N_in[j]) == L[k]);
    @constraint(model,[(i,j) in R], sum(x[i,j,k] for k in 1:K) <= Q*y[i,j]);

    f = open("bavarian_wood.lp", "w")
    print(f, model)
    close(f)

    #-------
    # SOLVE
    #-------

    optimize!(model)

    #------------------------
    # WRITE SOLUTION TO FILE
    #------------------------

    f = open("bavarian_wood.csv","w");

    for k in 1:K
        print(f,"commodity ",o[k],"-",d[k]," - qty = ",L[k]);
        for (i,j) in R
            if (value(x[i,j,k]) > 0)
                println(f,",route ",i,"-",j,",",value(x[i,j,k]));
            end
        end
    end

    for (i,j) in R
        if (value(y[i,j]) != 0)
            print(f,"route ",i,"-",j," - ",value(y[i,j])," trucks");
            for k in 1:K
                if (value(x[i,j,k]) != 0)
                    println(f,",commodity ",o[k],"-",d[k],",",value(x[i,j,k]));
                end
            end
        end
    end

    close(f);

end

solve_bavarian_wood();

end


function reciprocal_cycle_len(n::Int)
	seen=Dict{Int,Int}()
    x=1
    index=1
	while true

		if x in keys(seen)
			return index-seen[x]+1
		else
			seen[x]=index
			x=x*10 %n
		end
		index+=1
	end
end

function compute()
	ans=0
	for i =1:1000
		ans=max(ans,reciprocal_cycle_len(i))
	end

	return ans

end

println(compute())


function ldlt!(a::Matrix{Float64}, d::Vector{Float64}; small=1.0e-10)
  n = size(a,1)
  for k in 1:n-1
    d[1] = a[1,1]
    if abs(a[k,k]) > small
      for i in k+1:n
        x=a[i,k]/a[k,k]
        for j=k+1:n
          a[i,j] -= a[k,j]*x
        end
        d[i]=a[i,i]
      end
    else
      println("Zero pivot found in row $k")
    end
  end
end


# Patrick Keast, MODERATE-DEGREE TETRAHEDRAL QUADRATURE FORMULAS
function _get_gauss_tetdata(n::Int)
    if n == 1
        a = 1. / 4.
        w = 1. / 6.
        xw = [a a a w]
    elseif n == 2
        a = ( 5. + 3. * √(5.) ) / 20.
        b = ( 5. - √(5.) ) / 20.
        w = 1. / 24.
        xw = [a b b w
              b a b w
              b b a w
              b b b w]
    elseif n == 3
        a1 = 1. / 4.
        a2 = 1. / 2.
        b2 = 1. / 6.
        w1 = -2. / 15.
        w2 = 3. / 40.
        xw = [a1 a1 a1 w1
              a2 b2 b2 w2
              b2 a2 b2 w2
              b2 b2 a2 w2
              b2 b2 b2 w2]
    else
        throw(ArgumentError("unsupported order for tetraheder gauss-legendre integration"))
    end
    return xw
end


"""

  function procrustes(x,y) -> xnew

  aligns two structures [sets of points in 3D space]. Solves
  the "Procrustes" problem. Structures are expected to be of the same size, and the
  correspondence is assumed from the vector indices.

  Returns x aligned, by performing the rigid body transformation [rotation
  and translation that minimizes the RMSD between x and y].

  x, y, and xnew (return) are matrices of dimensions (n,3)
  (n is the number of points, 3 is the dimension of the space).

  L. Martinez, Institute of Chemistry - University of Campinas
  Jan 04, 2019

"""

using LinearAlgebra

function procrustes( x :: Matrix{}, y :: Matrix{} )

  n = size(x,1)

  # Computing centroid

  cmx = zeros(3)
  cmy = zeros(3)
  for i in 1:n
    for j in 1:3
      cmx[j] = cmx[j] + x[i,j]
      cmy[j] = cmy[j] + y[i,j]
    end
  end
  cmx = cmx / n
  cmy = cmy / n

  # Translating both sets to the origin

  for i in 1:n
    for j in 1:3
      x[i,j] = x[i,j] - cmx[j]
      y[i,j] = y[i,j] - cmy[j]
    end
  end

  # Computing the quaternion matrix

  xm = Vector{Float64}(undef,n)
  ym = Vector{Float64}(undef,n)
  zm = Vector{Float64}(undef,n)
  xp = Vector{Float64}(undef,n)
  yp = Vector{Float64}(undef,n)
  zp = Vector{Float64}(undef,n)
  for i in 1:n
    xm[i] = y[i,1] - x[i,1]
    ym[i] = y[i,2] - x[i,2]
    zm[i] = y[i,3] - x[i,3]
    xp[i] = y[i,1] + x[i,1]
    yp[i] = y[i,2] + x[i,2]
    zp[i] = y[i,3] + x[i,3]
  end

  q = zeros(4,4)
  for i in 1:n
    q[1,1] = q[1,1] + xm[i]^2 + ym[i]^2 + zm[i]^2
    q[1,2] = q[1,2] + yp[i]*zm[i] - ym[i]*zp[i]
    q[1,3] = q[1,3] + xm[i]*zp[i] - xp[i]*zm[i]
    q[1,4] = q[1,4] + xp[i]*ym[i] - xm[i]*yp[i]
    q[2,2] = q[2,2] + yp[i]^2 + zp[i]^2 + xm[i]^2
    q[2,3] = q[2,3] + xm[i]*ym[i] - xp[i]*yp[i]
    q[2,4] = q[2,4] + xm[i]*zm[i] - xp[i]*zp[i]
    q[3,3] = q[3,3] + xp[i]^2 + zp[i]^2 + ym[i]^2
    q[3,4] = q[3,4] + ym[i]*zm[i] - yp[i]*zp[i]
    q[4,4] = q[4,4] + xp[i]^2 + yp[i]^2 + zm[i]^2
  end
  q[2,1] = q[1,2]
  q[3,1] = q[1,3]
  q[3,2] = q[2,3]
  q[4,1] = q[1,4]
  q[4,2] = q[2,4]
  q[4,3] = q[3,4]

  # Computing the eigenvectors 'v' of the q matrix

  v = LinearAlgebra.eigvecs(q)

  # Compute rotation matrix

  u = Matrix{Float64}(undef,3,3)
  u[1,1] = v[1,1]^2 + v[2,1]^2 - v[3,1]^2 - v[4,1]^2
  u[1,2] = 2. * ( v[2,1]*v[3,1] + v[1,1]*v[4,1] )
  u[1,3] = 2. * ( v[2,1]*v[4,1] - v[1,1]*v[3,1] )
  u[2,1] = 2. * ( v[2,1]*v[3,1] - v[1,1]*v[4,1] )
  u[2,2] = v[1,1]^2 + v[3,1]^2 - v[2,1]^2 - v[4,1]^2
  u[2,3] = 2. * ( v[3,1]*v[4,1] + v[1,1]*v[2,1] )
  u[3,1] = 2. * ( v[2,1]*v[4,1] + v[1,1]*v[3,1] )
  u[3,2] = 2. * ( v[3,1]*v[4,1] - v[1,1]*v[2,1] )
  u[3,3] = v[1,1]^2 + v[4,1]^2 - v[2,1]^2 - v[3,1]^2

  # Rotate vector x [will be stored in xnew], and restore y

  xnew = zeros(n,3)
  for i in 1:n
    for j in 1:3
      for k in 1:3
        xnew[i,j] = xnew[i,j] + u[j,k] * x[i,k]
      end
    end
  end

  # Translate vector to the centroid of y [and restore x and y]

  for i in 1:n
    for j in 1:3
      xnew[i,j] = xnew[i,j] + cmy[j]
      y[i,j] = y[i,j] + cmy[j]
      x[i,j] = x[i,j] + cmx[j]
    end
  end

  return xnew

end


b1 = BSplineBasis(1, 0:5)

B₁ = x -> Int(0 ≤ x < 1)
B₂ = x -> Int(1 ≤ x < 2)
B₃ = x -> Int(2 ≤ x < 3)
B₄ = x -> Int(3 ≤ x < 4)
B₅ = x -> Int(4 ≤ x ≤ 5)

b1_exact = [B₁, B₂, B₃, B₄, B₅]

b2 = BSplineBasis(2, big(0):1:big(5))

B₁ = x -> 0 ≤ x < 1 ? -x + 1 : 0*one(x)
B₂ = x -> 0 ≤ x < 1 ?  x + 0 :
          1 ≤ x < 2 ? -x + 2 : 0*one(x)
B₃ = x -> 1 ≤ x < 2 ?  x - 1 :
          2 ≤ x < 3 ? -x + 3 : 0*one(x)
B₄ = x -> 2 ≤ x < 3 ?  x - 2 :
          3 ≤ x < 4 ? -x + 4 : 0*one(x)
B₅ = x -> 3 ≤ x < 4 ?  x - 3 :
          4 ≤ x ≤ 5 ? -x + 5 : 0*one(x)
B₆ = x -> 4 ≤ x ≤ 5 ?  x - 4 : 0*one(x)
# 1st derivatives
B₁′ = x -> 0 ≤ x < 1 ? -1*one(x) : 0*one(x)
B₂′ = x -> 0 ≤ x < 1 ?  1*one(x) :
           1 ≤ x < 2 ? -1*one(x) : 0*one(x)
B₃′ = x -> 1 ≤ x < 2 ?  1*one(x) :
           2 ≤ x < 3 ? -1*one(x) : 0*one(x)
B₄′ = x -> 2 ≤ x < 3 ?  1*one(x) :
           3 ≤ x < 4 ? -1*one(x) : 0*one(x)
B₅′ = x -> 3 ≤ x < 4 ?  1*one(x) :
           4 ≤ x ≤ 5 ? -1*one(x) : 0*one(x)
B₆′ = x -> 4 ≤ x ≤ 5 ?  1*one(x) : 0*one(x)

b2_exact = [B₁ B₁′; B₂ B₂′; B₃ B₃′; B₄ B₄′; B₅ B₅′; B₆ B₆′]

b3 = BSplineBasis(3, 0//1:5//1)

B₁ = x -> 0 ≤ x < 1 ? 1//1*x^2 - 2//1*x + 1//1 : 0//1*one(x)
B₂ = x -> 0 ≤ x < 1 ? -3//2*x^2 + 2//1*x        :
          1 ≤ x < 2 ?  1//2*x^2 - 2//1*x + 2//1 : 0//1*one(x)
B₃ = x -> 0 ≤ x < 1 ?  1//2*x^2                 :
          1 ≤ x < 2 ? -1//1*x^2 + 3//1*x - 3//2 :
          2 ≤ x < 3 ?  1//2*x^2 - 3//1*x + 9//2 : 0//1*one(x)
B₄ = x -> 1 ≤ x < 2 ?  1//2*x^2 - 1//1*x +  1//2 :
          2 ≤ x < 3 ? -1//1*x^2 + 5//1*x - 11//2 :
          3 ≤ x < 4 ?  1//2*x^2 - 4//1*x +  8//1 : 0//1*one(x)
B₅ = x -> 2 ≤ x < 3 ?  1//2*x^2 - 2//1*x +  2//1 :
          3 ≤ x < 4 ? -1//1*x^2 + 7//1*x - 23//2 :
          4 ≤ x ≤ 5 ?  1//2*x^2 - 5//1*x + 25//2 : 0//1*one(x)
B₆ = x -> 3 ≤ x < 4 ?  1//2*x^2 -  3//1*x +  9//2 :
          4 ≤ x ≤ 5 ? -3//2*x^2 + 13//1*x - 55//2 : 0//1*one(x)
B₇ = x -> 4 ≤ x ≤ 5 ? 1//1*x^2 - 8//1*x + 16//1 : 0//1*one(x)
# 1st derivatives
B₁′ = x -> 0 ≤ x < 1 ? 2//1*x - 2//1 : 0//1*one(x)
B₂′ = x -> 0 ≤ x < 1 ? -3//1*x + 2//1 :
           1 ≤ x < 2 ?  1//1*x - 2//1 : 0//1*one(x)
B₃′ = x -> 0 ≤ x < 1 ?  1//1*x        :
           1 ≤ x < 2 ? -2//1*x + 3//1 :
           2 ≤ x < 3 ?  1//1*x - 3//1 : 0//1*one(x)
B₄′ = x -> 1 ≤ x < 2 ?  1//1*x - 1//1 :
           2 ≤ x < 3 ? -2//1*x + 5//1 :
           3 ≤ x < 4 ?  1//1*x - 4//1 : 0//1*one(x)
B₅′ = x -> 2 ≤ x < 3 ?  1//1*x - 2//1 :
           3 ≤ x < 4 ? -2//1*x + 7//1 :
           4 ≤ x ≤ 5 ?  1//1*x - 5//1 : 0//1*one(x)
B₆′ = x -> 3 ≤ x < 4 ?  1//1*x -  3//1 :
           4 ≤ x ≤ 5 ? -3//1*x + 13//1 : 0//1*one(x)
B₇′ = x -> 4 ≤ x ≤ 5 ? 2//1*x - 8//1 : 0//1*one(x)
# 2nd derivatives
B₁′′ = x -> 0 ≤ x < 1 ? 2//1*one(x) : 0//1*one(x)
B₂′′ = x -> 0 ≤ x < 1 ? -3//1*one(x) :
            1 ≤ x < 2 ?  1//1*one(x) : 0//1*one(x)
B₃′′ = x -> 0 ≤ x < 1 ?  1//1*one(x) :
            1 ≤ x < 2 ? -2//1*one(x) :
            2 ≤ x < 3 ?  1//1*one(x) : 0//1*one(x)
B₄′′ = x -> 1 ≤ x < 2 ?  1//1*one(x) :
            2 ≤ x < 3 ? -2//1*one(x) :
            3 ≤ x < 4 ?  1//1*one(x) : 0//1*one(x)
B₅′′ = x -> 2 ≤ x < 3 ?  1//1*one(x) :
            3 ≤ x < 4 ? -2//1*one(x) :
            4 ≤ x ≤ 5 ?  1//1*one(x) : 0//1*one(x)
B₆′′ = x -> 3 ≤ x < 4 ?  1//1*one(x) :
            4 ≤ x ≤ 5 ? -3//1*one(x) : 0//1*one(x)
B₇′′ = x -> 4 ≤ x ≤ 5 ? 2//1*one(x) : 0//1*one(x)

b3_exact = [B₁ B₁′ B₁′′; B₂ B₂′ B₂′′; B₃ B₃′ B₃′′; B₄ B₄′ B₄′′; B₅ B₅′ B₅′′; B₆ B₆′ B₆′′; B₇ B₇′ B₇′′]

b4 = BSplineBasis(4, Int128[0,1,2,3,4,5])

B₁ = x -> 0 ≤ x < 1 ? -1//1*x^3 + 3//1*x^2 - 3//1*x + 1//1 : 0//1*one(x)
B₂ = x -> 0 ≤ x < 1 ?  7//4*x^3 - 9//2*x^2 + 3//1*x        :
          1 ≤ x < 2 ? -1//4*x^3 + 3//2*x^2 - 3//1*x + 2//1 : 0//1*one(x)
B₃ = x -> 0 ≤ x < 1 ? -11//12*x^3 + 3//2*x^2                 :
          1 ≤ x < 2 ?   7//12*x^3 - 3//1*x^2 + 9//2*x - 3//2 :
          2 ≤ x < 3 ?   -1//6*x^3 + 3//2*x^2 - 9//2*x + 9//2 : 0//1*one(x)
B₄ = x -> 0 ≤ x < 1 ?  1//6*x^3                      :
          1 ≤ x < 2 ? -1//2*x^3 + 2//1*x^2 -  2//1*x +  2//3 :
          2 ≤ x < 3 ?  1//2*x^3 - 4//1*x^2 + 10//1*x - 22//3 :
          3 ≤ x < 4 ? -1//6*x^3 + 2//1*x^2 -  8//1*x + 32//3 : 0//1*one(x)
B₅ = x -> 1 ≤ x < 2 ?  1//6*x^3 -  1//2*x^2 +  1//2*x -   1//6 :
          2 ≤ x < 3 ? -1//2*x^3 +  7//2*x^2 - 15//2*x +  31//6 :
          3 ≤ x < 4 ?  1//2*x^3 - 11//2*x^2 + 39//2*x - 131//6 :
          4 ≤ x ≤ 5 ? -1//6*x^3 +  5//2*x^2 - 25//2*x + 125//6 : 0//1*one(x)
B₆ = x -> 2 ≤ x < 3 ?   1//6*x^3 -  1//1*x^2 +   2//1*x -    4//3 :
          3 ≤ x < 4 ? -7//12*x^3 + 23//4*x^2 -  73//4*x + 227//12 :
          4 ≤ x ≤ 5 ? 11//12*x^3 - 49//4*x^2 + 215//4*x - 925//12 : 0//1*one(x)
B₇ = x -> 3 ≤ x < 4 ?  1//4*x^3 -  9//4*x^2 +  27//4*x -  27//4 :
          4 ≤ x ≤ 5 ? -7//4*x^3 + 87//4*x^2 - 357//4*x + 485//4 : 0//1*one(x)
B₈ = x -> 4 ≤ x ≤ 5 ? 1//1*x^3 - 12//1*x^2 + 48//1*x - 64//1 : 0//1*one(x)
# 1st derivatives
B₁′ = x -> 0 ≤ x < 1 ? -3//1*x^2 + 6//1*x - 3//1 : 0//1*one(x)
B₂′ = x -> 0 ≤ x < 1 ? 21//4*x^2 - 9//1*x + 3//1 :
           1 ≤ x < 2 ? -3//4*x^2 + 3//1*x - 3//1 : 0//1*one(x)
B₃′ = x -> 0 ≤ x < 1 ? -11//4*x^2 + 3//1*x        :
           1 ≤ x < 2 ?   7//4*x^2 - 6//1*x + 9//2 :
           2 ≤ x < 3 ?  -1//2*x^2 + 3//1*x - 9//2 : 0//1*one(x)
B₄′ = x -> 0 ≤ x < 1 ?  1//2*x^2                  :
           1 ≤ x < 2 ? -3//2*x^2 + 4//1*x -  2//1 :
           2 ≤ x < 3 ?  3//2*x^2 - 8//1*x + 10//1 :
           3 ≤ x < 4 ? -1//2*x^2 + 4//1*x -  8//1 : 0//1*one(x)
B₅′ = x -> 1 ≤ x < 2 ?  1//2*x^2 -  1//1*x +  1//2 :
           2 ≤ x < 3 ? -3//2*x^2 +  7//1*x - 15//2 :
           3 ≤ x < 4 ?  3//2*x^2 - 11//1*x + 39//2 :
           4 ≤ x ≤ 5 ? -1//2*x^2 +  5//1*x - 25//2 : 0//1*one(x)
B₆′ = x -> 2 ≤ x < 3 ?  1//2*x^2 -  2//1*x +   2//1 :
           3 ≤ x < 4 ? -7//4*x^2 + 23//2*x -  73//4 :
           4 ≤ x ≤ 5 ? 11//4*x^2 - 49//2*x + 215//4 : 0//1*one(x)
B₇′ = x -> 3 ≤ x < 4 ?   3//4*x^2 -  9//2*x +  27//4 :
           4 ≤ x ≤ 5 ? -21//4*x^2 + 87//2*x - 357//4 : 0//1*one(x)
B₈′ = x -> 4 ≤ x ≤ 5 ? 3//1*x^2 - 24//1*x + 48//1 : 0//1*one(x)
# 2nd derivatives
B₁′′ = x -> 0 ≤ x < 1 ? -6//1*x + 6//1 : 0//1*one(x)
B₂′′ = x -> 0 ≤ x < 1 ? 21//2*x - 9//1 :
            1 ≤ x < 2 ? -3//2*x + 3//1 : 0//1*one(x)
B₃′′ = x -> 0 ≤ x < 1 ? -11//2*x + 3//1 :
            1 ≤ x < 2 ?   7//2*x - 6//1 :
            2 ≤ x < 3 ?  -1//1*x + 3//1 : 0//1*one(x)
B₄′′ = x -> 0 ≤ x < 1 ?  1//1*x        :
            1 ≤ x < 2 ? -3//1*x + 4//1 :
            2 ≤ x < 3 ?  3//1*x - 8//1 :
            3 ≤ x < 4 ? -1//1*x + 4//1 : 0//1*one(x)
B₅′′ = x -> 1 ≤ x < 2 ?  1//1*x -  1//1 :
            2 ≤ x < 3 ? -3//1*x +  7//1 :
            3 ≤ x < 4 ?  3//1*x - 11//1 :
            4 ≤ x ≤ 5 ? -1//1*x +  5//1 : 0//1*one(x)
B₆′′ = x -> 2 ≤ x < 3 ?  1//1*x -  2//1 :
            3 ≤ x < 4 ? -7//2*x + 23//2 :
            4 ≤ x ≤ 5 ? 11//2*x - 49//2 : 0//1*one(x)
B₇′′ = x -> 3 ≤ x < 4 ?   3//2*x -  9//2 :
            4 ≤ x ≤ 5 ? -21//2*x + 87//2 : 0//1*one(x)
B₈′′ = x -> 4 ≤ x ≤ 5 ? 6//1*x - 24//1 : 0//1*one(x)
# 3rd derivatives
B₁′′′ = x -> 0 ≤ x < 1 ? -6//1 : 0//1*one(x)
B₂′′′ = x -> 0 ≤ x < 1 ? 21//2 :
             1 ≤ x < 2 ? -3//2 : 0//1*one(x)
B₃′′′ = x -> 0 ≤ x < 1 ? -11//2 :
             1 ≤ x < 2 ?   7//2 :
             2 ≤ x < 3 ?  -1//1 : 0//1*one(x)
B₄′′′ = x -> 0 ≤ x < 1 ?  1//1 :
             1 ≤ x < 2 ? -3//1 :
             2 ≤ x < 3 ?  3//1 :
             3 ≤ x < 4 ? -1//1 : 0//1*one(x)
B₅′′′ = x -> 1 ≤ x < 2 ?  1//1 :
             2 ≤ x < 3 ? -3//1 :
             3 ≤ x < 4 ?  3//1 :
             4 ≤ x ≤ 5 ? -1//1 : 0//1*one(x)
B₆′′′ = x -> 2 ≤ x < 3 ?  1//1 :
             3 ≤ x < 4 ? -7//2 :
             4 ≤ x ≤ 5 ? 11//2 : 0//1*one(x)
B₇′′′ = x -> 3 ≤ x < 4 ?   3//2 :
             4 ≤ x ≤ 5 ? -21//2 : 0//1*one(x)
B₈′′′ = x -> 4 ≤ x ≤ 5 ? 6//1 : 0//1*one(x)

b4_exact = [B₁ B₁′ B₁′′ B₁′′′; B₂ B₂′ B₂′′ B₂′′′; B₃ B₃′ B₃′′ B₃′′′; B₄ B₄′ B₄′′ B₄′′′;
            B₅ B₅′ B₅′′ B₅′′′; B₆ B₆′ B₆′′ B₆′′′; B₇ B₇′ B₇′′ B₇′′′; B₈ B₈′ B₈′′ B₈′′′]

b5 = BSplineBasis(3, [-5.0, -3.5, -2.2, 1.0, 2.0, 3.6])

# The exact B-spline functions are for the breakpoint sequence [-5, -7//2, -11//5, 1, 2, 18//5].
# Note that this is not exactly equal to [-5.0, -3.5, -2.2, 1.0, 2.0, 3.6], because the
# floating-point representations of -11//5 and 18//5 are not exact.
B₁ = x -> -5.0 ≤ x < -3.5 ? 4//9*x^2 + 28//9*x + 49//9 : 0//1*one(x)
B₂ = x -> -5.0 ≤ x < -3.5 ? -43//63*x^2 - 346//63*x - 655//63 :
          -3.5 ≤ x < -2.2 ?  25//91*x^2 + 110//91*x + 121//91 : 0//1*one(x)
B₃ = x -> -5.0 ≤ x < -3.5 ?     5//21*x^2 +    50//21*x +   125//21 :
          -3.5 ≤ x < -2.2 ? -365//819*x^2 - 1970//819*x - 1985//819 :
          -2.2 ≤ x <  1.0 ?     5//72*x^2 -     5//36*x +     5//72 : 0//1*one(x)
B₄ = x -> -3.5 ≤ x < -2.2 ?    20//117*x^2 + 140//117*x +  245//117 :
          -2.2 ≤ x <  1.0 ? -145//1008*x^2 -  95//504*x + 575//1008 :
           1.0 ≤ x <  2.0 ?      5//21*x^2 -   20//21*x +    20//21 : 0//1*one(x)
B₅ = x -> -2.2 ≤ x < 1.0 ?   25//336*x^2 +  55//168*x + 121//336 :
           1.0 ≤ x < 2.0 ? -170//273*x^2 + 470//273*x -  92//273 :
           2.0 ≤ x ≤ 3.6 ?   25//104*x^2 -   45//26*x +   81//26 : 0//1*one(x)
B₆ = x ->  1.0 ≤ x < 2.0 ?     5//13*x^2 -   10//13*x +    5//13 :
           2.0 ≤ x ≤ 3.6 ? -525//832*x^2 + 685//208*x - 765//208 : 0//1*one(x)
B₇ = x ->  2.0 ≤ x ≤ 3.6 ? 25//64*x^2 - 25//16*x + 25//16 : 0//1*one(x)
# 1st derivatives
B₁′ = x -> -5.0 ≤ x < -3.5 ? 8//9*x + 28//9 : 0//1*one(x)
B₂′ = x -> -5.0 ≤ x < -3.5 ? -86//63*x - 346//63 :
           -3.5 ≤ x < -2.2 ?  50//91*x + 110//91 : 0//1*one(x)
B₃′ = x -> -5.0 ≤ x < -3.5 ?    10//21*x +    50//21 :
           -3.5 ≤ x < -2.2 ? -730//819*x - 1970//819 :
           -2.2 ≤ x <  1.0 ?     5//36*x -     5//36 : 0//1*one(x)
B₄′ = x -> -3.5 ≤ x < -2.2 ?   40//117*x + 140//117 :
           -2.2 ≤ x <  1.0 ? -145//504*x -  95//504 :
            1.0 ≤ x <  2.0 ?    10//21*x -   20//21 : 0//1*one(x)
B₅′ = x -> -2.2 ≤ x < 1.0 ?   25//168*x +  55//168 :
            1.0 ≤ x < 2.0 ? -340//273*x + 470//273 :
            2.0 ≤ x ≤ 3.6 ?    25//52*x -   45//26 : 0//1*one(x)
B₆′ = x ->  1.0 ≤ x < 2.0 ?    10//13*x -   10//13 :
            2.0 ≤ x ≤ 3.6 ? -525//416*x + 685//208 : 0//1*one(x)
B₇′ = x ->  2.0 ≤ x ≤ 3.6 ? 25//32*x - 25//16 : 0//1*one(x)
# 2nd derivatives
B₁′′ = x -> -5.0 ≤ x < -3.5 ? 8//9*one(x) : 0//1*one(x)
B₂′′ = x -> -5.0 ≤ x < -3.5 ? -86//63*one(x) :
            -3.5 ≤ x < -2.2 ?  50//91*one(x) : 0//1*one(x)
B₃′′ = x -> -5.0 ≤ x < -3.5 ?    10//21*one(x) :
            -3.5 ≤ x < -2.2 ? -730//819*one(x) :
            -2.2 ≤ x <  1.0 ?     5//36*one(x) : 0//1*one(x)
B₄′′ = x -> -3.5 ≤ x < -2.2 ?   40//117*one(x) :
            -2.2 ≤ x <  1.0 ? -145//504*one(x) :
             1.0 ≤ x <  2.0 ?    10//21*one(x) : 0//1*one(x)
B₅′′ = x -> -2.2 ≤ x < 1.0 ?   25//168*one(x) :
             1.0 ≤ x < 2.0 ? -340//273*one(x) :
             2.0 ≤ x ≤ 3.6 ?    25//52*one(x) : 0//1*one(x)
B₆′′ = x ->  1.0 ≤ x < 2.0 ?    10//13*one(x) :
             2.0 ≤ x ≤ 3.6 ? -525//416*one(x) : 0//1*one(x)
B₇′′ = x ->  2.0 ≤ x ≤ 3.6 ? 25//32*one(x) : 0//1*one(x)

b5_exact = [B₁ B₁′ B₁′′; B₂ B₂′ B₂′′; B₃ B₃′ B₃′′; B₄ B₄′ B₄′′; B₅ B₅′ B₅′′; B₆ B₆′ B₆′′; B₇ B₇′ B₇′′]

b6 = BSplineBasis(4, Rational{Int64}[-5//1, -7//2, -11//5, 1//1, 2//1, 18//5])

B₁ = x ->  -5//1 ≤ x <  -7//2 ? -8//27*x^3 - 28//9*x^2 - 98//9*x - 343//27 : 0//1*one(x)
B₂ = x ->  -5//1 ≤ x <  -7//2 ? 1429//2646*x^3 + 5339//882*x^2 + 19429//882*x + 69635//2646 :
           -7//2 ≤ x < -11//5 ? -125//1274*x^3 - 825//1274*x^2 - 1815//1274*x -  1331//1274 : 0//1*one(x)
B₃ = x ->  -5//1 ≤ x <  -7//2 ?   -125//441*x^3 -   520//147*x^2 -   2075//147*x -    7750//441 :
           -7//2 ≤ x < -11//5 ? 2965//17199*x^3 + 7160//5733*x^2 + 15115//5733*x + 33890//17199 :
          -11//5 ≤ x <   1//1 ?     -5//432*x^3 +     5//144*x^2 -      5//144*x +       5//432 : 0//1*one(x)
B₄ = x ->  -5//1 ≤ x <  -7//2 ?       5//126*x^3 +       25//42*x^2 +      125//42*x +     625//126 :
           -7//2 ≤ x < -11//5 ? -5695//54054*x^3 - 16715//18018*x^2 - 42415//18018*x - 68015//54054 :
          -11//5 ≤ x <   1//1 ?  1255//33264*x^3 +   185//11088*x^2 -  3065//11088*x +  8825//33264 :
            1//1 ≤ x <   2//1 ?     -10//231*x^3 +       20//77*x^2 -       40//77*x +      80//231 : 0//1*one(x)
B₅ = x ->  -7//2 ≤ x < -11//5 ?       40//1287*x^3 +      140//429*x^2 +      490//429*x +     1715//1287 :
          -11//5 ≤ x <   1//1 ? -12535//321552*x^3 - 14585//107184*x^2 + 13385//107184*x + 188599//321552 :
            1//1 ≤ x <   2//1 ?   13120//87087*x^3 -  20465//29029*x^2 +  20140//29029*x +   34564//87087 :
            2//1 ≤ x ≤  18//5 ?     -125//3016*x^3 +     675//1508*x^2 -     1215//754*x +       729//377 : 0//1*one(x)
B₆ = x -> -11//5 ≤ x <   1//1 ?      125//9744*x^3 +      275//3248*x^2 +     605//3248*x +    1331//9744 :
            1//1 ≤ x <   2//1 ? -26275//102921*x^3 +   30500//34307*x^2 -  21205//34307*x + 41654//102921 :
            2//1 ≤ x ≤  18//5 ?  89125//313664*x^3 - 368175//156832*x^2 + 459135//78416*x - 153333//39208 : 0//1*one(x)
B₇ = x ->   1//1 ≤ x <   2//1 ?       25//169*x^3 -       75//169*x^2 +       75//169*x -      25//169 :
            2//1 ≤ x <  18//5 ? -42125//86528*x^3 + 145575//43264*x^2 - 155175//21632*x + 53325//10816 : 0//1*one(x)
B₈ = x ->   2//1 ≤ x ≤  18//5 ? 125//512*x^3 - 375//256*x^2 + 375//128*x - 125//64 : 0//1*one(x)
# 1st derivatives
B₁′ = x ->  -5//1 ≤ x <  -7//2 ? -8//9*x^2 - 56//9*x - 98//9 : 0//1*one(x)
B₂′ = x ->  -5//1 ≤ x <  -7//2 ?  1429//882*x^2 + 5339//441*x + 19429//882 :
            -7//2 ≤ x < -11//5 ? -375//1274*x^2 -  825//637*x - 1815//1274 : 0//1*one(x)
B₃′ = x ->  -5//1 ≤ x <  -7//2 ?  -125//147*x^2 -   1040//147*x -   2075//147 :
            -7//2 ≤ x < -11//5 ? 2965//5733*x^2 + 14320//5733*x + 15115//5733 :
           -11//5 ≤ x <   1//1 ?    -5//144*x^2 +       5//72*x -      5//144 : 0//1*one(x)
B₄′ = x ->  -5//1 ≤ x <  -7//2 ?        5//42*x^2 +      25//21*x +      125//42 :
            -7//2 ≤ x < -11//5 ? -5695//18018*x^2 - 16715//9009*x - 42415//18018 :
           -11//5 ≤ x <   1//1 ?  1255//11088*x^2 +   185//5544*x -  3065//11088 :
             1//1 ≤ x <   2//1 ?      -10//77*x^2 +      40//77*x -       40//77 : 0//1*one(x)
B₅′ = x ->  -7//2 ≤ x < -11//5 ?        40//429*x^2 +     280//429*x +      490//429 :
           -11//5 ≤ x <   1//1 ? -12535//107184*x^2 - 14585//53592*x + 13385//107184 :
             1//1 ≤ x <   2//1 ?   13120//29029*x^2 - 40930//29029*x +  20140//29029 :
             2//1 ≤ x ≤  18//5 ?     -375//3016*x^2 +     675//754*x -     1215//754 : 0//1*one(x)
B₆′ = x -> -11//5 ≤ x <   1//1 ?      125//3248*x^2 +     275//1624*x +     605//3248 :
             1//1 ≤ x <   2//1 ?  -26275//34307*x^2 +  61000//34307*x -  21205//34307 :
             2//1 ≤ x ≤  18//5 ? 267375//313664*x^2 - 368175//78416*x + 459135//78416 : 0//1*one(x)
B₇′ = x ->   1//1 ≤ x <   2//1 ?        75//169*x^2 -      150//169*x +       75//169 :
             2//1 ≤ x ≤  18//5 ? -126375//86528*x^2 + 145575//21632*x - 155175//21632 : 0//1*one(x)
B₈′ = x ->   2//1 ≤ x ≤  18//5 ? 375//512*x^2 - 375//128*x + 375//128 : 0//1*one(x)
# 2nd derivatives
B₁′′ = x ->  -5//1 ≤ x <  -7//2 ? -16//9*x - 56//9 : 0//1*one(x)
B₂′′ = x ->  -5//1 ≤ x <  -7//2 ? 1429//441*x + 5339//441 :
             -7//2 ≤ x < -11//5 ? -375//637*x -  825//637 : 0//1*one(x)
B₃′′ = x ->  -5//1 ≤ x <  -7//2 ?  -250//147*x -   1040//147 :
             -7//2 ≤ x < -11//5 ? 5930//5733*x + 14320//5733 :
            -11//5 ≤ x <   1//1 ?     -5//72*x +       5//72 : 0//1*one(x)
B₄′′ = x ->  -5//1 ≤ x <  -7//2 ?       5//21*x +      25//21 :
             -7//2 ≤ x < -11//5 ? -5695//9009*x - 16715//9009 :
            -11//5 ≤ x <   1//1 ?  1255//5544*x +   185//5544 :
              1//1 ≤ x <   2//1 ?     -20//77*x +      40//77 : 0//1*one(x)
B₅′′ = x ->  -7//2 ≤ x < -11//5 ?       80//429*x +     280//429 :
            -11//5 ≤ x <   1//1 ? -12535//53592*x - 14585//53592 :
              1//1 ≤ x <   2//1 ?  26240//29029*x - 40930//29029 :
              2//1 ≤ x ≤  18//5 ?    -375//1508*x +     675//754 : 0//1*one(x)
B₆′′ = x -> -11//5 ≤ x <   1//1 ?      125//1624*x +     275//1624 :
              1//1 ≤ x <   2//1 ?  -52550//34307*x +  61000//34307 :
              2//1 ≤ x ≤  18//5 ? 267375//156832*x - 368175//78416 : 0//1*one(x)
B₇′′ = x ->   1//1 ≤ x <   2//1 ?       150//169*x -      150//169 :
              2//1 ≤ x ≤  18//5 ? -126375//43264*x + 145575//21632 : 0//1*one(x)
B₈′′ = x ->   2//1 ≤ x ≤  18//5 ? 375//256*x - 375//128 : 0//1*one(x)
# 3rd derivative
B₁′′′ = x ->  -5//1 ≤ x <  -7//2 ? -16//9*one(x) : 0//1*one(x)
B₂′′′ = x ->  -5//1 ≤ x <  -7//2 ? 1429//441*one(x) :
              -7//2 ≤ x < -11//5 ? -375//637*one(x) : 0//1*one(x)
B₃′′′ = x ->  -5//1 ≤ x <  -7//2 ?  -250//147*one(x) :
              -7//2 ≤ x < -11//5 ? 5930//5733*one(x) :
             -11//5 ≤ x <   1//1 ?     -5//72*one(x) : 0//1*one(x)
B₄′′′ = x ->  -5//1 ≤ x <  -7//2 ?       5//21*one(x) :
              -7//2 ≤ x < -11//5 ? -5695//9009*one(x) :
             -11//5 ≤ x <   1//1 ?  1255//5544*one(x) :
               1//1 ≤ x <   2//1 ?     -20//77*one(x) : 0//1*one(x)
B₅′′′ = x ->  -7//2 ≤ x < -11//5 ?       80//429*one(x) :
             -11//5 ≤ x <   1//1 ? -12535//53592*one(x) :
               1//1 ≤ x <   2//1 ?  26240//29029*one(x) :
               2//1 ≤ x ≤  18//5 ?    -375//1508*one(x) : 0//1*one(x)
B₆′′′ = x -> -11//5 ≤ x <   1//1 ?      125//1624*one(x) :
               1//1 ≤ x <   2//1 ?  -52550//34307*one(x) :
               2//1 ≤ x ≤  18//5 ? 267375//156832*one(x) : 0//1*one(x)
B₇′′′ = x ->   1//1 ≤ x <   2//1 ?       150//169*one(x) :
               2//1 ≤ x ≤  18//5 ? -126375//43264*one(x) : 0//1*one(x)
B₈′′′ = x ->   2//1 ≤ x ≤  18//5 ? 375//256*one(x) : 0//1*one(x)

b6_exact = [B₁ B₁′ B₁′′ B₁′′′; B₂ B₂′ B₂′′ B₂′′′; B₃ B₃′ B₃′′ B₃′′′; B₄ B₄′ B₄′′ B₄′′′;
            B₅ B₅′ B₅′′ B₅′′′; B₆ B₆′ B₆′′ B₆′′′; B₇ B₇′ B₇′′ B₇′′′; B₈ B₈′ B₈′′ B₈′′′]

b7 = BSplineBasis(3, Int8[-6,-4,-2,0,0,1,2])

B₁ = x -> -6 ≤ x < -4 ? 1//4*x^2 + 2//1*x + 4//1 : 0//1*one(x)
B₂ = x -> -6 ≤ x < -4 ? -3//8*x^2 - 7//2*x - 15//2 :
          -4 ≤ x < -2 ?  1//8*x^2 + 1//2*x +  1//2 : 0//1*one(x)
B₃ = x -> -6 ≤ x < -4 ?  1//8*x^2 + 3//2*x + 9//2 :
          -4 ≤ x < -2 ? -1//4*x^2 - 3//2*x - 3//2 :
          -2 ≤ x <  0 ?  1//8*x^2                 : 0//1*one(x)
B₄ = x -> -4 ≤ x < -2 ?  1//8*x^2 + 1//1*x + 2//1 :
          -2 ≤ x <  0 ? -3//8*x^2 - 1//1*x        : 0//1*one(x)
B₅ = x -> -2 ≤ x <  0 ? 1//4*x^2 + 1//1*x + 1//1 :
           0 ≤ x <  1 ? 1//1*x^2 - 2//1*x + 1//1 : 0//1*one(x)
B₆ = x ->  0 ≤ x <  1 ? -3//2*x^2 + 2//1*x        :
           1 ≤ x ≤  2 ?  1//2*x^2 - 2//1*x + 2//1 : 0//1*one(x)
B₇ = x ->  0 ≤ x <  1 ?  1//2*x^2                 :
           1 ≤ x ≤  2 ? -3//2*x^2 + 4//1*x - 2//1 : 0//1*one(x)
B₈ = x ->  1 ≤ x ≤  2 ? 1//1*x^2 - 2//1*x + 1//1 : 0//1*one(x)
# 1st derivatives
B₁′ = x -> -6 ≤ x < -4 ? 1//2*x + 2//1 : 0//1*one(x)
B₂′ = x -> -6 ≤ x < -4 ? -3//4*x - 7//2 :
           -4 ≤ x < -2 ?  1//4*x + 1//2 : 0//1*one(x)
B₃′ = x -> -6 ≤ x < -4 ?  1//4*x + 3//2 :
           -4 ≤ x < -2 ? -1//2*x - 3//2 :
           -2 ≤ x <  0 ?  1//4*x        : 0//1*one(x)
B₄′ = x -> -4 ≤ x < -2 ?  1//4*x + 1//1 :
           -2 ≤ x <  0 ? -3//4*x - 1//1 : 0//1*one(x)
B₅′ = x -> -2 ≤ x <  0 ? 1//2*x + 1//1 :
            0 ≤ x <  1 ? 2//1*x - 2//1 : 0//1*one(x)
B₆′ = x ->  0 ≤ x <  1 ? -3//1*x + 2//1 :
            1 ≤ x ≤  2 ?  1//1*x - 2//1 : 0//1*one(x)
B₇′ = x ->  0 ≤ x <  1 ?  1//1*x        :
            1 ≤ x ≤  2 ? -3//1*x + 4//1 : 0//1*one(x)
B₈′ = x ->  1 ≤ x ≤  2 ? 2//1*x - 2//1 : 0//1*one(x)
# 2nd derivatives
B₁′′ = x -> -6 ≤ x < -4 ? 1//2*one(x) : 0//1*one(x)
B₂′′ = x -> -6 ≤ x < -4 ? -3//4*one(x) :
            -4 ≤ x < -2 ?  1//4*one(x) : 0//1*one(x)
B₃′′ = x -> -6 ≤ x < -4 ?  1//4*one(x) :
            -4 ≤ x < -2 ? -1//2*one(x) :
            -2 ≤ x <  0 ?  1//4*one(x) : 0//1*one(x)
B₄′′ = x -> -4 ≤ x < -2 ?  1//4*one(x) :
            -2 ≤ x <  0 ? -3//4*one(x) : 0//1*one(x)
B₅′′ = x -> -2 ≤ x <  0 ? 1//2*one(x) :
             0 ≤ x <  1 ? 2//1*one(x) : 0//1*one(x)
B₆′′ = x ->  0 ≤ x <  1 ? -3//1*one(x) :
             1 ≤ x ≤  2 ?  1//1*one(x) : 0//1*one(x)
B₇′′ = x ->  0 ≤ x <  1 ?  1//1*one(x) :
             1 ≤ x ≤  2 ? -3//1*one(x) : 0//1*one(x)
B₈′′ = x ->  1 ≤ x ≤  2 ? 2//1*one(x) : 0//1*one(x)

b7_exact = [B₁ B₁′ B₁′′; B₂ B₂′ B₂′′; B₃ B₃′ B₃′′; B₄ B₄′ B₄′′; B₅ B₅′ B₅′′; B₆ B₆′ B₆′′; B₇ B₇′ B₇′′; B₈ B₈′ B₈′′]

# Compare arrays element-wise, and use a non-zero atol
≈ₑₗ(x::AbstractArray, y::AbstractArray) = all(x .≈ₑₗ y)
≈ₑₗ(x::Number, y::Number) = isapprox(x, y; atol=1e-15, rtol = Base.rtoldefault(x,y,0))


let
    n = 5
    tf = 5
    t = collect(0:(tf/n):tf)
    f(t) = sin.(t)
    fdot(t) = cos.(t)

    x = f(t)
    spline = CubicSpline(t,x)
    interpolate(spline, t[1])
    deriv(spline, t[1])
    hess(spline, t[1])
    interpolate(spline, [t[1], t[end]])
    deriv(spline, [t[1], t[end]])
    hess(spline, [t[1], t[end]])

    dt = 1.0
    @test extrapolate(spline, t[end]+dt, 0)[1] == interpolate(spline, t[end])
    @test extrapolate(spline, t[end]+dt, 1)[1] == interpolate(spline, t[end]) + deriv(spline, t[end])*dt
    @test extrapolate(spline, t[1]-dt, 0)[1] == interpolate(spline, t[1])
    @test extrapolate(spline, t[1]-dt, 1)[1] == interpolate(spline, t[1]) - deriv(spline, t[1])*dt

    xdot = fdot(t)
    spline2 = CubicSpline(t,x,xdot) # prescribe knot tangents directly

    # plt = plot(t_vec, interpolate(spline, t_vec))
    # plot!(plt, t_vec, interpolate(spline2, t_vec))
    # plot!(plt, t_vec, f(t_vec))
    # plot!(plt, t, x, seriestype=:scatter)
end
let
    n = 10
    t = collect(0:(1/10):1)
    x = sin.(t)
    y = sin.(t)
    X = hcat(x,y)
    spline = ParametricSpline(t,X)
    interpolate(spline, t[1])
    deriv(spline, t[1])
    hess(spline, t[1])
    interpolate(spline, [t[1], t[end]])
    deriv(spline, [t[1], t[end]])
    hess(spline, [t[1], t[end]])
end
# test turn rate and tangent rate
let
    # circle in the plane
    tvec = collect(0:.01:2π)
    x = cos.(tvec)
    y = sin.(tvec)
    dx = -y
    dy = x
    ddx = -x
    ddy = -y
    z = zeros(length(tvec))
    dz = z
    ddz = z
    r = collect(hcat(x,y,z))
    dr = collect(hcat(dx,dy,dz))
    ddr = collect(hcat(ddx,ddy,ddz))
    spline = ParametricSpline(tvec,r,dr)

    # @show tangent_rate(spline,0.0)
    # @show tangent_rate(spline,π/4)
    #
    # tvec2 = (1/2)*(tvec[2:end].+tvec[1:end-1])
    # pts = interpolate(spline,tvec2)
    # plt = plot(map(pt->pt[1],pts),map(pt->pt[2],pts),aspect_ratio=:equal)
    # pts = deriv(spline,tvec2)
    # plot!(plt,map(pt->pt[1],pts),map(pt->pt[2],pts),aspect_ratio=:equal)
    # pts = hess(spline,tvec2)
    # plot(map(pt->pt[1],pts),map(pt->pt[2],pts),aspect_ratio=:equal)
end


# Benchmark script for comparing the convergence rate of GD, BiasSEGA, and SEGA
# for minimizing a multivariate quadratic on the unit ball

using Random, LinearAlgebra

function main(n=10, γ=1/(2n), niterations=30)

    Random.seed!(123)
    Q = Symmetric(randn(n, n))
    c = randn(n)
    f = (x) -> x'*Q*x ./ 2 .+ c'*x
    ∇ = (x) -> Q*x .+ c

    xgd = zeros(n, niterations)
    xbias = zeros(n, niterations)
    xsega = zeros(n, niterations)

    bsega = BiasSEGA(n)
    sega = SEGA(n)

    s = zeros(n)
    for i in 2:niterations
        # GD
        xgd[:, i] .= xgd[:, i-1] .- γ.*∇(xgd[:, i-1])
        xgd[:, i] ./= norm(xgd[:, i])

        # BiasSEGA
        s .= randn(n)
        project!(bsega, dot(s, ∇(xbias[:, i-1])), s)
        xbias[:, i] .= xbias[:, i-1] .- γ.*gradient(bsega)
        xbias[:, i] ./= norm(xbias[:, i])

        # SEGA
        project!(sega, 1/n, dot(s, ∇(xsega[:, i-1])), s)
        unbias!(sega)
        xsega[:, i] .= xsega[:, i-1] .- γ.*gradient(sega)
        xsega[:, i] ./= norm(xsega[:, i])
    end

    println("GD \t BiasSEGA \t SEGA")
    for i in 2:niterations
        println("$(f(xgd[:, i])) \t $(f(xbias[:, i])) \t $(f(xsega[:, i]))")
    end
end


using Test

using CompScienceMeshes
using BEAST
using StaticArrays

ω = 1.0

l1 = meshsegment(1.0,1/2)
l2 = meshsegment(1.0,1/4)

idcs = BEAST.interior_and_junction_vertices(l1, boundary(l1))
@test length(idcs) == 3

lag1 = lagrangec0d1(l1, boundary(l1))
lag2 = lagrangecxd0(l2)

id = Identity()
G = assemble(id, lag1, lag2)
H = assemble(id, lag2, lag1)

Gt = [
    3 1 0 0
    1 3 3 1
    0 0 1 3] // 16

@test norm(G-H') ≈ 0.0  atol = sqrt(eps()) #G == H'
@test norm(G-Gt) ≈ 0.0  atol = sqrt(eps())

# Test whether localoperator and localoperator 2 give the same result
# for test and basis functions defined on the same mesh.
id = Identity()
nc = NCross()

#fn = Pkg.dir("BEAST","test","sphere2.in")
fn = joinpath(dirname(@__FILE__),"assets","sphere316.in")
m = readmesh(fn)
rt = raviartthomas(m)

G1 = zeros(ComplexF64, numfunctions(rt), numfunctions(rt))
BEAST.assemble_local_matched!(id, rt, rt, (v,m,n)->(G1[m,n]+=v))

G2 = zeros(ComplexF64, numfunctions(rt), numfunctions(rt))
BEAST.assemble_local_mixed!(id, rt, rt, (v,m,n)->(G2[m,n]+=v))

# Test wether the gram matrix assembled works for
# different but overlapping meshes
m1 = meshrectangle(1.0,1.0,1.0)
m2 = translate(m1, point(0.5,0.5,0.0))
I = Identity()
x1 = lagrangecxd0(m1)
x2 = lagrangecxd0(m2)
G = assemble(I,x1,x2)
@test sum(G) ≈ 1/4


export DS, run_lt_test
const DS = DirectSearch

#= Following functions sourced from https://www.sfu.ca/~ssurjano/optimization.html =#
#Valley
#soln: -1.0316 @ [±0.0898, ∓0.7126]
camel6(x)=(4-2.1x[1]^2.0 + x[1]^4/3)x[1]^2.0 + x[1]x[2] + (-4 + 4x[2]^2.0)x[2]^2.0
#soln: 0 @ [1,...,1]
rosenbrock(x;a=1,b=100,d=length(x)) = sum([b*(x[i+1] - x[i]^2)^2 + (x[i]-a)^2 for i=1:d-1])

#Local Minima
ackley(x;a=20,b=0.2,c=2pi,d=length(x)) = -a * exp(-b*√(sum(x.^2)/d)) - exp(sum(cos.(c.*x))/d) + a + exp(1)
eggholder(x) = -(x[2]+47)sin(√abs(x[2] + x[1]/2 + 47)) - x[1]sin(√abs(x[1]-x[2]+47))

#Example Benchmarks from papers
#Audet & Dennis 2006 benchmark one, take x₀=[-2.1,1.7]
#soln: 0 @ [0,0]
bm_1(x;c=[30;40],d=[-30;-40]) = (1-exp(-norm(x)^2)) * max(norm(x-c)^2, norm(x-d)^2)

#Audet & Dennis 2006 benchmark three, should take x₀=[0,...,0]
#soln: -√(3)n @ [-√3,...,-√3]
bm_3(x)=sum(x)
bm_3_con(x)=sum(x.^2)-3*length(x)

function run_lt_test(;f=camel6,n=2,
                     constraints::Vector=[],
                     initial=zeros(Float64,n),
                     lim=1000,
                     lb=-5*ones(Float64,n),
                     ub=5*ones(Float64,n),
                     run=true)
    p = DSProblem(n)
    SetObjective(p, f)
    SetInitialPoint(p, initial)
    SetMaxEvals(p, 1)
    for c in constraints
        AddExtremeConstraint(p, c)
    end
    p.iteration_limit = lim
    SetVariableRanges(p, lb, ub)
    if run
        DS.Optimize!(p)
        println("$(p.iteration)\t$(p.x)\t$(p.x_cost)")
    end
    return p
end


### A Pluto.jl notebook ###
# v0.17.0

using Markdown
using InteractiveUtils

# ╔═╡ 0a57947c-3a56-11ec-1341-534834850daf
md"""
# Chapter 1
## Section 3
"""

# ╔═╡ 8fcfe29e-049c-4df3-9d4b-325d3f4e171e
function newton(f, fp, x0, tol)
	x = x0
	while abs(f(x)) > tol
		x = x - f(x) / fp(x)
	end
	x
end

# ╔═╡ 74555744-e6a7-47bd-96b9-7e8909664f42
newton(x -> x^2 - 5., x -> 2. * x, 1., 10e-6)

# ╔═╡ 40d3098f-4b1b-4ca1-be89-5eb9a18ef3a8
function secant(f, x0, x1, tol)
	x = x0
	xx = x1
	while abs(f(xx)) > tol
		tmp = xx
		xx = xx - f(xx) * (xx - x) / (f(xx) - f(x))
		x = xx
	end
	xx
end

# ╔═╡ 7d023bfd-53fb-46a1-84dc-1c34519387dc
secant(x -> x^2 - 5., 2, 2.3, 10e-6)
# Well ... I'm definitly not got at finding roots

# ╔═╡ Cell order:
# ╟─0a57947c-3a56-11ec-1341-534834850daf
# ╠═8fcfe29e-049c-4df3-9d4b-325d3f4e171e
# ╠═74555744-e6a7-47bd-96b9-7e8909664f42
# ╠═40d3098f-4b1b-4ca1-be89-5eb9a18ef3a8
# ╠═7d023bfd-53fb-46a1-84dc-1c34519387dc


"""
Cosine taper a N-dimensional array along given dimensions.

# Arguments
* `x::Array{Float64,N}` :
* `perc::Float64` : taper percentage
"""
function taper(x::AbstractArray,perc::Float64;dims=[1])
	xout=copy(x);
	taper!(xout,perc,dims=dims)
	return xout
end

"""
# no allocations
* `dims=[1]` : taper along these dimensions (default only first)
"""
function taper!(x, perc=0.0; bperc=perc, eperc=perc, dims=[1])
	for dim in dims
		nt=size(x,dim)
		nttb=min(round(Int,nt*bperc*0.01), nt)
		ntte=min(round(Int,nt*eperc*0.01), nt)
		kb=inv(2.0*round(Int,nt*bperc*0.01)-1)*pi
		ke=inv(2.0*round(Int,nt*eperc*0.01)-1)*pi
		for i in CartesianIndices(size(x))
			if(1≤i[dim]≤nttb)
				x[i] *= sin((i[dim]-1)*kb)
			end
			if(nt-ntte+1≤i[dim]≤nt)
				x[i] *= sin((-i[dim]+nt)*ke)
			end
		end
	end
end

"""
Tapering is necessary to be able to input random signal into finite-difference code
Filtering tapering are applied only if the length of the time series is greater than 10
"""
function get_tapered_random_tmax_signal(tgrid;
					fmin=nothing,
					fmax=nothing,
					tmaxfrac::Float64=1.0,
					dist=Uniform(-2.0, 2.0),
					sparsep=1.0,
					taperperc=20.
					)
	filt_flag=(length(tgrid) > 5) && (!(fmin === nothing)) && (!(fmax===nothing))

	fs = inv(step(tgrid));
	if(filt_flag)
		designmethod = Butterworth(6);
		filtsource = Bandpass(fmin, fmax; fs=fs);
	end

	itind = argmin(abs.(tgrid.-abs(tmaxfrac)*tgrid[end]))
	if(tmaxfrac>0.0)
		its=1:itind
	elseif(tmaxfrac<0.0)
		its=itind:length(tgrid)
	end
	# 20% taper window
	twin = taper(ones(length(its)),taperperc)
	X = zeros(length(its))
	wavsrc = zeros(length(tgrid))
	if(filt_flag)
		X[:] = rand(dist, length(its)) .* twin
	else
		X[:] = rand(dist, length(its))
	end
	if(sparsep ≠ 1.0)
		Xs=sprandn(length(X), sparsep)
		X[findall(Xs.==0.0)].=0.0
	end
	# band limit
	(filt_flag) && (filt!(X, digitalfilter(filtsource, designmethod), X))

	(length(X) ≠ 1) && normalize!(X)
	wavsrc[its] = X
	return wavsrc
end


loadcrabs(filename) = parse.(Int, split(readchomp(filename), ','))

if basename(pwd()) == "aoc"
    cd("2021/7")
end
examplecrabs = loadcrabs("example.txt")
inputcrabs = loadcrabs("input.txt")

using Statistics
part1(crabs) = sum(abs.(crabs .- median(crabs)))
part1(inputcrabs) # 331067

triangle(n) = n * (n + 1) ÷ 2
offset(a, b) = triangle(abs(a - b))

function part2(crabs)
    minimum(sum(offset(x, n) for x in crabs) for n in range(extrema(crabs)...))
end


function distances(resids::Array{Float64,1}, fitteds::Array{Float64})::Array{Float64,2}
    n = length(resids)
    d = zeros(Float64, n, n)
    @inbounds for i in 1:n
        for j in i:n
            if i != j
                p1 = [resids[i], fitteds[i]]
                p2 = [resids[j], fitteds[j]]
                d[i, j] = sqrt(sum((p1 .- p2).^2.0))
                d[j, i] = d[i, j]
            end
        end
    end
    return d
end

function majona(cluster::Hclust)::Float64
    heights = cluster.heights
    return mean(heights) + 1.25 * std(heights)
end

"""

    smr98(setting)

Perform the Sebert, Monthomery and Rollier (1998) algorithm for the given regression setting.

# Arguments
- `setting::RegressionSetting`: RegressionSetting object with a formula and dataset.

# Examples
```julia-repl
julia> reg0001 = createRegressionSetting(@formula(calls ~ year), phones);
julia> smr98(reg0001)
10-element Array{Int64,1}:
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
```

# References
Sebert, David M., Douglas C. Montgomery, and Dwayne A. Rollier. "A clustering algorithm for
identifying multiple outliers in linear regression." Computational statistics & data analysis
27.4 (1998): 461-484.
"""
function smr98(setting::RegressionSetting)
    X, y = @extractRegressionSetting setting
    return smr98(X, y)
end

"""

    smr98(X, y)

Perform the Sebert, Monthomery and Rollier (1998) algorithm for the given regression setting.

# Arguments
- `X::Array{Float64, 2}`: Desing matrix of the linear regression model.
- `y::Array{Float64, 1}`: Response vector of the linear regression model.

# References
Sebert, David M., Douglas C. Montgomery, and Dwayne A. Rollier. "A clustering algorithm for
identifying multiple outliers in linear regression." Computational statistics & data analysis
27.4 (1998): 461-484.
"""

function smr98(X::Array{Float64,2}, y::Array{Float64,1})
    olsreg = ols(X, y)
    stdres = standardize(ZScoreTransform, residuals(olsreg), dims=1)
    stdfit = standardize(ZScoreTransform, predict(olsreg), dims=1)
    n, p = size(X)
    d = distances(stdres, stdfit)
    h = floor((n + p - 1) / 2)
    hcl = hclust(d, linkage=:single)
    majonacrit = majona(hcl)
    clustermappings = cutree(hcl, h=majonacrit)
    uniquemappings = unique(clustermappings)
    for clustid in uniquemappings
        cnt = count(x -> x == clustid, clustermappings)
        if cnt >= h
            return filter(i -> clustermappings[i] != clustid, 1:n)
        end
    end
    return []
end


module TestThings
export foo, bar

greet() = print("Hello World!")

@doc raw"
    foo(xs::Vector{<:Integer})

return the maximum of the values in xs, i.e.
```math
foo(xs) = \{x \mid x \geq y \quad \forall y \in xs\}
```
"
foo(xs::Vector{<:Integer}) = foldl((x,y) -> x > y ? x : y, xs)

@doc raw"
    bar(xs::Vector{<:Integer})
Like [`foo`](@ref) but instead returns the *minimum* of the values in xs, i.e.
```math
bar(xs) = \{x \mid x \leq y \quad \forall y \in xs\}
```
"
bar(xs::Vector{<:Integer}) = -foo(-xs)

@doc raw"
    foobar(xs)
returns the extrema of `xs`
"

@doc raw"
    foobar(xs::Vector{<:Integer})
returns the extrema of a vector of integers
"
function foobar(xs::Vector{<:Integer})
    if length(xs) == 1
        x = first(xs)
        return (x,x)
    end

    let T = eltype(xs)
        foldl(((x1,x2),y) -> (ifelse(x1 > y, y, x1), ifelse(x2 > y, x2, y)), xs, init = (typemax(T), typemin(T)))
    end
end

@doc raw"
    foobar(xs::Vector{<:Real})
returns the extrema of a vector of integers
"
function foobar(xs::Vector{<:Real})
    if length(xs) == 1
        x = first(xs)
        return (x,x)
    end

    let T = eltype(xs)
        foldl(((x1,x2),y) -> (ifelse(x1 > y, y, x1), ifelse(x2 > y, x2, y)), xs, init = (typemax(T), typemin(T)))
    end
end

end # module


# 'cstrt_audet_ngon' provides the vertices coordinates (a,b) of a convex
# equilateral small n-gon for n = 2^s and s >= 4
function cstrt_audet_ngon(n)
if mod(log2(n),1)==0 && n>=16
    # initialization
    x(t) = sin(t)-(2*cos(t)-1)*cos((n/4-2)*t)*sin(n*t/4)/cos(2*t);
    F(t) = (2*x(t)+sin((n/2-1)*t))^2 + cos((n/2-1)*t)^2 - 4*sin(t/2)^2;
    z = find_zero(F,(2*asin(sin(pi/n)/2),pi/n));
    # construction
    a = zeros(n-1); b = zeros(n-1);
    a[Int(n/2-1)] = sin(z); a[Int(n/2+1)] = -a[Int(n/2-1)];
    b[Int(n/2-1)] = cos(z); b[Int(n/2+1)] = b[Int(n/2-1)];
    a[n-1] = a[Int(n/2-1)]-sin(2*z); a[1] = -a[n-1];
    b[n-1] = b[Int(n/2-1)]-cos(2*z); b[1] = b[n-1];
    a[n-2] = a[Int(n/2-1)]-sin(3*z); a[2] = -a[n-2];
    b[n-2] = b[Int(n/2-1)]-cos(3*z); b[2] = b[n-2];
    for i = 1:Int(n/8-1)
        a[Int(mod(n/2-1+i*(3*n/2-2),n))] = sin(z)-(2*cos(z)-1)*sin(i*(2*z-pi/2))*sin((i+1)*(2*z-pi/2))/cos(2*z);
        a[Int(n-mod(n/2-1+i*(3*n/2-2),n))] = -a[Int(mod(n/2-1+i*(3*n/2-2),n))];
        b[Int(mod(n/2-1+i*(3*n/2-2),n))] = cos(z)-(2*cos(z)-1)*sin(i*(2*z-pi/2))*cos((i+1)*(2*z-pi/2))/cos(2*z);
        b[Int(n-mod(n/2-1+i*(3*n/2-2),n))] = b[Int(mod(n/2-1+i*(3*n/2-2),n))];
        a[Int(mod(n-1+i*(3*n/2-2),n))] = a[Int(mod(n/2-1+i*(3*n/2-2),n))]-(-1)^i*sin((4*i+2)*z);
        a[Int(n-mod(n-1+i*(3*n/2-2),n))] = -a[Int(mod(n-1+i*(3*n/2-2),n))];
        b[Int(mod(n-1+i*(3*n/2-2),n))] = b[Int(mod(n/2-1+i*(3*n/2-2),n))]-(-1)^i*cos((4*i+2)*z);
        b[Int(n-mod(n-1+i*(3*n/2-2),n))] = b[Int(mod(n-1+i*(3*n/2-2),n))];
        a[Int(mod(n-2+i*(3*n/2-2),n))] = a[Int(mod(n/2-1+i*(3*n/2-2),n))]-(-1)^i*sin((4*i+3)*z);
        a[Int(n-mod(n-2+i*(3*n/2-2),n))] = -a[Int(mod(n-2+i*(3*n/2-2),n))];
        b[Int(mod(n-2+i*(3*n/2-2),n))] = b[Int(mod(n/2-1+i*(3*n/2-2),n))]-(-1)^i*cos((4*i+3)*z);
        b[Int(n-mod(n-2+i*(3*n/2-2),n))] = b[Int(mod(n-2+i*(3*n/2-2),n))];
        a[Int(mod(n+i*(3*n/2-2),n))] = a[Int(mod(n/2-1+i*(3*n/2-2),n))]-(-1)^i*sin((4*i+1)*z);
        a[Int(n-mod(n+i*(3*n/2-2),n))] = -a[Int(mod(n+i*(3*n/2-2),n))];
        b[Int(mod(n+i*(3*n/2-2),n))] = b[Int(mod(n/2-1+i*(3*n/2-2),n))]-(-1)^i*cos((4*i+1)*z);
        b[Int(n-mod(n+i*(3*n/2-2),n))] = b[Int(mod(n+i*(3*n/2-2),n))];
    end
    a[Int(n/2)] = 0; b[Int(n/2)] = 1;
    return(a,b)
end
end


#=
 # 121: 1D Poisson with point charge
 ([source code](SOURCE_URL))

Solve a Poisson equation
```math
- \Delta u = 0
```

in $\Omega=(-1,1)$
with a point charge $Q$ at $x=0$.
=#

module Example121_PoissonPointCharge1D

using Printf

using VoronoiFVM
using ExtendableGrids
using GridVisualize

function main(;nref=0,Plotter=nothing, verbose=false, unknown_storage=:sparse, brea=false)

    ## Create grid in (-1,1) refined around 0
    hmax=0.2/2.0^nref
    hmin=0.05/2.0^nref
    X1=geomspace(-1.0,0.0, hmax,hmin)
    X2=geomspace(0.0,1.0, hmin,hmax)
    X=glue(X1,X2)
    grid=VoronoiFVM.Grid(X)

    ## Edit default region numbers:
    ##   additional boundary region 3 at 0.0
    bfacemask!(grid, [0.0],[0.0],3)
    ## Material 1 left of 0
    cellmask!(grid, [-1.0],[0.0],1)
    ## Material 2 right of 0
    cellmask!(grid, [0.0],[1.0],2)

    Q=0.0

    function flux!(f,u0,edge)
        u=unknowns(edge,u0)
        f[1]=u[1,1]-u[1,2]
    end
    function storage!(f,u,node)
        f[1]=u[1]
    end

    ## Define boundary reaction defining charge
    ## Note that the term  is written on  the left hand side, therefore the - sign
    function breaction!(f,u,node)
        if node.region==3
            f[1]=-Q
        end
    end

   ## Create physics
    physics=VoronoiFVM.Physics(
        flux=flux!,
        storage=storage!,
        breaction=breaction!
    )

    ## Create system
    sys=VoronoiFVM.System(grid,physics,unknown_storage=:dense)

    ##  put potential into both regions
    enable_species!(sys,1,[1,2])

    ## Set boundary conditions

    boundary_dirichlet!(sys,1,1,1.0)
    boundary_dirichlet!(sys,1,2,0.0)

    ## Create a solution array
    inival=unknowns(sys)
    U=unknowns(sys)
    inival.=0

    ## Create solver control info
    control=VoronoiFVM.NewtonControl()
    control.verbose=verbose
    if ispyplot(Plotter)
        Plotter.clf()
    end

    ## Solve and plot for several values of charge
    for q in [0.1,0.2,0.4,0.8,1.6]

        if brea
            ## Charge in reaction term
            Q=q
        else
            ## Charge as boundary condition
            sys.boundary_values[1,3]=q
        end
        solve!(U,inival,sys, control=control)

        ## Plot data
        if ispyplot(Plotter)
            Plotter.grid()
            coord=grid[Coordinates]
            Plotter.plot(coord[1,:],U[1,:],label=@sprintf("Q=%.2f",q))
            Plotter.xlabel("x")
            Plotter.ylabel("\$\\phi\$")
            Plotter.legend(loc="upper right")
            Plotter.pause(1.0e-10)
        end
    end
    return sum(U)
end

function test()
    main()≈20.254591679579015
end
end


# # Tutorial 04 Perturbation Theory
#
# ## Introduction
#
# This tutorial demonstrates how perturbation theory is utilized using the
# WavesAndEigenvalues package. You may ask: 'What is perturbation theory?'
# Well, perturbation theory deals with the approximation of a solution of a
# mathematical problem based on a *known* solution of a problem similar to the
# problem of interest. (Puhh, that was a long sentence...) The problem is said
# to be perturbed from a baseline solution.
# You can find a comprehensive presentation of the internal algorithms in
# [1,2,3].
## #jl
# ## Model set-up and baseline-solution
# The model is the same Rijke tube configuration as in Tutorial 01:
using WavesAndEigenvalues.Helmholtz
mesh=Mesh("Rijke_mm.msh",scale=0.001) #load mesh
dscrp=Dict() #initialize model descriptor
dscrp["Interior"]=(:interior, ()) #define resonant cavity
dscrp["Outlet"]=(:admittance, (:Y,1E15)) #specify outlet BC
γ=1.4 #ratio of specific heats
ρ=1.225 #density at the reference location upstream to the flame in kg/m^3
Tu=300.0    #K unburnt gas temperature
Tb=1200.0    #K burnt gas temperature
P0=101325.0 # ambient pressure in Pa
A=pi*0.025^2 # cross sectional area of the tube
Q02U0=P0*(Tb/Tu-1)*A*γ/(γ-1) #the ratio of mean heat release to mean velocity Q02U0
x_ref=[0.0; 0.0; -0.00101] #reference point
n_ref=[0.0; 0.0; 1.00] #directional unit vector of reference velocity
n=0.01 #interaction index
τ=0.001 #time delay
dscrp["Flame"]=(:flame,(γ,ρ,Q02U0,x_ref,n_ref,:n,:τ,n,τ)) #flame dynamics
R=287.05 # J/(kg*K) specific gas constant (air)
speedofsound(x,y,z) = z<0. ? sqrt(γ*R*Tu) : sqrt(γ*R*Tb)
c=generate_field(mesh,speedofsound)
L=discretize(mesh,dscrp,c)

# To obtain a baseline solution we solve it using the housholder iteration with
# a very small stopping tollerance of `tol=1E-11`.
sol,nn,flag=householder(L,340*2*pi,maxiter=20,tol=1E-11)

# this small tolerance is necessary because as the name suggests the base-line
# solution is the basis to our subsequent steps. If it is inaccurate we will
# definetly also encounter inaccurate approximations to other configurations
# than the base-line set-up.
## #jl
# ## Taylor series
#
# Probably, you are somewhat familiar with the concept of Taylor series
# which is a basic example of perturbation theory. There is some function
# f from which the value f(x0) is known together with some derivatives at the
# same point, the first N say. Then, we can approximate f(x0+Δ) as:
#
# f(x0+Δ)≈∑_n=0^N f_n(x0)Δ^n

# Let's consider the time delay `τ`. Our problem depends exponentially on this
# value.  We can utilize a fast perturbation algorithm to compute the first
# 20 Taylor-series coefficients of the eigenfrequency `ω` w.r.t. `τ` by just
# typing
perturb_fast!(sol,L,:τ,20)
# The output shows you how long it takes to compute the coefficients.
# Obviously, it takes longer and longer for higher order coefficients.
# Note, that there is also an algorithm `perturb!(sol,L,:τ,20)` which does
# exactly the same as the fast algorithm but is slower, when it comes to high
# orders (N>5).

# Both algorithms populate a field in the solution object holding the Taylor
# coefficients.
sol.eigval_pert

# We can use these values to form the Taylor-series approximation and for
# convenience we can just do so by calling to the solution object itself.
# For instance let's assume we would like to approximate the value of the
# eigenfrequency when "τ==0.00125" based on the 20th order series expansion.
Δ=0.00001
ω_approx=sol(:τ,τ+Δ,20)

# Let's compare this value to the true solution:
L.params[:τ]=τ+Δ #change parameter
sol_exact,nn,flag=householder(L,340*2*pi,maxiter=20,tol=1E-11) #solve again
ω_exact=sol_exact.params[:ω]
println(" exact=$(ω_exact/2/pi)  vs  approx=$(ω_approx/2/pi))")
# Clearly, the approximation matches the first 4 digits after the point!

# We can also compute the approximation at any lower order than 20.
# For instance, the first-order approximation is:
ω_approx=sol(:τ,τ+Δ,1)
println(" first-order approx=$(ω_approx/2/pi)")

# Note that the accuracy is less than at twentieth order.
#
# Also note that we cannot compute the perturbation at higher order than 20
# because we have only computed the Taylor series coefficients up to 20th order.
# Of course we could, prepare higher order coefficients, let's say up to 30th
# order by
perturb_fast!(sol,L,:τ,30)
# and then
ω_approx=sol(:τ,τ+Δ,30)
println(" 30th-order approx=$(ω_approx/2/pi)")

# Indeed, `30` is a special limit because per default the WavesAndEigenvalues
# package installs the perturbation algorithm on your machine up to 30th order.
# This is because higher orders would consume significantly more memory in
# the installation directory and also the computation of higher orders may take
# some time.  However, if you really want to use higher orders. You can rebuild
# your package by first setting a special environment variable to your desired
# order -- say 40 -- by `ENV["JULIA_WAE_PERT_ORDER"]=40` and then run
# `Pkg.build("WavesAndEigenvalues")`
# Note, that if you don't make the environment variable permanent,
# these steps will be necessary once every time you got any update to the
# package from julias package manager.

#TODO:speed

## #jl
# Ok, we've seen how we can compute Taylor-series coefficients and how to
# evaluate them as a Taylor series. But how do we choose parameters like
# the baseline set-up or the eprturbation order to get a reasonable estimate?
# Well there is a lot you can do but, unfornately, there are a lot of
# misunderstandings when it comes to the quality perturbative approximations.
#
# Take a second and try to answer the following questions:
# 1. What is the range of Δ in which you can expect reliable results from
# the approximation, i.e., the difference from the true solution is small?
# 2. How does the quality of your approximation improve if you increase the
# number of known derivatives?
# 3. What else can you do to improve your solution?
#
# Regarding the first question, many people misbelieve that the approximation is
# good as long as Δ (the shift from the expansion point) is small. This is right
# and wrong at the same time. Indeed, to classify Δ as small, you need to
# compare it agains some value. For Taylor series this is the radius of
# convergence. Convergence radii can be quite huge and sometimes super small.
# Also keep in mind that the nuemrical value of Δ in most engineering
# applications is meaningless if it is not linked to some unit of measure.
# (You know the joke: What is larger, 1 km or a million mm?)

# So how do we get the radius of convergence? It's as simple like
r=conv_radius(sol,:τ)
#The return value here is an array that holds N-1 values, where N is the order
# of Taylor-series coefficients that have been computed for `:τ`. This is
# because the convergenvce radius is estimated based on the ratio of two
# consecutive Taylor-series coefficients. Usually, the last entry of r should
# give you the best approximate.
println("Best estimate available: r=$(r[end])")
# However, there are cases where the estimation procedure for the convergence
# radius is not appropriate. That is why you can have a look on the other entries
# in r to judge whether there is smooth convergence.
## #jl
# Let's see what's happening if we evaluate the Taylor series beyond it's radius
# of convergence.
ω_approx=sol(:τ,τ+r[end]+0.001,20)
L.params[:τ]=τ+r[end]+0.001
sol_exact,nn,flag=householder(L,340*2*pi,maxiter=20,tol=1E-11) #solve again
ω_exact=sol_exact.params[:ω]
println(" exact=$(ω_exact/2/pi)  vs  approx=$(ω_approx/2/pi))")
# Outch, the approximation is completely off.
# Remember question 2? Maybe the estimate gets better if we increase the
# perturbation order ? At least the last time we've seen an improvement.
# But this time...
ω_approx=sol(:τ,τ+r[end]+0.001,30)
println(" exact=$(ω_exact/2/pi)  vs  approx=$(ω_approx/2/pi))")
# things get *way* worse! This is exaclty the reason why some clever person
# named r the radius of *convergence*. Beyond that radius we cannot make the
# Taylor-series converge without shifting the expansion point. You might think:
# 'Well, then let's shift the expansion point!' While this is definitely a valid
# approach, there is something better you can do...

# ## Series accelartion by Padé approximation
#
# The Taylor-series cannot converge beyond its radius of convergence. So why
# not trying someting else than a Taylor-series?. The truncated Taylor series is
# a polynomial approximation to our unknown relation ω=ω(τ). An alternative
# (if not to say a generalazation) of this approach is to consider rational
# polynomial approximations.  So isntead of
# f(x0+Δ)≈∑_n=0^N f_n(x0)Δ^n
# we try something like
# f(x0+Δ)≈[ ∑_l=0^L a_l(x0)Δ^l ] / [ 1 + ∑_m=0^M b_m(x0)Δ^m ]
# In order to get the same asymptotic behaviour close to our expansion point, we
# demand that the truncated Taylor series expansion of our Padé approximant is
# identical to the approximation of our unknown function. Here comes the clou:
# we already know these values because we computed the Taylor-series
# coefficients. Only little algebra is needed to convert the Taylor coefficients
# to Padé coefficients and all of this is build in to the solution type. All you
# need to know is that the number of Padé coefficients is related to the number
# of Taylor coefficients by the simple formula L+M=N. Let's give it a try and
# compute the Padé approximant for L=M=10 (a so-called diagonal Padé
# approximant). This is just achieve by an extra argument for the call to our
# solution object.
ω_approx=sol(:τ,τ+r[end]+0.001,10,10)
println(" exact=$(ω_exact/2/pi)  vs  approx=$(ω_approx/2/pi))")
# Wow! There is now only a diffrence of about 1hz between the true solution and
# it's estimate. I'would say that's fine for many egineering applications.
# What's your opinion?
## #jl
# ## Conclusion
# You learned how to use perturbation theory. The main computational costs for
# this approach is the computation of Taylor-series coefficients. Once you have
# them everything comes down to ta few evaluations of scalar polynomials.
# Especially, when you like to rapidly evalaute your model for a bunch of values
# in a given parameter range. This approach should speed up your computations.
#
# The next tutorial will familiarize you with the use of higher order
# finite elements.


function svdbyeigen(A; nsv::Integer=3)
	P,N = size(A)
	K = Symmetric(N<=P ? A'A : A*A')
	M = size(K,1)
	F = eigen(K, M-nsv+1:M)
	S = sqrt.(max.(0.,reverse(F.values)))

	V = F.vectors[:,end:-1:1]
	N<=P ? SVD(A*V./S',S,V') : SVD(V,S,V'A./S)
end