"""
    AbstractVectorTransportMethod

Abstract type for methods for transporting vectors. Such vector transports are not
necessarily linear.

# See also

[`AbstractLinearVectorTransportMethod`](@ref)
"""
abstract type AbstractVectorTransportMethod end

"""
    AbstractLinearVectorTransportMethod <: AbstractVectorTransportMethod

Abstract type for linear methods for transporting vectors, that is transport of a linear
combination of vectors is a linear combination of transported vectors.
"""
abstract type AbstractLinearVectorTransportMethod <: AbstractVectorTransportMethod end

@doc raw"""
    DifferentiatedRetractionVectorTransport{R<:AbstractRetractionMethod} <:
        AbstractVectorTransportMethod

A type to specify a vector transport that is given by differentiating a retraction.
This can be introduced in two ways. Let ``\mathcal M`` be a Riemannian manifold,
``p\in\mathcal M`` a point, and ``X,Y\in T_p\mathcal M`` denote two tangent vectors at ``p``.

Given a retraction (cf. [`AbstractRetractionMethod`](@ref)) ``\operatorname{retr}``,
the vector transport of `X` in direction `Y` (cf. [`vector_transport_direction`](@ref))
by differentiation this retraction, is given by

```math
\mathcal T^{\operatorname{retr}}_{p,Y}X
= D_Y\operatorname{retr}_p(Y)[X]
= \frac{\mathrm{d}}{\mathrm{d}t}\operatorname{retr}_p(Y+tX)\Bigr|_{t=0}.
```
see [^AbsilMahonySepulchre2008], Section 8.1.2 for more details.

This can be phrased similarly as a [`vector_transport_to`](@ref) by introducing
``q=\operatorname{retr}_pX`` and defining

```math
\mathcal T^{\operatorname{retr}}_{q \gets p}X = \mathcal T^{\operatorname{retr}}_{p,Y}X
```

which in practice usually requires the [`inverse_retract`](@ref) to exists in order to
compute ``Y = \operatorname{retr}_p^{-1}q``.

# Constructor

    DifferentiatedRetractionVectorTransport(m::AbstractRetractionMethod)

[^AbsilMahonySepulchre2008]:
    > Absil, P.-A., Mahony, R. and Sepulchre R.,
    > _Optimization Algorithms on Matrix Manifolds_
    > Princeton University Press, 2008,
    > doi: [10.1515/9781400830244](https://doi.org/10.1515/9781400830244)
    > [open access](http://press.princeton.edu/chapters/absil/)
"""
struct DifferentiatedRetractionVectorTransport{R<:AbstractRetractionMethod} <:
       AbstractLinearVectorTransportMethod end
function DifferentiatedRetractionVectorTransport(::R) where {R<:AbstractRetractionMethod}
    return DifferentiatedRetractionVectorTransport{R}()
end

"""
    ParallelTransport = DifferentiatedRetractionVectorTransport{ExponentialRetraction}

Specify to use parallel transport as vector transport method within
[`vector_transport_to`](@ref), [`vector_transport_direction`](@ref), or
[`vector_transport_along`](@ref).

Note that since it is technically the [`DifferentiatedRetractionVectorTransport`](@ref) of
the [`exp`](@ref exp(M::Manifold, p, X)) (cf. [`ExponentialRetraction`](@ref)), we define
`ParallelTransport` as an alias.
"""
const ParallelTransport = DifferentiatedRetractionVectorTransport{ExponentialRetraction}

"""
    ProjectionTransport <: AbstractVectorTransportMethod

Specify to use projection onto tangent space as vector transport method within
[`vector_transport_to`](@ref), [`vector_transport_direction`](@ref), or
[`vector_transport_along`](@ref). See [`project`](@ref) for details.
"""
struct ProjectionTransport <: AbstractVectorTransportMethod end

@doc raw"""
    PoleLadderTransport <: AbstractVectorTransportMethod

Specify to use [`pole_ladder`](@ref) as vector transport method within
[`vector_transport_to`](@ref), [`vector_transport_direction`](@ref), or
[`vector_transport_along`](@ref), i.e.

Let $X\in T_p\mathcal M$ be a tangent vector at $p\in\mathcal M$ and $q\in\mathcal M$ the
point to transport to. Then $x = \exp_pX$ is used to call
`y = `[`pole_ladder`](@ref)`(M, p, x, q)` and the resulting vector is obtained by computing
$Y = -\log_qy$.

The [`PoleLadderTransport`](@ref) posesses two advantages compared to
[`SchildsLadderTransport`](@ref):
* it is cheaper to evaluate, if you want to transport several vectors, since the
  mid point $c$ then stays unchanged.
* while both methods are exact if the curvature is zero, pole ladder is even exact in
  symmetric Riemannian manifolds[^Pennec2018]

The pole ladder was was proposed in [^LorenziPennec2014]. Its name stems from the fact that
it resembles a pole ladder when applied to a sequence of points usccessively.

# Constructor
````julia
PoleLadderTransport(
    retraction = ExponentialRetraction(),
    inverse_retraction = LogarithmicInverseRetraction(),
)
````
Construct the classical pole ladder that employs exp and log, i.e. as proposed
in[^LorenziPennec2014]. For an even cheaper transport the inner operations can be
changed to an [`AbstractRetractionMethod`](@ref) `retraction` and an
[`AbstractInverseRetractionMethod`](@ref) `inverse_retraction`, respectively.

[^LorenziPennec2014]:
    > Lorenzi, M. and Pennec, X: Efficient parallel transport of deformations in time
    > series of images: From Schild’s to pole ladder.
    > Journal of Mathematical Imaging and Vision (2014), 50(1), pp. 5–17
    > doi [10.1007/s10851-013-0470-3](https://doi.org/10.1007/s10851-013-0470-3),
    > hal: [hal-00870489](https://hal.inria.fr/hal-00870489)
[^Pennec2018]:
    > Pennec, X: Parallel Transport with Pole Ladder: a Third Order Scheme in Affine
    > Connection Spaces which is Exact in Affine Symmetric Spaces.
    > arXiv: [1805.11436](https://arxiv.org/abs/1805.11436)

"""
struct PoleLadderTransport{
    RT<:AbstractRetractionMethod,
    IRT<:AbstractInverseRetractionMethod,
} <: AbstractLinearVectorTransportMethod
    retraction::RT
    inverse_retraction::IRT
    function PoleLadderTransport(
        retraction = ExponentialRetraction(),
        inverse_retraction = LogarithmicInverseRetraction(),
    )
        return new{typeof(retraction),typeof(inverse_retraction)}(
            retraction,
            inverse_retraction,
        )
    end
end

@doc raw"""
    ScaledVectorTransport{T} <: AbstractVectorTransportMethod

Introduce a scaled variant of any [`AbstractVectorTransportMethod`](@ref) `T`,
as introduced in [^SatoIwai2013] for some ``X\in T_p\mathcal M`` as

```math
    \mathcal T^{\mathrm{S}}(X) = \frac{\lVert X\rVert_p}{\lVert \mathcal T(X)\rVert_q}\mathcal T(X).
```

Note that the resulting point `q` has to be known, i.e. for [`vector_transport_direction`](@ref)
the curve or more precisely its end point has to be known (via an exponential map or a
retraction). Therefore a default implementation is only provided for the [`vector_transport_to`](@ref)

# Constructor

    ScaledVectorTransport(m::AbstractVectorTransportMethod)

[^SatoIwai2013]:
    > Sato, H., Iwai, T.: _A new, globally convergent Riemannian conjugate gradient method_,
    > Optimization, 2013, Volume 64(4), pp. 1011–1031.
    > doi: [10.1080/02331934.2013.836650](https://doi.org/10.1080/02331934.2013.836650),
    > arXiv: [1302.0125](https://arxiv.org/abs/1302.0125).
"""
struct ScaledVectorTransport{T<:AbstractVectorTransportMethod} <:
       AbstractVectorTransportMethod
    method::T
end

@doc raw"""
    SchildsLadderTransport <: AbstractVectorTransportMethod

Specify to use [`schilds_ladder`](@ref) as vector transport method within
[`vector_transport_to`](@ref), [`vector_transport_direction`](@ref), or
[`vector_transport_along`](@ref), i.e.

Let $X\in T_p\mathcal M$ be a tangent vector at $p\in\mathcal M$ and $q\in\mathcal M$ the
point to transport to. Then

````math
P^{\mathrm{S}}_{q\gets p}(X) =
    \log_q\bigl( \operatorname{retr}_p ( 2\operatorname{retr}_p^{-1}c ) \bigr),
````

where $c$ is the mid point between $q$ and $d=\exp_pX$.

This method employs the internal function [`schilds_ladder`](@ref)`(M, p, d, q)` that avoids
leaving the manifold.

The name stems from the image of this paralleltogram in a repeated application yielding the
image of a ladder. The approximation was proposed in [^EhlersPiraniSchild1972].

# Constructor
````julia
SchildsLadderTransport(
    retraction = ExponentialRetraction(),
    inverse_retraction = LogarithmicInverseRetraction(),
)
````
Construct the classical Schilds ladder that employs exp and log, i.e. as proposed
in[^EhlersPiraniSchild1972]. For an even cheaper transport these inner operations can be
changed to an [`AbstractRetractionMethod`](@ref) `retraction` and an
[`AbstractInverseRetractionMethod`](@ref) `inverse_retraction`, respectively.

[^EhlersPiraniSchild1972]:
    > Ehlers, J., Pirani, F.A.E., Schild, A.: The geometry of free fall and light
    > propagation. In: O’Raifeartaigh, L. (ed.) General Relativity: Papers in Honour of
    > J. L. Synge, pp. 63–84. Clarendon Press, Oxford (1972).
    > reprint doi: [10.1007/s10714-012-1353-4](https://doi.org/10.1007/s10714-012-1353-4)

"""
struct SchildsLadderTransport{
    RT<:AbstractRetractionMethod,
    IRT<:AbstractInverseRetractionMethod,
} <: AbstractLinearVectorTransportMethod
    retraction::RT
    inverse_retraction::IRT
    function SchildsLadderTransport(
        retraction = ExponentialRetraction(),
        inverse_retraction = LogarithmicInverseRetraction(),
    )
        return new{typeof(retraction),typeof(inverse_retraction)}(
            retraction,
            inverse_retraction,
        )
    end
end

@doc raw"""
    pole_ladder(
        M,
        p,
        d,
        q,
        c = mid_point(M, p, q);
        retraction=ExponentialRetraction(),
        inverse_retraction=LogarithmicInverseRetraction()
    )

Compute an inner step of the pole ladder, that can be used as a [`vector_transport_to`](@ref).
Let $c = \gamma_{p,q}(\frac{1}{2})$ mid point between `p` and `q`, then the pole ladder is
given by

````math
    \operatorname{Pl}(p,d,q) = \operatorname{retr}_d (2\operatorname{retr}_d^{-1}c)
````

Where the classical pole ladder employs $\operatorname{retr}_d=\exp_d$
and $\operatorname{retr}_d^{-1}=\log_d$ but for an even cheaper transport these can be set
to different [`AbstractRetractionMethod`](@ref) and [`AbstractInverseRetractionMethod`](@ref).

When you have $X=log_pd$ and $Y = -\log_q \operatorname{Pl}(p,d,q)$,
you will obtain the [`PoleLadderTransport`](@ref). When performing multiple steps, this
method avoidsd the switching to the tangent space. Keep in mind that after $n$ successive
steps the tangent vector reads $Y_n = (-1)^n\log_q \operatorname{Pl}(p_{n-1},d_{n-1},p_n)$.

It is cheaper to evaluate than [`schilds_ladder`](@ref), sinc if you want to form multiple
ladder steps between `p` and `q`, but with different `d`, there is just one evaluation of a geodesic
each., since the center `c` can be reused.
"""
function pole_ladder(
    M,
    p,
    d,
    q,
    c = mid_point(M, p, q);
    retraction = ExponentialRetraction(),
    inverse_retraction = LogarithmicInverseRetraction(),
)
    return retract(M, d, 2 * inverse_retract(M, d, c, inverse_retraction), retraction)
end
@doc raw"""
    pole_ladder(
        M,
        pl,
        p,
        d,
        q,
        c = mid_point(M, p, q),
        X = allocate_result_type(M, log, d, c);
        retraction = ExponentialRetraction(),
        inverse_retraction = LogarithmicInverseRetraction()
    )

Compute the [`pole_ladder`](@ref), i.e. the result is saved in `pl`.
`X` is used for storing intermediate inverse retraction.
"""
function pole_ladder!(
    M,
    pl,
    p,
    d,
    q,
    c = mid_point(M, p, q),
    X = allocate_result(M, log, d, c);
    retraction = ExponentialRetraction(),
    inverse_retraction = LogarithmicInverseRetraction(),
)
    inverse_retract!(M, X, d, c, inverse_retraction)
    X *= 2
    return retract!(M, pl, d, X, retraction)
end

@doc raw"""
    schilds_ladder(
        M,
        p,
        d,
        q,
        c = mid_point(M, q, d);
        retraction = ExponentialRetraction(),
        inverse_retraction = LogarithmicInverseRetraction(),
    )

Perform an inner step of schilds ladder, which can be used as a
[`vector_transport_to`](@ref), see [`SchildsLadderTransport`](@ref).
Let $c = \gamma_{q,d}(\frac{1}{2})$ denote the mid point
on the shortest geodesic connecting $q$ and the point $d$. Then Schild's ladder reads as

````math
\operatorname{Sl}(p,d,q) = \operatorname{retr}_x( 2\operatorname{retr}_p^{-1} c)
````

Where the classical Schilds ladder employs $\operatorname{retr}_d=\exp_d$
and $\operatorname{retr}_d^{-1}=\log_d$ but for an even cheaper transport these can be set
to different [`AbstractRetractionMethod`](@ref) and [`AbstractInverseRetractionMethod`](@ref).

In consistency with [`pole_ladder`](@ref) you can change the way the mid point is computed
using the optional parameter `c`, but note that here it's the mid point between `q` and `d`.

When you have $X=log_pd$ and $Y = \log_q \operatorname{Sl}(p,d,q)$,
you will obtain the [`PoleLadderTransport`](@ref).
Then the approximation to the transported vector is given by $\log_q\operatorname{Sl}(p,d,q)$.

When performing multiple steps, this method avoidsd the switching to the tangent space.
Hence after $n$ successive steps the tangent vector reads
$Y_n = \log_q \operatorname{Pl}(p_{n-1},d_{n-1},p_n)$.
"""
function schilds_ladder(
    M,
    p,
    d,
    q,
    c = mid_point(M, q, d);
    retraction = ExponentialRetraction(),
    inverse_retraction = LogarithmicInverseRetraction(),
)
    return retract(M, p, 2 * inverse_retract(M, p, c, inverse_retraction), retraction)
end
@doc raw"""
    schilds_ladder!(
        M,
        sl
        p,
        d,
        q,
        c = mid_point(M, q, d),
        X = allocate_result_type(M, log, d, c);
        retraction = ExponentialRetraction(),
        inverse_retraction = LogarithmicInverseRetraction()
    )

Compute [`schilds_ladder`](@ref) and return the value in the parameter `sl`.
If the required mid point `c` was computed before, it can be passed using `c`,
and the allocation of new memory can be avoided providing a tangent vector `X`
for the interims result.
"""
function schilds_ladder!(
    M,
    sl,
    p,
    d,
    q,
    c = mid_point(M, q, d),
    X = allocate_result(M, log, d, c);
    retraction = ExponentialRetraction(),
    inverse_retraction = LogarithmicInverseRetraction(),
)
    inverse_retract!(M, X, p, c, inverse_retraction)
    X *= 2
    return retract!(M, sl, p, X, retraction)
end

function show(io::IO, ::ParallelTransport)
    return print(io, "ParallelTransport()")
end
function show(io::IO, m::ScaledVectorTransport)
    return print(io, "ScaledVectorTransport($(m.method))")
end

"""
    vector_transport_along(M::Manifold, p, X, c)
    vector_transport_along(M::Manifold, p, X, c, method::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`Manifold`](@ref) `M`
along the curve represented by `c` using the `method`, which defaults to
[`ParallelTransport`](@ref).
"""
function vector_transport_along(M::Manifold, p, X, c)
    return vector_transport_along(M, p, X, c, ParallelTransport())
end
function vector_transport_along(M::Manifold, p, X, c, m::AbstractVectorTransportMethod)
    Y = allocate_result(M, vector_transport_along, X, p)
    vector_transport_along!(M, Y, p, X, c, m)
    return Y
end

"""
    vector_transport_along!(M::Manifold, Y, p, X, c)
    vector_transport_along!(M::Manifold, Y, p, X, c, method::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`Manifold`](@ref) `M`
along the curve represented by `c` using the `method`, which defaults to
[`ParallelTransport`](@ref). The result is saved to `Y`.
"""
function vector_transport_along!(M::Manifold, Y, p, X, c)
    return vector_transport_along!(M, Y, p, X, c, ParallelTransport())
end
function vector_transport_along!(
    M::Manifold,
    Y,
    p,
    X,
    c,
    method::AbstractVectorTransportMethod,
)
    return error(
        manifold_function_not_implemented_message(
            M,
            vector_transport_along!,
            M,
            Y,
            p,
            X,
            c,
            method,
        ),
    )
end
@doc raw"""
    vector_transport_along!(
        M::Manifold,
        Y,
        p,
        X,
        c::AbstractVector,
        method::AbstractVectorTransportMethod
    ) where {T}

Compute the vector transport along a discretized curve `c` using an
[`AbstractVectorTransportMethod`](@ref) `method` succesively along the sampled curve.
"""
function vector_transport_along!(
    M::Manifold,
    Y,
    p,
    X,
    c::AbstractVector,
    method::AbstractVectorTransportMethod,
)
    n = length(c)
    if n == 0
        copyto!(Y, X)
    else
        # we shouldn't assume that vector_transport_to! works when both input and output
        # vectors are the same object
        Y2 = allocate(X)
        vector_transport_to!(M, Y2, p, X, c[1], method)
        for i in 1:(length(c) - 1)
            vector_transport_to!(M, Y, c[i], Y2, c[i + 1], method)
            copyto!(Y2, Y)
        end
    end
    return Y
end
@doc raw"""
    function vector_transport_along!(
        M::Manifold,
        Y,
        p,
        X,
        c::AbstractVector,
        method::PoleLadderTransport
    )

Compute the vector transport along a discretized curve using
[`PoleLadderTransport`](@ref) succesively along the sampled curve.
This method is avoiding additional allocations as well as inner exp/log by performing all
ladder steps on the manifold and only computing one tangent vector in the end.
"""
function vector_transport_along!(
    M::Manifold,
    Y,
    p,
    X,
    c::AbstractVector,
    method::PoleLadderTransport,
)
    clen = length(c)
    if clen == 0
        copyto!(Y, X)
    else
        d = retract(M, p, X, method.retraction)
        m = mid_point(M, p, c[1])
        pole_ladder!(
            M,
            d,
            p,
            d,
            c[1],
            m,
            Y;
            retraction = method.retraction,
            inverse_retraction = method.inverse_retraction,
        )
        for i in 1:(clen - 1)
            # precompute mid point inplace
            ci = c[i]
            cip1 = c[i + 1]
            mid_point!(M, m, ci, cip1)
            # compute new ladder point
            pole_ladder!(
                M,
                d,
                ci,
                d,
                cip1,
                m,
                Y;
                retraction = method.retraction,
                inverse_retraction = method.inverse_retraction,
            )
        end
        inverse_retract!(M, Y, c[clen], d, method.inverse_retraction)
        Y *= (-1)^clen
    end
    return Y
end
@doc raw"""
    vector_transport_along!(
        M::Manifold,
        Y,
        p,
        X,
        c::AbstractVector,
        method::SchildsLadderTransport
    )

Compute the vector transport along a discretized curve using
[`SchildsLadderTransport`](@ref) succesively along the sampled curve.
This method is avoiding additional allocations as well as inner exp/log by performing all
ladder steps on the manifold and only computing one tangent vector in the end.
"""
function vector_transport_along!(
    M::Manifold,
    Y,
    p,
    X,
    c::AbstractVector,
    method::SchildsLadderTransport,
)
    clen = length(c)
    if clen == 0
        copyto!(Y, X)
    else
        d = retract(M, p, X, method.retraction)
        m = mid_point(M, c[1], d)
        schilds_ladder!(
            M,
            d,
            p,
            d,
            c[1],
            m,
            Y;
            retraction = method.retraction,
            inverse_retraction = method.inverse_retraction,
        )
        for i in 1:(clen - 1)
            ci = c[i]
            cip1 = c[i + 1]
            # precompute mid point inplace
            mid_point!(M, m, cip1, d)
            # compute new ladder point
            schilds_ladder!(
                M,
                d,
                ci,
                d,
                cip1,
                m,
                Y;
                retraction = method.retraction,
                inverse_retraction = method.inverse_retraction,
            )
        end
        inverse_retract!(M, Y, c[clen], d, method.inverse_retraction)
    end
    return Y
end

"""
    vector_transport_direction(M::Manifold, p, X, d)
    vector_transport_direction(M::Manifold, p, X, d, method::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`Manifold`](@ref) `M`
in the direction indicated by the tangent vector `d` at `p`. By default, [`exp`](@ref) and
[`vector_transport_to!`](@ref) are used with the `method`, which defaults
to [`ParallelTransport`](@ref).
"""
function vector_transport_direction(M::Manifold, p, X, d)
    return vector_transport_direction(M, p, X, d, ParallelTransport())
end
function vector_transport_direction(
    M::Manifold,
    p,
    X,
    d,
    method::AbstractVectorTransportMethod,
)
    Y = allocate_result(M, vector_transport_direction, X, p, d)
    vector_transport_direction!(M, Y, p, X, d, method)
    return Y
end

"""
    vector_transport_direction!(M::Manifold, Y, p, X, d)
    vector_transport_direction!(M::Manifold, Y, p, X, d, method::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`Manifold`](@ref) `M`
in the direction indicated by the tangent vector `d` at `p`. By default, [`exp`](@ref) and
[`vector_transport_to!`](@ref) are used with the `method`, which defaults
to [`ParallelTransport`](@ref). The result is saved to `Y`.
"""
function vector_transport_direction!(M::Manifold, Y, p, X, d)
    return vector_transport_direction!(M, Y, p, X, d, ParallelTransport())
end
function vector_transport_direction!(
    M::Manifold,
    Y,
    p,
    X,
    d,
    method::AbstractVectorTransportMethod,
)
    y = exp(M, p, d)
    return vector_transport_to!(M, Y, p, X, y, method)
end

"""
    vector_transport_to(M::Manifold, p, X, q)
    vector_transport_to(M::Manifold, p, X, q, method::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`Manifold`](@ref) `M`
along the [`shortest_geodesic`](@ref) to the tangent space at another point `q`.
By default, the [`AbstractVectorTransportMethod`](@ref) `method` is
[`ParallelTransport`](@ref).
"""
function vector_transport_to(M::Manifold, p, X, q)
    return vector_transport_to(M, p, X, q, ParallelTransport())
end
function vector_transport_to(M::Manifold, p, X, q, method::AbstractVectorTransportMethod)
    Y = allocate_result(M, vector_transport_to, X, p, q)
    vector_transport_to!(M, Y, p, X, q, method)
    return Y
end

"""
    vector_transport_to!(M::Manifold, Y, p, X, q)
    vector_transport_to!(M::Manifold, Y, p, X, q, method::AbstractVectorTransportMethod)

Transport a vector `X` from the tangent space at a point `p` on the [`Manifold`](@ref) `M`
along the [`shortest_geodesic`](@ref) to the tangent space at another point `q`.
By default, the [`AbstractVectorTransportMethod`](@ref) `method` is
[`ParallelTransport`](@ref). The result is saved to `Y`.
"""
function vector_transport_to!(M::Manifold, Y, p, X, q)
    return vector_transport_to!(M, Y, p, X, q, ParallelTransport())
end
"""
    vector_transport_to!(M::Manifold, Y, p, X, q, method::ProjectionTransport)

Transport a vector `X` from the tangent space at `p` on the [`Manifold`](@ref) `M` by
interpreting it as an element of the embedding and then projecting it onto the tangent space
at `q`. This function needs to be separately implemented for each manifold because
projection [`project`](@ref project(M::Manifold, p, X)) may also change vector
representation (if it's different than in the embedding) and it is assumed that the vector
`X` already has the correct representation for `M`.
"""
vector_transport_to!(M::Manifold, Y, p, X, q, m::ProjectionTransport)

@doc raw"""
    vector_transport_to!(M::Manifold, Y, p, X, q, method::PoleLadderTransport)

Perform a vector transport by using [`PoleLadderTransport`](@ref).
"""
function vector_transport_to!(M::Manifold, Y, p, X, q, m::PoleLadderTransport)
    inverse_retract!(
        M,
        Y,
        q,
        pole_ladder(
            M,
            p,
            retract(M, p, X, m.retraction),
            q;
            retraction = m.retraction,
            inverse_retraction = m.inverse_retraction,
        ),
        m.inverse_retraction,
    )
    copyto!(Y, -Y)
    return Y
end

function vector_transport_to!(
    M::Manifold,
    Y,
    p,
    X,
    q,
    m::ScaledVectorTransport{T},
) where {T<:AbstractVectorTransportMethod}
    vector_transport_to!(M, Y, p, X, q, m.method)
    Y .*= norm(M, p, X) / norm(M, q, Y)
    return Y
end

@doc raw"""
    vector_transport_to!(M::Manifold, Y, p, X, q, method::SchildsLadderTransport)

Perform a vector transport by using [`SchildsLadderTransport`](@ref).
"""
function vector_transport_to!(M::Manifold, Y, p, X, q, m::SchildsLadderTransport)
    return inverse_retract!(
        M,
        Y,
        q,
        schilds_ladder(
            M,
            p,
            retract(M, p, X, m.retraction),
            q;
            retraction = m.retraction,
            inverse_retraction = m.inverse_retraction,
        ),
        m.inverse_retraction,
    )
end

function vector_transport_to!(
    M::Manifold,
    Y,
    p,
    X,
    q,
    method::AbstractVectorTransportMethod,
)
    return error(
        manifold_function_not_implemented_message(
            M,
            vector_transport_to!,
            Y,
            p,
            X,
            q,
            method,
        ),
    )
end

const VECTOR_TRANSPORT_DISAMBIGUATION =
    [PoleLadderTransport, ScaledVectorTransport, SchildsLadderTransport]


using StatsModels, BlockDiagonals
"""
    olsranef(model::LinearMixedModel, method=:simultaneous)

Compute the group-level estimates using ordinary least squares.

This is somewhat similar to the conditional modes / BLUPs computed without
shrinkage.

There is no shrinkage of the group-level estimates with this approach, which
means singular estimates can be avoided. However, this also means the random
effects design matrix must not be singular.

Two methods are provided:
1. (default) OLS estimates computed for all strata (blocking variables)
   simultaneously with `method=simultaneous`. This pools the variance
   across estimates but does not shrink the estimates. Note that this method
   internal reparameterizes the random effects matrix `Z` to use effects coding
   and only use a single intercept shared across all grouping variables.
2. OLS estimates computed within each stratum with `method=stratum`. This
   method is equivalent for example to computing each subject-level and each
   item-level regression separately.

For fully balanced designs with a single blocking variable, these methods will
give the same results.

!!! warning
    If the design matrix for the random effects is rank deficient (e.g., through
    the use of `MixedModels.fulldummy` or missing cells in the data), then these
    methods will fail because no shrinkage/regularization is applied.
"""
function olsranef(model::LinearMixedModel{T}, method=:simultaneous) where {T}
    fixef_res = copy(response(model))
    # what's not explained by the fixed effects has to be explained by the RE
    X = model.X
    β = model.β # (X'X) \ (X'fixef_res)
    mul!(fixef_res, X, β, -one(T), one(T))

    return olsranef(model, fixef_res, Val(method))
end

function olsranef_org(model::LinearMixedModel{T}, fixef_res, ::Val{:stratum}) where {T}

    blups = Vector{Matrix{T}}()
    for trm in model.reterms
        re = []

        z = trm.z
        refs = unique(trm.refs)
        ngrps = length(refs)
        npreds = length(trm.cnames)

        j = 1
        for r in refs
            i = trm.refs .== r
            X = trm[i, j:(j+npreds-1)]
            b = (X'X) \ (X'fixef_res[i])
            push!(re, b)
            j += npreds
        end

        push!(blups, hcat(re...))
    end
    return blups, dummy_scalings(model.reterms)
end

function dummy_scalings(reterms)
     scalings = repeat([LinearAlgebra.I],length(reterms))
      scalings = vcat(scalings,1.) # add sigma scaling
      return scalings

end
function olsranef(model::LinearMixedModel{T}, fixef_res, ::Val{:simultaneous}) where {T}
    n_reterms = size(model.reterms)[1]

    code = Vector{Matrix{T}}(undef, n_reterms) # new contrast matrices
    mat = Vector{Matrix{T}}(undef, n_reterms) # new Z model matrices

    ### create new Z matrices with orthogonal contrasts
    for (i, trm) in enumerate(model.reterms)
        dim = size(trm.z)[1]
        cd = StatsModels.ContrastsMatrix(EffectsCoding(), trm.levels).matrix
        cd = kron(cd, I(dim))
        code[i] = cd
        mat[i] = trm * cd
    end

    # add in intercept
    # note that we pass two dims to ones to create a matrix
    pushfirst!(mat, ones(sum(first ∘ size, mat), 1))
    Z = hcat(mat...)
    # compute the BLUPs
    flatblups = Z'Z \ Z'fixef_res
    # get back to original coding
    flatblups = BlockDiagonal(code) * @view flatblups[2:end, :]

    blups = Vector{Matrix{T}}()
    offset = 1
    for trm in model.reterms
        chunksize = size(trm, 2)
        ngrps = length(trm.levels)
        npreds = length(trm.cnames)
        re = Matrix{T}(reshape(view(flatblups, offset:(offset+chunksize-1)),
                               npreds, ngrps))
        offset += chunksize
        push!(blups, re)
    end

    return blups, dummy_scalings(model.reterms)
end

"""
    residuals(model::LinearMixedModel, blups)

Compute the residuals of a mixed model using the specified group-level BLUPs/predictors.

This is useful for, e.g., comparing the residuals from a mixed-effects model with shrunken
group-level predictors against a non-shrunken classical OLS model fit within each group.
"""
function MixedModels.residuals(model::LinearMixedModel{T}, blups::Vector{<:AbstractMatrix{T}}) where T
    # XXX This is kinda type piracy, if it weren't developed by one of the MixedModels.jl devs....

    y = response(model) # we are now modifying the model

    ŷ = zeros(T, length(y))

    for (re, trm) in zip(blups, model.reterms)
        mul!(ŷ, trm, re, one(T), one(T))
    end

    mul!(ŷ, model.X, model.β, one(T), one(T))

    # TODO: do this inplace to avoid an allocation
    return y .- ŷ
end


using SumOfSquares
using DynamicPolynomials
using Test

include("sparse_polynomials.jl")

function test(test_function, optimizer_constructor)
    sparse_model = sos_lower_bound(test_function, optimizer_constructor, VariableSparsity())
    dense_model  = sos_lower_bound(test_function, optimizer_constructor, NoSparsity())
    return abs(objective_value(sparse_model) - objective_value(dense_model))
end

function chordal_sos(optimizer_constructor, tol=1e-5)
    val = test(chained_singular(4), optimizer_constructor)
    @test val < tol

    val = test(broyden_banded(2), optimizer_constructor)
    @test val < tol

    val = test(broyden_tridiagonal(4), optimizer_constructor)
    @test val < tol

    val = test(chained_wood(4), optimizer_constructor)
    @test val < tol

    val = test(generalized_rosenbrock(4), optimizer_constructor)
    @test val < tol
end

import CSDP
chordal_sos(CSDP.Optimizer)


mutable struct PerfectBipartiteMatchingHungarianSolver <: PerfectBipartiteMatchingSolver
  weights_matrix::Matrix{Float64}

  function PerfectBipartiteMatchingHungarianSolver()
    return new(zeros(0, 0))
  end
end

function build!(solver::PerfectBipartiteMatchingHungarianSolver, n_arms::Int)
  solver.weights_matrix = zeros(n_arms, n_arms)
  nothing
end

function solve_linear(solver::PerfectBipartiteMatchingHungarianSolver, rewards::Dict{Tuple{Int, Int}, Float64})
  fill!(solver.weights_matrix, 0.0)
  for (k, v) in rewards
    solver.weights_matrix[k[1], k[2]] = v
  end
  solver.weights_matrix = maximum(solver.weights_matrix) .- solver.weights_matrix

  matching = Hungarian.munkres!(solver.weights_matrix)
  indices = CartesianIndices(matching)
  return [(indices[v][1], indices[v][2]) for v in findall(matching .== Hungarian.STAR)]
end

has_lp_formulation(::PerfectBipartiteMatchingHungarianSolver) = false


using MIToS.MSA
using MIToS.Information

const msa_long = read("../data/PF00089_aligned.fasta", FASTA)
const msa_wide = read("../data/PF16957_aligned.fasta", FASTA)

mapcolfreq!(entropy, msa_long, Probabilities{Float64,1,UngappedAlphabet}(ContingencyTable(Float64,Val{1},UngappedAlphabet())))
mapcolfreq!(entropy, msa_wide, Probabilities{Float64,1,UngappedAlphabet}(ContingencyTable(Float64,Val{1},UngappedAlphabet())))

println("[BENCH] Shannon entropy PF00089: ", @elapsed mapcolfreq!(entropy, msa_long, Probabilities{Float64,1,UngappedAlphabet}(ContingencyTable(Float64,Val{1},UngappedAlphabet()))))
println("[BENCH] Shannon entropy PF16957: ", @elapsed mapcolfreq!(entropy, msa_wide, Probabilities{Float64,1,UngappedAlphabet}(ContingencyTable(Float64,Val{1},UngappedAlphabet()))))


function getTableauVSPARK(s, σ, o, tsym, g, h, lq, lp, ω, d=Nothing)
    # α_q_1 and α_p_2 need to be conjugate symplectic
    # α_q_1 and α_p_3 need to be conjugate symplectic
    # α_q_2 and α_p_2 need to be conjugate symplectic
    # α_q_2 and α_p_3 need to be conjugate symplectic
    # -> α_q_1 and α_q_2 need to be identical
    # -> α_p_2 and α_p_3 need to be identical

    α_q_1 = lq.a
    α_q_2 = lq.a
    α_p_2 = lp.a
    α_p_3 = lp.a

    b_q_1 = lq.b
    b_q_2 = lq.b
    b_p_2 = lp.b
    b_p_3 = lp.b

    # α_p_1 is free, but determines a_q_1 and a_q_2
    α_p_1 = h.a

    # a_p_1, a_p_2 and a_p_3 are free (they are not used in VPSARKpSecondary integrators anymore)
    a_p_1 = g.a
    b_p_1 = g.b

    β_q_1 = lq.b
    β_q_2 = lq.b

    β_p_1 = lp.b
    β_p_2 = lp.b
    β_p_3 = lp.b

    # a_q_1_ij = b_q_1_j / b_p_1_i * (b_p_1_i - α_p_1_ji)
    a_q_1 = zeros(s,σ)
    for i in 1:s
        for j in 1:σ
            a_q_1[i,j] = b_q_1[j] / b_p_1[i] * (b_p_1[i] - α_p_1[j,i])
        end
    end

    # a_q_2_ij = b_q_2_j / b_p_1_i * (b_p_1_i - α_p_1_ji)
    a_q_2 = zeros(s,σ)
    for i in 1:s
        for j in 1:σ
            a_q_2[i,j] = b_q_2[j] / b_p_1[i] * (b_p_1[i] - α_p_1[j,i])
        end
    end

    a_p_2 = a_q_1
    a_p_3 = a_q_2

    a_q = (a_q_1, a_q_2)
    b_q = (b_q_1, b_q_2)
    c_q = g.c

    a_p = (a_p_1, a_p_2, a_p_3)
    b_p = (b_p_1, b_p_2, b_p_3)
    c_p = g.c

    α_q = (α_q_1, α_q_2)
    β_q = (β_q_1, β_q_2)
    γ_q = lq.c

    α_p = (α_p_1, α_p_2, α_p_3)
    β_p = (β_p_1, β_p_2, β_p_3)
    γ_p = lp.c

    coeff_q = CoefficientsSPARK(tsym, o, s, σ, a_q, b_q, c_q)
    coeff_p = CoefficientsSPARK(tsym, o, s, σ, a_p, b_p, c_p)
    coeff_q̃ = CoefficientsSPARK(tsym, o, σ, s, α_q, β_q, γ_q)
    coeff_p̃ = CoefficientsSPARK(tsym, o, σ, s, α_p, β_p, γ_p)

    TableauVSPARKsecondary(tsym, o, s, σ, coeff_q, coeff_p, coeff_q̃, coeff_p̃, ω, d)
end

function getTableauVSPARKLobIII(s, lq, lp)
    o = 2s-2
    tsym = Symbol("VSPARKLobIII")
    getTableauVSPARK(s, s, o, tsym, lq, lp, lq, lp, get_lobatto_ω_matrix(s), get_lobatto_d_vector(s))
end

function getTableauVSPARKLobIIIAB(s)
    lq = getCoefficientsLobIIIA(s)
    lp = getCoefficientsLobIIIB(s)
    getTableauVSPARKLobIII(s, lq, lp)
end

function getTableauVSPARKLobIIIC(s)
    l = getCoefficientsLobIII(s)
    getTableauVSPARKLobIII(s, l, l)
end

function getTableauVSPARKLobIIID(s)
    l = getCoefficientsLobIIID(s)
    getTableauVSPARKLobIII(s, l, l)
end

function getTableauVSPARKLobIIIE(s)
    l = getCoefficientsLobIIIE(s)
    getTableauVSPARKLobIII(s, l, l)
end

function getTableauVSPARKGLRKLobIII(s, σ, lq, lp)
    o = 2s
    g = getCoefficientsGLRK(s)
    tsym = Symbol("VSPARKGLRKLobIII")
    getTableauVSPARK(s, σ, o, tsym, g, get_lobatto_interstage_coefficients(s, σ), lq, lp, get_GLRK_ω_matrix(σ), get_lobatto_d_vector(σ))
end

function getTableauVSPARKGLRKLobIIIAB(s, σ=s+1)
    getTableauVSPARKGLRKLobIII(s, σ, getCoefficientsLobIIIA(σ), getCoefficientsLobIIIB(σ))
end

function getTableauVSPARKGLRKLobIIIC(s, σ=s+1)
    getTableauVSPARKGLRKLobIII(s, σ, getCoefficientsLobIII(σ), getCoefficientsLobIIIC(σ))
end

function getTableauVSPARKGLRKLobIIID(s, σ=s+1)
    getTableauVSPARKGLRKLobIII(s, σ, getCoefficientsLobIIID(σ), getCoefficientsLobIIID(σ))
end

function getTableauVSPARKGLRKLobIIIE(s, σ=s+1)
    getTableauVSPARKGLRKLobIII(s, σ, getCoefficientsLobIIIE(σ), getCoefficientsLobIIIE(σ))
end


using LinearAlgebra
using MAT
using AdFem
using PyPlot; matplotlib.use("agg")
using SparseArrays

ADCME.options.sparse.auto_reorder = false

function compute_residual_and_jacobian(k_chip, S)
    # compute r and J in Jx=r for Newton's method
    # read in current step
    k_fem = k_air * constant(ones(ndof))
    k_fem = scatter_update(k_fem, solid_fem_idx, k_mold * ones(length(solid_fem_idx)))
    # k_fem = scatter_update(k_fem, chip_fem_idx, k_chip * ones(length(chip_fem_idx))) # if k_chip is a constant
    k_fem = scatter_update(k_fem, chip_fem_idx, k_chip)

    kgauss = dof_to_gauss_points(k_fem, mesh)
    LaplaceK = constant(compute_fem_laplace_matrix1(kgauss, mesh))

    u, v, p, T = S[1:ndof],
        S[ndof+1:2*ndof],
        S[2*ndof+1:2*ndof+nelem],
        S[2*ndof+nelem+1:end]

    grad_u = eval_grad_on_gauss_pts1(u, mesh)
    grad_v = eval_grad_on_gauss_pts1(v, mesh)

    ugauss = dof_to_gauss_points(u, mesh)
    vgauss = dof_to_gauss_points(v, mesh)
    ux, uy, vx, vy = grad_u[:,1], grad_u[:,2], grad_v[:,1], grad_v[:,2]

    # comupute jacobian
    M1 = constant(compute_fem_mass_matrix1(ux, mesh))
    M2 = constant(compute_fem_advection_matrix1(constant(ugauss), constant(vgauss), mesh)) # a julia kernel needed
    M3 = Laplace
    Fu = M1 + M2 + M3

    Fv = constant(compute_fem_mass_matrix1(uy, mesh))

    N1 = constant(compute_fem_mass_matrix1(vy, mesh))
    N2 = constant(compute_fem_advection_matrix1(constant(ugauss), constant(vgauss), mesh))
    N3 = Laplace
    Gv = N1 + N2 + N3

    Gu = constant(compute_fem_mass_matrix1(vx, mesh))

    M = LaplaceK + constant(compute_fem_advection_matrix1(ugauss,vgauss, mesh))

    gradT = eval_grad_on_gauss_pts1(T, mesh)
    Tx, Ty = gradT[:,1], gradT[:,2]
    DU_TX = constant(compute_fem_mass_matrix1(Tx, mesh))
    DV_TY = constant(compute_fem_mass_matrix1(Ty, mesh))

    T_mat = constant(compute_fem_mass_matrix1(-buoyance_coef * constant(ones(ndof)), mesh))
    T_mat = [SparseTensor(spzeros(ndof, ndof)); T_mat]

    J0 = [Fu Fv
          Gu Gv]

    J1 = [J0 -B' T_mat
        -B spdiag(zeros(size(B,1))) SparseTensor(spzeros(nelem, ndof))]

    J = [J1
        [DU_TX DV_TY SparseTensor(spzeros(ndof, nelem)) M]]

    # compute residual
    interaction = compute_interaction_term(p, mesh)
    f1 = compute_fem_source_term1(ugauss.*ux, mesh)
    f2 = compute_fem_source_term1(vgauss.*uy, mesh)
    f3 = -interaction[1:ndof]
    f4 = Laplace*u
    F = f1 + f2 + f3 + f4

    g1 = compute_fem_source_term1(ugauss.*vx, mesh)
    g2 = compute_fem_source_term1(vgauss.*vy, mesh)
    g3 = -interaction[ndof+1:end]
    g4 = Laplace*v
    T_gauss = dof_to_gauss_points(T, mesh)
    buoyance_term = - buoyance_coef * compute_fem_source_term1(T_gauss, mesh)
    G = g1 + g2 + g3 + g4 + buoyance_term

    H0 = -B * [u;v]

    T0 = LaplaceK * T + compute_fem_advection_matrix1(ugauss,vgauss, mesh) * T - heat_source

    R = [F;G;H0;T0]

    return R, J

end

function solve_one_step(θ, S)
    function f(θ, S)
        r, J = compute_residual_and_jacobian(θ, S)
        J, r = impose_Dirichlet_boundary_conditions(J, r, bd, zeros(length(bd)))
        # residual_norm = norm(r)
        # op = tf.print("residual norm", residual_norm)
        # r = bind(r, op)
        return r, J
    end

    S_new = newton_raphson_with_grad(f, S, θ)
    return S_new
end

function solve_navier_stokes(S0, NT, θ)
    function condition(i, S_arr)
        i <= NT
    end

    function body(i, S_arr)
        S = read(S_arr, i)
        op = tf.print("i=",i)
        i = bind(i, op)
        S_new = solve_one_step(θ, S)
        S_arr = write(S_arr, i+1, S_new)
        return i+1, S_arr
    end

    i = constant(1, dtype=Int32)
    S_arr = TensorArray(NT+1)
    S_arr = write(S_arr, 1, S0)
    _, S = while_loop(condition, body, [i, S_arr])
    S = set_shape(stack(S), NT+1, length(S0))
end


# ----------------------------------------------------------------------------------------------------------------------
# Question 5
# Fomulation of Matrix A
mat_a1_2 = zeros(numNodes^2, numNodes^2)
for mn = 1: numNodes^2
    mat_a1_2[mn, mn] = -1
end
mat_a3_2 = zeros(numNodes, numNodes^2)
seq_zeroTenNinety = collect(0:10:90)
for m = 1: numNodes
    mat_a3_2[m, (10 * m - 9): (10 * m)] = repeat([-1], 10)  # sent out
    mat_a3_2[m, (seq_zeroTenNinety + repeat([m],10))] = repeat([1], 10)  # sent in
    mat_a3_2[:, (11 * m - 10)] = repeat([0], 10)  # self-sending doesn't count
end
mat_a_2 = vcat(mat_a1_2, mat_a3_2)
# Fomulation of Matrix B
mat_b1_2 = zeros(numNodes^2, numNodes^2)
for mn = 1: numNodes^2
    mat_b1_2[mn, mn] = 170
end
mat_b3_2 = zeros(numNodes, numNodes^2)
mat_b_2 = vcat(mat_b1_2, mat_b3_2)
# Fomulation of Vector b
vec_b1_2 = zeros(numNodes^2)
vec_b3_2 = zeros(numNodes)
for n = 1: numNodes
    vec_b3_2[n] = vec_netInject[n] - vec_netEject[n]
end
vec_b_2 = vcat(vec_b1_2, vec_b3_2)
vec_b_2 = hcat(vec_b_2)
#
vec_max_y_2 = repeat([1], numNodes^2)
vec_max_y_2 = hcat(vec_max_y_2)
#
vec_c_2 = zeros(numNodes^2)
for m = 1: numNodes
    for n = 1: numNodes
        vec_c_2[10 * (m - 1) + n] = mat_distance[m, n]
    end
end
vec_c_2 = hcat(vec_c_2)
#
vec_f_2 = zeros(numNodes^2)
for m = 1: numNodes
    for n = 1: numNodes
        vec_f_2[10 * (m - 1) + n] = mat_fixedCost[m, n]
    end
end
vec_f_2 = hcat(vec_f_2)
#
Benders.milp(n_x = numNodes^2,
            n_y = numNodes^2,
            vec_min_y = repeat([0], numNodes^2),
            vec_max_y = vec_max_y_2,
            vec_c = vec_c_2,
            vec_f = vec_f_2,
            vec_b = vec_b_2,
            mat_a = mat_a_2,
            mat_b = mat_b_2,
            epsilon = 0.0001,
            timesIterationMax = 1000)
# obj - sum(mat_arcTwoNodes .* mat_fixedCost)
# ----------------------------------------------------------------------------------------------------------------------
# using LightGraphs
# using GraphPlot
# using Compose
# using Cairo
# using Fontconfig
# mat_result_y = [0 1 0 0 0 0 0 0 0 0;
#                 0 0 0 0 0 1 0 0 0 0;
#                 0 0 0 0 0 0 0 0 0 1;
#                 1 0 0 0 1 0 0 0 0 0;
#                 0 0 0 0 0 1 0 0 0 0;
#                 1 1 0 1 1 0 0 1 0 0;
#                 0 1 1 0 0 0 0 0 0 0;
#                 0 0 0 0 0 0 0 0 1 0;
#                 0 0 0 0 1 0 0 0 0 0;
#                 0 0 1 0 0 1 1 1 1 0]
# graph_initial = DiGraph(mat_arcTwoNodes)
# plot_graph_initial = gplot(graph_initial, nodelabel = vec_nameNodes)
# graph_result = graph_initial
# mat_arcTwoNodes_new = mat_result_y - mat_arcTwoNodes
# for m = 1: numNodes
#     for n = 1: numNodes
#         if mat_arcTwoNodes_new[m,n] == 1
#             add_edge!(graph_result, m, n)
#         end
#     end
# end
# gplot(graph_result, nodelabel = vec_nameNodes)
# draw(PNG("graph_initial.png", 16cm, 16cm), gplot(graph_initial))
# draw(PNG("graph_result.png", 16cm, 16cm), gplot(result))

# Final Corrected
vec_x= [0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        10 10 0 10 40 0 0 20 0 0
        0 20 20 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 65 0 15 0]
vec_y= [0 1 0 0 0 0 0 0 0 0
        0 0 0 0 0 1 0 0 0 0
        0 0 0 0 0 0 0 0 0 1
        1 0 0 0 1 0 0 0 0 0
        0 0 0 0 0 1 0 0 0 0
        1 1 0 1 1 0 0 1 0 0
        0 1 1 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 1 0
        0 0 0 0 1 0 0 0 0 0
        0 0 0 0 0 1 1 0 1 0]

vec_y: [0 1 0 0 0 0 0 0 0 0
        0 0 0 0 0 1 0 0 0 0
        0 0 0 0 0 0 0 0 0 1
        1 0 0 0 1 0 0 0 0 0
        0 0 0 0 0 1 0 0 0 0
        1 1 0 1 1 0 0 1 0 0
        0 1 1 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 1 0
        0 0 0 0 1 0 0 0 0 0
        0 0 1 0 0 1 1 1 1 0]
result_q: 600.0

vec_x= [0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        10 30 0 10 40 0 0 0 0 0
        0 0 0 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 15 0
        0 0 0 0 0 0 0 0 0 0
        0 0 20 0 0 0 25 35 0 0]

# vec_result_x = reshape(transpose(vec_x), numNodes^2)
vec_result_node = zeros(numNodes)
for m = 1: numNodes
    vec_result_node[m] = sum(vec_x[(10 * m - 9): (10 * m)]) - sum(vec_x[(seq_zeroTenNinety + repeat([m],10))])
end
vec_y= [0 1 0 0 0 0 0 0 0 0
        0 0 0 0 0 1 0 0 0 0
        0 0 0 0 0 0 0 0 0 1
        1 0 0 0 1 0 0 0 0 0
        0 0 0 0 0 1 0 0 0 0
        1 1 0 1 1 0 0 1 0 0
        0 1 1 0 0 0 0 0 0 0
        0 0 0 0 0 0 0 0 1 0
        0 0 0 0 1 0 0 0 0 0
        0 0 1 0 0 1 1 1 1 0]


# minimal use of the package, user manages all the plotting
# PyPlot version

module M
using Pseudospectra
using PyPlot

A = randn(100,100)
# we want a true Schur decomposition, so force complexity
Tschur,U,eigA  = schur(A .+ 0im)
ax = [-12,12,-12,12]
npts = 100
opts = Dict{Symbol,Any}(:real_matrix => true)
Z,x,y,levels,err,Tproj,eigAproj,algo = psa_compute(Tschur,npts,
                                                   ax,eigA,opts)
plot(real(eigA),imag(eigA),"k.")
if isempty(levels)
    contour(x,y,log10.(Z),extend="both",linewidths=2)
else
    contour(x,y,log10.(Z),levels=levels,extend="both",linewidths=2)
end
colorbar()
end


abstract type BasisFunction end

#1s Gaussian-type function
abstract type Gaussian1s <: BasisFunction end
    #Gaussian orbital exponent
    alpha :: Float64
    #nuclear coordinates
    center::Float64
end

#Slater Type Orbital fit with N primative gausians (STO-NG) type basis
type STONG <: BasisFunction
    n::Integer
    #contraction coeffiecents
    d::Array{Float64}
    #primative gaussians
    g::Array{Gaussian1s}
end

#STO-3G basis for hydrogen
function sto3g_hydrogen(center)
    sto3g(center, 1.24)
end

#STO-3G basis for helium
function sto3g_helium(center)
    sto3g(center, 2.0925)
end

#Builds a STO-3G basis that best approximates a single slater type
#orbital with Slater orbital exponent zeta
function sto3g(center, zeta)
    scaling = zeta^2
    STONG(3,[0.444635, 0.535328, 0.154329],
            [Gaussian1s(scaling*.109818, center),
             Gaussian1s(scaling*.405771, center),
             Gaussian1s(scaling*2.22766, center)])
end

#The overlap integrals describe how the basis functions overlap
#as the atom centered gaussian basis functions are non-orthognal
#they have a non-zero overlap. The integral has the following form:
#S_{ij} = \int \phi_i(r-R_a) \phi_j(r-R_b) \mathrm{d}r
function overlap_integral(b1::STONG, b2::STONG)
    two_center_contraction(b1, b2, overlap_integral)
end

#This function calculates the overlap integral for 1s Gaussian
#orbitals using a closed-form expression.
function overlap_integral(g1::Gaussian1s, g2::Gaussian1s)
    alpha = g1.alpha
    beta = g2.alpha
    Ra = g1.center
    Rb = g2.center

    #normalization constant
    n = (2*alpha/pi)^(3/4) * (2*beta/pi)^(3/4)

    I  = n * (pi/(alpha+beta))^(3/2)
    I *= exp(-alpha*beta/(alpha+beta) * abs(Ra-Rb)^2)

    return I
end

#
function nuclear_attraction_integral(Zc::Int, Rc::Float64, b1::STONG,
                                     b2::STONG)
    integral(g1, g2) = nuclear_attraction_integral(Zc, Rc, g1, g2)
    two_center_contraction(b1, b2, integral)
end

function nuclear_attraction_integral(Zc::Int, Rc::Float64, g1::Gaussian1s, g2::Gaussian1s)
    alpha = g1.alpha
    beta  = g2.alpha
    Ra = g1.center
    Rb = g2.center
    Rp = (alpha*Ra + beta*Rb)/(alpha + beta)

    n = (2*alpha/pi)^(3/4) * (2*beta/pi)^(3/4)
    matrix_element  = n*-2*pi/(alpha+beta)*Zc
    matrix_element *= exp(-alpha*beta/(alpha+beta)*abs(Ra-Rb)^2)

    t = (alpha+beta)*abs(Rp-Rc)^2
    if abs(t) < 1e-8
        return matrix_element
    end

    matrix_element *= 0.5 * sqrt(pi/t) * erf(sqrt(t))
    return matrix_element
end

function kinetic_energy_integral(b1::STONG, b2::STONG)
    two_center_contraction(b1, b2, kinetic_energy_integral)
end

function kinetic_energy_integral(g1::Gaussian1s, g2::Gaussian1s)
    alpha = g1.alpha
    beta = g2.alpha
    Ra = g1.center
    Rb = g2.center

    n = (2*alpha/pi)^(3/4) * (2*beta/pi)^(3/4)

    matrix_element  = n * alpha*beta/(alpha+beta)
    matrix_element *= (3-2*alpha*beta/((alpha+beta)/abs(Ra-Rb)^2 ))
    matrix_element *= (pi/(alpha+beta))^(3/2)
    matrix_element *= exp(-alpha*beta/(alpha+beta) * abs(Ra-Rb)^2)
end

function two_electron_integral(g1::STONG, g2::STONG, g3::STONG, g4::STONG)
    four_center_contraction(g1, g2, g3, g4, two_electron_integral)
end

function two_electron_integral(g1::Gaussian1s, g2::Gaussian1s, g3::Gaussian1s,
                               g4::Gaussian1s)
    alpha = g1.alpha
    beta  = g2.alpha
    gamma = g3.alpha
    delta = g4.alpha
    Ra = g1.center
    Rb = g2.center
    Rc = g3.center
    Rd = g4.center
    Rp = (alpha*Ra + beta*Rb)/(alpha + beta)
    Rq = (gamma*Rc + delta*Rd)/(gamma + delta)

    n  = (2*alpha/pi)^(3/4) * (2*beta/pi)^(3/4)
    n *= (2*gamma/pi)^(3/4) * (2*delta/pi)^(3/4)

    matrix_element  = n*2*pi^(5/2)
    matrix_element /= ((alpha+beta)*(gamma+delta)*sqrt(alpha+beta+gamma+delta))
    matrix_element *= exp(-alpha*beta/(alpha+beta)*abs(Ra-Rb)^2 - gamma*delta/(gamma+delta)*abs(Rc-Rd)^2)
    t = (alpha+beta)*(gamma+delta)/(alpha+beta+gamma+delta)*abs(Rp-Rq)^2
    if abs(t) < 1e-8
        return matrix_element
    end

    matrix_element *= 0.5 * sqrt(pi/t) * erf(sqrt(t))
    return matrix_element
end

function two_center_contraction(b1::STONG, b2::STONG, integral::Function)
    total = 0.0
    for p = 1:b1.n
        for q = 1:b2.n
            d1 = b1.d[p]
            d2 = b2.d[q]
            total += d1*d2*integral(b1.g[p], b2.g[q])
        end
    end
    return total
end

function four_center_contraction(b1::STONG, b2::STONG, b3::STONG, b4::STONG,
                                 integral::Function)
    total = 0.0
    for p = 1:b1.n
        for q = 1:b2.n
            for r = 1:b3.n
                for s = 1:b4.n
                    dp = b1.d[p]
                    dq = b2.d[q]
                    dr = b3.d[r]
                    ds = b4.d[s]
                    total += dp*dq*dr*ds*integral(b1.g[p], b2.g[q], b3.g[r], b4.g[s])
                end
            end
        end
    end
    total
 end

function hartree_fock(R, Z)
    #println("constructing basis set")
    phi = Array(BasisFunction, length(Z))
    for A = 1:length(Z)
        if Z[A] == 1
            phi[A] = sto3g_hydrogen(R[A])
        elseif Z[A] == 2
            phi[A] = sto3g_helium(R[A])
        end
    end
    #calculate the overlap matrix S
    #the matrix should be symmetric with diagonal entries equal to one
    #println("building overlap matrix")
    S = eye(length(phi))
    for i = 1:length(phi)
        for j = (i+1):length(phi)
            S[i,j] = S[j,i] = overlap_integral(phi[i], phi[j])
        end
    end

    #println("S: ", S)

    #calculate the kinetic energy matrix T
    #println("building kinetic energy matrix")
    T = zeros(length(phi), length(phi))
    for i = 1:length(phi)
        for j = i:length(phi)
            T[i,j] = T[j,i] = kinetic_energy_integral(phi[i], phi[j])
        end
    end

    #println("T: ", T)

    #calculate nuclear attraction matrices V_i
    #println("building nuclear attraction matrices")
    V = zeros(length(Z), length(Z))
    for A = 1:length(Z)
        for i = 1:length(phi)
            for j = i:length(phi)
                v = nuclear_attraction_integral(Z[A], R[A], phi[i], phi[j])
                V[i,j] += v
                if i != j
                    V[j,i] += v
                end
            end
        end
    end

    #println("V: ", V)

    #build core-Hamiltonian matrix
    #println("building core-Hamiltonian matrix")
    Hcore = T + V

    #println("Hcore: ", Hcore)

    #diagonalize overlap matrix to get transformation matrix X
    #println("diagonalizing overlap matrix")
    s, U = eig(S)
    #println("building transformation matrix")
    X = U*diagm(s.^(-1/2))*U'
    #println("X: ", X)

    #calculate all of the two-electron integrals
    K = length(phi)
    two_electron = zeros(K,K,K,K)
    for mu = 1:K
        for v = 1:K
            for lambda = 1:K
                for sigma = 1:K
                    coulomb  = two_electron_integral(phi[mu], phi[v],
                                                     phi[sigma], phi[lambda])
                    two_electron[mu,v,sigma,lambda] = coulomb
                    #println("coulomb  ($mu $v | $sigma $lambda): $coulomb")
                    exchange = two_electron_integral(phi[mu], phi[lambda],
                                                     phi[sigma], phi[v])
                    #println("exchange ($mu $lambda | $sigma $v): $exchange")
                    two_electron[mu,lambda,sigma,v] = exchange
                end
            end
        end
    end

    P = zeros(K,K)

    total_energy = 0.0
    old_energy = 0.0
    electronic_energy = 0.0

    printf("%4s %13s de\n", "iter", "total energy")
    for scf_iter = 1:100
        #calculate the two electron part of the Fock matrix
        G = zeros(size(Hcore))
        K = length(phi)
        for mu = 1:K
            for v = 1:K
                for lambda = 1:K
                    for sigma = 1:K
                        coulomb  = two_electron[mu,v,sigma,lambda]
                        exchange = two_electron[mu,lambda,sigma,v]
                        G[mu,v] += P[lambda,sigma]*(coulomb - 0.5*exchange)
                    end
                end
            end
        end

        #println("G: ", G)
        F = Hcore + G

        nuclear_energy = 0.0
        for A = 1:length(Z)
            for B = (A+1):length(Z)
                nuclear_energy += Z[A]*Z[B]/abs(R[A]-R[B])
            end
        end
        #println("E_nclr: $nuclear_energy")

        electronic_energy = 0.0
        for mu = 1:length(phi)
            for v = 1:length(phi)
                electronic_energy += P[v,mu]*(Hcore[mu,v]+F[mu,v])
            end
        end
        electronic_energy *= 0.5
        #println("E_elec: $electronic_energy")
        total_energy = electronic_energy + nuclear_energy
        printf("%3i %12.8f %12.4e\n", scf_iter, total_energy,
               total_energy - old_energy)

        if scf_iter > 2 && abs(old_energy - total_energy) < 1e-6
            break
        end

        #println("F: ", F)
        Fprime = X' * F * X
        #println("F': $Fprime")
        epsilon, Cprime = eig(Fprime)
        #println("epsilon: ", epsilon)
        #println("C': ", Cprime)
        C = real(X*Cprime)
        #println("C: ", C)

        P = zeros(size(Hcore))
        for mu = 1:length(phi)
            for v = 1:length(phi)
                P[mu,v] = 2*C[mu,1]*C[v,1]
            end
        end
        #println("P: ", P)

        old_energy = total_energy
    end

    return total_energy, electronic_energy
end

function test_h2()
    println("TESTING H2")
    total_energy, electronic_energy = hartree_fock([0., 1.4], [1, 1])
    szabo_energy = -1.8310
    if abs(electronic_energy - szabo_energy) > 1e-6
        println("TEST FAILED")
    else
        println("TEST PASSED")
    end

end

function test_heh()
    println("TESTING HEH+")
    total_energy, electronic_energy = hartree_fock([0., 1.4632], [2, 1])
    szabo_energy = -4.227529
    if abs(electronic_energy - szabo_energy) > 1e-6
        println("TEST FAILED")
    else
        println("TEST PASSED")
    end

end

function heh_pes()
    file = open("heh_pes.dat", "w")
    energy_he, energy_he = hartree_fock([0.0], [2])
    for r in linspace(0.7, 3.5, 25)
        total_energy, electronic_energy = hartree_fock([0., r], [2, 1])
        write(file, "$r $(total_energy - energy_he)\n")
    end
    close(file)
end

function tests()
    test_h2()
    test_heh()
end

tests()
heh_pes()


# The branching factors for the various INode levels, as determined by
# _config_for_size(N). A NodeConfig remains valid until the number of dictionary
# entries N reaches next_config_size - changing configurations is handled by
# insert!(dict,h,k,v).
struct NodeConfig
    inode_capacity::Vector{Int}
    next_config_size::Int
end

function _config_for_size2(N::Int)::NodeConfig
    if N < 16
        return NodeConfig(Int64[], 16)
    elseif N < 65536
        # For small dictionaries, use inode_capacity=4, and height increasing with N.
        num_bits = ceil(log2(N+1))
        leaf_bits = min(num_bits,4)
        num_inodes = max(1, convert(Int, ceil((num_bits-leaf_bits)/2)))
        inode_capacity = [4 for i in 1:num_inodes]
        next_config_size = 2^(4+2*(num_inodes))
    else
        # For large dictionaries, we use height 7, and inode capacity increasing with
        # N.  We partition ceil(log2(N)) bits among 7 levels, and to grow capacity as
        # N increases we add a single bit to one level.

        # We will have 1/7 of the bits at the leaf node, so 6/7 of the bits will be
        # partitioned among the inodes.  We start by giving all levels the same number
        # of bits, then take care of the remainder by putting one extra bit at deeper
        # levels.
        num_bits = ceil(log2(N+1))
        leaf_bits = max(4, convert(Int, floor(num_bits/7)))
        inode_bits = num_bits - leaf_bits
        m = convert(Int, floor(inode_bits/6))
        inode_capacity = [2^m for i in 1:6]
        have_bits = m*6
        for k=6:-1:1
            if have_bits < inode_bits
                inode_capacity[k] *= 2
                @dassert1 inode_capacity[k] <= 256   # N <= 2^56
                have_bits += 1
            else
                break
            end
        end
        next_config_size = convert(Int, 2^(floor(log2(N))+1))
    end
    shifts = [0 for i in 1:6]
    return NodeConfig(inode_capacity, next_config_size)
end

const config1 = _config_for_size2(1)
const config16 = _config_for_size2(16)
const config64 = _config_for_size2(64)
const config256 = _config_for_size2(256)

function _config_for_size(N::Int)::NodeConfig
    # For N < 1024, use one of the pre-allocated instances config1/config16/
    # config64/config256, to reduce space use by small dictionaries.
    if N < 64
        if N < 16
            @dassert1 config1.next_config_size == 16
            return config1
        else
            @dassert1 config16.next_config_size == 64
            return config16
        end
    elseif N < 1024
        if N < 256
            @dassert1 config64.next_config_size == 256
            return config64
        else
            @dassert1 config256.next_config_size == 1024
            return config256
        end
    else
        return _config_for_size2(N)
    end
end


# Sum over states solution for systems with few modes.

"""
Sum over states solution for a small system.
"""
struct SumOverStates <: Solution
    "Partition function."
    Z::Float64
    "Energy."
    E::Float64
    "Heat capacity."
    Cv::Float64
end

"""
    SumOverStates(sys::System, beta::Float64, basis_size::Int)

Calculate the solution for `sys` at `beta` with `basis_size` basis functions.
"""
function SumOverStates(sys::System, beta::Float64, basis_size::Int)
    basis = Basis(sys, basis_size)
    h0, V = operators(basis, sys)

    Es = eigvals(Symmetric(h0 + V))

    Z = sum(exp.(-beta * Es))
    E = sum(exp.(-beta * Es) .* Es) / Z
    Cv = sum(exp.(-beta * Es) .* (Es .- E).^2) / Z * beta^2

    SumOverStates(Z, E, Cv)
end


# nested-simulations.jl
# SimLynx Nested Simulation Environments Example

using SimLynx
SimLynx.greet()

using Distributions: Exponential, Uniform
using Random

const N_TELLERS = 2
const MEAN_INTERARRIVAL_TIME = 4.0
const MIN_SERVICE_TIME = 2.0
const MAX_SERVICE_TIME = 10.0

tellers = nothing

"Process to generate n customers arriving into the system."
@process generator(n::Integer) begin
    dist = Exponential(MEAN_INTERARRIVAL_TIME)
    for i = 1:n
        @schedule now customer(i)
        # We don't want to wait after the last customer, which could extend the
        # simulation time past the last customer leaving.
        if i < n
            work(rand(dist))
        end
    end
end

"The ith customer into the system."
@process customer(i::Integer) begin
    dist = Uniform(MIN_SERVICE_TIME, MAX_SERVICE_TIME)
    @with_resource tellers begin
        work(rand(dist))
    end
end

"Run the simulation for n customers."
function run_simulation(n1::Integer, n2::Integer)
    @assert n1 > 0 && n2 > 0
    println("SimLynx Nested Simulation Environments Example")
    println("Number of runs = $n1, number of customers = $n2")
    println("Single Queue, $N_TELLERS Teller Bank Model")
    @simulation begin
        for i = 1:n1
            @simulation begin
                global tellers = Resource(N_TELLERS, "tellers")
                @schedule at 0.0 generator(n2)
                start_simulation()
                print_stats(tellers.allocated, title="Allocated Statistics")
                print_stats(tellers.queue_length, title="Queue Length Statistics")
                plot_history(tellers.queue_length, title="Queue Length History")
                print_stats(tellers.wait, title="Queue Wait Statistics")
            end
        end
    end
end

run_simulation(10, 100)


@testset "Linear" begin
    xmax = 10
    g1(x) = sin((x-3)*2pi/(xmax-1)-1)
    f(x) = g1(x)
    A1 = Float64[f(x) for x in 1:xmax]
    fr(x) = (x^2) // 40 + 2

    ymax = 10
    g2(y) = cos(y/6)
    f(x,y) = g1(x)*g2(y)
    A2 = Float64[f(x,y) for x in 1:xmax, y in 1:ymax]

    for (constructor, copier) in ((interpolate, identity), (interpolate!, copy))
        isinplace = constructor == interpolate!
        itp1 = @inferred(constructor(copier(A1), BSpline(Linear())))
        itp2 = @inferred(constructor(copier(A2), BSpline(Linear())))

        @test parent(itp1) === itp1.coefs
        @test Interpolations.lbounds(itp1) == (1,)
        @test Interpolations.ubounds(itp1) == (xmax,)

        for (itp, A) in ((itp1, A1), (itp2, A2))
            check_axes(itp, A, isinplace)
            check_inbounds_values(itp, A)
            check_oob(itp)
            can_eval_near_boundaries(itp)
            I = first(eachindex(itp))
            @test itp(I) == itp(Tuple(I)...)
        end

        # Just interpolation
        for x in 1:.2:xmax
            @test f(x) ≈ itp1(x) atol=abs(0.1 * f(x))
        end

        # 2D
        for x in 2.1:.2:xmax-1, y in 1.9:.2:ymax-.9
            @test ≈(f(x,y),itp2(x,y),atol=abs(0.25 * f(x,y)))
        end

        # Rational element types
        A1R = Rational{Int}[fr(x) for x in 1:10]
        itp1r = @inferred(constructor(copier(A1R), BSpline(Linear())))
        @test @inferred(size(itp1r)) == size(A1R)
        @test itp1r(23 // 10) ≈ fr(23 // 10) atol=abs(0.1 * fr(23 // 10))
        @test typeof(itp1r(23//10)) == Rational{Int}
        @test eltype(itp1r) == Rational{Int}
    end

    # Issue #183
    x = rand(3,3,3)
    itp = interpolate(x, BSpline(Linear()))
    @test itp(1.5, CartesianIndex((2, 3))) === itp(1.5, 2, 3)
    @test itp(CartesianIndex((1, 2)), 1.5) === itp(1, 2, 1.5)

    # Issue #289
    for (x, it) in ((ones(1, 3), (NoInterp(), BSpline(Linear()))),
                    (ones(3, 1), (BSpline(Linear()), NoInterp())))
        @test_throws ArgumentError interpolate(x, BSpline(Linear()))
        itp = interpolate(x, it)
        etp = extrapolate(itp, Flat())
        @test sum(isnan.(etp)) == 0
    end

    @testset "Linear periodic" begin
        # Constructors
        @test Linear() === Linear(Throw(OnGrid())) === Linear{Throw{OnGrid}}()
        @test Linear() isa Linear{Throw{OnGrid}}
        @test Linear(Periodic()) === Linear(Periodic(OnCell())) == Linear{Periodic{OnCell}}()
        @test Linear(Periodic()) isa Linear{Periodic{OnCell}}

        xmax = 10
        f2(x) = sin((x-3)*2pi/xmax - 1) # Periodicity is xmax, not xmax-1
        A1 = Float64[f2(x) for x in 1:xmax]

        for (constructor, copier) in ((interpolate, identity), (interpolate!, copy))
            isinplace = constructor == interpolate!
            itp = @inferred(constructor(copier(A1), BSpline(Linear())))
            itp_periodic = @inferred(constructor(copier(A1), BSpline(Linear(Periodic()))))

            @test all(Interpolations.lbounds(itp_periodic) .≈ (0.5,))
            @test all(Interpolations.ubounds(itp_periodic) .≈ (10.5,))

            for x in 1:.2:xmax
                @test itp(x) ≈ itp_periodic(x)
            end

            # Interpolation range for periodic should be [0.5, xmax + 0.5]
            for x in 0.5:.1:0.9
                @test f2(x) ≈ itp_periodic(x) atol=abs(0.1 * f2(x))
                @test_throws BoundsError itp(x)
            end
            for x in xmax+0.1:.1:xmax+0.5
                @test f2(x) ≈ itp_periodic(x) atol=abs(0.1 * f2(x))
                @test_throws BoundsError itp(x)
            end
        end
    end
end


"""
	2D
"""

function integralArray( img::AbstractArray{<:Real,2}; type=Float32, fun=(x)->(x) )
	return integralArray!( img, zeros( type, size(img) .+ 1 ), fun=fun )
end

function integralArray!( img::AbstractArray{<:Real,2}, intArr::AbstractArray{<:AbstractFloat,2}; fun=(x)->(x) )

	@inbounds for c in 1:size(img,2), r in 1:size(img,1)
		intArr[1+r,1+c] = fun(img[r,c]) + intArr[1+r-1,1+c] + intArr[1+r,1+c-1] - intArr[1+r-1,1+c-1]
	end
	return intArr
end

function integralArea( intArr::AbstractArray{<:Real,2}, TL, BR )
	TL   = TL .+ 1;
	BR   = BR .+ 1;
	area = intArr[BR[1],BR[2]] - intArr[BR[1],TL[2]] - intArr[TL[1],BR[2]] + intArr[TL[1],TL[2]]
end

# integral array for ZNCC, copied from quickPIV

function integralArrayZNCC( padData::AbstractArray{Complex{T},N} ) where {T<:AbstractFloat,N}
	return integralArrayZNCC!( padData, zeros( T, size(padData).+1 ) );
end

function integralArrayZNCC!( padData::AbstractArray{Complex{T},2}, intArr::Array{T,2} ) where {T<:AbstractFloat}

	h, w = size(intArr) .- 1

	@inbounds for c in 2:w+1, r in 2:h+1
		intArr[r,c] = (padData[r-1,c-1].re)^2 + intArr[r-1,c] + intArr[r,c-1] - intArr[r-1,c-1]
	end

	return intArr
end

function integralArrayZNCC!( padData::AbstractArray{Complex{T},3}, intArr::Array{T,3} ) where {T<:AbstractFloat}

	h, w, d = size(intArr) .- 1;
	@inbounds for z in 2:d+1, c in 2:w+1
		tmp = 0.0;
		for r in 2:h+1
			val2          = real( padData[r-1,c-1,z-1] )^2
			intArr[r,c,z] = val2 + intArr[r,c-1,z] + intArr[r,c,z-1] - intArr[r,c-1,z-1] + tmp;
			tmp          += val2;
	end end

	return intArr
end

# what did IIM mean...?

function integralArrayIIM!( img::AbstractArray{<:Real,2}, intArr::AbstractArray{<:AbstractFloat,3}, z )

	@inbounds for c in 1:size(img,2), r in 1:size(img,1)
		intArr[1+r,1+c,z] = img[r,c] + intArr[1+r-1,1+c,z] + intArr[1+r,1+c-1,z] - intArr[1+r-1,1+c-1,z]
	end
	return intArr
end

function integralAreaIIM( intArr::AbstractArray{<:Real,3}, z,TL, BR )
	TL   = TL .+ 1;
	BR   = BR .+ 1;
	area = intArr[BR[1],BR[2],z] - intArr[BR[1],TL[2],z] - intArr[TL[1],BR[2],z] + intArr[TL[1],TL[2],z]
end

# Considering offset of the img within the integral array

function integralArray_off( img::AbstractArray{<:Real,2}, off=(0,0); type=Float32, fun=(x)->(x) )
	return integralArray_off!( img, zeros( type, size(img) .+ 1 ), off, fun=fun )
end

function integralArray_off!( img::AbstractArray{<:Real,2}, intArr::AbstractArray{<:AbstractFloat,2}, off=(0,0); fun=(x)->(x))

	@inbounds for c in 1:size(img,2), r in 1:size(img,1)
		intArr[1+r+off[1],1+c+off[2]] = fun(img[r,c]) + intArr[1+r-1+off[1],1+c+off[2]] + intArr[1+r+off[1],1+c-1+off[2]] - intArr[1+r-1+off[1],1+c-1+off[2]]
	end
	return intArr
end

# Specific function to compute mean maxima and minima, which requires padded integral arrays

function integralArray_pad( img::AbstractArray{<:Real,2}, pad=(0,0); type=Float32, fun=(x)->(x) )
	return integralArray_pad!( img, zeros( type, size(img) .+ 1 .+ 2 .* pad ), pad, fun=fun )
end

function integralArray_pad!( img::AbstractArray{<:Real,2}, intArr::AbstractArray{<:AbstractFloat,2}, pad; fun=(x)->(x) )

	for c in 1:size(img,2)      # c    = column in input image
		c_ia = 1+c+pad[2];      # c_ia = offset column index in integral array
		for r in 1:size(img,1)  # r    = row in input image
			r_ia = 1+r+pad[1];  # r_ia = offset row index in integral array
			intArr[r_ia,c_ia] = fun(img[r,c]) + intArr[r_ia-1,c_ia] + intArr[r_ia,c_ia-1]- intArr[r_ia-1,c_ia-1]
	end end

	lastr = 1+size(img,1)+pad[1];
	lastc = 1+size(img,2)+pad[2];

	for c in 1:lastc
		val = intArr[lastr,c]
		for r in lastr+1:size(intArr,1)
			intArr[r,c] = val
	end end

	for c in lastc+1:size(intArr,2)
		intArr[:,c] .= intArr[:,c-1]
	end

	return intArr
end

function integralArray_pad( vol::AbstractArray{<:Real,3}, pad=(0,0,0); type=Float32, fun=(x)->(x) )
	return integralArray_pad!( vol, zeros( type, size(vol) .+ 1 .+ 2 .*pad ), pad, fun=fun )
end

function integralArray_pad!( vol::AbstractArray{<:Real,3}, intArr::AbstractArray{<:AbstractFloat,3}, pad; fun=(x)->(x))

	for z in 1:size(vol,3)         # z    = depth index in input volume
		z_ia = z+pad[3]+1;         # z_ia = offset depth index in integral array
		for c in 1:size(vol,2)     # c    = columns in input volume
			c_ia = c+pad[2]+1;     # c_ia = offset column index in integral array
			reuse = 0.0
			for r in 1:size(vol,1) # r    = row in input volume
				r_ia = r+pad[1]+1; # r_ia = offset row index in integral array
				tmp = fun(vol[r,c,z])
				intArr[r_ia,c_ia,z_ia] = tmp + intArr[r_ia,c_ia-1,z_ia] + intArr[r_ia,c_ia,z_ia-1] - intArr[r_ia,c_ia-1,z_ia-1] + reuse;
				reuse += tmp
	end end end

	lastr = 1+size(vol,1)+pad[1];
	lastc = 1+size(vol,2)+pad[2];
	lastz = 1+size(vol,3)+pad[3];

	for z in 1:lastz
		for c in 1:lastc
			val = intArr[lastr,c,z]
			for r in lastr+1:size(intArr,1)
				intArr[r,c,z] = val
		end end

		for c in lastc+1:size(intArr,2)
			intArr[:,c,z] .= intArr[:,c-1,z]
		end
	end

	for z in lastz+1:size(intArr,3)
		intArr[:,:,z] .= intArr[:,:,z-1]
	end

	return intArr
end

# Reversed variant, which is used during normalized cross-correlation
function integralArrayRev!( img::AbstractArray{<:Real,2}, intArr::AbstractArray{<:AbstractFloat,2}, fun=(x)->(x) )
	h, w = size(img)
	@inbounds for c in 2:w+1, r in 2:h+1
		intArr[r,c] = fun(img[h-(r-2),w-(c-2)]) + intArr[r-1,c] + intArr[r,c-1] - intArr[r-1,c-1]
	end
	return intArr
end

function integralArrayRev( img::AbstractArray{<:Real,2}; type=Float32, fun=(x)->(x) )

	return integralArrayRev!( img, zeros( type, size(img) .+ 1 ), fun )
end

"""
	3D: optimized integral array by reusing previous operations
"""
# Below is the unoptimized code, in case I want to check again:
#    for z in 2:d+1, c in 2:w+1, r in 2:h+1
#        arr[r,c,z] = fun(vol[r-1,c-1,z-1]) + arr[r-1,c,z] + arr[r,c-1,z] + arr[r,c,z-1] -
#                     arr[r,c-1,z-1] - arr[r-1,c,z-1] - arr[r-1,c-1,z] + arr[r-1,c-1,z-1]
#    end

function integralArray!( vol::AbstractArray{<:Real,3}, intArr::AbstractArray{<:AbstractFloat,3}, fun=(x)->(x) )

	@inbounds for z in 2:size(vol,3)+1, c in 2:size(vol,2)+1
		reuse = 0.0
		for r in 2:size(vol,1)+1
			tmp = fun(vol[r-1,c-1,z-1])
			intArr[r,c,z] = tmp + intArr[r,c-1,z] + intArr[r,c,z-1] - intArr[r,c-1,z-1] + reuse;
			reuse += tmp
		end
	end
	return intArr
end

function integralArray( vol::AbstractArray{<:Real,3}; type=Float32, fun=(x)->(x) )

	return integralArray!( vol, zeros( type, size(vol) .+ 1 ), fun )
end

function integralArea( intArr::AbstractArray{<:Real,3}, TL, BR )
	TL = TL .+ 1;
	BR = BR .+ 1;
	area  = intArr[BR[1],BR[2],BR[3]] - intArr[TL[1],TL[2],TL[3]]
	area -= intArr[TL[1],BR[2],BR[3]] + intArr[BR[1],TL[2],BR[3]] + intArr[BR[1],BR[2],TL[3]]
	area += intArr[BR[1],TL[2],TL[3]] + intArr[TL[1],BR[2],TL[3]] + intArr[TL[1],TL[2],BR[3]]
    return area
end

# Reversed variant, which is used during normalized cross-correlation
function integralArrayRev!( vol::AbstractArray{<:Real,3}, intArr::AbstractArray{<:AbstractFloat,3}, fun=(x)->(x) )

	h, w, d = size(vol)
	@inbounds for z in 2:d+1, c in 2:w+1
		reuse = 0.0
		for r in 2:h+1
			tmp = fun(vol[h-(r-2),w-(c-2),d-(z-2)])
			intArr[r,c,z] = tmp + intArr[r,c-1,z] + intArr[r,c,z-1] - intArr[r,c-1,z-1] + reuse;
			reuse += tmp
		end
	end
	return intArr
end

function integralArrayRev( vol::AbstractArray{<:Real,3}; type=Float32, fun=(x)->(x) )

	return integralArrayRev!( vol, zeros( type, size(vol) .+ 1 ), fun )
end

"""
	Derived functions
"""
function scan4Simmetry( intArr, rad )

	score = zeros( length( rad+1:size(intArr,1)-rad), length( rad+1:size(intArr,2)-rad ) )

	cont = 1;
	for c in rad+1:size(intArr,2)-rad;     c1, c2 = c - rad, c + rad
		for r in rad+1:size(intArr,1)-rad; r1, r2 = r - rad, r + rad

			Q1  = integralArea( intArr, (r1,c ), ( r-1,c2-1) )
			Q2  = integralArea( intArr, (r1,c1), ( r-1,c -1) )
			Q3  = integralArea( intArr, (r ,c1), (r2-1,c -1) )
			Q4  = integralArea( intArr, (r ,c ), (r2-1,c2-1) )

			acc = Q1 + Q2 + Q3 + Q4;
			ps  = [ Q1, Q2, Q3, Q4 ] ./ acc

			score[ cont ] = 1/( 1 + sum( abs.( ps .- 0.25 ) ) )
			score[ cont ] = isnan( score[cont] ) ? 0.0 : score[cont];
			cont += 1
	end	end

	return score
end

function followCentroids( data, intArr, rad; iters=5 )

	# Goal: At any position, compute centroid, move to centroid
	# recompute centroid and so on until converging

	score = zeros( Int64, size(data) )

	xx = [ x for y in -rad:rad, x in -rad:rad ]
	yy = [ y for y in -rad:rad, x in -rad:rad ]

	h, w = size( data );
	for c in 1:w;
		for r in 1:h;

			pos = r, c;
			#println( pos )
			# Contemplating the case where the position is closer than "rad"
			# to the edges of the images. As we iterate and update the position
			# of the centroid, it becomes necessary. Here is not needed though.
			left   = rad + min(0, pos[1]-1-rad)
			up     = rad + min(0, pos[2]-1-rad)
			right  = min(  h - pos[ 1 ], rad  )
			down   = min(  w - pos[ 2 ], rad  )

			TL     = pos .- ( left , up   ) .- 1
			BR     = pos .+ ( right, down )

			acc    = integralArea( intArr, TL, BR )
			( acc == 0 ) && ( continue; )

			subimg = pos[1]-left:pos[1]+right, pos[2]-up:pos[2]+down
			subxy  =  rad+1-left:rad+1+right ,  rad+1-up:rad+1+down

			xmean  = sum( (data[ subimg... ] ./ acc) .* xx[ subxy... ] )
			ymean  = sum( (data[ subimg... ] ./ acc) .* yy[ subxy... ] )

			for i in 1:iters

				offset = round.( Int64, (ymean,xmean) )

				if offset[1] == 0 && offset[2] == 0
					break
				end

				pos    = pos .+ offset
				#println( "\t", pos )
				left   = rad + min(0, pos[1]-1-rad)
				up     = rad + min(0, pos[2]-1-rad)
				right  = min(  h - pos[ 1 ], rad  )
				down   = min(  w - pos[ 2 ], rad  )

				TL     = pos .- ( left , up   ) .- 1
				BR     = pos .+ ( right, down )

				acc    = integralArea( intArr, TL, BR )
				( acc == 0 ) && ( break; )

				subimg = pos[1]-left:pos[1]+right, pos[2]-up:pos[2]+down
				subxy  =  rad+1-left:rad+1+right ,  rad+1-up:rad+1+down

				xmean  = sum( (data[ subimg... ] ./ acc) .* xx[ subxy... ] )
				ymean  = sum( (data[ subimg... ] ./ acc) .* yy[ subxy... ] )
			end

			pos = pos .+ round.( Int64, (ymean,xmean) )

			score[ pos... ] += 1;
	end	end

	return score
end

function integralAreaZNCC( sumS::Array{T,2}, sumS2::Array{T,2},
                           row, col, ss, si ) where {T<:Real}
    pad = 1;
    row = row + pad;
    col = col + pad;
    r0, r1 = max( 1, row - si[1] ), min( ss[1] + pad, row );
    c0, c1 = max( 1, col - si[2] ), min( ss[2] + pad, col );

    opS  = 0.0
    opS += sumS[ r1, c1 ];
    opS -= sumS[ r1, c0 ];
    opS -= sumS[ r0, c1 ];
    opS += sumS[ r0, c0 ];

    opS2  = 0.0
    opS2 += sumS2[ r1, c1 ];
    opS2 -= sumS2[ r1, c0 ];
    opS2 -= sumS2[ r0, c1 ];
    opS2 += sumS2[ r0, c0 ];

    return opS, opS2
end


# ---
# jupyter:
#   jupytext:
#     formats: ipynb,jl:hydrogen
#     text_representation:
#       extension: .jl
#       format_name: hydrogen
#       format_version: '1.3'
#       jupytext_version: 1.11.2
#   kernelspec:
#     display_name: Julia 1.6.2
#     language: julia
#     name: julia-1.6
# ---

# %%
using Plots
using Distributions

# %%
d = MixtureModel([Poisson(100), Poisson(250)], [0.95, 0.05])
n = 10^4
sample = sum.([rand(d, 20) for _ in 1:n])
histogram(sample)

# %%
using StatsPlots
x=rand(1:6, 10^5)
density(x; label="default")
density!(x; label="bandwidth=1", bandwidth=1)

# %%
using StatsPlots
x = rand(1:6, 10^5)
histogram(x; norm=true, alpha=0.3, bin=0.5:6.5, label="histogram of true sample")
density!(x; label="kde with bandwidth=1", bandwidth=1, lw=2)
plot!(; ylim=(-0.005, 0.22), xtick=-10:10)

# %%


"""
    TMJets20{N, DM<:AbstractDisjointnessMethod} <: AbstractContinuousPost

Validated integration using Taylor models.

### Fields

- `orderQ`       -- (optional, default: `2`) order of the Taylor models for jet transport variables
- `orderT`       -- (optional, default: `8`) order of the Taylor model in time
- `abstol`       -- (optional, default: `1e-10`) absolute tolerance
- `maxsteps`     -- (optional, default: `2000`) maximum number of steps in the
                     validated integration ``x' = f(x)``
- `adaptive`     -- (optional, default: `true`) if `true`, try decreasing the absolute
                     tolerance each time step validation fails, until `min_abs_tol` is reached
- `minabstol`    -- (optional, default: `1e-29`) minimum absolute tolerance for the adaptive algorithm
- `disjointness` -- (optional, default: `ZonotopeEnclosure()`) defines the method to
                     perform the disjointness check between the taylor model flowpipe and the invariant

### Notes

The argument `disjointness` allows to control how are disjointness checks
computed, in the case where the invariant is not universal. In particular,
`ZonotopeEnclosure()` pre-processes the taylor model with a zonotopic
overapproximation, then performs the disjointness check with that zonotope and the
invariant. For other options, see the documentation of `AbstractDisjointnessMethod`.

This algorithm is an adaptation of the implementation in `TaylorModels.jl`
(see copyright license in the file `reach.jl` of the current folder). The package
`TaylorIntegration.jl` is used for jet-transport of ODEs using the Taylor method,
and `TaylorSeries.jl` is used to work with truncated Taylor series.
"""
@with_kw struct TMJets20{N, DM<:AbstractDisjointnessMethod} <: AbstractContinuousPost
    orderQ::Int=DEFAULT_ORDER_Q_TMJETS
    orderT::Int=DEFAULT_ORDER_T_TMJETS
    abstol::N=DEFAULT_ABS_TOL_TMJETS
    maxsteps::Int=DEFAULT_MAX_STEPS_TMJETS
    adaptive::Bool=true
    minabstol::N=1e-29
    disjointness::DM=ZonotopeEnclosure()
end

const DEFAULT_MAX_STEPS_TMJETS = 2000
const DEFAULT_ABS_TOL_TMJETS = 1e-10
const DEFAULT_ORDER_T_TMJETS = 8
const DEFAULT_ORDER_Q_TMJETS = 2

numtype(::TMJets20{N}) where {N} = N
rsetrep(::TMJets20{N}) where {N} = TaylorModelReachSet{N}

include("post.jl")
include("reach.jl")


using QMTK
using BenchmarkTools

@generated function _kronprod(list...)
    ex = Expr(:call, :kron, :(list[1]), :(list[2]))
    for i in 3:length(list)
        ex = Expr(:call, kron, ex, :(list[$i]))
    end
    return ex
end

const sigmax = sprand(10, 10, 0.5)
⊗ = kron

println("--------------------")
println("baseline")
display(@benchmark kron(kron(kron(sigmax, sigmax), sigmax), sigmax))
println("--------------------")
println("QMTK")
display(@benchmark kronprod(sigmax, sigmax, sigmax, sigmax))
println("--------------------")
println("generated")
display(@benchmark _kronprod(sigmax, sigmax, sigmax, sigmax))
println("--------------------")
println("product")
display(@benchmark sigmax ⊗ sigmax ⊗ sigmax ⊗ sigmax)


const TEST_FLOAT32 = false
const TEST_DOUBLE64 = false
const TEST_STATIC_SIZED = false

using Manifolds
using ManifoldsBase
using ManifoldsBase: number_of_coordinates

using LinearAlgebra
using Distributions
using DoubleFloats
using ForwardDiff
using Quaternions
using Random
using ReverseDiff
using StaticArrays
using Statistics
using StatsBase
using Test
using LightGraphs
using SimpleWeightedGraphs

using Manifolds.ManifoldTests


using Shannon
using Base.Test

v = collect(0:10) ./ 10

@test [1,1,2,3,4,5,6,7,8,9,10] == bin_vector(v, 0.0, 1.0, 10)

r = zeros((10,5))
i = 1
for j=1:10
  for k=1:5
    r[j,k]=i
    i = i+1
  end
end
v = r ./ 50
r = convert(Matrix{Int64}, r)

@test r == bin_matrix(v, 0.0, 1.0, 50)

c = [[1 0 0 0]; [1 2 0 0]; [1 2 3 0]; [1 2 3 4]]
r = [1; 1 + 2 * 4^1; 1 + 2*4^1 + 3*4^2; 1 + 2*4^1 + 3*4^2 + 4*4^3]

@test 1    == combine_binned_vector(squeeze(c[1,:],1), 10)
@test 21   == combine_binned_vector(squeeze(c[2,:],1), 10)
@test 321  == combine_binned_vector(squeeze(c[3,:],1), 10)
@test 4321 == combine_binned_vector(squeeze(c[4,:],1), 10)

@test r[1] == combine_binned_vector(squeeze(c[1,:],1), 4)
@test r[2] == combine_binned_vector(squeeze(c[2,:],1), 4)
@test r[3] == combine_binned_vector(squeeze(c[3,:],1), 4)
@test r[4] == combine_binned_vector(squeeze(c[4,:],1), 4)

@test r    == combine_binned_matrix(c)

# entropy tests
@test 0           == entropy([1,1,1,1,1])
@test abs(log(2,6) - entropy([1,2,3,4,5,6])) < 0.0000001
@test abs(log(4,6) - entropy([1,2,3,4,5,6], base=4)) < 0.0000001

#= @test abs(5.540741 - entropy([1,2,3,4,5,6], mode="ChaoShen")) < 0.00001 =#
#= @test abs(5.540741 - entropy([1,2,3,4,5,6], mode="ChaoShen", base=2)) < 0.00001 =#
#= @test abs(1.667929 - entropy([1,2,3,4,5,6], mode="ChaoShen", base=10)) < 0.00001 =#
#= @test abs(3.840549 - entropy([1,2,3,4,5,6], mode="ChaoShen", base=e)) < 0.00001 =#

# test basic functions - bin a value
@test 1  == bin_value(-1.0, 10)
@test 1  == bin_value(-0.9, 10)
@test 1  == bin_value(-0.8, 10)
@test 2  == bin_value(-0.7, 10)
@test 2  == bin_value(-0.6, 10)
@test 3  == bin_value(-0.5, 10)
@test 3  == bin_value(-0.4, 10)
@test 4  == bin_value(-0.3, 10)
@test 4  == bin_value(-0.2, 10)
@test 5  == bin_value(-0.1, 10)
@test 5  == bin_value(0.0,  10)
@test 6  == bin_value(0.1,  10)
@test 6  == bin_value(0.2,  10)
@test 7  == bin_value(0.3,  10)
@test 7  == bin_value(0.4,  10)
@test 8  == bin_value(0.5,  10)
@test 8  == bin_value(0.6,  10)
@test 9  == bin_value(0.7,  10)
@test 9  == bin_value(0.8,  10)
@test 10 == bin_value(0.9,  10)
@test 10 == bin_value(1.0,  10)

# test basic functions - bin a vector
@test [1, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10] == bin_vector([-1.0, -0.9, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0], -1.0, 1.0, 10)
@test [0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10] != bin_vector([-1.0, -0.9, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0], -1.0, 1.0, 10)

# test basic functions - bin a matrix
@test [[1, 2, 3] [4, 5, 6] [7, 8, 9]] == bin_matrix([[-1.0, -0.7, -0.5] [-0.3, -0.1, 0.1] [0.3, 0.5, 0.7]], -1.0, 1.0, 10)
@test [[2, 2, 3] [4, 5, 6] [7, 8, 9]] != bin_matrix([[-1.0, -0.7, -0.5] [-0.3, -0.1, 0.1] [0.3, 0.5, 0.7]], -1.0, 1.0, 10)

# test inverse binning
@test abs(-1.0 - unbin_value(1,  10, -1.0, 1.0, mode="lower")) < 0.0001
@test abs(-0.8 - unbin_value(2,  10, -1.0, 1.0, mode="lower")) < 0.0001
@test abs(-0.6 - unbin_value(3,  10, -1.0, 1.0, mode="lower")) < 0.0001
@test abs(-0.4 - unbin_value(4,  10, -1.0, 1.0, mode="lower")) < 0.0001
@test abs(-0.2 - unbin_value(5,  10, -1.0, 1.0, mode="lower")) < 0.0001
@test abs( 0.0 - unbin_value(6,  10, -1.0, 1.0, mode="lower")) < 0.0001
@test abs( 0.2 - unbin_value(7,  10, -1.0, 1.0, mode="lower")) < 0.0001
@test abs( 0.4 - unbin_value(8,  10, -1.0, 1.0, mode="lower")) < 0.0001
@test abs( 0.6 - unbin_value(9,  10, -1.0, 1.0, mode="lower")) < 0.0001
@test abs( 0.8 - unbin_value(10, 10, -1.0, 1.0, mode="lower")) < 0.0001

@test abs(-0.8 - unbin_value(1,  10, -1.0, 1.0, mode="upper")) < 0.0001
@test abs(-0.6 - unbin_value(2,  10, -1.0, 1.0, mode="upper")) < 0.0001
@test abs(-0.4 - unbin_value(3,  10, -1.0, 1.0, mode="upper")) < 0.0001
@test abs(-0.2 - unbin_value(4,  10, -1.0, 1.0, mode="upper")) < 0.0001
@test abs(-0.0 - unbin_value(5,  10, -1.0, 1.0, mode="upper")) < 0.0001
@test abs( 0.2 - unbin_value(6,  10, -1.0, 1.0, mode="upper")) < 0.0001
@test abs( 0.4 - unbin_value(7,  10, -1.0, 1.0, mode="upper")) < 0.0001
@test abs( 0.6 - unbin_value(8,  10, -1.0, 1.0, mode="upper")) < 0.0001
@test abs( 0.8 - unbin_value(9,  10, -1.0, 1.0, mode="upper")) < 0.0001
@test abs( 1.0 - unbin_value(10, 10, -1.0, 1.0, mode="upper")) < 0.0001

@test abs(-0.9 - unbin_value(1,  10, -1.0, 1.0, mode="centre")) < 0.0001
@test abs(-0.7 - unbin_value(2,  10, -1.0, 1.0, mode="centre")) < 0.0001
@test abs(-0.5 - unbin_value(3,  10, -1.0, 1.0, mode="centre")) < 0.0001
@test abs(-0.3 - unbin_value(4,  10, -1.0, 1.0, mode="centre")) < 0.0001
@test abs(-0.1 - unbin_value(5,  10, -1.0, 1.0, mode="centre")) < 0.0001
@test abs( 0.1 - unbin_value(6,  10, -1.0, 1.0, mode="centre")) < 0.0001
@test abs( 0.3 - unbin_value(7,  10, -1.0, 1.0, mode="centre")) < 0.0001
@test abs( 0.5 - unbin_value(8,  10, -1.0, 1.0, mode="centre")) < 0.0001
@test abs( 0.7 - unbin_value(9,  10, -1.0, 1.0, mode="centre")) < 0.0001
@test abs( 0.9 - unbin_value(10, 10, -1.0, 1.0, mode="centre")) < 0.0001


#=
MIT License

Copyright (c) 2021 David-W-Wallis

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
=#

module TwoDerrors

import Printf
import JSON
import Random
import Statistics
import JLD
import PyPlot

const C = 330

# Sensor positions
sensorA=(10,-5,0,"A")
sensorB=(40,-5,0,"B")
sensorC=(-5,45,-90,"C")
sensorsAB=(sensorA,sensorB)

# For constructing file names.
sensor2string(s) = Printf.@sprintf("%+04d%+04d%+04d",s[1],s[2],s[3])
sensors2string(s) = join(map(sensor2string,s))

# Angle from microphone array to a point on the modelled area.
theta(x,y,sensorPos) = atan.(x-sensorPos[1], y-sensorPos[2])

# Equation 8
function phi(theta, ifParams)
    f,d,C=ifParams
    2*pi*f*d.*(sin.(theta)./C)
end

# Equation 8
function phi(theta,f,d)
    return 2*pi*f*d/C * sin(deg2rad(theta))
end

function errorMapFileName(sensors,d,f)
    fmt=Printf.@sprintf("jld/%s_%02d_%05d",sensors2string(sensors),
                        convert(Int64,d*1000),convert(Int64,f))
    return "errormap_$fmt.jld"
end

coeffsFileName(froot,f,d) = return Printf.@sprintf("json/%s_%05d_%02d_%02d.json",
                                                   froot,
                                                   Int(floor(f)),
                                                   Int(d*1000),
                                                   5)

# Reads pre-calculated model coefficients from json files.
function makeCoeffs(froot)
    dict=Dict()
    for d in 1:200
        for f in [2000,4000,6000,8000]#,4000,6000,8000]
            di=Dict()
            open(coeffsFileName(froot,f,d/1000.0), "r") do f
                dicttxt = read(f,String)  # file information to string
                di=JSON.parse(dicttxt)
            end
            dict[(f,d)]=di
        end
    end
    return dict
end

BiasCoeffs=makeCoeffs("biascoeffs")
ErrorBarCoeffs=makeCoeffs("polycoeffs")

# Equation 9
# Takes radians returns degrees
function biasModel(theta,f,d)
    c=BiasCoeffs[(f,d)]
    alpha=c["alpha"]
    beta=c["beta"]
    gamma=c["gamma"]
    model = alpha/(cos(gamma*theta)^beta) - alpha
    return model*sign(theta)
end

# Equation 10
# Takes radians and returns degrees
function errorBarModel(theta,f,d)
    c=ErrorBarCoeffs[(f,d)]["coeffs"]
    return c[1]*theta^4 + c[2]*theta^3 + c[3]*theta^2 + c[4]*theta + c[1]
end

# Purturb the angle
# Takes radians returns degrees
function newAngleRad(theta,f,d)
    bias=biasModel(theta,f,d)
    err=errorBarModel(theta,f,d)
    #println("Bias: ",bias," error: ",err)
    return rad2deg(theta)-bias + err*Random.rand() - err/2
end

# Purturb the angle
# Takes degrees returns degrees
function newAngle(theta,f,d)
    return newAngleRad(deg2rad(theta),f,d)
end

# Generates and saves an error map
function errorMapN(sensors;f=2000.0,d=0.020,M=1000)
    N=length(sensors)
    GZ=51
    ifparams=(f,d,330)
    posx=Array{Float64}(undef,GZ,GZ)
    posy=Array{Float64}(undef,GZ,GZ)
    errx=Array{Float64}(undef,GZ,GZ)
    erry=Array{Float64}(undef,GZ,GZ)
    th_=zeros(N)
    th=Array{Float64}(undef,N)
    phi_=Array{Float64}(undef,N)
    slopeErr=zeros(N)
    for x = 0:GZ-1
        for y = 0:GZ-1
            for i = 1:N
                th_[i] = theta(x,y,sensors[i])
            end
            xp=zeros(M)
            yp=zeros(M)
            for test = 1:M
                th=newAngle.(th_,Int(floor(f)),Int(floor(d*1000)))
                if false
                    println("x: ",x," y: ",y)
                    println("Angle:")
                    println(map(x -> Printf.@sprintf("%.1f",x), rad2deg.(th_)))
                    println("Error:")
                    println(map(x -> Printf.@sprintf("%.1f",x), slopeErr))
                    println("New angle:")
                    println(map(x -> Printf.@sprintf("%.1f",x), rad2deg.(th)))
                    println()
                end

                A=hcat(tan.(th),ones(N))
                b=[sensors[i][1]*tan(th[i]) + sensors[i][2] for i in 1:N]
                if N > 2
                    m=GLM.lm(A,b)
                    xp[test],yp[test]=GLM.coef(m)
                else
                    xp[test],yp[test]=A\b
                end
            end
            errx[x+1,y+1]=Statistics.std(xp)
            erry[x+1,y+1]=Statistics.std(yp)
            posx[x+1,y+1]=Statistics.mean(xp)
            posy[x+1,y+1]=Statistics.mean(yp)
        end
    end
    fname=errorMapFileName(sensors,d,f)
    JLD.save(fname,"errx",errx,"erry",erry,"posx",posx,"posy",posy)
end

# Load a pre-calculated map
function loadMap(sensors,f,d)
    fname=errorMapFileName(sensors,d,f)
    return JLD.load(fname)#,"errx",errx,"erry",erry,"posx",posx,"posy",posy)
end

function plotErrorMapN(sensors,f,d;colour=false,levelsx=false,levelsy=false,PLOTSENSORS=false,contcolours="none",cmap="gist_rainbow",vmin=0,vmax=15)
    println("cmap=",cmap)
    emap=loadMap(sensors,f,d)
    errx=emap["errx"]
    erry=emap["erry"]
    posx=emap["posx"]
    posy=emap["posy"]
    fmt=Printf.@sprintf("%s_%02d_%04d",sensors2string(sensors),
                        convert(Int64,d*1000),convert(Int64,f))

    fint=convert(Int64, f)
    fig1=PyPlot.figure(1)
    fig1.clf()
    if colour
        if levelsx==false
            PyPlot.contourf(errx',cmap=cmap,vmin=vmin,vmax=vmax)
        else
            PyPlot.contourf(errx',cmap=cmap,levels=levelsx,vmin=vmin,vmax=vmax)
        end
    end

    if colour
        if contcolours == "none"
            contcolours="white"
        end
    else
        contcolours="black"
    end

    if levelsx==false
        CS = PyPlot.contour(errx',colors=contcolours)
    else
        CS = PyPlot.contour(errx',colors=contcolours,levels=levelsx)
    end

    PyPlot.clabel(CS)
    PyPlot.xlabel("Distance (meters)")
    PyPlot.ylabel("Distance (meters)")
    PyPlot.savefig("mc_error_x_$fmt.png")

    fig2=PyPlot.figure(2)
    fig2.clf()
    if colour
        if levelsy==false
            PyPlot.contourf(erry',cmap=cmap)
        else
            PyPlot.contourf(erry',cmap=cmap,levels=levelsy)
        end
    end

    if levelsy==false
        CS = PyPlot.contour(erry',colors=contcolours)
    else
        CS = PyPlot.contour(erry',colors=contcolours,levels=levelsy)
    end

    if PLOTSENSORS
        for s=sensors
            x,y=s[1],s[2]
            PyPlot.plot(x,y,"ko")
            PyPlot.annotate(s[4],xy=(x+2,y))
        end
        PyPlot.ylim([-7,50])
        PyPlot.xlim([-7,50])
    end

    PyPlot.clabel(CS)
    PyPlot.xlabel("Distance (meters)")
    PyPlot.ylabel("Distance (meters)")
    PyPlot.savefig("mc_error_y_$fmt.png")

    (errx,erry,posx,posy)
end

function paperPlot(sensors,f,d;M=600,colour=false,contcolours="black",levs=[0,1,1.5,2,3,4,5,6,8,10,15],cmap="gist_rainbow",vmin=0,vmax=15,calculate=true)
    if calculate
        errorMapN(sensors,f=f,d=d,M=M)#,levelsx=mylevs,levelsy=mylevs)#,contcolours="black")
    end
    plotErrorMapN(sensors,f,d;colour=colour,levelsx=levs,levelsy=levs,vmin=vmin,vmax=vmax,cmap=cmap,contcolours=contcolours)
    fig=PyPlot.figure(1)
    froot="$(sensors[1][4])$(sensors[2][4])$(Int(f))-$(Int(d*1000))"
    println(froot)
    PyPlot.savefig(froot*"x.png")
    fig=PyPlot.figure(2)
    PyPlot.savefig(froot*"y.png")
end

# For interactive testing
function ppRun(f,d;M=1000)
    paperPlot(TwoDerrors.sensorsAB,f,d;M=M,levs=[0,1,1.5,2,3,4,5,6,8,10,15],colour=true,cmap="jet",calculate=true)
end

# Plots for paper
function makePlots(;M=1000)
    for (f,d) in zip((2000,8000),(0.008,0.012,0.020))
        paperPlot(TwoDerrors.sensorsAB,f,d;M=M,levs=[0,1,1.5,2,3,4,5,6,8,10,15],colour=true,cmap="jet",calculate=true)
    end
    paperPlot(TwoDerrors.sensorsAB,2000,0.060;M=M,levs=[0,1,1.5,2,3,4,5,6,8,10,15],colour=true,cmap="jet",calculate=true)
end

# To generate the area coverage.
percentUnder(a,thresh) = (sum(a.<thresh) / length(a)) * 100

# To generate the area coverage.
function percentUnder(sensors,f,d,thresh)
    emap=loadMap(sensors,f,d)
    errx=emap["errx"]
    erry=emap["erry"]
    percentUnder(errx,thresh),percentUnder(erry,thresh)
end

end


using IterativeSolvers
using FactCheck

facts("svdvals_gkl") do

context("Small diagonal matrix") do
    A = full(Diagonal([10.0, 9, 8, 6, 1]))
    @fact norm(svdvals_gkl(A)[1] - svdvals(A)) --> less_than(1e-10)
end

for T in (Float32, Float64)
    context("Medium random UpperTriangular{$T}") do
        srand(1)
        n = 150
        nvals = 6

        Base.Ac_mul_B!(p::Vector, A::UpperTriangular, u::Vector) = Ac_mul_B!(A, copy!(p, u))
        Base.At_mul_B!(p::Vector, A::UpperTriangular, u::Vector) = At_mul_B!(A, copy!(p, u))

        A = UpperTriangular(convert(Matrix{T}, rand(n, n)))
        σth = 0.1*√eps(T)

        svals, = svdvals_gkl(A, nvals, σth=σth)
        @fact length(svals) --> greater_than_or_equal(nvals)

        svals = svals[1:nvals]
        svals2 = svdvals(A)[1:nvals]
        @fact norm(svals - svals2) --> less_than(nvals*σth) "Disagreement in top $nvals singular values:\nsvdvals_gkl\tsvdvals\n"*repr([svals svals2])
    end
end

for T in (Float32, Float64)
    context("Medium wide rectangular Matrix{$T}") do
        srand(1)
        n = 500
        m = 200
        nvals = 10

        A = convert(Matrix{T}, rand(n, m))
        σth = 0.1*√eps(T)

        svals, = svdvals_gkl(A, nvals, σth=σth)
        @fact length(svals) --> greater_than_or_equal(nvals)

        nvals = 4
        svals = svals[1:nvals]
        svals2 = svdvals(A)[1:nvals]
        @fact norm(svals - svals2) --> less_than(nvals*σth) "Disagreement in top $nvals singular values:\nsvdvals_gkl\tsvdvals\n"*repr([svals svals2])
    end

    context("Medium tall rectangular Matrix{$T}") do
        srand(1)
        n = 200
        m = 500
        nvals = 10

        A = convert(Matrix{T}, rand(n, m))
        σth = 0.1*√eps(T)

        svals, = svdvals_gkl(A, nvals, σth=σth)
        @fact length(svals) --> greater_than_or_equal(nvals)

        nvals = 4
        svals = svals[1:nvals]
        svals2 = svdvals(A)[1:nvals]
        @fact norm(svals - svals2) --> less_than(nvals*σth) "Disagreement in top $nvals singular values:\nsvdvals_gkl\tsvdvals\n"*repr([svals svals2])
    end
e
end

end


using SQP, Ipopt
using JuMP

ipopt_solver = optimizer_with_attributes(
    Ipopt.Optimizer,
    "print_level" => 0,
    "warm_start_init_point" => "yes",
)
optimizer = optimizer_with_attributes(
    SQP.Optimizer,
    "external_optimizer" => ipopt_solver,
    "max_iter" => 100,
    "algorithm" => "SQP-TR",
)
model = Model(optimizer)

@variable(model, X);
@variable(model, Y);
@objective(model, Min, X^2 + X);
@NLconstraint(model, X^2 - X == 2);
@NLconstraint(model, X*Y == 1);
@NLconstraint(model, X*Y >= 0);
@constraint(model, X >= -2);

println("________________________________________");
print(model);
println("________________________________________");
JuMP.optimize!(model);

xsol = JuMP.value.(X)
ysol = JuMP.value.(Y)
status = termination_status(model)

println("Xsol = ", xsol);
println("Ysol = ", ysol);

println("Status: ", status);