#   Unit "timefrequency" of the FourierAnalysis Package for julia language
#
#   MIT License
#   Copyright (c) 2019, Marco Congedo, CNRS, Grenobe, France:
#   https://sites.google.com/site/marcocongedo/home

# ? CONTENTS :
#   This unit implements time-frequency representations based
#   on analytic signal estimations using FFTW and DSP filters.

#### Time-Frequency Analytic Signal ####

"""
```
(1)
function TFanalyticsignal(x         :: Vector{T},
                          sr        :: Int,
                          wl        :: Int          = 0,
                          bandwidth :: IntOrReal    = 2;
                      fmin          :: IntOrReal    = bandwidth,
                      fmax          :: IntOrReal    = sr÷2,
                      filtkind      :: FilterDesign = Butterworth(2),
                      nonlinear     :: Bool         = false,
                      fsmoothing    :: Smoother     = noSmoother,
                      tsmoothing    :: Smoother     = noSmoother,
                      planner       :: Planner      = getplanner,
                      ⏩           :: Bool         = true) where T<:Real

(2)
function TFanalyticsignal(𝐱         :: Vector{Vector{T}},
                      <same arguments as method (1)>

```
(1)

Given sampling rate `sr`, construct a [TFAnalyticSignal](@ref) object
from univariate data `x`, that is, a time-frequency representation of real
vector `x`.

Call three functions in series performing the three following operations:

i. pass the data throught a bank of pass-band filters calling function
[`filterbank`](@ref),

ii. compute the analytic signal calling function [`analyticsignal`](@ref)
(method (1) therein),

iii. If requested, smooth the analytic signal along the time and/or frequency
dimension calling function [`smooth`](@ref).

The arguments passed to each of these functions are:

|   filterbank   | analyticsignal |   smooth      |
|:--------------:|:--------------:|:-------------:|
|      `x`       |     `wl`       | `fsmoothing`  |
|     `sr`       |  `nonlinear`   | `tsmoothing`  |
|  `bandwidth`   |   `planner`    |               |
|  `filtkind`    |    `⏩`       |               |
|    `fmin`      |                |               |
|    `fmax`      |                |               |
|     `⏩`      |                |               |

Refer to the documentation of each function to learn the meaning
of each argument.

By default the `wl` argument is set to `length(x)`.

If `⏩` is true (default), filtering and computation fo the analytic signal
are multi-threaded across band-pass regions as long as the number of the regions
is at least twice the number of threads Julia is instructed to use.
See [Threads](@ref).

(2)

Given sampling rate `sr`, construct a
[TFAnalyticSignalVector](@ref) object from a vector of univariate data `𝐱`,
that is, from the time-frequency representations of the vectors in `𝐱`.

This method operates as method (1) with the following exceptions:

By default the `wl` argument is set to `length(x)` for each vectors in `𝐱`.
If another values is given, it will be used for all of them.

If `⏩` is true (default), the method is run in multi-threaded mode across the
vectors in `𝐱` as long as their number is at least twice the number of
threads Julia is instructed to use, otherwise this method attempts to run
each analytic signal estimation in multi-threaded mode like in method (1).
See [Threads](@ref).

If a `Planner` is not explicitly passed as an argument,
the FFTW plan is computed once and applied for all analytic signal
estimations.

**See**: [`filterbank`](@ref), [`analyticsignal`](@ref), [`smooth`](@ref).

**Examples**:
```
using Plots, FourierAnalysis

# generate some data
sr, t, bandwidth=128, 512, 2
h=taper(harris4, t)
x1=sinusoidal(10, 8, sr, t, 0)
x2=sinusoidal(10, 19, sr, t, 0)
x=Vector((x1+x2).*h.y+randn(t))
y1=sinusoidal(10, 6, sr, t, 0)
y2=sinusoidal(10, 16, sr, t, 0)
y=Vector((y1+y2).*h.y+randn(t))
plot([x, y])

# TFAnalyticSignal object for x (method (1))
Y = TFanalyticsignal(x, sr, sr*4; fmax=32)

# vector of TFAnalyticSignal objects for x and y (method (2))
𝒀 = TFanalyticsignal([x, y], sr, sr*4; fmax=32)

# (for shortening comments: TF=time-frequency, AS=analytic signal)

# mean AS in a TF region (8:12Hz and samples 1:128) for the first object
m=mean(𝒀[1], (8, 12), (1, 128)) # Output a complex number

# extract the AS in a TF region (8:12Hz and samples 1:128) for the first object
E=extract(𝒀[1], (8, 12), (1, 128)) # Output a complex matrix

# mean AS in a TF region (8:12Hz and samples 1:128) for the two objects
𝐦=mean(𝒀, (8, 12), (1, 128)) # Output a vector of complex numbers
# same computation without checking homogeneity of the two objects in 𝒀 (faster)
𝐦=mean(𝒀, (8, 12), (1, 128); check=false)

# extract the AS in a TF region (8:12Hz and samples 1:128) for the two objects
𝐄=extract(𝒀, (8, 12), (8, 12)) # Output a vector of complex matrices

# plot the real part of the AS of x (see unit recipes.jl)

# gather first useful attributes for the plot
using Plots.Measures
tfArgs=(right_margin = 2mm,
        top_margin = 2mm,
        xtickfont = font(10, "Times"),
        ytickfont = font(10, "Times"))

heatmap(tfAxes(Y)..., real(Y.y);
        c=:pu_or, tfArgs...)

# ...the imaginary part
heatmap(tfAxes(Y)..., imag(Y.y);
        c=:bluesreds, tfArgs...)

# ...the amplitude
heatmap(tfAxes(Y)..., amplitude(Y.y);
        c=:amp, tfArgs...)

# ...the amplitude of the AS smoothed in frequency and time
heatmap(tfAxes(Y)..., amplitude(smooth(hannSmoother, hannSmoother, Y).y);
        c=:fire, tfArgs...)

# ...the phase
heatmap(tfAxes(Y)..., phase(Y.y);
        c=:bluesreds, tfArgs...)

# or generate a TFAmplitude object from the AS
A=TFamplitude(Y)
# and plot it (with different colors)
heatmap(tfAxes(A)..., A.y;
        c=:fire, tfArgs...)

# generate a TFPhase object
ϴ=TFphase(Y)
# and plot it (with custom colors)
heatmap(tfAxes(ϴ)..., ϴ.y;
        c=:pu_or, tfArgs...)

# compute and plot phase in [0, 2π]
heatmap(tfAxes(Y)..., TFphase(Y; func=x->x+π).y;
        c=:amp, tfArgs...)

# compute and plot unwrapped phase
heatmap(tfAxes(Y)..., TFphase(Y; unwrapped=true).y;
        c=:amp, tfArgs...)

# smooth time-frequency AS: smooth frequency
Z=smooth(blackmanSmoother, noSmoother, Y)

# plot amplitude of smoothed analytic signal
heatmap(tfAxes(Z)..., amplitude(Z.y);
        c=:amp, tfArgs...)

# not equivalently (!), create an amplitude object and smooth it:
# in this case the amplitude is smoothed, not the AS
A=smooth(blackmanSmoother, noSmoother, TFamplitude(Y))
heatmap(tfAxes(A)..., A.y;
        c=:fire, tfArgs...)

# Smoothing raw phase estimates is unappropriate
# since the phase is a discontinous function, however it makes sense
# to smooth phase if the phase is unwrapped.
heatmap(tfAxes(Y)...,
        smooth(blackmanSmoother, noSmoother, TFphase(Y; unwrapped=true)).y;
        c=:amp, tfArgs...)

# smooth AS: smooth both frequency and time
E=smooth(blackmanSmoother, blackmanSmoother, Y)

# plot amplitude of smoothed analytic signal
heatmap(tfAxes(E)..., amplitude(E.y);
        c=:fire, tfArgs...)

# plot phase of smoothed analytic signal
heatmap(tfAxes(E)..., phase(E.y);
        c=:bluesreds, tfArgs...)

# not equivalently (!), create amplitude and phase objects and smooth them
A=smooth(blackmanSmoother, blackmanSmoother, TFamplitude(Y))
heatmap(tfAxes(A)..., A.y;
        c=:fire, tfArgs...)

ϴ=smooth(blackmanSmoother, blackmanSmoother, TFphase(Y, unwrapped=true))
heatmap(tfAxes(ϴ)..., ϴ.y;
        c=:pu_or, tfArgs...)

# smooth again
ϴ=smooth(blackmanSmoother, blackmanSmoother, ϴ)
heatmap(tfAxes(ϴ)..., ϴ.y;
        c=:pu_or, tfArgs...)
# and again ...

# create directly smoothed AS
Y=TFanalyticsignal(x, sr, t, bandwidth;
                   fmax=32,
                   fsmoothing=hannSmoother,
                   tsmoothing=hannSmoother)

# plot amplitude of smoothed analytic signal
heatmap(tfAxes(Y)..., amplitude(Y.y);
        c=:amp, tfArgs...)

# create directly smoothed Amplitude
A=TFamplitude(x, sr, t, bandwidth;
              fmax=32,
              fsmoothing=hannSmoother,
              tsmoothing=hannSmoother)

# plot smoothed amplitude
heatmap(tfAxes(A)..., A.y;
        c=:amp, tfArgs...)

# compute a TFAnalyticSignal object with non-linear AS
Y=TFanalyticsignal(x, sr, t, bandwidth; fmax=32, nonlinear=true)

# check that it is non-linear
Y.nonlinear

# check that the amplitude is now 1.0 everywhere
norm(amplitude(Y.y)-ones(eltype(Y.y), size(Y.y))) # must be zero

# plot non-linear phase
heatmap(tfAxes(Y)..., phase(Y.y);
        c=:bkr, tfArgs...)

# get the center frequencies of TFAmplitude object A
A.flabels

# extract the amplitude in a time-frequency region
extract(A, (2, 10), (1, 256)) # output a real matrix

# extract the amplitude in a time-frequency region at only one frequency
extract(A, 10, (1, 256)) # output a row vector

# extract the amplitude at one temporal sample at one frequency
extract(A, 10, 12) # or extract(A, 10.0, 12)

# extract amplitude at one temporal sample in a frequency region
extract(A, (10, 12), 12) # or extract(A, (10.0, 12.0), 12)

# extract amplitude at one temporal sample and all frequencies
extract(A, :, 12) # output a (column) vector

# compute the mean in a time-frequency region:
mean(A, (2, 10), (1, 256)) # output a real number
# is equivalent to (but may be less efficient than)
mean(extract(A, (2, 10), (1, 256)))

# using column sign for extracting all time samples
extract(A, (2, 10), :)

# This:
extract(A, :, :)
# is equivalent to this:
A.y
# but if you don't need to extract all frequencies,
# use the extract function to control what frequencies will be extracted:
# This
extract(A, (4, 5), 10)
# is not equivalent to this
A.y[4:5, 10]
# since the `extract` function finds the correct rows corresponding
# to the sought frequencies (in Hz), while A.y[4:5, 10]
# just returns the elements [4:5, 10] in the TF amplitude object

# Although the first center frequency in A is 2Hz, its
# band-pass region is 1-3Hz, therefore the frequency range 1:10 is accepted
mean(A, (1, 10), (1, 256))
# but this result in an error (see the REPL) since 0.5 is out of range:
mean(A, (0.5, 10), (1, 256))

# using a colon sign for time range
a=mean(A, (1, 10), :)
# using an integer for time range (indicates one specific sample)
a=mean(A, (1, 10), 16)

# using a colon sign for frequency range
a=mean(A, :, (1, 16))
# using a real number for frequency range
a=mean(A, 8.5, (1, 16))

# NB: the `extract` and `mean` functions work with the same syntax
#     for objects TFAnayticSignal, TFAmplitude and TFPhase.
```
"""
function TFanalyticsignal(x         :: Vector{T},
                          sr        :: Int,
                          wl        :: Int          = 0,
                          bandwidth :: IntOrReal    = 2;
                      fmin          :: IntOrReal    = bandwidth,
                      fmax          :: IntOrReal    = sr÷2,
                      filtkind      :: FilterDesign = Butterworth(2),
                      nonlinear     :: Bool         = false,
                      fsmoothing    :: Smoother     = noSmoother,
                      tsmoothing    :: Smoother     = noSmoother,
                      planner       :: Planner      = getplanner,
                      ⏩           :: Bool         = true) where T<:Real
    # NB  if `t` > 2^14 then `t` is set to 2^10 by `anaytic signal function`.
    f, F=filterbank(x, sr, bandwidth;
                filtkind=filtkind, fmin=fmin, fmax=fmax, ⏩=⏩)

    smooth(fsmoothing,
           tsmoothing,
           TFAnalyticSignal(Matrix(transpose(analyticsignal(F,
                                                            wl==0 ? length(x) : wl;
                                                        nonlinear=nonlinear,
                                                        planner=planner,
                                                        ⏩=⏩))),
                            bandwidth,
                            f,
                            nonlinear,
                            noSmoother,
                            noSmoother),
            "TFanalyticsignal")
end

function TFanalyticsignal(𝐱         :: Vector{Vector{T}},
                          sr        :: Int,
                          wl        :: Int,
                          bandwidth :: IntOrReal    = 2;
                      filtkind      :: FilterDesign = Butterworth(2),
                      fmin          :: IntOrReal    = bandwidth,
                      fmax          :: IntOrReal    = sr÷2,
                      nonlinear     :: Bool         = false,
                      fsmoothing    :: Smoother     = noSmoother,
                      tsmoothing    :: Smoother     = noSmoother,
                      planner       :: Planner      = getplanner,
                      ⏩           :: Bool         = true) where T<:Real
    # NB  if `t` > 2^14 then `t` is set to 2^10 by `anaytic signal function`.
    # differently from previous method, wl here must be explicitly provided
    k = length(𝐱)
    𝐙 = TFAnalyticSignalVector(undef, k)

    planner ≠ getplanner ? plan=planner : plan=Planner(plan_estimate, -1.0, wl, T, true)

    function TF!(i::Int, threaded::Bool)
        f, F = filterbank(𝐱[i],
                          sr,
                          bandwidth;
                      filtkind=filtkind,
                      fmin=fmin,
                      fmax=fmax,
                      ⏩=threaded)

        𝐙[i] = TFAnalyticSignal(Matrix(transpose(analyticsignal(F,
                                                                wl;
                                                            nonlinear=nonlinear,
                                                            planner=plan,
                                                            ⏩=threaded))),
                                bandwidth,
                                f,
                                nonlinear,
                                noSmoother,
                                noSmoother)
    end

    _thread(⏩, k) ? (@threads for i=1:k TF!(i, false) end) : (for i=1:k TF!(i, true) end)
    return smooth(fsmoothing, tsmoothing, 𝐙, "TFanalyticsignal")
end

# ++++++++++++++++++++  Show override  +++++++++++++++++++ # (REPL output)
function Base.show(io::IO, ::MIME{Symbol("text/plain")}, Y::TFAnalyticSignal)
    r=size(Y.y, 1)
    c=size(Y.y, 2)
    l=length(Y.flabels)
    println(io, titleFont, "▤ TFAnayticSignal type; $r freq. x $c samples")
    #println(io, "□  □    □      □        □           □", defaultFont)
    println(io, separatorFont, "⭒  ⭒    ⭒      ⭒        ⭒           ⭒", defaultFont)
    println(io, "non-linear(.nonlinear): $(Y.nonlinear)")
    println(io, "freq. sm.(.fsmoothing): ", string(Y.fsmoothing))
    println(io, "time  sm.(.tsmoothing): ", string(Y.tsmoothing))
    println(io, "bandwidth             : $(Y.bandwidth) Hz")
    println(io, "freq. lab.  (.flabels): $(l)-", typeof(Y.flabels))
    println(io, "data              (.y): $(r)x$(c)-", typeof(Y.y))
    r≠l && @warn "number of frequency labels does not match the data matrix size" l r
end

function Base.show(io::IO, ::MIME{Symbol("text/plain")}, 𝐘::TFAnalyticSignalVector)
    println(io, titleFont, "▤ ⋯ ▤ TFAnalyticSignalVector Type")
    println(io, separatorFont, "⭒  ⭒    ⭒      ⭒        ⭒           ⭒", defaultFont)
    println(io, "$(length(𝐘))-element Vector{TFAnalyticSignal}")
end
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #

#### Time-Frequency Amplitude ####

#############################################################################
# Generic constructors of TFAmplitude objects:
##############################################
# Enable construction giving only `y`, `bandwidth`, `flabels`, `fsmoothing`
# and `tsmoothing`. `func` is set to `identity`.
TFAmplitude(y, bandwidth, flabels, fsmoothing, tsmoothing) =
    TFAmplitude(y, bandwidth, flabels, fsmoothing, tsmoothing, identity)
#
# As above, but setting by default also both
# `fsmoothing` and `tsmoothing` to `noSmoother`.
TFAmplitude(y, bandwidth, flabels) =
    TFAmplitude(y, bandwidth, flabels, noSmoother, noSmoother, identity)

#############################################################################
# Constructors from Data
##############################################
"""
```
(1)
function TFamplitude(Z::TFAnalyticSignal;
                func::Function=identity)

(2)
function TFamplitude(𝐙::TFAnalyticSignalVector;
                func::Function=identity)

(3)
function TFamplitude(x         :: Vector{T},
                     sr        :: Int,
                     wl        :: Int,
                     bandwidth :: IntOrReal = 2;
                func       :: Function     = identity,
                filtkind   :: FilterDesign = Butterworth(2),
                fmin       :: IntOrReal    = bandwidth,
                fmax       :: IntOrReal    = sr÷2,
                fsmoothing :: Smoother     = noSmoother,
                tsmoothing :: Smoother     = noSmoother,
                planner    :: Planner      = getplanner,
                ⏩        :: Bool         = true) where T<:Real =

(4)
function TFamplitude(𝐱 :: Vector{Vector{T}},
                < same arguments as method (3) >
```

(1)

Construct a [TFAmplitude](@ref) object computing the amplitude of
[TFAnalyticSignal](@ref) object `Z`.
Optional keyword argument `func` is a function to be applied
element-wise to the data matrix of the output. By default,
the `identity` (do nothing) function is applied.

(2)

Construct a [TFAmplitudeVector](@ref) object from a
[TFAnalyticSignalVector](@ref) object executing method (1)
for all [TFAnalyticSignal](@ref) objects in `𝐙`

(3)

Call [`TFanalyticsignal`](@ref) to obtain the time-frequency
analytic signal of real signal vector `x` and construct a [TFAmplitude](@ref)
object holding the time-frequency amplitude (the mudulus, often
referred to as the *envelope*) of `x`.

All arguments are used for regulating the estimation of the analytic signal,
with the exception of `func`, `fsmoothing` and `fsmoothing`:

`func` is an optional function to be applied to the amplitude data matrix output.

In order to estimate the analytic signal in the time-frequency domain
this function calls the [`TFanalyticsignal`](@ref) constructor
(method (1) therein), with both `fsmoothing` and `tsmoothing` arguments
set to `noSmoother`.
Arguments `fsmoothing` and `fsmoothing` are then used to smooth the amplitude.

In order to obtain amplitude estimations on smoothed analytic signal instead,
create a [TFAnalyticSignal](@ref) object passing a
[Smoother](@ref) to the [`TFanalyticsignal`](@ref)
constructor and then use method (1) to obtain the amplitude.
Such amplitude estimation can be further smoothed
using the [`smooth`](@ref) function, as shown in the examples.

For the meaning of all other arguments, which are passed to function
[`TFanalyticsignal`](@ref), see the documentation therein.

(4)

Construct a [TFAmplitudeVector](@ref) object from a
vector of real signal vectors `𝐱`, executing method (3) for all of them.
In order to estimate the time-frequency analytic signal for a
vector of signals, method (2) of [`TFanalyticsignal`](@ref) is called.

**See**: [`TFanalyticsignal`](@ref), [TFAmplitude](@ref).

**See also**: [`amplitude`](@ref).

**Examples**: see the examples of [`TFanalyticsignal`](@ref).
"""
TFamplitude(Z::TFAnalyticSignal; func::Function=identity) =
    TFAmplitude(amplitude(Z.y; func=func), Z.bandwidth, Z.flabels, Z.fsmoothing, Z.tsmoothing, func)

TFamplitude(𝐙::TFAnalyticSignalVector; func::Function=identity) =
    TFAmplitudeVector([TFAmplitude(amplitude(Z.y; func=func), Z.bandwidth, Z.flabels, Z.fsmoothing, Z.tsmoothing, func) for Z ∈ 𝐙])

TFamplitude(𝐱         :: Union{Vector{T}, Vector{Vector{T}}},
            sr        :: Int,
            wl        :: Int,
            bandwidth :: IntOrReal    = 2;
        func          :: Function     = identity,
        filtkind      :: FilterDesign = Butterworth(2),
        fmin          :: IntOrReal    = bandwidth,
        fmax          :: IntOrReal    = sr÷2,
        fsmoothing    :: Smoother     = noSmoother,
        tsmoothing    :: Smoother     = noSmoother,
        planner       :: Planner      = getplanner,
        ⏩           :: Bool         = true) where T<:Real =

    smooth(fsmoothing,
           tsmoothing,
           TFamplitude(TFanalyticsignal(𝐱,
                                        sr,
                                        wl,
                                        bandwidth;
                                    filtkind  = filtkind,
                                    fmin      = fmin,
                                    fmax      = fmax,
                                    planner   = planner,
                                    ⏩       = ⏩);
                        func=func),
           "TFamplitude")

# ++++++++++++++++++++  Show override  +++++++++++++++++++ # (REPL output)
function Base.show(io::IO, ::MIME{Symbol("text/plain")}, Y::TFAmplitude)
   r=size(Y.y, 1)
   c=size(Y.y, 2)
   l=length(Y.flabels)
   println(io, titleFont, "▤ TFAmplitude type; $r freq. x $c samples")
   #println(io, "□  □    □      □        □           □", defaultFont)
   println(io, separatorFont, "⭒  ⭒    ⭒      ⭒        ⭒           ⭒", defaultFont)
   println(io, "freq. sm.(.fsmoothing): ", string(Y.fsmoothing))
   println(io, "time  sm.(.tsmoothing): ", string(Y.tsmoothing))
   println(io, "bandwidth             : $(Y.bandwidth) Hz")
   println(io, "function     (.func)  : ", string(Y.func))
   println(io, "freq. lab.(.flabels)  : $(l)-", typeof(Y.flabels))
   println(io, "data            (.y)  : $(r)x$(c)-", typeof(Y.y))
   r≠l && @warn "number of frequency labels does not match the data matrix size" l r
end

function Base.show(io::IO, ::MIME{Symbol("text/plain")}, 𝐘::TFAmplitudeVector)
   println(io, titleFont, "▤ ⋯ ▤ TFAmplitudeVector Type")
   println(io, separatorFont, "⭒  ⭒    ⭒      ⭒        ⭒           ⭒", defaultFont)
   println(io, "$(length(𝐘))-element Vector{TFAmplitude}")
end
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #

#### Time-Frequency Phase ####

#############################################################################
# Generic constructors of TFAmplitude objects:
##############################################
#
# Enable construction giving only `y`, `bandwidth`, `flabels`, `fsmoothing`
# and `tsmoothing`. `unwrapped` is set to false and `func` is set to `identity`.
TFPhase(y, bandwidth, flabels, nonlinear, fsmoothing, tsmoothing) =
    TFPhase(y, bandwidth, flabels, nonlinear, fsmoothing, tsmoothing, false, identity)
#
# As above, but setting by default also `nonlinear` to true and both
# `fsmoothing` and `tsmoothing` to `noSmoother`.
TFPhase(y, bandwidth, flabels) =
    TFPhase(y, bandwidth, flabels, true, noSmoother, noSmoother, false, identity)

#############################################################################
# Constructors from Data
##############################################
"""
```
(1)
function TFphase(Z :: TFAnalyticSignal;
            func      :: Function = identity,
            unwrapped :: Bool     = false)

(2)
function TFphase(𝐙 :: TFAnalyticSignalVector;
            func      ::Function = identity,
            unwrapped ::Bool     = false)

(3)
function TFphase(x         :: Vector{T},
                 sr        :: Int,
                 wl        :: Int,
                 bandwidth :: IntOrReal = 2;
           unwrapped  :: Bool         = false,
           func       :: Function     = identity,
           filtkind   :: FilterDesign = Butterworth(2),
           fmin       :: IntOrReal    = bandwidth,
           fmax       :: IntOrReal    = sr÷2,
           nonlinear  :: Bool         = false,
           fsmoothing :: Smoother     = noSmoother,
           tsmoothing :: Smoother     = noSmoother,
           planner    :: Planner      = getplanner,
           ⏩        :: Bool         = true) where T<:Real

(4)
function TFphase(𝐱 :: Vector{Vector{T}},
                < same arguments as method (3) >
```

(1)

Construct a [TFPhase](@ref) object computing the phase of
[TFAnalyticSignal](@ref) object `Z`. By default the phase is
represented in ``[−π, π]``.

If optional keyword argument `unwrapped` is true (false by defalut),
the phase is unwrapped, that is, it holds the cumulative sum of the phase
along the time dimension once this is represented in ``[0, 2π]``.

Optional keyword argument `func` is a function to be applied
element-wise to the data matrix of the output. By default,
the `identity` (do nothing) function is applied. If `unwrapped`
is true, the function is applied on the unwrapped phase.

(2)

Construct a [TFPhaseVector](@ref) object from a
[TFAnalyticSignalVector](@ref) object executing method (1)
for all [TFAnalyticSignal](@ref) objects in `𝐙`

(3)

Call [`TFanalyticsignal`](@ref) to obtain the time-frequency
analytic signal of real signal vector `x` and construct a [TFPhase](@ref)
object holding the time-frequency phase (argument) of `x`.

All arguments are used for regulating the estimation of the analytic signal,
with the exception of `unwrapped`, `func`, `fsmoothing` and `fsmoothing`.

`unwrapped` has the same meaning as in method (1) and (2).

`func` is an optional function to be applied to the phase data matrix output.
If `unwrapped` is true, it is applied to the unwrapped phase.

In order to estimate the analytic signal in the time-frequency domain
this function calls the [`TFanalyticsignal`](@ref) constructor
(method (1) therein),
with both `fsmoothing` and `tsmoothing` arguments set to `noSmoother`.
`fsmoothing` and `fsmoothing` are then used to smooth the phase if `unwrapped`
is true.

In order to obtain phase estimations on smoothed analytic signal instead,
create a [TFAnalyticSignal](@ref) object passing a
[Smoother](@ref) to the [`TFanalyticsignal`](@ref)
constructor and then use method (1) to obtain the phase.

For the meaning of all other arguments, which are passed to function
[`TFanalyticsignal`](@ref), see the documentation therein.

(4)

Construct a [TFPhaseVector](@ref) object from a
vector of real signal vectors `𝐱`, executing method (3) for all of them.
In order to estimate the time-frequency analytic signal for a
vector of signals, method (2) of [`TFanalyticsignal`](@ref) is called.

**See**: [`TFanalyticsignal`](@ref), [TFPhase](@ref).

**See also**: [`phase`](@ref), [`unwrapPhase`](@ref), [`polar`](@ref).

**Examples**: see the examples of [`TFanalyticsignal`](@ref).
"""
TFphase(Z::TFAnalyticSignal; func::Function=identity, unwrapped::Bool=false) =
    TFPhase(phase(Z.y; unwrapdims=2*unwrapped, func=func), Z.bandwidth, Z.flabels, Z.nonlinear, Z.fsmoothing, Z.tsmoothing, unwrapped, func)

TFphase(𝐙::TFAnalyticSignalVector; func::Function=identity, unwrapped::Bool=false) =
    TFPhaseVector([TFphase(Z; func=func, unwrapped=unwrapped) for Z in 𝐙])

function TFphase(𝐱         :: Union{Vector{T}, Vector{Vector{T}}},
                 sr        :: Int,
                 wl        :: Int,
                 bandwidth :: IntOrReal    = 2;
           unwrapped  :: Bool         = false,
           func       :: Function     = identity,
           filtkind   :: FilterDesign = Butterworth(2),
           fmin       :: IntOrReal    = bandwidth,
           fmax       :: IntOrReal    = sr÷2,
           nonlinear  :: Bool         = false,
           fsmoothing :: Smoother     = noSmoother,
           tsmoothing :: Smoother     = noSmoother,
           planner    :: Planner      = getplanner,
           ⏩        :: Bool         = true) where T<:Real

   TFP = TFphase(TFanalyticsignal(𝐱,
                                 sr,
                                 wl,
                                 bandwidth;
                                 filtkind  = filtkind,
                                 fmin      = fmin,
                                 fmax      = fmax,
                                 nonlinear = nonlinear,
                                 planner   = planner,
                                 ⏩       = ⏩);
                         unwrapped = unwrapped,
                         func      = func)

   !unwrapped && (fsmoothing≠noSmoother || tsmoothing≠noSmoother) &&
   @warn 📌*", call to TFPhase constructor; smoothing is meaningful only on unwrapped phase. Smoothing has been disabled.)" unwrapped

   return unwrapped ? smooth(fsmoothing,
                             tsmoothing,
                             TFP,
                             "TFphase") :
                      TFP
end

# ++++++++++++++++++++  Show override  +++++++++++++++++++ # (REPL output)
function Base.show(io::IO, ::MIME{Symbol("text/plain")}, Y::TFPhase)
    r=size(Y.y, 1)
    c=size(Y.y, 2)
    l=length(Y.flabels)
    println(io, titleFont, "▤ TFPhase type; $r freq. x $c samples")
    #println(io, "□  □    □      □        □           □", defaultFont)
    println(io, separatorFont, "⭒  ⭒    ⭒      ⭒        ⭒           ⭒", defaultFont)
    println(io, "non-linear(.nonlinear): $(Y.nonlinear)")
    println(io, "freq. sm.(.fsmoothing): ", string(Y.fsmoothing))
    println(io, "time  sm.(.tsmoothing): ", string(Y.tsmoothing))
    println(io, "bandwidth             : $(Y.bandwidth) Hz")
    println(io, "unwrapped             : $(Y.unwrapped)")
    println(io, "function     (.func)  : ", string(Y.func))
    println(io, "freq. lab.(.flabels)  : $(l)-", typeof(Y.flabels))
    println(io, "data            (.y)  : $(r)x$(c)-", typeof(Y.y))
    r≠l && @warn "number of frequency labels does not match the data matrix size" l r
end

function Base.show(io::IO, ::MIME{Symbol("text/plain")}, 𝐘::TFPhaseVector)
    println(io, titleFont, "▤ ⋯ ▤ TFPhaseVector Type")
    println(io, separatorFont, "⭒  ⭒    ⭒      ⭒        ⭒           ⭒", defaultFont)
    println(io, "$(length(𝐘))-element Vector{TFPhase}")
end
# ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #


"""
Generic function to estimate decay time on a single vector

Args:
	PP (scipy matrix): approximation of the discretized matrix to use
	v (numpy vector):
	my_norm (function):
	M, K (reals with RNDU): constants such that ||P-PP|| ≤ K` and ||P^n|| ≤ M`,
	where P is the exact discretized matrix.

Returns:
	n such that ||PP^n v|| ≤ target`.

Raises:
	ValueError if insufficient precision is detected
"""
function vector_decay_time(B::Basis, PP, v, M=0, K=0, target = 0.5)
	current_norm = weak_norm(B, v)
	error_on_computed_norm = zero(M)
	n = 0
	MK = M * K
	error_propagation_constant = K #this constant is K at the first step and MK afterwards; see notes
	while current_norm + error_on_computed_norm >= target
		n += 1
		v = PP * v
		current_norm = weak_norm(B, v)
		error_on_computed_norm += error_propagation_constant * current_norm
		error_propagation_constant = MK
		if error_on_computed_norm > target
			raise() # ValueError, 'Insufficient precision'
		end
	end
	return n
end

"""
Number of iterations needed to contract all vectors in `basis.contracting_pairs()` to a given target alpha
"""
function decay_time(D::Dynamic, B::Basis, P::AbstractMatrix{Interval{T}}, alpha = 0.5, n_jobs = 1) where {T}

	PP = mid.(P)

	M = bound_on_norms_of_powers(B, D, project_left=True, project_right=True)
	K = numerical_error(B, D, P, PP)
	my_norm(v) = weak_norm(B, v)

	#alpha = Rdown(alpha) # we need a lower bound to alpha*s to make sure we contract below it
	#decay_times = Parallel(n_jobs=n_jobs, verbose=1)(delayed(vector_decay_time)(PP, v, my_norm, M, K, alpha*s) for v, s in basis.contracting_pairs())

	return max(decay_times)
end


# Utilities to generate a grid with the following inputs

@inline adapt_if_vector(to, var) = var
@inline adapt_if_vector(to, var::AbstractArray) = Adapt.adapt(to, var)

@inline show_coordinate(Δ::Number, T)            = "Regular, with spacing $Δ"
@inline show_coordinate(Δ::Number, ::Type{Flat}) = "Flattened"
@inline show_coordinate(Δ::AbstractVector, T)    = "Stretched, with spacing min=$(minimum(parent(Δ))), max=$(maximum(parent(Δ)))"

get_domain_extent(coord, N)                 = (coord[1], coord[2])
get_domain_extent(coord::Function, N)       = (coord(1), coord(N+1))
get_domain_extent(coord::AbstractVector, N) = (coord[1], coord[N+1])

get_coord_face(coord::Nothing, i) = 1
get_coord_face(coord::Function, i) = coord(i)
get_coord_face(coord::AbstractVector, i) = CUDA.@allowscalar coord[i]

lower_exterior_Δcoordᶜ(topology,        Fi, Hcoord) = [Fi[end - Hcoord + i] - Fi[end - Hcoord + i - 1] for i = 1:Hcoord]
lower_exterior_Δcoordᶜ(::Type{Bounded}, Fi, Hcoord) = [Fi[2]  - Fi[1] for i = 1:Hcoord]

upper_exterior_Δcoordᶜ(topology,        Fi, Hcoord) = [Fi[i + 1] - Fi[i] for i = 1:Hcoord]
upper_exterior_Δcoordᶜ(::Type{Bounded}, Fi, Hcoord) = [Fi[end]   - Fi[end - 1] for i = 1:Hcoord]

# generate a stretched coordinate passing the explicit coord faces as vector of functionL
function generate_coordinate(FT, topology, N, H, coord, architecture)

    # Ensure correct type for F and derived quantities
    interiorF = zeros(FT, N+1)

    for i = 1:N+1
        interiorF[i] = get_coord_face(coord, i)
    end

    L = interiorF[N+1] - interiorF[1]

    # Build halo regions
    ΔF₋ = lower_exterior_Δcoordᶜ(topology, interiorF, H)
    ΔF₊ = reverse(upper_exterior_Δcoordᶜ(topology, interiorF, H))

    c¹, cᴺ⁺¹ = interiorF[1], interiorF[N+1]

    F₋ = [c¹   - sum(ΔF₋[i:H]) for i = 1:H]          # locations of faces in lower halo
    F₊ = reverse([cᴺ⁺¹ + sum(ΔF₊[i:H]) for i = 1:H]) # locations of faces in width of top halo region

    F = vcat(F₋, interiorF, F₊)

    # Build cell centers, cell center spacings, and cell interface spacings
    TC = total_length(Center, topology, N, H)
     C = [ (F[i + 1] + F[i]) / 2 for i = 1:TC ]
    ΔF = [  C[i] - C[i - 1]      for i = 2:TC ]

    # Trim face locations for periodic domains
    TF = total_length(Face, topology, N, H)
    F  = F[1:TF]

    ΔC = [F[i + 1] - F[i] for i = 1:TF-1]

    ΔF = OffsetArray(ΔF, -H)
    ΔC = OffsetArray(ΔC, -H)

    # Seems needed to avoid out-of-bounds error in viscous dissipation
    # operators wanting to access ΔF[N+2].
    ΔF = OffsetArray(cat(ΔF[0], ΔF..., ΔF[N], dims=1), -H-1)

    ΔF = OffsetArray(arch_array(architecture, ΔF.parent), ΔF.offsets...)
    ΔC = OffsetArray(arch_array(architecture, ΔC.parent), ΔC.offsets...)

    F = OffsetArray(F, -H)
    C = OffsetArray(C, -H)

    # Convert to appropriate array type for arch
    F = OffsetArray(arch_array(architecture, F.parent), F.offsets...)
    C = OffsetArray(arch_array(architecture, C.parent), C.offsets...)

    return L, F, C, ΔF, ΔC
end

# generate a regular coordinate passing the domain extent (2-tuple) and number of points
function generate_coordinate(FT, topology, N, H, coord::Tuple{<:Number, <:Number}, architecture)

    @assert length(coord) == 2

    c₁, c₂ = coord
    @assert c₁ < c₂
    L = FT(c₂ - c₁)

    # Convert to get the correct type also when using single precision
    ΔF = ΔC = Δ = convert(FT, L / N)

    F₋ = c₁ - H * Δ
    F₊ = F₋ + total_extent(topology, H, Δ, L)

    C₋ = F₋ + Δ / 2
    C₊ = C₋ + L + Δ * (2H - 1)

    TF = total_length(Face,   topology, N, H)
    TC = total_length(Center, topology, N, H)

    F = range(F₋, F₊, length = TF)
    C = range(C₋, C₊, length = TC)

    F = OffsetArray(F, -H)
    C = OffsetArray(C, -H)

    return L, F, C, ΔF, ΔC
end

# Flat domains
function generate_coordinate(FT, ::Type{Flat}, N, H, coord::Tuple{<:Number, <:Number}, architecture)
    return FT(1), range(1, 1, length=N), range(1, 1, length=N), FT(1), FT(1)
end


#####
##### confusion matrices
#####

"""
    confusion_matrix(class_count::Integer, hard_label_pairs = ())

Given the iterable `hard_label_pairs` whose `k`th element takes the form
`(first_classifiers_label_for_sample_k, second_classifiers_label_for_sample_k)`,
return the corresponding confusion matrix where `matrix[i, j]` is the number of
samples that the first classifier labeled `i` and the second classifier labeled
`j`.

Note that the returned confusion matrix can be updated in-place with new labels
via `Lighthouse.increment_at!(matrix, more_hard_label_pairs)`.
"""
function confusion_matrix(class_count::Integer, hard_label_pairs=())
    confusion = zeros(Int, class_count, class_count)
    increment_at!(confusion, hard_label_pairs)
    return confusion
end

"""
    accuracy(confusion::AbstractMatrix)

Returns the percentage of matching classifications out of total classifications,
or `missing` if `all(iszero, confusion)`.

Note that `accuracy(confusion)` is equivalent to overall percent agreement
between `confusion`'s row classifier and column classifier.
"""
function accuracy(confusion::AbstractMatrix)
    total = sum(confusion)
    total == 0 && return missing
    return tr(confusion) / total
end

"""
    binary_statistics(confusion::AbstractMatrix, class_index)

Treating the rows of `confusion` as corresponding to predicted classifications
and the columns as corresponding to true classifications, return a `NamedTuple`
with the following fields for the given `class_index`:

- `predicted_positives`
- `predicted_negatives`
- `actual_positives`
- `actual_negatives`
- `true_positives`
- `true_negatives`
- `false_positives`
- `false_negatives`
- `true_positive_rate`
- `true_negative_rate`
- `false_positive_rate`
- `false_negative_rate`
- `precision`
"""
function binary_statistics(confusion::AbstractMatrix, class_index::Integer)
    total = sum(confusion)
    predicted_positives = sum(view(confusion, class_index, :))
    predicted_negatives = total - predicted_positives
    actual_positives = sum(view(confusion, :, class_index))
    actual_negatives = total - actual_positives
    true_positives = confusion[class_index, class_index]
    true_negatives = sum(diag(confusion)) - true_positives
    false_positives = predicted_positives - true_positives
    false_negatives = actual_positives - true_positives
    true_positive_rate = (true_positives == 0 && actual_positives == 0) ?
                         (one(true_positives) / one(actual_positives)) :
                         (true_positives / actual_positives)
    true_negative_rate = (true_negatives == 0 && actual_negatives == 0) ?
                         (one(true_negatives) / one(actual_negatives)) :
                         (true_negatives / actual_negatives)
    false_positive_rate = (false_positives == 0 && actual_negatives == 0) ?
                          (zero(false_positives) / one(actual_negatives)) :
                          (false_positives / actual_negatives)
    false_negative_rate = (false_negatives == 0 && actual_positives == 0) ?
                          (zero(false_negatives) / one(actual_positives)) :
                          (false_negatives / actual_positives)
    precision = (true_positives == 0 && predicted_positives == 0) ? missing :
                (true_positives / predicted_positives)
    return (predicted_positives=predicted_positives,
            predicted_negatives=predicted_negatives, actual_positives=actual_positives,
            actual_negatives=actual_negatives, true_positives=true_positives,
            true_negatives=true_negatives, false_positives=false_positives,
            false_negatives=false_negatives, true_positive_rate=true_positive_rate,
            true_negative_rate=true_negative_rate, false_positive_rate=false_positive_rate,
            false_negative_rate=false_negative_rate, precision=precision)
end

function binary_statistics(confusion::AbstractMatrix)
    return [binary_statistics(confusion, i) for i in 1:size(confusion, 1)]
end

#####
##### interrater agreement
#####

"""
    cohens_kappa(class_count, hard_label_pairs)

Return `(κ, p₀)` where `κ` is Cohen's kappa and `p₀` percent agreement given
`class_count` and `hard_label_pairs` (these arguments take the same form as
their equivalents in [`confusion_matrix`](@ref)).
"""
function cohens_kappa(class_count, hard_label_pairs)
    @assert all(issubset(pair, 1:class_count) for pair in hard_label_pairs)
    p₀ = accuracy(confusion_matrix(class_count, hard_label_pairs))
    pₑ = _probability_of_chance_agreement(class_count, hard_label_pairs)
    return _cohens_kappa(p₀, pₑ), p₀
end

_cohens_kappa(p₀, pₑ) = (p₀ - pₑ) / (1 - ifelse(pₑ == 1, zero(pₑ), pₑ))

function _probability_of_chance_agreement(class_count, hard_label_pairs)
    labels_1 = (pair[1] for pair in hard_label_pairs)
    labels_2 = (pair[2] for pair in hard_label_pairs)
    x = sum(k -> count(==(k), labels_1) * count(==(k), labels_2), 1:class_count)
    return x / length(hard_label_pairs)^2
end

#####
##### probability distributions
#####

# source: https://github.com/FluxML/Flux.jl/blob/fe85a38d78e225e07a0b75c12b55c8398ae3fe5d/src/layers/stateless.jl#L6
mse(ŷ, y) = sum((ŷ .- y) .^ 2) * 1 // length(y)

"""
    calibration_curve(probabilities, bitmask; bin_count=10)

Given `probabilities` (the predicted probabilities of the positive class) and
`bitmask` (a vector of `Bool`s indicating whether or not the element actually
belonged to the positive class), return `(bins, fractions, totals, mean_squared_error)`
where:

- `bins` a vector with `bin_count` `Pairs` specifying the calibration curve's probability bins
- `fractions`: a vector where `fractions[i]` is the number of values in `probabilities`
  that falls within `bin[i]` over the total number of values within `bin[i]`, or `missing`
  if the total number of values in `bin[i]` is zero.
- `totals`: a vector where `totals[i]` the total number of values within `bin[i]`.
- `mean_squared_error`: The mean squared error of `fractions` vs. an ideal calibration curve.

This method is similar to the corresponding scikit-learn method:

https://scikit-learn.org/stable/modules/generated/sklearn.calibration.calibration_curve.html
"""
function calibration_curve(probabilities, bitmask; bin_count=10)
    bins = probability_bins(bin_count)
    per_bin = [fraction_within(probabilities, bitmask, bin...) for bin in bins]
    fractions, totals = first.(per_bin), last.(per_bin)
    nonempty_indices = findall(!ismissing, fractions)
    if !isempty(nonempty_indices)
        ideal = range(mean(first(bins)), mean(last(bins)); length=length(bins))
        mean_squared_error = mse(fractions[nonempty_indices], ideal[nonempty_indices])
    else
        mean_squared_error = missing
    end
    return (bins=bins, fractions=fractions, totals=totals,
            mean_squared_error=mean_squared_error)
end

function probability_bins(bin_count)
    r = range(0.0, 1.0; length=(bin_count + 1))
    return [begin
                start, stop = r[i], r[i + 1]
                stop = (i + 1) < length(r) ? prevfloat(stop) : stop
                start => stop
            end for i in 1:(length(r) - 1)]
end

function fraction_within(values, bitmask, start, stop)
    count = 0
    total = 0
    for (i, value) in enumerate(values)
        if start <= value <= stop
            count += bitmask[i]
            total += 1
        end
    end
    fraction = iszero(total) ? missing : (count / total)
    return (fraction=fraction, total=total)
end


#=
testODE:
- Julia version:
- Author: ymocquar
- Date: 2019-11-13
=#
using HOODESolver
using DifferentialEquations
using LinearAlgebra
using Plots
using Random
function twoscales_solve( par_u0::PrepareU0, order, t, nb)
end
function fctMain(n_tau)
    seed=123321
    Random.seed!(seed)
    u0 = rand(BigFloat, 4)
    B = 2rand(BigFloat, 4, 4) - ones(BigFloat, 4, 4)
    println("seed = $seed B=$B")
    nbeps=48
    nbmaxtest=3
    paseps = big"10.0"^0.125
    ordmin=3
    ordmax=14
    ordpas = 1
    t_max = big"1.0"
    A = [0 0 1 0; 0 0 0 0;-1 0 0 0; 0 0 0 0]
    for order = ordmin:ordpas:ordmax
        y = ones(Float64, nbeps, nbmaxtest)
        x=zeros(Float64,nbeps)
        for i_eps=1:nbeps
            epsilon = big"1.0"/paseps^i_eps
            x[i_eps] = epsilon
            fct = u -> B*u
            parphi = PreparePhi(epsilon, n_tau, A, fct, B)
            println("prepareU0 eps=$epsilon n_tau=$n_tau")
            @time par_u0 = PrepareU0(parphi, order+2, u0)
            solref = getexactsol(parphi, u0, t_max)
            eps_v = convert(Float32,epsilon)
            println("epsilon = $eps_v solref=$solref")
            for indc=1:nbmaxtest
                nb = 10*10^indc
                @time parGen = PrepareTwoScalePureAB(nb, t_max, order, par_u0)
                @time sol = twoscales_pure_ab(parGen, only_end=true)
                println("solref=$solref")
                println("nb=$nb sol=$sol")
                diff=solref-sol
                println("nb=$nb dt=$(1.0/nb) normInf=$(norm(diff,Inf)) norm2=$(norm(diff))")
                y[i_eps, indc] = norm(diff,Inf)
                println("epsilon=$epsilon result=$y")
            end
        end
        labels=Array{String,2}(undef, 1, nbmaxtest)
        for i=1:nbmaxtest
            nb = 10*10^i
            labels[1,i] = " delta t,order=$(1/nb), $order "
            mindiff = minimum(y[:,i])
            bornediff = min(mindiff*1e20,1)
            for j=1:nbeps
                if y[j,i] >= bornediff
                    y[j,i] = NaN
                end
            end
        end
        gr()
        p=Plots.plot(
    x,
    y,
    xlabel="epsilon",
    xaxis=:log,
    ylabel="error",
    yaxis=:log,
    legend=:topleft,
    label=labels,
    marker=2
)
        prec_v = precision(BigFloat)
        Plots.savefig(p,"out/r_$(prec_v)_$(order)_$(n_tau)_epsilon_lgn_2.pdf")
    end
end
fctMain(32)


@doc """
	boundary_coef_mat(F::BoundaryFilter) -> Matrix

The boundary coefficients collected in a matrix where the `i`'th row
contains the coefficients of the `i`'th boundary scaling function.
"""->
function boundary_coef_mat(F::BoundaryFilter)
	vm = van_moment(F)
	coef_mat = zeros(Float64, vm, vm)

	for row in 1:vm
		coef = bfilter(F, row-1)
		@inbounds coef_mat[row,:] = sqrt2*coef[1:vm]
	end

	return coef_mat
end

@doc """
	DaubScaling(p::Integer, side::Char, R::Integer) -> x, Y

Compute the boundary scaling function at resolution `R` on `side` with `p` vanishing moments.
"""->
function DaubScaling(p::Integer, side::Char, R::Integer)
	B = bfilter(p, side)
	IF = ifilter(p, true)

	x = dyadic_rationals( support(B), R )
	Y = DaubScaling(B, IF, R)

	return x, Y'
end

#=
	DaubScaling(B::BoundaryFilter) -> Vector

Compute the boundary scaling function values at 0.
=#
function DaubScaling(B::BoundaryFilter)
	boundary_mat = boundary_coef_mat(B)
	E = eigval1(boundary_mat)
	# TODO: How to scale E?

	return E
end

#=
	DaubScaling(B, I) -> Matrix

Compute the boundary scaling function defined by boundary filter `B` and
interior filter `I` at the non-zero integers in their support.

The ouput is a matrix where the `k`'th row are the functions values of the `k-1` scaling function.
=#
function DaubScaling(B::BoundaryFilter, IF::InteriorFilter)
	internal = DaubScaling(IF)
	IS = support(IF)
	BS = support(B)

	# Function values at 0
	vm = van_moment(B)
	xvals = integers(B)
	Y = zeros(Float64, vm, length(xvals)+1)
	Y[:, x2index(0,BS)] = DaubScaling(B)

	xvals = integers(B)
	# The translations of the interior scaling function differ for the two sides
	interior_start = ( side(B) == 'L' ? vm : -vm-1 )
	interior_iter = ( side(B) == 'L' ? 1 : -1 )

	for x in xvals
		xindex = x2index(x, BS)
		doublex = 2*x
		doublex_index = x2index(doublex, BS)

		for k in 1:vm
			filterk = bfilter(B, k-1)

			# Boundary contribution
			if isinside(doublex, BS)
				for l in 1:vm
					Y[k, xindex] += sqrt2 * filterk[l] * Y[l, doublex_index]
				end
			end

			# Interior contribution
			fidx = vm
			interior_arg = doublex - interior_start
			flength = vm + 2*(k-1) #length(filterk)
			while fidx <= flength
				fidx += 1
				if isinside(interior_arg, IS)
					Y[k,xindex] += sqrt2 * filterk[fidx] * internal[ x2index(interior_arg,IS) ]
				end
				interior_arg -= interior_iter
			end
		end
	end

	return Y
end

@doc """
	DaubScaling(B, I, R) -> Matrix

Compute the boundary scaling function defined by boundary filter `B` and interior filter `I` at the dyadic rationals up to resolution `R` in their support.

The ouput is a matrix where the `k`'th row are the functions values of the `k-1` scaling function.
"""->
function DaubScaling(B::BoundaryFilter, IF::InteriorFilter, R::Int)
	R >= 0 || throw(DomainError())

	internal = DaubScaling(IF,R)
	Ny = length(internal)
	vm = van_moment(B)
	Y = zeros(Float64, vm, Ny)

	IS = support(IF)
	BS = support(B)

	# Base level
	cur_idx = dyadic_rationals(BS, R, 0)
	Y[:,cur_idx] = DaubScaling(B, IF)

	interior_start = ( side(B) == 'L' ? vm : -vm-1 )
	interior_iter = ( side(B) == 'L' ? 1 : -1 )

	# Recursion: Fill remaining levels
	xvals = dyadic_rationals(BS, R)
	for L in 1:R
		# Indices of x values on scale L
		cur_idx = dyadic_rationals(BS, R, L)

		for xindex in cur_idx
			doublex = 2*xvals[xindex]
			doublex_index = x2index(doublex, BS, R)

			for k in 1:vm
				filterk = bfilter(B, k-1)

				# Boundary contribution
				if isinside(doublex, BS)
					for l in 1:vm
						Y[k,xindex] += sqrt2 * filterk[l] * Y[l,doublex_index]
					end
				end

				# Interior contribution
				fidx = vm
				interior_arg = doublex - interior_start
				flength = vm + 2*(k-1) #length(filterk)
				while fidx <= flength
					fidx += 1
					if isinside(interior_arg, IS)
						Y[k,xindex] += sqrt2 * filterk[fidx] * internal[ x2index(interior_arg,IS,R) ]
					end
					interior_arg -= interior_iter
				end
			end
		end
	end

	# Hack until the right 0 values are computed
	# TODO: Fix this
	if side(B) == 'L'
		Y[:,1] = Y[:,2]
	else
		Y[:,end] = Y[:,end-1]
	end

	return Y
end


println("Starting tests with $(Threads.nthreads()) threads out of `Sys.CPU_THREADS = $(Sys.CPU_THREADS)`...")
using Polyester, PolyesterForwardDiff, Test, ForwardDiff

x = randn(800);
dxref = similar(x);
dx = similar(x);
f(x) = -sum(sum ∘ sincos, x)
println("Running threaded ForwardDiff test...")
PolyesterForwardDiff.threaded_gradient!(f, dx, x, ForwardDiff.Chunk(8));
ForwardDiff.gradient!(dxref, f, x, ForwardDiff.GradientConfig(f, x, ForwardDiff.Chunk(8), nothing));
@test dx == dxref

dx .= NaN;
batch((length(x), max(1,num_threads()>>1), 2), dx, x) do (dx,x), start, stop
    PolyesterForwardDiff.threaded_gradient!(f, view(dx, start%Int:stop%Int), view(x, start%Int:stop%Int), ForwardDiff.Chunk(8))
end;
@test dx ≈ dxref

dxref = similar(x, 100, 800);
dx = similar(dxref);
yref = similar(x, 100);
y = similar(x, 100);
A = randn(100, 800);
g!(y, x) = (y .= A*x)
g(x) = A*x

PolyesterForwardDiff.threaded_jacobian!(g, dx, x, ForwardDiff.Chunk(8));
ForwardDiff.jacobian!(dxref, g, x, ForwardDiff.JacobianConfig(g, x, ForwardDiff.Chunk(8), nothing));
@test dx ≈ dxref

PolyesterForwardDiff.threaded_jacobian!(g!, y, dx, x, ForwardDiff.Chunk(8));
ForwardDiff.jacobian!(dxref, g!, yref, x, ForwardDiff.JacobianConfig(g!, yref, x, ForwardDiff.Chunk(8), nothing));
@test dx ≈ dxref
@test y ≈ yref


import Test: @test, @testset, @test_throws
import Luna: Polarisation, Output, Modes
import LinearAlgebra: norm

@testset "Jones" begin
    h = Polarisation.H()
    @test norm(Polarisation.Stokes(h) - [1; 1; 0; 0]) < 1e-15

    QWP = Polarisation.rotate(Polarisation.WP(π/2), 0)
    @test norm(Polarisation.Stokes(QWP*h) - [1; 1; 0; 0]) < 1e-15
    QWP = Polarisation.rotate(Polarisation.WP(π/2), π/4)
    @test norm(Polarisation.Stokes(QWP*h) - [1; 0; 0; 1]) < 1e-15
    QWP = Polarisation.rotate(Polarisation.WP(π/2), -π/4)
    @test norm(Polarisation.Stokes(QWP*h) - [1; 0; 0; -1]) < 1e-15

    HWP = Polarisation.rotate(Polarisation.WP(π), 0)
    @test norm(Polarisation.Stokes(HWP*h) - [1; 1; 0; 0]) < 1e-15
    HWP = Polarisation.rotate(Polarisation.WP(π), π/4)
    @test norm(Polarisation.Stokes(HWP*h) - [1; -1; 0; 0]) < 1e-15
    HWP = Polarisation.rotate(Polarisation.WP(π), π/8)
    @test norm(Polarisation.Stokes(HWP*h) - [1; 0; 1; 0]) < 1e-15
    HWP = Polarisation.rotate(Polarisation.WP(π), -π/8)
    @test norm(Polarisation.Stokes(HWP*h) - [1; 0; -1; 0]) < 1e-15

    @test isapprox(Polarisation.ellipticity(Polarisation.Stokes(h)), 0.0)
    QWP = Polarisation.rotate(Polarisation.WP(π/2), π/4)
    @test isapprox(Polarisation.ellipticity(Polarisation.Stokes(QWP*h)), 1.0)
    QWP = Polarisation.rotate(Polarisation.WP(π/2), -π/4)
    @test isapprox(Polarisation.ellipticity(Polarisation.Stokes(QWP*h)), 1.0)

    QWP = Polarisation.rotate(Polarisation.WP(π/2), π/4)
    C = QWP*h
    L = Polarisation.LP()*C
    @test norm(Polarisation.Stokes(L, normalise=true) - [1; 1; 0; 0]) < 1e-15
    @test isapprox(Polarisation.Stokes(L)[1], 0.5)
end


using Pkg
Pkg.activate(".")
using SymEngine

struct CustomFunc
    name::String
    cfunc::Base.CFunction
    func1
    func2
end

struct CFunctionWrapperClass
    name::Ptr{Cchar}
    create_ptr::Ptr{Cvoid}
    diff_ptr::Ptr{Cvoid}
end

function create_func(name, f)
    function _create_func(a::Basic, args_ptr::Ptr{Cvoid})::Cint
        args = SymEngine.CVecBasic(args_ptr)
        try
            res = f(convert(Vector, args))
        catch e
            res = nothing
        end
        if res == nothing
            return 1
        else
            res = Basic(res)
            ccall((:basic_assign, SymEngine.libsymengine), Nothing, (Ref{Basic}, Ref{Basic}), a, res)
            return 0
        end
    end
    create_ptr = @cfunction($_create_func, Cint, (Ref{Basic}, Ptr{Cvoid}))
    return CustomFunc(name, create_ptr, f, _create_func)
end

function my_real(x::Vector{Basic})
   return real(N(x[1]))
end

function my_norm(x::Vector{Basic})
   a, b = N(x[1]), N(x[2])
   return sqrt(a^2+b^2)
end

@vars x

function (c::CustomFunc)(args...)
    v = convert(SymEngine.CVecBasic, args...)
    a = Basic()
    ccall((:basic_create_function_wrapper, SymEngine.libsymengine), Nothing, (Ref{Basic}, Ptr{Cvoid}, Cstring, Ptr{Cvoid}, Ptr{Cvoid}), a, v.ptr, pointer(c.name), c.cfunc.ptr, C_NULL)
    return a
end

c = create_func("Re", my_real)
c(x)

d = create_func("Norm", my_norm)
d(2, 3)


__precompile__()

module ArtificialBeeColony

export ABC
export search!

abstract type TargetSystem end
struct TimeVariant <: TargetSystem end
struct TimeInvariant <: TargetSystem end

mutable struct Bee
    data :: Vector
    fitness :: AbstractFloat
    count :: Integer
end

function Bee(data::Vector)
    Bee(data, zero(eltype(data)), zero(Integer))
end

const Bees = Vector{Bee}

function Base.copyto!(t::Bee, s::Bee)
    copyto!(t.data, s.data)
    t.fitness = s.fitness
    t.count = s.count
    return t
end

function Base.copy(s::Bee)
    return Bee(copy(s.data), s.fitness, s.count)
end

struct ABC
    bees :: Bees
    best :: Bee
    init :: Function
    function ABC(N::Integer, init::Function)
        dim = length(init())
        bees = Bee[Bee(init()) for i=1:N]
        new(bees, Bee(zeros(dim)), init)
    end
end

@inline function update_fitness!(bee::Bee, g::Function)
    bee.fitness = fitness(g(bee.data))
end

@inline function update_fitness!(bees::Bees, g::Function)
    for bee in bees
        update_fitness!(bee, g)
    end
end

function update_bee!(bee::Bee, beem::Bee, g::Function)
    dim = length(bee.data)
    phi = 2.0*(rand()-0.5)
    j = rand(1:dim)

    val = phi*(bee.data[j] - beem.data[j])

    bee.data[j] += val
    fitnew = fitness(g(bee.data))

    if fitnew > bee.fitness
        bee.fitness = fitnew
        bee.count = 0
    else
        bee.data[j] -= val
        bee.count += 1
    end
end

function update_employed!(bees::Bees, g::Function, mode::TimeInvariant)
    N = length(bees)
    for bee in bees
        update_bee!(bee, bees[rand(1:N)], g)
    end
end

function update_employed!(bees::Bees, g::Function, mode::TimeVariant)
    N = length(bees)
    for bee in bees
        update_fitness!(bee, g)
        update_bee!(bee, bees[rand(1:N)], g)
    end
end

function roulette_select(bees::Bees)
    sf = sum(map(x->x.fitness, bees))
    r = rand()
    rs = zero(r)

    for bee in bees
        rs += bee.fitness/sf
        if r<=rs
            return bee
        end
    end
    return bees[end]
end

function update_outlook!(bees::Bees, g::Function, No::Integer, mode::TimeInvariant)
    N = length(bees)
    for i=1:No
        bee = roulette_select(bees)
        update_bee!(bee, bees[rand(1:N)], g)
    end
end

function update_outlook!(bees::Bees, g::Function, No::Integer, mode::TimeVariant)
    N = length(bees)
    for i=1:No
        bee = roulette_select(bees)
        update_fitness!(bee, g)
        update_bee!(bee, bees[rand(1:N)], g)
    end
end

function update_scout!(bees::Bees, g::Function, init::Function, limit::Integer, mode::TimeInvariant)
    bees_scout = filter(x->x.count >= limit, bees)
    for bee in bees_scout
        bee.data = init()
        bee.count = 0
    end
    update_fitness!(bees_scout, g)
end

function update_scout!(bees::Bees, g::Function, init::Function, limit::Integer, mode::TimeVariant)
    bees_scout = filter(x->x.count >= limit, bees)
    for bee in bees_scout
        bee.data = init()
        bee.count = 0
    end
end

function find_best(bees::Bees)
    best = bees[1]
    for bee in bees
        if bee.fitness > best.fitness
            best = bee
        end
    end
    return best
end

function update_best!(bees::Bees, bee_best::Bee, mode::TimeInvariant)
    bee_cand = find_best(bees)
    if bee_best.fitness < bee_cand.fitness
        copyto!(bee_best, bee_cand)
    end
end

function update_best!(bees::Bees, bee_best::Bee, mode::TimeVariant)
    copyto!(bee_best, find_best(bees))
end

function search!(abc::ABC, g::Function; epoch::Integer=1000, time_invariant::Bool=false)
    mode = time_invariant ? TimeInvariant() : TimeVariant()

    Ne = length(abc.bees)
    No = length(abc.bees)
    dim = length(abc.bees[1].data)
    limit = round(Integer, 0.1*dim*(Ne+No))

    if time_invariant
        update_fitness!(abc.bees, g)
    end
    for i=1:epoch
        update_employed!(abc.bees, g, mode)
        update_outlook!(abc.bees, g, No, mode)
        update_best!(abc.bees, abc.best, mode)
        update_scout!(abc.bees, g, abc.init, limit, mode)
    end
    return abc.best.data
end

function fitness(val::T) where T<:AbstractFloat
    if val >= zero(T)
        return one(T)/(one(T)+val)
    else
        return one(T)+abs(val)
    end
end

end


#module CenterOfMass

using PyPlot
using Convex
using SCS
using GLPKMathProgInterface
include("Histogram.jl")
include("plotting_tools.jl")

# all wrenches computed around the origin
point_to_wrench(p::Array{Float64,1}) = [1.0;-p[2]; p[1]]

# normal(x, var) = exp(-0.5*x*x/var)/sqrt(2*pi*var)
const one_over_sqrt_2pi = 1.0 / sqrt(2*pi)
function normal(x, var::Float64)
  ov = 1.0/var
  sqrt2pivar = sqrt(ov) * one_over_sqrt_2pi
  normal(x, -0.5*ov, sqrt2pivar)
end

normal(x::Float64, nhalfov::Float64, ov2sqrt2pi::Float64) = exp(x*x*nhalfov) * ov2sqrt2pi

function critical_force(wrench_applied, wrench_offset, W_boundary)
  fa = Variable(1)
  Fr = Variable(size(W_boundary, 2))

  stability = wrench_applied * fa +
              W_boundary * Fr +
              wrench_offset == 0
  feasibility = [fa >= 0; Fr >= 0]

  problem = maximize(fa, [stability; feasibility])

  solver = SCSSolver(verbose = 0)

  solve!(problem, solver)

  problem.optval, evaluate(Fr)
end

function solve_minimal(a, w1, w2, b)
  A = [a w1 w2]

  #if abs(det(A)) <= 1e-3
    #return -1, [-1;-1]
  #end

  x = A \ (-b)

  for ii = 1:3
    if x[ii] < -1e-3
      return -1.0
    end
  end

  return x[1]
end

# assume points form the convex hull and are arranged clockwise or
# counter-clockwise
function critical_force_iterative(wrench_applied, wrench_offset, W_boundary,
  actuator_limit::Float64 = 1e6)

  max_val::Float64 = -1

  n = size(W_boundary, 2)

  # TODO: implement binary search
  for ii = 1:n
    i2 = mod(ii, n) + 1
    val = solve_minimal(wrench_applied, W_boundary[:,ii], W_boundary[:,i2], wrench_offset)

    if val > max_val
      if val > actuator_limit
        max_val = actuator_limit
      else
        max_val = val
      end

      break
    end
  end

  max_val
end

flatten(v::Array) = hcat(v...)
flatten(v) = v

function rand_in_circle()
  r = sqrt(rand())
  theta = 2*pi*rand()
  r * [cos(theta);sin(theta);0]
end

function to_wrench_matrix(points)
  n = length(points)
  ws = zeros(3, n)
  for ii = 1:n
    ws[:,ii] = point_to_wrench(points[ii])
  end

  ws
end

function critical_force_from_points(boundary_ps, com_p, applied_p, mass,
    actuator_limit = 1e6, solver = critical_force_iterative)

  g = -9.8

  boundary_ws = to_wrench_matrix(boundary_ps)
  gravity_w = mass*g*point_to_wrench(com_p)
  applied_w = point_to_wrench(applied_p)

  fa = solver(applied_w, gravity_w, boundary_ws, actuator_limit)

  #stability = applied_w * fa +
              #boundary_ws * f_boundary +
              #gravity_w

  fa
end

function get_reaction_points(boundary_ps, boundary_fs)
  inds = find(boundary_fs .>= 1e-3)
  force_points = boundary_ps[inds]

  force_points
end

function plot_solution(applied_p)
  scatter3D(applied_p[1,:]', applied_p[2,:]', [0], color="k", s=800, alpha=0.8)
end

function plot_attachment_points(attachment_ps)
  for p = attachment_ps
    #scatter3D(p[1,:]', p[2,:]', [0], s=400, facecolors="",edgecolors = "k",
    #alpha = 0.8)
    plot_circle(p, radius = 0.2)
  end
end

function plot_new_point(point)
  # nothing to see here
end

function plot_occupied_points(occupied_points)
  for p = occupied_points
    plot_quadrotor(p+[0;0;0.025], color="k", scale=0.15)
  end
end

function plot_measurement_points(measurement_points)
  for p = measurement_points
    plot_quadrotor(p+[0;0;0.025], color="r", scale=0.15)
  end
end

function initialize_prior(boundary_ps, resolution, interior_q, masses)
  boundary_vec = flatten(boundary_ps)

  min_p = minimum(boundary_vec, 2)[1:2]
  max_p = maximum(boundary_vec, 2)[1:2]
  position_ranges = map(x->x[1]:resolution:x[2], zip(min_p, max_p))
  ranges = (position_ranges..., masses)

  prior = Histogram(ranges)

  num_inside = 0
  for kk = 1:length(1:length(get_range(prior, 3)))
    for jj = 1:length(1:length(get_range(prior, 2)))
      for ii = 1:length(1:length(get_range(prior, 1)))
        p = from_indices(prior, [ii,jj,kk])
        if interior_q(p[1:2])::Bool
          num_inside += 1
        end
      end
    end
  end
  p_interior = (1 / (num_inside))

  for kk = 1:length(1:length(get_range(prior, 3)))
    for jj = 1:length(1:length(get_range(prior, 2)))
      for ii = 1:length(1:length(get_range(prior, 1)))
        p = from_indices(prior, [ii,jj,kk])
        value = 0
        if interior_q(p[1:2])::Bool
          value = p_interior
        end
        get_data(prior)[ii,jj,kk] = value
      end
    end
  end

  prior
end

function update_prior!(prior, interior_q, boundary_ws, applied_w, f_hat, sigma, actuator_limit = 1e6)
  critical_fs = get_critical_values(boundary_ws, applied_w, prior, interior_q, actuator_limit)
  update_prior!(prior, critical_fs, f_hat, sigma)
end

shift_dims(field) = reshape(field[1,:,:], (size(field, 2), size(field, 3)))

function update_prior!(prior, critical_fs, f_hat, sigma)
  p_prior = get_data(prior)

  p_f_given_com = map(y->normal(y, sigma^2), f_hat - critical_fs)

  p_f_and_com = p_f_given_com .* p_prior

  p_f = sum(p_f_and_com)

  p_com_given_f = p_f_and_com / p_f

  get_data(prior)[:] = p_com_given_f[:]

  assert(abs(sum(get_data(prior)) - 1.0) < 1e-6)

  prior
end

function get_critical_values(boundary_ws, applied_w, prior, interior_q, actuator_limit = 1e6)
  values = zeros(size(prior))

  for kk = 1:length(get_range(prior, 3))
    for jj = 1:length(get_range(prior, 2))
      for ii = 1:length(get_range(prior, 1))
        p = from_indices(prior, [ii, jj, kk])
        if interior_q(p[1:2])::Bool
          mass = p[3]
          gravity_w = mass*g*point_to_wrench(p)
          applied_f = critical_force_iterative(applied_w, gravity_w, boundary_ws, actuator_limit)
          values[ii,jj,kk] = applied_f
          if isnan(applied_f)
            values[ii,jj,kk] = 0.0
          end
        end
      end
    end
  end

  values
end

function plot_attachment_csqmis(boundary_ps, attachment_ps, csqmis)
  attachment_vec = hcat(attachment_ps...)

  csqmi_scaled = 800 * csqmis / maximum(csqmis)
  scatter3D(attachment_vec[1,:]', attachment_vec[2,:]',
    zeros(size(attachment_vec, 2)), s = csqmi_scaled,
    color="b", alpha = 0.5, edgecolor="k", linewidth=2)
end

function to_cloud(prior, interior_q)
  out = Any[]
  ind = 1
  for kk = 1:length(get_range(prior, 3))
    for jj = 1:length(get_range(prior, 2))
      for ii = 1:length(get_range(prior, 1))
        p = from_indices(prior, [ii, jj, kk])
        if(interior_q(p[1:2]))::Bool
          mass = p[3]
          probability = get_data(prior)[ii, jj, kk]
          push!(out, [p; probability])
        end
      end
    end
  end
  out = hcat(out...)

  out
end

function plot_field(field, title_string="")
  fig, ax = plt[:subplots](1)
  imshow(field' , cmap= "BuPu",
         interpolation="nearest", origin="lower")
  fig[:axes][1][:get_yaxis]()[:set_visible](false)
  fig[:axes][1][:get_xaxis]()[:set_visible](false)
  title(title_string)
  axis("off")
end

include("csqmi.jl")

typealias CombinationMap Dict{Array{Int64,1},Array{Float64,2}}
function generate_over_combinations(action_ws, num_choose, prior,
  boundary_ws, actuator_limit = 1e6, interior_q = x->true)

  total_limit = actuator_limit * num_choose

  d = CombinationMap()
  for c = combinations(1:length(action_ws), num_choose)
    action_w = mean(action_ws[c])
    field = get_critical_values(boundary_ws, action_w, prior, interior_q, total_limit)
    normals = normal_matrix(field[:], sigma^2)
    d[c] = normals
  end
  d
end

function generate_combinations_maps(action_ws, max_choose, prior,
  boundary_ws, actuator_limit = 1e6, interior_q = x->true)
  out = Array{CombinationMap,1}()
  for ii = 1:max_choose
    println("Generating normals for $(ii)")
    @time combination_map = generate_over_combinations(action_ws, ii, prior,
      boundary_ws, actuator_limit, interior_q)
    push!(out, combination_map)
  end
  out
end

function maximize_csqmi_combinations(robot_indices, num_select, prior,
  boundary_ws, action_ws, sigma, interior_q, actuator_limit)

  max_csqmi = 0.0
  best_combination = Array{Float64,1}()
  data = get_data(prior)[:]

  total_limit = actuator_limit * num_select

  for c = combinations(robot_indices, num_select)
    action_w = mean(action_ws[c])

    field = get_critical_values(boundary_ws, action_w, prior, interior_q, total_limit)
    normals = normal_matrix(field[:], sigma^2)

    csqmi = compute_mutual_information(data, normals)

    if csqmi > max_csqmi
      max_csqmi = csqmi
      best_combination = c
    end
  end

  max_csqmi, best_combination
end

function maximize_csqmi_available_robots(robot_indices, prior, boundary_ws,
  action_ws, sigma, interior_q, actuator_limit)

  max_csqmi = 0.0
  best_combination = Array{Float64,1}()

  for ii = 1:length(robot_indices)
    csqmi, c = maximize_csqmi_combinations(robot_indices, ii, prior,
      boundary_ws, action_ws, sigma, interior_q, actuator_limit)

    if csqmi > max_csqmi
      max_csqmi = csqmi
      best_combination = c
    end
  end

  max_csqmi, best_combination
end

function maximize_csqmi_additional_robot(robot_indices, prior, boundary_ws,
  action_ws, sigma, interior_q, actuator_limit)

  max_csqmi = 0.0
  best_combination = Array{Float64,1}()
  additional_robot = 0

  all_robots = collect(1:length(action_ws))

  remaining_robots = setdiff(all_robots, robot_indices)

  # iterate over all combinations of robots including the additional robot
  for remaining_robot = remaining_robots
    for ii = 0:length(robot_indices)
      for c = combinations(robot_indices, ii)
        robots = [remaining_robot; c]
        total_limit = actuator_limit * length(robots)

        action_w = mean(action_ws[robots])

        field = get_critical_values(circle_ws, action_w, prior, interior_q, total_limit)
        normals = normal_matrix(field[:], sigma^2)

        csqmi = compute_mutual_information(get_data(prior)[:], normals)

        if csqmi > max_csqmi
          max_csqmi = csqmi
          best_combination = robots
          additional_robot = remaining_robot
        end
      end
    end
  end

  max_csqmi, best_combination, additional_robot
end

function maximize_csqmi_additional_robot_feasibility(robot_indices, prior, boundary_ws,
  action_ws, sigma, interior_q, actuator_limit, feasibility_constraint)

  max_csqmi = 0.0
  best_combination = Array{Float64,1}()
  additional_robot = 0

  all_robots = collect(1:length(action_ws))

  remaining_robots = setdiff(all_robots, robot_indices)

  # iterate over all combinations of robots including the additional robot
  for remaining_robot = remaining_robots
    robot_indices_p = [robot_indices; remaining_robot]
    robots = action_ws[robot_indices_p]
    doubly_remaining = action_ws[setdiff(all_robots, robot_indices_p)]

    fp = feasibility_probability(belief, robots, doubly_remaining, actuator_limit, max_robots)

    if fp >= feasibility_constraint
      println("adding robot $(remaining_robot) is feasible: $(fp) >= $(feasibility_constraint)")
      for ii = 0:length(robot_indices)
        for c = combinations(robot_indices, ii)
          robots = [remaining_robot; c]
          total_limit = actuator_limit * length(robots)

          action_w = mean(action_ws[robots])

          field = get_critical_values(circle_ws, action_w, prior, interior_q, total_limit)
          normals = normal_matrix(field[:], sigma^2)

          csqmi = compute_mutual_information(get_data(prior)[:], normals)

          if csqmi > max_csqmi
            max_csqmi = csqmi
            best_combination = robots
            additional_robot = remaining_robot
          end
        end
      end
    else
      println("adding robot $(remaining_robot) isn't feasible: $(fp) < $(feasibility_constraint)")
    end
  end

  max_csqmi, best_combination, additional_robot
end

# feasibility
function lifting_feasibility(applied_wrenches, wrench_offset, actuator_limit)
  W = hcat(applied_wrenches...)

  fs = Variable(length(applied_wrenches))

  lifting_condition = W*fs + wrench_offset == 0

  feasibility = [fs >= 0.0; fs <= actuator_limit]

  problem = maximize(0, [lifting_condition; feasibility])

  solver = SCSSolver(verbose = 0)
  solve!(problem, solver)

  ret = problem.status == :Optimal

  ret
end

function check_feasible_configuration(chosen_ws, remaining_ws, offset_w, actuator_limit,
  max_num_robots)

  num_remaining = max_num_robots - length(chosen_ws)

  W_chosen = hcat(chosen_ws...)
  W_remaining = hcat(remaining_ws...)

  fs_chosen = Variable(length(chosen_ws))
  fs_remaining = Variable(length(remaining_ws))

  fs_enabled = Variable(length(remaining_ws), :Bin)

  feasibility = Convex.Constraint[]
  if length(remaining_ws) > 0 && length(chosen_ws) > 0
    #println("remaining and chosen")
    feasibility = [
      W_chosen*fs_chosen + W_remaining*fs_remaining + offset_w == 0.0;
      fs_chosen >= 0.0;
      fs_remaining >= 0.0;
      fs_chosen <= actuator_limit;
      fs_remaining <= fs_enabled*actuator_limit;
      sum(fs_enabled) <= num_remaining
    ]
  elseif length(remaining_ws) > 0
    #println("remaining")
    feasibility = [
      W_remaining*fs_remaining + offset_w == 0.0;
      fs_remaining >= 0.0;
      fs_remaining <= fs_enabled*actuator_limit;
      sum(fs_enabled) <= num_remaining
    ]
  else
    #println("chosen")
    feasibility = [
      W_chosen*fs_chosen + offset_w == 0.0;
      fs_chosen >= 0.0;
      fs_chosen <= actuator_limit;
    ]
  end

  problem = minimize(0.0, feasibility)

  # solver was giving incorrect results with presolve set to false
  solver = GLPKSolverMIP(presolve=false, msg_lev=GLPK.MSG_OFF)

  #TT=STDERR
  #out_read, out_write = redirect_stderr()
  #close(out_write)

  try
    solve!(problem, solver)
  catch ex
    #data = readavailable(out_read)
    #close(out_read)

    #redirect_stderr(TT)

    if isa(ex, InterruptException)
      println("Caught Interrupt")
    else
      println("Caught other exception")
      @show ex
    end
    throw(ex)
  end

  ret = problem.status == :Optimal

  ret
end

function milp()
  x = Variable(4, :Bin)

  problem = minimize(sum(x), x>=0.5)

  solver = GLPKSolverMIP(presolve=true)
  solve!(problem, solver)

  ret = problem.optval

  if problem.status == :Infeasible
    ret = 1000
  end

  ret
end

# probability of feasible configuration
function feasibility_probability(belief, chosen_ws, remaining_ws,
  actuator_limit, max_num_robots)
  g = -9.8
  ranges = get_range(belief)

  data = get_data(belief)

  total = 0.0

  for ii = 1:length(ranges[1])
    for jj = 1:length(ranges[2])
      for kk = 1:length(ranges[3])
        p = from_indices(belief, [ii;jj;kk])
        gravity_w = g * p[3] * point_to_wrench(p[1:2])

        feasible = check_feasible_configuration(chosen_ws, remaining_ws,
          gravity_w, actuator_limit, max_num_robots)

        total += feasible * data[ii,jj,kk]
      end
    end
  end

  total
end

# optimality

# expected configuration value

#end


module QuPhys

using LinearAlgebra
using SparseArrays
using KrylovKit
using IterativeSolvers
using IncompleteLU
using Statistics
using DifferentialEquations

include("quantum_operators.jl")
include("general_functions.jl")
include("time_evolution.jl")

export EnsembleSerial, EnsembleThreads, EnsembleDistributed

export spre, spost, sprepost, lindblad_dissipator
export destroy, eye, fock, projection
export sinm, cosm
export expect
export row_major_reshape, chop_op, gaussian, gaussian_derivative, trunc_op, eigensystem
export ptrace, entropy_vn, entanglement
export mesolve, mcsolve, sesolve

end


#=
Copyright 2017 INSIGNEO Institute for in silico Medicine

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

   http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=#

# Inlet and outlet boundary conditions are applied at the beginning and at the
# ending of each time step respectively
# (see [`solveModel`](godunov.html#solveModel)). Inlet (outlet) condition is
# applied by setting flow (pressure) value at the first (last) vessel node.
# [Inlet](boundary_conditions.html#inletCompatibility) and
# [outlet](boundary_conditions.html#outletCompatibility) compatibility
# relations are defined to compute the remaining quantities:
# `P`, `u`, `A`, `c`, and `Q`, `u`, `A`, `c` respectively.

# *function* __`setInletBC`__
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `t`           `::Float` current time in seconds.
#
# `dt`          `::Float` $\Delta t$, current time step.
#
# `v`           `::Vessel` vessel data structure.
#
# `h`           `::Heart` heart data structure.
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# The flow (pressure) at the first node of the vessel (`Q[1]`) can be set by
# several
# functions. This function is selected by `BC_switch` from `h` data structure.
# The inlet compatibility relations are handled by
# [`inletCompatibility`](boundary_conditions.html#inletCompatibility).
# ----------------------------------------------------------------------------
#
function setInletBC(t :: Float64, dt :: Float64,
                    v :: Vessel,  h  :: Heart)

	if h.BC_switch == 1
		v.Q[1] = sinHeaviside(t, h)

	elseif h.BC_switch == 2
		v.Q[1] = gauss(t, h)

	elseif h.BC_switch == 3

  	if h.inlet_type == "Q"
			v.Q[1] = inputFromData(t, h)
  	else
			v.P[1] = inputFromData(t, h)
  	end

	end

	inletCompatibility(dt, v, h)
end

# `heart_data.BC_switch` = 1 sets a half sine flow waveform at the vessel
# inlet. Flow values at each time step are computed by the following function:

# *function* __`sinHeaviside`__ $\rightarrow$ `flow::Float`
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `t`           `::Float` current time in seconds.
#
# `h`           `::Heart` heart data structure.
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# The flow waveform is always bigger than the initial flow rate assigned at
# the initialisation step, i.e. `initial_flow` ($Q_i$ in the picture). The
# sinusoid is multiplied by the desired maximum flow rate `flow_amplitude`
# ($A_Q$), and by the [`heaviside`](boundary_conditions.html#heaviside) step
# function. This is done in order to ensure only a positive inlet flow. We
# want also the waveform to be different than zero for the entire length of
# the systole, defined by `h.sys_T` variable. This is done by setting the
# `sin` argument as $\frac{2 \pi t}{T_s}$.
# In this way we can define a systolic impulse as narrow as wanted without
# worrying about the cardiac period which is completely unrelated.
# ----------------------------------------------------------------------------
#

#

#
# ----------------------------------------------------------------------------
# Returns:
# --------- ------------------------------------------------------------------
# `Q`       `::Float` flow rate value at current time.
# ----------------------------------------------------------------------------
#
function sinHeaviside(t :: Float64, h :: Heart)

	return h.flow_amplitude*(sin(2*pi*t/h.sys_T)*heaviside(-t+h.sys_T*0.5)) +
          h.initial_flow
end

# *function* __`heaviside`__ $\rightarrow$ `::Float`
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `n`           `::Float`
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# $$
#   H(n) = \begin{cases}
#           0 \quad \text{if } n < 0, \\
#           1 \quad \text{if } n \geq 0.
#          \end{cases}
# $$
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Returns:
# ------------- --------------------------------------------------------------
# `H`           `::Float` either 1 or 0.
# ----------------------------------------------------------------------------
#
function heaviside(n :: Float64)

	if n < 0.

		return 0.

	else

		return 1.

	end

end

# `heart_data.BC_switch` = 2 sets a gaussian shaperd flow waveform at the
# vessel inlet. The bell shape has smoother transition at $T_s$ than
# [`sinHeaviside`](boundary_conditions.html#sinHeaviside). As a result, the
# numerical scheme produces less oscillations due to that discontinuity.
# Flow values at each time step are computed by the following function:

# *function* __`gauss`__ $\rightarrow$ `flow::Float`
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `t`           `::Float` current time in seconds.
#
# `h`           `::Heart` heart data structure.
# ----------------------------------------------------------------------------
#
function gauss(t :: Float64, h :: Heart)

	# --------------------------------------------------------------------------
	# Functioning
	# --------------------------------------------------------------------------
	# `t_hat` variable tells us how many cardiac cycles have already been
	# simulated. This is calculated with `div(a, b)` which returns `a/b`,
	# truncating to an integer.
	# --------------------------------------------------------------------------
	t_hat = div(t,h.cardiac_T)
	# The gaussian bell should span the entire systolic period `h.sys_T` length.
	# For `gauss` to return a value different than zero, the current time `t`
	# must be within the current systolic period `h.sys_T` ($T_s$).
	if t < h.sys_T + h.cardiac_T*t_hat && t >= h.cardiac_T*t_hat
		# When this is confirmed, the inlet flow rate reads
		# $$
		#   Q = A_Q \exp{\left(- \frac{\left(t - \hat{t} T -
		#   \tfrac{T_s}{2}\right)^2}{2 \left(\tfrac{T_s}{8}\right)^2 }\right)}
		#   + Q_i.
		# $$
		# Variables are the same as those used in
		# [`sinHeaviside`](boundary_conditions.html#sinHeaviside).
		return h.flow_amplitude * exp( - (t-t_hat*h.cardiac_T -h.sys_T*0.5)^2 /
            (2*(h.sys_T*0.125)^2)) + initial_flow
	# Otherwise only the `initial_flow` is returned.
	#
	# --------------------------------------------------------------------------
	# Returns:
	# ---------- ---------------------------------------------------------------
	# `Q`        `::Float` inlet flow rate at current time.
	# --------------------------------------------------------------------------
	else
		return 0. + initial_flow

	end
end

# `heart_data.BC_switch` = 3 requires a flow waveform to be supplied by the
# user. The flow waveform is read by the following function:

# *function* __`inputFromData`__ $\rightarrow$ `flow::Float`
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `t`           `::Float` current time in seconds.
#
# `h`           `::Heart` heart data structure.
# ----------------------------------------------------------------------------
#
function inputFromData(t :: Float64, h :: Heart)

	# --------------------------------------------------------------------------
	# Functioning
	# --------------------------------------------------------------------------
	# Inlet flow waveform is stored in `h` data structure inside `input_data`
	# matrix. `input_data` has two columns and as many rows as time steps. The
	# first column (`input_data[:,1]`) contains the time variable. The second
	# column (`input_data[:,2]`) contains the flow variable. The two arrays are
	# copied into two variables, `idt` and `idq` respectively.
	# --------------------------------------------------------------------------
	idt = h.input_data[:,1]
	idq = h.input_data[:,2]
	# `t_hat` variable tells us how many cardiac cycles have already been
	# simulated. This is calculated with `div(a, b)` which returns `a/b`,
	# truncating to an integer.
	t_hat = div(t,h.cardiac_T)
	# The current time is then updated by subtracting as many cardiac cycles as
	# those already passed. This is done in order to have a reference within the
	# cardiac cycle, i.e. in order to know at what point (*when*) in the cardiac
	# cycle the simulation is running.
	t -= t_hat*h.cardiac_T
	# The waveform is given in a discrete form and usually with a small amount
	# of points along the cardiac cycle. Usually, the numerical $\Delta t$ is
	# small enough to fall between two time steps reported in `idt`. This
	# loop skims all the entries in `idt` and finds the time interval containing
	# the current time `t`. The variable `idx` is initialised to zero for
	# simplicity.
	#
	#

	#

	idx = 0
	for i in 1:length(idt)
		if ((t >= idt[i]) && (t <= idt[i+1]))
			idx=i
			break
		end
	end
	# The flow `qu` is then computed at the current time by a linear
	# interpolation between the two known flows (`idq[idx]` and `idq[idx+1]`)
	# at the ends of the time interval.
	qu = idq[idx] + (t - idt[idx]) * (idq[idx+1] - idq[idx]) /
       (idt[idx+1] - idt[idx])
	# --------------------------------------------------------------------------
	# Returns:
	# ---------- ---------------------------------------------------------------
	# `qu`       `::Float` inlet flow rate at current time.
	# --------------------------------------------------------------------------
	return qu
end

# In the case of veins, the inlet is always coupled with three-element Windkessel.
# The inlet function is re-defined as follows

# *function* __`setInletBC`__
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `v`           `::Vessel` vein data structure.
#
# `dt`          `::Float` $\Delta t$, current time step.
#
# `Pc`           `::Float` pressure across the peripheral compliance of the
#                three-element Windkessel.
#
# `R2`           `::Float` peripeheral resistance of the coupled three-element
#                Windkessel.
#
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# The flow at the first node of the vein (`Q[1]`) is set as the flow exiting
# the three-element Windkessel computed as
# $$
#   Q_{vein-in} = \frac{P_c}{R_2},
# $$
# and a small factor of `1e-10` is added to avoid numerical instabilities.
# Then, the inlet compatibility relations are computed.
# ----------------------------------------------------------------------------
function setInletBC(v  :: Vessel,  dt :: Float64,
                    Pc :: Float64, R2 :: Float64)
	v.Q[1] = Pc/R2 + 1e-10

	inletCompatibility(dt, v)
end

# The inlet compatibility relations are handled by
# [`inletCompatibility`](boundary_conditions.html#inletCompatibility).

# *function* __`inletCompatibility`__
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `dt`          `::Float` $\Delta t$, current time step.
#
# `v`           `::Vessel` vessel data structure.
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# Compatibility relations are derived by using a technique called
# *extrapolation of characteristics* [^1].
# ----------------------------------------------------------------------------
#
function inletCompatibility(dt :: Float64, v :: Vessel, h :: Heart)

	# Riemann invariants are [computed](converter.html#riemannInvariants)
	# starting from variables at time `t`.
	# $$
	#   W_{11}^t = u - 4c, \quad W_{21}^t = u + 4c,
	# $$
	# are computed at the inlet node of the vessel.
	# `W12` and `W22` are the same quantities computed at the second node, i.e.
	# the closest node at the right hand side of the inlet node.
	W11, W21 = riemannInvariants(1, v)
	W12, W22 = riemannInvariants(2, v)

	# The two Riemann invariants within the two nodes are extrapolated with a
	# linear law. They reads
	# $$
	#   W_{11}^{t+1} = W_{11}^t + \left(W_{12}^t - W_{11}^t \right) (c_1 - u_1)
	#    \frac{\Delta t}{\Delta x},
	# $$
	# $$
	#   W_{21}^{t+1} = W_{21}^t - W_{11}^{t+1} + 2 \frac{Q_1}{A_1},
	# $$
	# where the 1 subscript indicates the inlet node.
	W11 += (W12-W11)*(v.c[1] - v.u[1])*dt/v.dx
	W21 = 2*v.Q[1]/v.A[1] - W11
	# Longitudinal velocity and wave velocity at the inlet node are retrieved
	# by [`rI2uc`](converter.html#rI2uc) (*Riemann invariants to u and c*)
	# function.
	v.u[1], v.c[1] = rI2uc(W11, W21)
	# `A` and `P` quantities follow from their definitions.

	if h.inlet_type == "Q"
		v.A[1] = v.Q[1]/v.u[1]
		v.P[1] = pressure(v.A[1], v.A0[1], v.beta[1], v.Pext)
	else
		v.A[1] = areaFromPressure(v.P[1], v.A0[1], v.beta[1], v.Pext)
		v.Q[1] = v.u[1]*v.A[1]
	end

end

function inletCompatibility(dt :: Float64, v :: Vessel)

	W11, W21 = riemannInvariants(1, v)
	W12, W22 = riemannInvariants(2, v)

	W11 += (W12-W11)*(v.c[1] - v.u[1])*dt/v.dx
	W21 = 2*v.Q[1]/v.A[1] - W11

	v.u[1], v.c[1] = rI2uc(W11, W21)

	v.A[1] = v.Q[1]/v.u[1]
	v.P[1] = pressure(v.A[1], v.A0[1], v.beta[1], v.Pext)

end

# Outlet boundary condition is applied to the last node of the involved
# vessel. Two boundary conditions are available. Either a reflection
# coefficient is provided or a three elements windkessel is coupled.

# *function* __`setOutletBC`__
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `dt`          `::Float` $\Delta t$, current time step.
#
# `v`           `::Vessel` vessel data structure.
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# Outlet boundary conditions are selected in the `project.csv` file.
# ----------------------------------------------------------------------------
#
function setOutletBC(dt :: Float64, v :: Vessel)

	# A proximal resistance `R1` set to zero means that a reflection coefficient
	# was specified. Thus, the pressure `P` is first linearly extrapolated [^2]
	# as
	# $$
	#   P_M = 2 P_{M-1} - P_{M-2},
	# $$
	# where $M$ is the index of the vessel outlet node. Then, the compatibility
	# relations are applied by means of
	# [`outletCompatibility`](boundary_conditions.html#outletCompatibility)
	# function.
	if v.R1 == 0.

		v.P[end] = 2*v.P[end-1] - v.P[end-2]
		outletCompatibility(dt, v)
	# When all the parameters of the three element windkessel are specified,
	# [`wk3`](boundary_conditions.html#wk3) function is called. This function
	# solves lumped parameter equations and it also applies compatibility
	# relations.
	else
		wk3(dt, v)

	end

end

# Outlet compatibility relations compute all the quantities not directly
# assigned by the outlet boundary condition.

# *function* __`outletCompatibility`__
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `dt`          `::Float` $\Delta t$, current time step.
#
# `v`           `::Vessel` vessel data structure.
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# Compatibility relations are derived by using a technique called
# *extrapolation of characteristics* as for the
# [inlet](boundary_conditions.html#inletCompatibility).
# ----------------------------------------------------------------------------
#
function outletCompatibility(dt :: Float64, v :: Vessel)

	W1M1, W2M1 = riemannInvariants(v.M-1, v)
	W1M, W2M   = riemannInvariants(v.M, v)

	# $$
	#   W_{2M}^{t+1} = W_2^t + \frac{W_{2 M-1}^t - W_{2M}^t}{\Delta x} (u_M^t
	#                   + c_M^t) \Delta t,
	# $$
	# $$
	#   W_{1M}^{t+1} = W_{1M}^0 - R_t \left(W_{2L}^{t+1} - W_{2L}^0 \right).
	# $$
	W2M += (W2M1 - W2M)*(v.u[end] + v.c[end])*dt/v.dx
	W1M = v.W1M0 - v.Rt * (W2M - v.W2M0)
	# Here the remaining quatities are computed by means of their definitions.
	v.u[end], v.c[end] = rI2uc(W1M, W2M)
	v.Q[end] = v.A[end]*v.u[end]

end

# The three element windkessel simulates the perfusion of downstream vessels.
# This 0D model is coupled by [`wk3`](boundary_conditions.html#wk3)
# function to 1D model terminal branches via the solution of a Riemann
# problem at the 0D/1D interface.
#
#

#

#
# $R_1$ is the proximal resistance, $R_2$ is the peripeheral reisistance,
# $C_c$ is the peripheral compliance, $P_c$ is the pressure across the
# peripheral compliance, $P_out$ is the pressure at the artery-vein interface,
# $P_e$ is the pressure at the 0D/1D interface.

# *function* __`wk3`__
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `dt`          `::Float` $\Delta t$, current time step.
#
# `v`           `::Vessel` vessel data structure.
# ----------------------------------------------------------------------------
#
function wk3(dt :: Float64, v :: Vessel)

	# --------------------------------------------------------------------------
	# Functioning [^3]
	# --------------------------------------------------------------------------
	# At capillary level the pressure is assumed to be zero, i.e. $P_{out}=0.$
	# --------------------------------------------------------------------------
	Pout = 0.
	# The coupling is performed by assuming that an intermediate state
	# $(A^*,u^*)$ generates from $(A_l,u_l)$ (1D outlet) and $(A_r,u_r)$ (0D
	# inlet). This intermediate state must satisfy the windkessel equation
	# $$
	#   A^*u^* \left(1 + \frac{R_1}{R_2}\right) + C_c R_1 \frac{\partial
	#   (A^*u^*)}{\partial t} = \frac{P_e - P_{out}}{R_2} + C_c \frac{\partial
	#   P_e}{\partial t}.
	# $$
	Al = v.A[end]
	ul = v.u[end]
	# $P_c$ is computed at each time step from
	# $$
	#   C_c \frac{\partial P_c}{\partial t} = A^*u^* - \frac{P_c -
	#   P_{out}}{R_2},
	# $$
	# which is discretised numerically with a first-order scheme. $P_c$ is
	# is [initialised](initialise.html#initialiseVessel) to zero.
	v.Pc += dt/v.Cc * (Al*ul - (v.Pc-Pout)/v.R2)
	# @printf "%8.3e\n" v.Pc/v.R2*1e6
	# We consider $\beta$ and $A_0$ to be the same on both sides of the 0D/1D
	# interface. It yields the nonlinear equation
	# $$
	#   \mathcal{F}(A^*) = A^*R_1\left(u_l+4c_l\right)
	#   -4A^*R_1c^* - \frac{\beta}{A_0}\left(
	#   \sqrt{A^*}-\sqrt{A_0}\right) + P_c = 0,
	# $$
	# where $c_l$ and $c^*$ are the wave speeds calculated with $A_l$ and $A^*$,
	# respectively.
	As = Al

	# fun(As) = v.R1*(ul+4*sqrt(3*v.gamma[end]*sqrt(Al)*0.5))*As -
	# 	4*v.R1*sqrt(3*v.gamma[end]*sqrt(As)*0.5)*As -
	# 	(v.Pext + v.beta[end]*(sqrt(As/v.A0[end]) - 1)) + v.Pc

	# dfun(As) = v.R1*( ul +4*sqrt(1.5*v.gamma[end])*(sqrt(sqrt(Al)) - 1.25*sqrt(sqrt(As))) ) -
	# 	v.beta[end]*0.5/(sqrt(As*v.A0[end]))

	ssAl = sqrt(sqrt(Al))
	sgamma =  2*sqrt(6*v.gamma[end])
	sA0 = sqrt(v.A0[end])
	bA0 = v.beta[end]/sA0

	fun(As) = As*v.R1*(ul + sgamma * (ssAl - sqrt(sqrt(As)))) -
		(v.Pext + bA0*(sqrt(As) - sA0)) + v.Pc

	dfun(As) = v.R1*(ul + sgamma * (ssAl - 1.25*sqrt(sqrt(As)))) - bA0*0.5/sqrt(As)

	# $\mathcal{F}(A^*)=0$ is solved for $A^*$ with the Newton's method
	# implemented in [__Roots__](https://github.com/JuliaLang/Roots.jl) library.
	# $A^*$ is initialised equal to $A_l$.
	# As = newton(fun, dfun, As)
	As = newtone(fun, dfun, As)
	# Once $A^*$ is found, $u^*$ reads
	# $$
	#   u^* = \frac{P_e^* - P{out}}{A^*R_1},
	# $$
	# where $P_e^*$ is $P_e$ calculated with $A^*$.
	us = (pressure(As, v.A0[end], v.beta[end], v.Pext) - Pout)/(As*v.R1)

	v.A[end] = As
	v.u[end] = us

end

function newtone(f, df, x0)

	xn = x0 - f(x0)/df(x0)
	if abs(xn-x0)<= 1e-5
		return xn
	else
		newtone(f, df, xn)
	end
end

# *function* __`updateGhostCells`__
#
# ----------------------------------------------------------------------------
# Parameters:
# ------------- --------------------------------------------------------------
# `v`           `::Vessel` vessel data structure.
# ----------------------------------------------------------------------------
#
# ----------------------------------------------------------------------------
# Functioning
# ----------------------------------------------------------------------------
# Ghost cells are updated by assuming both ends as transmissive boundaries.
# ----------------------------------------------------------------------------
#
function updateGhostCells(v :: Vessel)

	v.U00A = v.A[1]
	v.U00Q = v.Q[1]
	v.U01A = v.A[2]
	v.U01Q = v.Q[2]

	v.UM1A = v.A[v.M]
	v.UM1Q = v.Q[v.M]
	v.UM2A = v.A[v.M-1]
	v.UM2Q = v.Q[v.M-1]

end

# `updateGhostCells` exploits julia's multiple dispatch property. This
# function can handle one or more than one vessels organised in an `Array`
# structure.
function updateGhostCells(vessels :: Array{Vessel, 1})

	for vessel in vessels
		updateGhostCells(vessel)
	end
#   if length(vessels) == 1
#     updateGhostCells(vessels[1])
#   else
#     pmap(updateGhostCells, vessels)
#   end
end

# ### References
#
# [^1]: Peiro, Joaquim, and Alessandro Veneziani.
# [*"Reduced models of the cardiovascular system
# ."*](http://link.springer.com/chapter/10.1007/978-88-470-1152-6_10#page-1)
# In Cardiovascular mathematics, pp. 347-394. Springer Milan, 2009.
#
# [^2]: Anderson, John David, and J. Wendt. *Computational fluid dynamics*.
# Vol. 206. New York: McGraw-Hill, 1995.
#
# [^3]: Fernandez, Miguel Angel, Vuk Milisic, and Alfio Quarteroni.
# [*"Analysis of a geometrical multiscale blood flow model based on
# the coupling of ODEs and hyperbolic
# PDEs."*](http://epubs.siam.org/doi/abs/10.1137/030602010)
# Multiscale Modeling & Simulation 4.1 (2005): 215-236.


### A Pluto.jl notebook ###
# v0.12.16

using Markdown
using InteractiveUtils

# ╔═╡ 2d73bd62-3739-11eb-35c0-09836bdd3d24
file = open(f->read(f, String), "day_2.txt")

# ╔═╡ 431ed85e-3739-11eb-0f7d-bd408abb654f
begin
	#separing each line
	input = eachmatch(r"(\d+)-(\d+) (\w): (\w+)\n",file)

	#getting first match of regex and converting to int
	input = map(x-> x.captures, input)
end

# ╔═╡ db441a22-3739-11eb-3657-319f8fbfbbe2
[x[1] for x in input]

# ╔═╡ ebaf6628-3739-11eb-3ec3-3120c2937e13
function valid_line(min ,max ,password, letter::AbstractString)
	min = parse(Int64, min)
	max = parse(Int64, max)

	matchs = collect( eachmatch( Regex(letter) , password) )
	return size(matchs)[1] ≥ min && size(matchs)[1] ≤ max
end

# ╔═╡ a9690290-373b-11eb-0312-671f6f134c36
resp1 = sum([valid_line(x[1],x[2],x[4],x[3]) for x in input])

# ╔═╡ ba4b5356-373b-11eb-2dc9-65a12a13c5d2
md"awnser for problem1 $(resp1)"

# ╔═╡ 65c76852-373c-11eb-3b42-8bdfacf8c6b3
md"# Problem 2"

# ╔═╡ 769c461e-373c-11eb-050a-1d496a98c2ab
function valid_line2(pos1 ,pos2 ,password, letter::AbstractString)
	pos1 = parse(Int64, pos1)
	pos2 = parse(Int64, pos2)

	matchs =  collect(eachmatch( Regex(letter) , password))
	if size(matchs)[1] == 0
		return false
	end
	apperance= map(x -> x.offset, matchs)
	return xor((pos1 in apperance) ,(pos2 in apperance))
end

# ╔═╡ b71c9022-373c-11eb-1168-599806156629
resp2 = sum([valid_line2(x[1],x[2],x[4],x[3]) for x in input])

# ╔═╡ Cell order:
# ╠═2d73bd62-3739-11eb-35c0-09836bdd3d24
# ╠═431ed85e-3739-11eb-0f7d-bd408abb654f
# ╠═db441a22-3739-11eb-3657-319f8fbfbbe2
# ╠═ebaf6628-3739-11eb-3ec3-3120c2937e13
# ╠═a9690290-373b-11eb-0312-671f6f134c36
# ╟─ba4b5356-373b-11eb-2dc9-65a12a13c5d2
# ╠═65c76852-373c-11eb-3b42-8bdfacf8c6b3
# ╠═769c461e-373c-11eb-050a-1d496a98c2ab
# ╠═b71c9022-373c-11eb-1168-599806156629