mutable struct BezierPathSegment <: AbstractArray{Luxor.Point, 1}
    p1::Point
    cp1::Point
    cp2::Point
    p2::Point
end

Base.size(bps::BezierPathSegment) = (4, )
Base.length(bps::BezierPathSegment) = 4
Base.eltype(::Type{BezierPathSegment}) = Luxor.Point
Base.getindex(bps::BezierPathSegment, i::T) where T <: Integer = [bps.p1, bps.cp1, bps.cp2, bps.p2][i]
Base.setindex!(bp::BezierPathSegment, v, i::T) where T <: Integer = [bps.p1, bps.cp1, bps.cp2, bps.p2][i] = v
Base.IndexStyle(::Type{<:BezierPathSegment}) = IndexLinear()

# state is integer referring to one of the four points
function Base.iterate(bps::BezierPathSegment)
    return (bps.p1, 2)
end

function Base.iterate(bps::BezierPathSegment, s)
    if (s > length(bps))
        return
    end
    return bps[s], s+1
end

# function Base.show(io::IO, bps::BezierPathSegment)
#     println(io, "p1  $(bps.p1)  cp1 $(bps.cp1)")
#     println(io, "cp2  $(bps.cp2)  p2 $(bps.p2)")
# end

mutable struct BezierPath <: AbstractArray{BezierPathSegment, 1}
    segments::Vector{BezierPathSegment}
end

BezierPath(bps::BezierPathSegment) = BezierPath([bps])
BezierPath() = BezierPath([])

Base.size(bp::BezierPath) = size(bp.segments)
Base.length(bp::BezierPath) = length(bp.segments)
Base.eltype(::Type{BezierPath}) = BezierPathSegment
Base.getindex(bp::BezierPath, i::T) where T <: Integer = bp.segments[i]
Base.setindex!(bp::BezierPath, v, i::T) where T <: Integer = bp.segments[i] = v
Base.IndexStyle(::Type{<:BezierPath}) = IndexLinear()

Base.push!(bp::BezierPath, v::BezierPathSegment) = Base.push!(bp.segments, v)

# Base.show(io::IO, bp::BezierPath) =
#     print(io, "BezierPath with $(length(bp.segments)) segments")

"""
    bezier(t, A::Point, A1::Point, B1::Point, B::Point)

Return the result of evaluating the Bezier cubic curve function, `t` going from
0 to 1, starting at A, finishing at B, control points A1 (controlling A), and B1
(controlling B).
"""
bezier(t, A::Point, A1::Point, B1::Point, B::Point) =
      A * (1.0 - t)^3 +
      (A1 * 3t * (1.0 - t)^2) +
      (B1 * 3t^2 * (1.0 - t)) +
      (B * t^3)

"""
  bezier′(t, A::Point, A1::Point, B1::Point, B::Point)

Return the first derivative of the Bezier function.
"""
bezier′(t, A, A1, B1, B) = 3(1.0-t)^2 * (A1-A) + 6(1.0-t) * t * (B1-A1) + 3t^2 * (B-B1)

"""
    bezier′′(t, A::Point, A1::Point, B1::Point, B::Point)

Return the second derivative of Bezier function.
"""
bezier′′(t, A, A1, B1, B) = 6(1-t) * (B1-2A1+A) + 6t * (B-2B1+A1)

"""
    beziercurvature(t, A::Point, A1::Point, B1::Point, B::Point)

Return the curvature of the Bezier curve at `t` ([0-1]), given start and end
points A and B, and control points A1 and B1. The value (kappa) will typically
be a value between -0.001 and 0.001 for points with coordinates in the 100-500
range.

κ(t) is the curvature of the curve at point t, which for a parametric planar
curve is:

```math
\\begin{equation}
\\kappa = \\frac{\\mid \\dot{x}\\ddot{y}-\\dot{y}\\ddot{x}\\mid}
    {(\\dot{x}^2 + \\dot{y}^2)^{\\frac{3}{2}}}
\end{equation}
```

The radius of curvature, or the radius of an osculating circle at a point, is
1/κ(t). Values of 1/κ will typically be in the range -1000 to 1000 for points
with coordinates in the 100-500 range.

TODO Fix overshoot...

...The value of kappa can sometimes collapse near 0, returning NaN (and Inf for
radius of curvature).
"""
function beziercurvature(t, A::Point, A1::Point, B1::Point, B::Point)
    x′ = bezier′(t, A, A1, B1, B).x
    y′ = bezier′(t, A, A1, B1, B).y
    x′′ = bezier′′(t, A, A1, B1, B).x
    y′′ = bezier′′(t, A, A1, B1, B).y
    return ((x′ * y′′) - (y′ * x′′)) / (x′^2 + y′^2)^(3/2)
end

"""
    findbeziercontrolpoints(previouspt::Point,
        pt1::Point,
        pt2::Point,
        nextpt::Point;
        smooth_value=0.5)

Find the Bézier control points for the line between `pt1` and `pt2`, where the
point before `pt1` is `previouspt` and the next point after `pt2` is `nextpt`.
"""
function findbeziercontrolpoints(previouspt::Point, point1::Point, point2::Point, nextpt::Point;
    smoothing=0.5)

    xc1 = (previouspt.x + point1.x)/2.0 ; yc1 = (previouspt.y + point1.y)/2.0
    xc2 = (point1.x + point2.x)/2.0     ; yc2 = (point1.y + point2.y)/2.0
    xc3 = (point2.x + nextpt.x)/2.0     ; yc3 = (point2.y + nextpt.y)/2.0

    l1, l2, l3 = distance(previouspt, point1), distance(point2, point1), distance(nextpt, point2)
    k1 = l1 / (l1 + l2)   ; k2 = l2 / (l2 + l3)
    xm1 = xc1 + (xc2-xc1) * k1  ; ym1 = yc1 + (yc2-yc1) * k1
    xm2 = xc2 + (xc3-xc2) * k2  ; ym2 = yc2 + (yc3-yc2) * k2
    c1x = xm1 + (xc2-xm1) * smoothing + point1.x-xm1
    c1y = ym1 + (yc2-ym1) * smoothing + point1.y-ym1
    c2x = xm2 + (xc2-xm2) * smoothing + point2.x-xm2
    c2y = ym2 + (yc2-ym2) * smoothing + point2.y-ym2
    return (Point(c1x, c1y),  Point(c2x, c2y))
end

"""
    makebezierpath(pgon::Array{Point, 1}; smoothing=1.0)

Return a Bézier path (a BezierPath) that represents a polygon (an array of points). The Bézier
path is an array of segments (tuples of 4 points); each segment contains the
four points that make up a section of the entire Bézier path. `smoothing`
determines how closely the curve follows the polygon. A value of 0 returns a
straight-sided path; as values move above 1 the paths deviate further from
the original polygon's edges.
"""
function makebezierpath(pgon::Array{Point, 1}; smoothing=1.0)
    lpg = length(pgon)
    newpath = BezierPath()
    for i in 1:lpg
        cp1, cp2 = findbeziercontrolpoints(
            pgon[mod1(i - 1, lpg)],
            pgon[mod1(i,     lpg)],
            pgon[mod1(i + 1, lpg)],
            pgon[mod1(i + 2, lpg)], smoothing=smoothing)
        push!(newpath, BezierPathSegment(pgon[mod1(i, lpg)], cp1, cp2, pgon[mod1(i + 1, lpg)]))
    end
    return newpath
end

"""
    drawbezierpath(bezierpath::BezierPath, action=:none;
        close=true)

Draw the Bézier path, and apply the action, such as `:none`, `:stroke`, `:fill`,
etc. By default the path is closed.
"""
function drawbezierpath(bezierpath::BezierPath, action=:none; close=true)
    move(bezierpath[1].p1)
    for i in 1:length(bezierpath) - 1
        c = bezierpath[i]
        curve(c.cp1, c.cp2, c.p2)
    end
    if close == true
        c = bezierpath[end]
        curve(c.cp1, c.cp2, c.p2)
    end
    do_action(action)
end

"""
    drawbezierpath(bps::BezierPathSegment, action=:none;
        close=false)

Draw the Bézier path segment, and apply the action, such as `:none`, `:stroke`,
`:fill`, etc. By default the path is open.
"""
function drawbezierpath(bps::BezierPathSegment,
    action=:none; close=false)
    move(bps.p1)
    curve(bps.cp1, bps.cp2, bps.p2)
    if close == true
        closepath()
    end
    do_action(action)
end

drawbezierpath(bpsa::Array{BezierPathSegment, 1}, action=:none) =
   foreach(b -> drawbezierpath(b, action), bpsa)

"""
    beziertopoly(bpseg::BezierPathSegment; steps=10)

Convert a Bezier segment to a polygon (an array of points).
"""
function beziertopoly(bpseg::BezierPathSegment; steps=10)
    thepolygon = Point[]
    anchor1, control1, control2, anchor2 = bpseg.p1, bpseg.cp1, bpseg.cp2, bpseg.p2
    dx1 = control1.x - anchor1.x
    dy1 = control1.y - anchor1.y
    dx2 = control2.x - control1.x
    dy2 = control2.y - control1.y
    dx3 = anchor2.x - control2.x
    dy3 = anchor2.y - control2.y

    ss1 = 1.0/(steps + 1)
    ss2 = ss1 * ss1
    ss3 = ss2 * ss1

    p1 = 3.0 * ss1; p2 = 3.0 * ss2; p4 = 6.0 * ss2; p5 = 6.0 * ss3

    t1x = anchor1.x - control1.x * 2.0 + control2.x
    t1y = anchor1.y - control1.y * 2.0 + control2.y

    t2x = (control1.x - control2.x) * 3.0 - anchor1.x + anchor2.x
    t2y = (control1.y - control2.y) * 3.0 - anchor1.y + anchor2.y

    fx = anchor1.x ; fy = anchor1.y

    dfx = (control1.x - anchor1.x) * p1 + t1x * p2 + t2x * ss3
    dfy = (control1.y - anchor1.y) * p1 + t1y * p2 + t2y * ss3

    d2fx = t1x * p4 + t2x * p5 ; d2fy = t1y * p4 + t2y * p5
    d3fx = t2x * p5            ; d3fy = t2y * p5

    n = steps
    push!(thepolygon, Point(anchor1.x, anchor1.y))
    while n > 0
        fx   += dfx
        fy   += dfy
        dfx  += d2fx
        dfy  += d2fy
        d2fx += d3fx
        d2fy += d3fy
        push!(thepolygon, Point(fx, fy))
        n -= 1
    end
    # should we add the last point? no, do that downstream if required:
    # push!(thepolygon, Point(x4, y4))
    return thepolygon
end

"""
    bezierpathtopoly(bezierpath::BezierPath; steps=10)

Convert a Bezier path (an array of Bezier segments, where each segment is a
tuple of four points: anchor1, control1, control2, anchor2) to a polygon.

To make a Bezier path, use `makebezierpath()` on a polygon.

The `steps` optional keyword determines how many line sections are used for each
path.
"""
function bezierpathtopoly(bezierpath::BezierPath; steps=10)
    resultpoly = Point[]
    for bp in bezierpath
        p = beziertopoly(bp, steps=steps)
        append!(resultpoly, p)
    end
    if resultpoly[1] == resultpoly[end]
        pop!(resultpoly)
    end
    return resultpoly
end

"""
    pathtobezierpaths(
        ; flat=true)

Convert the current path (which may consist of one or more paths) to an array of
Bezier paths. Each Bezier path is, in turn, an array of path segments. Each path
segment is a tuple of four points. A straight line is converted to a Bezier segment
in which the control points are set to be the same as the end points.

If `flat` is true, use `getpathflat()` rather than `getpath()`.

# Example

This code draws the Bezier segments and shows the control points as "handles", like
a vector-editing program might.

```
@svg begin
    fontface("MyanmarMN-Bold")
    st = "goo"
    thefontsize = 100
    fontsize(thefontsize)
    sethue("red")
    fontsize(thefontsize)
    textpath(st)
    nbps = pathtobezierpaths()
    for nbp in nbps
        setline(.15)
        sethue("grey50")
        drawbezierpath(nbp, :stroke)
        for p in nbp
            sethue("red")
            circle(p[2], 0.16, :fill)
            circle(p[3], 0.16, :fill)
            line(p[2], p[1], :stroke)
            line(p[3], p[4], :stroke)
            if p[1] != p[4]
                sethue("black")
                circle(p[1], 0.26, :fill)
                circle(p[4], 0.26, :fill)
            end
        end
    end
end
```

"""
function pathtobezierpaths(; flat=true)
    flat ? (originalpath = getpath()) : (originalpath = getpathflat())
    # to store all the Bezier paths
    result = BezierPath[]
    # to store one Bezier path
    newbezpath = BezierPath()
    # to store a Bezier segment
    currentpos = Point(0, 0)
    if length(originalpath) > 0
        for e in originalpath
            if e.element_type == Cairo.CAIRO_PATH_MOVE_TO                # 0
                currentpos = Point(e.points[1], e.points[2])
            elseif e.element_type == Cairo.CAIRO_PATH_LINE_TO            # 1
                # add a straight line segment in the form of a bezsegment
                bezsegment = BezierPathSegment(currentpos,
                              currentpos,
                              Point(e.points[1], e.points[2]),
                              Point(e.points[1], e.points[2]))
                push!(newbezpath, bezsegment)
                currentpos = Point(e.points[1], e.points[2])
            elseif e.element_type == Cairo.CAIRO_PATH_CURVE_TO           # 2
                bezsegment = BezierPathSegment(currentpos,
                              Point(e.points[1], e.points[2]),
                              Point(e.points[3], e.points[4]),
                              Point(e.points[5], e.points[6]))
                currentpos = Point(e.points[5], e.points[6])
                push!(newbezpath, bezsegment)
            elseif e.element_type == Cairo.CAIRO_PATH_CLOSE_PATH         # 3
                # finish this path and store it away
                if length(newbezpath) > 0
                    push!(result, newbezpath)
                end
                # make new path
                newbezpath = BezierPath()
            else
                error("unknown CairoPathEntry " * repr(e.element_type))
                error("unknown CairoPathEntry " * repr(e.points))
            end
        end
        # tidy up, not everything gets closed
        if length(newbezpath) > 0
            push!(result, newbezpath)
        end
    end
    return result
end

# More Bezier madness

function _solvexy(a, b, c, d, e, f)
# solve linear equation ai + bj = c and di + ej = f
    j = (c - a / d * f) / (b - a * e / d)
    i = (c - (b * j)) / a
    return i, j
end

# basis functions
_b0(t) = (1 - t) ^ 3
_b1(t) = t * (1 - t) * (1 - t) * 3
_b2(t) = (1 - t) * t * t * 3
_b3(t) = (t ^ 3)

"""
    bezierfrompoints(startpoint::Point, pointonline1::Point,
        pointonline2::Point, endpoint::Point)

Given four points, return the Bezier curve that passes through all four points,
starting at `startpoint` and ending at `endpoint`. The two middle points of the
returned BezierPathSegment are the two control points that make the curve pass
through the two middle points supplied.
"""
function bezierfrompoints(startpoint::Point, pointonline1::Point, pointonline2::Point, endpoint::Point)
    # chord lengths
    c1 = sqrt((pointonline1.x - startpoint.x) * (pointonline1.x - startpoint.x) + (pointonline1.y - startpoint.y) * (pointonline1.y - startpoint.y))
    c2 = sqrt((pointonline2.x - pointonline1.x) * (pointonline2.x - pointonline1.x) + (pointonline2.y - pointonline1.y) * (pointonline2.y - pointonline1.y))
    c3 = sqrt((endpoint.x - pointonline2.x) * (endpoint.x - pointonline2.x) + (endpoint.y - pointonline2.y) * (endpoint.y - pointonline2.y))
    # best guess for t
    t1 = c1 / (c1 + c2 + c3)
    t2 = (c1 + c2) / (c1 + c2 + c3)
    (x1, x2) = _solvexy(_b1(t1), _b2(t1), pointonline1.x - (startpoint.x * _b0(t1)) - (endpoint.x * _b3(t1)), _b1(t2), _b2(t2), pointonline2.x - (startpoint.x * _b0(t2)) - (endpoint.x * _b3(t2)))
    (y1, y2) = _solvexy(_b1(t1), _b2(t1), pointonline1.y - (startpoint.y * _b0(t1)) - (endpoint.y * _b3(t1)), _b1(t2), _b2(t2), pointonline2.y - (startpoint.y * _b0(t2)) - (endpoint.y * _b3(t2)))
    return BezierPathSegment(startpoint, Point(x1, y1), Point(x2, y2), endpoint)
end

"""
    bezierfrompoints(ptslist::Array{Point, 1})

Given four points, return the Bezier curve that passes through all four points.
"""
bezierfrompoints(ptslist::Array{Point, 1}) = bezierfrompoints(ptslist...)

"""
    bezierstroke(point1, point2, width=0.0)

Return a BezierPath, a stroked version of a straight line between two points.

It wil have 2 or 6 Bezier path segments that define a brush or pen shape. If
width is 0, the brush shape starts and ends at a point. Otherwise the brush shape
starts and ends with the thick end.

To draw it, use eg `drawbezierpath(..., :fill)`.
"""
function bezierstroke(p1::Point, p2::Point, width=0.0)
    bezpath = BezierPath()
    if isapprox(width, 0.0)
        # simple wavy stroke starting ending at point
        push!(bezpath, BezierPathSegment(p1, p1, p2, p2))
        push!(bezpath, BezierPathSegment(p2, p2, p1, p1))
        result = setbezierhandles.(bezpath, angles=[0.1, -0.1],handles=[0.3, 0.3])
    else
        # stroke with broad opening and closing
        # make the paths, then give them some control power
        cp1 = perpendicular(p1, p2, -width)
        seg = BezierPathSegment(p1, p1, cp1, cp1)
        adjustedseg = setbezierhandles(seg, angles=[0.1, -0.1], handles=[0.3, 0.3])
        push!(bezpath, adjustedseg)

        cp2 = perpendicular(p2, p1, width)
        seg = BezierPathSegment(cp1, cp1, cp2, cp2)
        adjustedseg = setbezierhandles(seg, angles=[0.1, -0.1], handles=[0.3, 0.3])
        push!(bezpath, adjustedseg)

        seg = BezierPathSegment(cp2, cp2, p2, p2)
        adjustedseg = setbezierhandles(seg, angles=[0.1, -0.1], handles=[0.3, 0.3])
        push!(bezpath, adjustedseg)

        # TODO the segments should be collinear, perhaps?
        cp3 = perpendicular(p2, p1, -width)
        seg = BezierPathSegment(p2, p2, cp3, cp3)
        adjustedseg = setbezierhandles(seg, angles=[0.1, -0.1], handles=[0.3, 0.3])
        push!(bezpath, adjustedseg)

        cp4 = perpendicular(p1, p2, width)
        seg = BezierPathSegment(cp3, cp3, cp4, cp4)
        adjustedseg = setbezierhandles(seg, angles=[0.1, -0.1], handles=[0.3, 0.3])
        push!(bezpath, adjustedseg)

        seg = BezierPathSegment(cp4, cp4, p1, p1)
        adjustedseg = setbezierhandles(seg, angles=[0.1, -0.1], handles=[0.3, 0.3])
        push!(bezpath, adjustedseg)
        result = bezpath
    end
    return result
end

"""
    setbezierhandles(bps::BezierPathSegment;
            angles  = [0.05, -0.1],
            handles = [0.3, 0.3])

Return a new Bezier path segment with new locations for the Bezier control
points in the Bezier path segment `bps`.

`angles` are the two angles that the "handles" make with the line direciton.

` handles` are the lengths of the "handles". 0.3 is a typical value.
"""
function setbezierhandles(bps::BezierPathSegment;
            angles=[0.05, -0.1],
            handles=[0.3, 0.3])
    d = distance(bps[1], bps[4])
    s = slope(bps[1], bps[4])
    bezhandle1 = bps[1] + polar(d * handles[1], s - angles[1])
    s = slope(bps[4], bps[1])
    bezhandle2 = bps[4] + polar(d * handles[2], s + angles[2])
    return BezierPathSegment(bps[1], bezhandle1, bezhandle2, bps[4])
end

"""
    setbezierhandles(bezpath::BezierPath;
            angles=[0 .05, -0.1],
            handles=[0.3, 0.3])

Return a new Bezierpath with new locations for the Bezier control points in
every Bezier path segment of the BezierPath in `bezpath`.

`angles` are the two angles that the "handles" make with the line direciton.

` handles` are the lengths of the "handles". 0.3 is a typical value.
"""
function setbezierhandles(bezpath::BezierPath;
        angles=[0.05, -0.1],
        handles=[0.4, 0.4])
    newbezpath = BezierPath()
    for bps in bezpath
        push!(newbezpath, setbezierhandles(bps, angles=angles, handles=handles))
    end
    return newbezpath
end

"""
    shiftbezierhandles(bps::BezierPathSegment;
        angles=[0.1, -0.1], handles=[1.1, 1.1])

Return a new BezierPathSegment that modifies the Bezier path in `bps` by moving
the control handles. The values in `angles` increase the angle of the handles;
the values in `handles` modifies the lengths: 1 preserves the length, 0.5 halves
the length of the  handles, 2 doubles them.
"""
function shiftbezierhandles(bps::BezierPathSegment;
        angles  = [0.1, -0.1],
        handles = [0.1, 0.1])
    p1, cp1, cp2, p2 = bps
    # find slope of curve at the end points
    spt1 = bezier′(0.0, p1, cp1, cp2, p2)
    spt2 = bezier′(1.0, p1, cp1, cp2, p2)
    # angle of handle to curve
    s1 = slope(p1, cp1)
    s2 = slope(p2, cp2)
    # length of handles
    l1 = distance(p1, cp1)
    l2 = distance(p2, cp2)
    # new angle
    s1 += angles[1]
    s2 += angles[2]
    # new handle lengths
    l1 = l1 * handles[1]
    l2 = l2 * handles[2]
    newcontrolpoint1 = p1 + polar(l1, s1)
    newcontrolpoint2 = p2 + polar(l2, s2)
    return BezierPathSegment(p1, newcontrolpoint1, newcontrolpoint2, p2)
end

"""
    brush(pt1, pt2, width=10;
        strokes=10,
        minwidth=0.01,
        maxwidth=0.03,
        twist = -1,
        lowhandle  = 0.3,
        highhandle = 0.7,
        randomopacity = true,
        tidystart = false,
        action = :fill,
        strokefunction = (nbpb) -> nbpb))

Draw a composite brush stroke made up of some randomized individual filled
Bezier paths.

`strokefunction` allows a function to process a BezierPathSegment or do other things before it's drawn.

!!! note

    There is a lot of randomness in this function. Results are unpredictable.
"""
function brush(pt1, pt2, width=10;
        strokes=10,
        minwidth=0.01,
        maxwidth=0.03,
        twist = -1,
        lowhandle  = 0.3,
        highhandle = 0.7,
        randomopacity = true,
        tidystart = false,
        action = :fill,
        strokefunction = (nbpb) -> nbpb)
    @layer begin
        sl = slope(pt1, pt2)
        n = distance(pt1, pt2)
        translate(pt1)
        rotate(sl - pi/2)
        widthsteps = (maxwidth - minwidth)/10
        for j in 1:strokes
            shiftedline = [O + (0, 0), O + (0, n)]
            if tidystart
                shiftedline .+= Point(rand(-width/2:width/2), 0)
            else
                shiftedline .+= Point(rand(-width/2:width/2), rand(-width/2:width/2))
            end
            pbp = bezierstroke(shiftedline[1],
                shiftedline[2],
                rand(Bool) ? 0.0 : rand(minwidth:widthsteps:maxwidth)
                )
            for bps in pbp
                randomopacity ? setopacity(rand(0.3:0.1:0.9)) : setopacity(1.0)
                nbpb = shiftbezierhandles(bps,
                    angles  = [twist * rand(minwidth:widthsteps:maxwidth), -twist * rand(minwidth:widthsteps:maxwidth)],
                    handles = [rand(lowhandle:0.01:highhandle, 2)...]
                    )
                strokefunction(nbpb)
                drawbezierpath(nbpb, action, close=false)
            end
        end
    end
end


### A Pluto.jl notebook ###
# v0.12.21

using Markdown
using InteractiveUtils

# ╔═╡ 896f6942-76a2-11eb-01e9-1156629a4a14
begin
	using Pkg;using Plots; gr()
	Pkg.add("LaTeXStrings")
	using LaTeXStrings
end

# ╔═╡ 0eea2678-7641-11eb-1c89-b53ca4514973
function circleShape(h,k,r)
	θ = LinRange(0,2π,500)
	h .+ r*sin.(θ), k .+ r*cos.(θ)
end

# ╔═╡ 58cc6b66-7641-11eb-2aa0-3b5f7a52eac9
function mohr2D(ϵx,ϵy,ϵxy)
	center = (ϵx+ϵy)/2.
	R = sqrt(((ϵx-ϵy)/2.)^2+ϵxy^2)
	return center,R
end

# ╔═╡ a8d106fa-7641-11eb-0248-1ddac2014484
function mohr2D(ϵ1,ϵ2)
	center = (ϵ1+ϵ2)/2
	R = abs((ϵ1-ϵ2)/2)
	return center,R
end

# ╔═╡ a62626fc-7643-11eb-14ef-c110a134ce50
function plotCircle(ϵ1,ϵ2)
	c,R =  mohr2D(ϵ1,ϵ2)
	p = plot(circleShape(c,0,R),seriestype = [:shape], 		 lw=0.5, c = :blue, linecolor =:black, legend = false, fillalpha = 0.05, aspect_ratio = 1)
	scatter!(p,[c],[0],markercolor = :black)
	scatter!(p,[ϵ1],[0],markercolor = :black)
	scatter!(p,[ϵ2],[0],markercolor = :black)
	xlabel!(L"\varepsilon_n")

	ylabel!(L"\tau_n")

	if R≉0
		annotate!(p, ϵ1 + 0.1*R, 0.1*R, L"\epsilon_1", :color)
		annotate!(p, ϵ2 + 0.1*R, 0.1*R, L"\epsilon_2", :color)
	end

	if R ≈ 0
		ylims!(-maximum([ϵ1,ϵ2]),maximum([ϵ1,ϵ2]))
	end

	hline!(p,[0], linecolor = :black)
	return p
end

# ╔═╡ eaf4f830-7643-11eb-09e3-e1109e9cdb65
function plotCircle(ϵx,ϵy,ϵxy)
	c,R =  mohr2D(ϵx,ϵy,ϵxy)
	ϵ1 = c + R
	ϵ2 = c - R
	p = plot(circleShape(c,0,R),seriestype = [:shape], 		 lw=0.5, c = :blue, linecolor =:black, legend = false, fillalpha = 0.05, aspect_ratio = 1)
	scatter!(p,[c],[0],markercolor = :black)
	scatter!(p,[ϵ1],[0],markercolor = :black)
	scatter!(p,[ϵ2],[0],markercolor = :black)
	scatter!(p,[ϵx],[ϵxy],markercolor = :blue)
	scatter!(p,[ϵy],[-ϵxy],markercolor = :red)

	plot!(p,[ϵx,ϵy],[ϵxy, -ϵxy],linecolor=:black)

	if R≉0
		annotate!(p, ϵ1 + 0.1*R, 0.1*R, L"\epsilon_1", :color)
		annotate!(p, ϵ2 + 0.1*R, 0.1*R, L"\epsilon_2", :color)
		annotate!(p, ϵx+0.06*R, ϵxy+0.1*R, L"X", :color)
		annotate!(p, ϵy+0.06*R, -ϵxy+0.1*R, L"Y", :color)
	end

	xlabel!(L"\varepsilon_n")

	ylabel!(L"\tau_n")
	if R ≈ 0
		ylims!(-maximum([ϵ1,ϵ2]),maximum([ϵ1,ϵ2]))
		annotate!(p, ϵ1 + 0.1*ϵ1, 0.1*ϵ1, L"\epsilon_1 = \epsilon_2 = C", :color)
	end

	hline!(p,[0], linecolor = :black)
	return p
end

# ╔═╡ d56c2fda-7641-11eb-29e8-579e1f95cada
begin
	ϵ1 = 1.
	ϵ2 = 5.
	p = plotCircle(3,5,2)
end

# ╔═╡ Cell order:
# ╠═d56c2fda-7641-11eb-29e8-579e1f95cada
# ╠═896f6942-76a2-11eb-01e9-1156629a4a14
# ╠═0eea2678-7641-11eb-1c89-b53ca4514973
# ╠═58cc6b66-7641-11eb-2aa0-3b5f7a52eac9
# ╠═a8d106fa-7641-11eb-0248-1ddac2014484
# ╠═a62626fc-7643-11eb-14ef-c110a134ce50
# ╠═eaf4f830-7643-11eb-09e3-e1109e9cdb65


using RandomVariates
using Gadfly
import Cairo, Fontconfig

N = 1000000
A = [1 .8; .8 1]

struct Dist
    title
    x
    xmin
    xmax
end

function plot_dist(d::Dist)
    plot(x=d.x, Stat.density, Geom.polygon(fill=true, preserve_order=true), Guide.title(d.title), Coord.cartesian(xmin=d.xmin, xmax=d.xmax))
end

function plot_hist(d::Dist)
    plot(x=d.x, Geom.histogram(density=true), Guide.title(d.title), Coord.cartesian(xmin=d.xmin, xmax=d.xmax))
end

uniform = Dist("Standard Uniform", uniform_rng(0,1,N), 0, 1)
normal = Dist("Standard Normal", normal_rng(0,1,N), nothing, nothing)
bernoulli = Dist("Bernoulli(.34)", bernoulli_rng(.34, N), nothing, nothing)
beta = Dist("Beta(2,8)", beta_rng(2,8, N), 0, 1)
binomial = Dist("Binomial(.43, 10)", binomial_rng(.43, 10, N), nothing, nothing)
erlang = Dist("Erlang(7, 4)", erlang_rng(7, 4, N), 0, nothing)
expon = Dist("Exponential(4)", expon_rng(4, N), 0, nothing)
gamma = Dist("Gamma(6,4)", gamma_rng(6,4,N), 0, nothing)
geom = Dist("Geometric(.2)", geometric_rng(.2, N), 0, nothing)
neg_binom = Dist("Negative Binomial(.25, 20)", conv_neg_binomial_rng(.25, 20, N), 0, nothing)
poisson = Dist("Poission(10)", poisson_rng(10, N), 0, nothing)
triang = Dist("Triangular(1,5,2)", triag_rng(1,5,2,N), nothing, nothing)
weibull = Dist("Weibull(1,5)", weibull_rng(1,5,N), 0, nothing)
mv_norm = Dist("Multivariate Normal([0,2], Σ)", mv_normal_rng([0,2], A, Int(N/1000)), nothing, nothing)

for d in [uniform, normal, expon, erlang, gamma, triang, weibull, beta]
    out = plot_dist(d)
    out |> SVG("images/$(d.title).svg", 5inch, 5inch)
    out |> PNG("images/$(d.title).png", 5inch, 5inch)
end

for d in [bernoulli, binomial, geom, neg_binom, poisson]
    out = plot_hist(d)
    out |> SVG("images/$(d.title).svg", 5inch, 5inch)
    out |> PNG("images/$(d.title).png", 5inch, 5inch)
end

out = plot(x=mv_norm.x[:, 1], y=mv_norm.x[:, 2], Geom.density2d(levels=10), Geom.point, alpha=[0.4], Guide.title(mv_norm.title))
out |> SVG("images/$(mv_norm.title).svg", 5inch, 5inch)
out |> PNG("images/$(mv_norm.title).png", 5inch, 5inch)


#=
Given an array of time intervals (start, end) for classroom lectures (possibly overlapping), find the minimum number of rooms required.

For example, given [(30, 75), (0, 50), (60, 150)], you should return 2.
=#

using Test
include("Solutions/problem21_number_of_rooms_for_lectures.jl")

@test number_of_rooms_required([(30, 75), (0, 50), (60, 150)]) == 2


using SumBassSolver

p=0.001
q=0.1
m=100
c0=10
alpha = 1
F = 10
T = 50
n = 50
Pi = 600

x1,v1 = sumBassHomogeneousSolver(p,q,m,c0,alpha,F,T,n,Pi)

pp=fill(p,n)
qq=fill(q,n)
mm=fill(m,n)
cc0 = fill(c0,n)
alphas=fill(alpha,n)
FF=fill(F,n)
TT=fill(T,n)

# The general algorithm does find the right solution but takes really long
# The local optimization finishes in a local optimum
x2,v2 = @time sumBassSolver(pp,qq,mm,cc0,alphas,FF,TT,Pi)
x3,v3 = @time sumBassSolver(pp,qq,mm,cc0,alphas,FF,TT,Pi,alg="SB")
x4,v4 = @time sumBassSolver(pp,qq,mm,cc0,alphas,FF,TT,Pi,alg="SB",localopt=true)


#=
design a modular device using parts from a catalog:
- for each module we pick one corresponding part from the catalog, satisfying a constraint
associated with the part
- these constraints involve certain global design variables (e.g. weight, size,
lifetime, battery power)
- we minimize the cost of the parts plus a linear function of the global design variables

sets:
i ∈ 1..m          modules
j ∈ J[i], i ∈ m   parts for module i
k ∈ 1..n          global design variables

variables:
xᵢⱼ ∈ {0,1}   whether part j ∈ J[i] is selected for module i
y   ∈ ℝ₊ⁿ     global design characteristics

objective:
min ∑ᵢⱼ cᵢⱼ xᵢⱼ + ∑ₖ dₖ yₖ   where c ≥ 0

constraints:
∑ⱼ xᵢⱼ = 1, ∀i                 disjunction for module i
ymin ≤ y ≤ ymax                bounds
(xᵢⱼ = 1) ⟹ (y ∈ Sᵢⱼ), ∀i,j   constraint for part i,j where Sᵢⱼ is some set

use the conic copies-of-variables formulation for the disjunctions
=#

struct ModularDesign <: ExampleInstance
    m::Int # number of modules
    p::Int # number of part options per module
    n::Int # number of design variables
    use_nat::Bool # for convex case, use natural formulations
    use_nonconvex::Bool # use nonconvex equality constraints, else convex constraints
    pwl::PWLSOS2 # type of piecewise linear SOS2 formulation to use
    num_pts::Int # maximum number of points per piecewise linearization
end

function ModularDesign(m::Int, p::Int, n::Int, use_nat::Bool)
    return ModularDesign(m, p, n, use_nat, false, SOS2(), 0)
end

function build(inst::ModularDesign)
    (m, p, n) = (inst.m, inst.p, inst.n)
    @assert m >= 1
    @assert p >= 1
    @assert n >= 2
    ymin = 1e-3
    ymax = 2.0
    @assert ymax > ymin > 0
    q = 1 + div(n, 2) # dimension of convex epigraph sets
    @assert q >= 2

    # generate data
    c = 10 * rand(m, p)
    d = vec(sprandn(n, 1, 0.3))
    # random solution to ensure feasibility
    y0 = (ymax - ymin) * rand(n) .+ ymin

    # functions and affine data defining the conic constraints
    # TODO -log has high slope near zero, so need non-evenly-spaced points
    if inst.use_nat
        f_list = VecSepSpecPrim[
            # VecNegLog(),
            VecNegEntropy(),
            VecNegSqrt(),
            VecNegPower01(0.8),
            VecPower12(2.0),
        ]
    else
        f_list = VecSepSpecPrimEF[
            # VecNegLogEF(),
            VecNegEntropyEF(),
            VecNegSqrtEF(),
            VecNegPower01EF(0.8),
            VecPower12EF(2.0),
        ]
    end
    fs = rand(f_list, m, p)
    Gs = Matrix{Matrix{Float64}}(undef, m, p)
    hs = Matrix{Vector{Float64}}(undef, m, p)
    G_sparsity = min(0.7, 4 / (q * n))
    for i in 1:m, j in 1:p
        f_ij = fs[i, j]
        G_ij = sprandn(q, n, G_sparsity)
        for k in 2:q
            if iszero(G_ij[k, :])
                G_ij[k, rand(1:n)] = randn()
            end
        end
        h_ij = G_ij * y0
        w = rand(q - 1) .+ 1e-3
        h_ij[2:end] .+= w
        h_ij[1] += get_val(w, f_ij)
        if !inst.use_nonconvex
            h_ij[1] += rand()
        end
        Gs[i, j] = G_ij
        hs[i, j] = h_ij
    end

    # build model
    model = JuMP.Model()
    JuMP.@variable(model, y[1:n])
    JuMP.@variable(model, x[1:m, 1:p], Bin)
    JuMP.@objective(model, Min, dot(c, x) + dot(d, y))

    # disjunctive copies-of-variables formulation
    # x[i, j] switches on conic constraint for part j of module i
    JuMP.@variable(model, z[1:m, 1:p, 1:n])
    JuMP.@constraint(model, [i in 1:m], sum(x[i, :]) == 1)
    JuMP.@constraint(model, [i in 1:m], sum(z[i, j, :] for j in 1:p) .== y)

    # x[i, j] = 0 ⇒ z[i, j, :] = 0, x[i, j] = 1 ⇒ z[i, j, :] = y
    affs = Matrix{Vector{JuMP.AffExpr}}(undef, m, p)
    for i in 1:m, j in 1:p
        x_ij = x[i, j]
        z_ij = z[i, j, :]
        JuMP.@constraint(model, z_ij .<= ymax * x_ij)
        JuMP.@constraint(model, ymin * x_ij .<= z_ij)

        h_ij = hs[i, j]
        G_ij = Gs[i, j]
        aff = affs[i, j] = h_ij * x_ij - G_ij * z_ij
        u_aff = aff[1]
        w_aff = aff[2:end]
        K = length(w_aff)
        f = fs[i, j]

        if !inst.use_nonconvex
            # add convex constraint
            add_spectral(f, K, vcat(u_aff, x_ij, w_aff), model)
            continue
        end

        # relaxation of nonconvex constraint
        λ = JuMP.@variable(model, [1:K])
        JuMP.@constraint(model, u_aff == sum(λ))

        for k in 1:K
            # auxiliary variable for w_aff[k]
            w_k = JuMP.@variable(model)
            JuMP.@constraint(model, w_k == w_aff[k])

            # convex constraint (above graph)
            add_spectral(f, 1, vcat(λ[k], x_ij, 1.0 * w_k), model)

            # bounds
            G_k = G_ij[1 + k, :]
            min_ij = h_ij[1 + k] - sum((g < 0 ? ymin : ymax) * g for g in G_k)
            max_ij = h_ij[1 + k] - sum((g < 0 ? ymax : ymin) * g for g in G_k)
            @assert min_ij <= max_ij
            max_ij = max(1e-4, max_ij)
            min_ij = max(1e-4, min_ij)

            # interpolation
            pts = collect(range(min_ij, max_ij, length = inst.num_pts))
            f_pts = [get_val([pt], f) for pt in pts]

            # auxiliary variables and SOS2 formulation
            σ = JuMP.@variable(model, [1:length(pts)], lower_bound = 0)
            PWL_SOS2(inst.pwl, model, σ, x_ij)

            # data constraints
            JuMP.@constraints(model, begin
                dot(σ, f_pts) >= λ[k] # below PWL graph
                sum(σ) == x_ij
                dot(σ, pts) == w_k
            end)
        end
    end

    # save for use in tests
    model.ext[:x] = x
    model.ext[:y] = y
    model.ext[:z] = z
    model.ext[:fs] = fs
    model.ext[:affs] = affs

    return model
end

function test_extra(inst::ModularDesign, model::JuMP.Model)
    stat = JuMP.termination_status(model)
    @test stat in OPT_OR_LIMIT
    (stat == MOI.OPTIMAL) || return

    tol = eps()^0.2
    x = JuMP.value.(model.ext[:x])
    x_int = round.(Int, x)
    @test x ≈ x_int atol = tol rtol = tol
    @test all(-tol .<= x .<= 1 + tol)

    # check feasibility for disjunctive linear constraints
    (m, p) = (inst.m, inst.p)
    y = JuMP.value.(model.ext[:y])
    z = JuMP.value.(model.ext[:z])
    @test all(sum(x_int[i, :]) == 1 for i in 1:m)
    z0ij(i, j) = (iszero(x_int[i, j]) ? zero(y) : y)
    approxij(i, j) = isapprox(z[i, j, :], z0ij(i, j), atol = tol, rtol = tol)
    @test all(approxij(i, j) for i in 1:m for j in 1:p)

    # check feasibility for nonlinear constraints
    fs = model.ext[:fs]
    affs = model.ext[:affs]
    viol(fvaff) = begin
        (f, v, aff) = fvaff
        (u, w) = (aff[1], aff[2:end])
        iszero(v) && return -u
        @assert v == 1
        any(<(-tol), w) && return NaN
        return get_val(max.(w, 1e-9), f) - u
    end
    if inst.use_nonconvex
        tol_loose = eps()^0.1
        # check approximate equality (and inside cone)
        conicfeas = (a -> -tol_loose < a < tol)
    else
        # convex constraints: check inside cone
        conicfeas = <(tol)
    end
    ϵs = [viol((fs[i, j], x_int[i, j], JuMP.value.(affs[i, j]))) for i in 1:m, j in 1:p]
    @show maximum(abs, ϵs) # TODO delete
    @test all(conicfeas, ϵs)
    return
end


@doc """
Euler method

Step Propagation using the Euler formula.
""" ->
immutable QuEuler <: QuPropagatorMethod
end

@doc """
Crank Nicolson Method

Step Propagation using the Crank Nicolson formula.
""" ->
immutable QuCrankNicolson <: QuPropagatorMethod
end

@doc """
Krylov subspace Method

Step Propagation using the Krylov subspace method.

### Fields :

* options :: Dict

  Dictionary to set the size of basis by using the key `:NC`.
""" ->
immutable QuKrylov <: QuPropagatorMethod
    options::Dict
end

QuKrylov() = QuKrylov(@compat Dict(:NC=>10))

@doc """
Propagate for different solver types.

Method used to propagate one time step using the solver.

### Arguments

Inputs :
* prob <: QuPropagatorMethod

  Solver type (Method to be used  -> QuEuler, QuCrankNicolson,
                       QuKrylov, QuODE45, QuODE78, QuODE23s,
                       QuExpmV, QuExpokit, QuMCWF)
* eq <: QuEquation

  Equation type
* t :: Float64

  Final time
* current_t :: Float64

  Current time
* current_qustate

  Quantum state corresponding to current time

Output :
* Same type as current_qustate.
""" ->
function propagate(prob::QuEuler, eq::QuEquation, t, current_t, current_qustate)
    dt = t - current_t
    dims = size(current_qustate)
    op = operator(eq, t)
    next_state = (eye(op)-im*op*dt)*vec(current_qustate)
    CQST = QuBase.similar_type(current_qustate)
    return CQST(reshape(coeffs(next_state), dims), bases(current_qustate))
end

function propagate(prob::QuCrankNicolson, eq::QuEquation, t, current_t, current_qustate)
    dt = t - current_t
    dims = size(current_qustate)
    op = operator(eq, t)
    uni = eye(op)-im*op*dt/2
    next_state = \(uni', uni*vec(current_qustate))
    CQST = QuBase.similar_type(current_qustate)
    return CQST(reshape(coeffs(next_state), dims), bases(current_qustate))
end

function propagate(prob::QuKrylov, eq::QuEquation, t, current_t, current_qustate)
    dt = t - current_t
    dims = size(current_qustate)
    cqs = vec(current_qustate)
    basis_size = get(prob.options,:NC, length(cqs))
    N = min(basis_size, length(cqs))
    v = Array(typeof(cqs),0)
    @compat sizehint!(v, N+1)
    push!(v,zeros(cqs))
    push!(v,cqs)
    alpha = Array(Complex{Float64},0)
    @compat sizehint!(alpha, N)
    beta = Array(Complex{Float64},0)
    @compat sizehint!(beta, N+1)
    op = operator(eq, t)
    push!(beta,0.)
    for i=2:N
        w = op*v[i]
        push!(alpha, w'*v[i])
        w = w-alpha[i-1]*v[i]-beta[i-1]*v[i-1]
        push!(beta, norm(w))
        push!(v, w/beta[i])
    end
    w = op*v[end]
    push!(alpha, w'*v[end])
    deleteat!(v,1)
    H_k = full(Tridiagonal(beta[2:end], alpha, beta[2:end]))
    U_k = expm(-im*dt*H_k)
    next_state = zeros(cqs)
    for j=1:N
        next_state = next_state + v[j]*U_k[j,1]
    end
    CQST = QuBase.similar_type(current_qustate)
    return CQST(reshape(coeffs(next_state), dims), bases(current_qustate))
end

export QuEuler,
     QuCrankNicolson,
     QuKrylov


using PyPlot
using SpecialFunctions
Np=Int(1e4)
Nx=Int(6)
dt=0.05
tmax=1000
deg=0; #Smoothness of periodic boundary
sigmaL=4*pi;
vth=0.1;
gamma=1; # Electrons (-1), gravity 1
eps=0
pad=0

function f(x,v)
  return exp.(-0.5.*(v/vth).^2)/sqrt(2*pi)/vth.*
              exp.(-0.5.*(x/sigmaL).^2).*(1+ eps*sqrt(2)*((x/sigmaL).^2-0.5) )/sqrt(2*pi)/(1+0.5*eps)
end
function g(x,v)
  return exp.(-0.5.*(v/vth).^2)./sqrt(2*pi)/vth.*exp.(-0.5.*(x/sigmaL).^2)/sqrt(2*pi)/sigmaL
end

using Sobol
sob = SobolSeq(2)
skip(sob, 5) # Skip some entries
xk=Array(Float64,Np); vk=similar(xk)
for i=1:Np
  xk[i],vk[i]=Sobol.next(sob)
end

# Sampling
norminv=p->sqrt(2.).*erfinv.(2.*p - 1.0)
xk.=norminv(xk)*sigmaL
vk.=norminv(vk)*vth

fk=f(xk,vk)
wk=f(xk,vk)./g(xk,vk)

# Hermite functions
function HermiteH{T}(x::T,N::Int)
  Hn=Array(T, N)
  Hn[1]=1.
  Hn[2]=sqrt(2)*x
  @inbounds for idx=3:N
    n=idx-2
    Hn[idx]=x*sqrt(2/(n+1))*Hn[idx-1]- sqrt(n/(n+1))*Hn[idx-2]
  end
  return Hn*exp(x^2/2)
end

 # Hermite functions
 function HermiteHdx{T}(x::T,N::Int)
   Hdxn=Array(T, N)
   H0::T=1.0
   H1::T=sqrt(2)*x
   Hdxn[1]=-x   #-1./sqrt(2)*H1
   @inbounds for idx=2:N
     n=idx-1
     H2::T=x*sqrt(2/(n+1))*H1- sqrt(n/(n+1))*H0
     Hdxn[idx]=sqrt(n/2)*H0 -sqrt((n+1)/2)*H2
     H0=H1; H1=H2
   end
   return Hdxn.*exp(-x^2/2)
 end

function HermiteHdx_series{T}( x::Array{T,1}, u::Array{T,1} )
  E=Array(T,length(x))
  Np=length(x)
  Nx=length(u)
  @inbounds for pdx=1:Np
      E[pdx]=dot(HermiteHdx(x[pdx],Nx),u)
    end
  return E
end

function diff_Hermite{T}(H::Array{T,1})
  N=length(H)-1
  dH=zeros(T,length(H))
  dH[1]=1/sqrt(2)*H[2]
  dH[end]=-sqrt(N/2)*H[end-1]
  for idx=2:N-1
    n=idx-1
    dH[idx]=sqrt((n+1)/2)*H[idx+1] -sqrt(n/2)*H[idx-1]
  end
  return dH
end

function poissonK_Hermite(N::Int)
  K=spdiagm([-sqrt( (3:N).*((3:N)-1)*pi)/2,
             ((1:N)+0.5)*sqrt(pi)  ,
             -sqrt(((1:N-2)+2).*((1:N-2)+1)*pi)/2],
                   [2,0,-2])
  return K
end

@inbounds function  rhs_particle_Hermite{T}(xk::Array{T,1},wk::Array{T,1}, Nx::Int)
    const Np=length(xk)
    rho=zeros(T,Nx)
    # Precalculate coefficients
    alpha=Array(T,Nx-2)
    beta=Array(T,Nx-2)
    @inbounds for n=1:Nx-2
      alpha[n]=sqrt(2/(n+1))
      beta[n]=-sqrt(n/(n+1))
    end

    @inbounds for pdx=1:Np
      w::T=exp(-xk[pdx].^2/2)*wk[pdx]
      P0::T=1.0
      P1::T=sqrt(2)*P0*xk[pdx]
      rho[1]+=P0.*w
      rho[2]+=P1.*w
      for idx=1:Nx-2
        P2::T=xk[pdx].*alpha[idx].*P1+ beta[idx].*P0
        P0=P1; P1=P2;
        rho[idx+2]+=P2.*w
      end
    end
  return rho
end

@inbounds function HermiteH_series{T}(x::Array{T,1}, u::Array{T,1})
  N::Int=length(u)-1;
  alpha=Array(T,N+1)
  beta=Array(T,N+1)
  #
  for n=0:N
     alpha[n+1]=sqrt(2/(n+1))
     beta[n+1]=-sqrt((n+1)/(n+2))
  end
  E=Array(T,length(x))
  for pdx=1:length(x)
   b1::T=0
   b2::T=0
   for n=N:-1:0
     b0::T=u[n+1]+ x[pdx]*alpha[n+1]*b1 + beta[n+1]*b2
     b2=b1; b1=b0;
    end
    E[pdx]=b1*exp(-x[pdx]^2/2)
  end
  return E
end

# Coefficients for Runge Kutta
rksd=[2/3.0, -2/3.0, 1  ]
rksc=[ 7/24.0, 3/4.0, -1/24.0]

Nt=Int(ceil(tmax/dt))
fieldenergy=zeros(Float64,Nt)
kineticenergy=zeros(Float64,Nt)
momentum=zeros(Float64,Nt)
rho=zeros(Float64,Nx)
Phi=zeros(Float64,Nx+pad)
E=zeros(Float64,Nx+pad+2)
K=poissonK_Hermite(Nx+pad)

figure()
tic()
@inbounds for tdx=1:Nt
  if (mod(tdx-1,50)==0)
    PyPlot.clf()

    scatter(xk,vk,s=2, c=fk,lw=0)
    axis([-sigmaL*5,sigmaL*5,-3.5*vth,3.5*vth])

    sleep(0.01)
  end
  @inbounds for rkdx=1:length(rksd)

    # Bin to right hand side
    rhs=rhs_particle_Hermite( xk/sigmaL,wk,Nx)/Np
    # Hermite L^2 projection
    rho=rhs./sqrt(pi)
    # Remove ion background (quick'n dirty)
    rhs[1]=0
    Phi.=\(-K, [rhs;zeros(Float64,pad)])

    E.=diff_Hermite(gamma*[Phi;0;0])

    if rkdx==1
      fieldenergy[tdx]=(Phi'*K*Phi)[1]/2*sigmaL  #sum(E.^2)*sqrt(pi) #
      kineticenergy[tdx]=0.5.*sum(vk.^2.*wk )/Np
      momentum[tdx]=sum(vk.*wk)/Np
    end

    vk.+=dt*rksc[rkdx]*HermiteH_series(xk/sigmaL,E)
    #vk.+=dt*rksc[rkdx]*HermiteHdx_series(xk/sigmaL,-Phi)
    xk.+=dt*rksd[rkdx].*vk

    # Reflecting boundary
  end

  #print(tdx)
end
toc()
ttime=(0:Nt-1)*dt

prefix="plots/hermite_sheath"
#prefix="legendre"

#
# xg=collect(linspace(minimum(xk),maximum(xk)))
# figure()
# plot(xg, HermiteH_series(xg/sigmaL,Phi))

# HermiteHdx_series(xk/sigmaL,-Phi)
# HermiteH_series(xk/sigmaL,E)
#
# figure()
# plot(abs(rhs))

# plot(abs(Phi))
# plot(abs(E))
# plot(1:Nx,abs(rho))

# figure()
# title("magnitude of coefficients")
# semilogy(abs(coefficients(rho)),label=L"density $\rho$")
# semilogy(abs(coefficients(Phi)),label=L"potential $\Phi$")
# semilogy(abs(coefficients(D1*Phi)),label=L"field $E$")
# xlabel("index")
# legend()
# grid()
# savefig("$prefix\_field_coeffs.png")

# figure()
# plot(kineticenergy)
# plot(kineticenergy+fieldenergy*sigmaL)

PyPlot.matplotlib[:rc]("font", size=18)

figure()
semilogy(ttime,fieldenergy)
omega=1.415661888604536 - 0.153359466909605im;
semilogy(ttime[ttime.<8.],
0.5*fieldenergy[1]*abs(exp(-im*omega*(ttime[ttime.<8.]-0.4))).^2,
lw=3,"r")
autoscale(tight="True");grid()
ylabel("electrostatic energy");
xlabel("time")
#tight_layout()
# savefig("$prefix\_fieldenergy.png")

# figure()
# semilogy(ttime,kineticenergy)
# grid();
# #autoscale(tight="True")
# ylabel("kinetic energy")
# xlabel("time")
# savefig("$prefix\_kineticenergy.png")

figure()
energy=kineticenergy+fieldenergy
semilogy(ttime,abs((energy-energy[1])./energy[1]))
grid()
ylabel("rel. energy error")
xlabel("time ")
# savefig("$prefix\_energy_error.png")
figure()
semilogy(ttime,abs(momentum-momentum[1]))
xlabel("time "); grid()
ylabel("absolute momentum error")
# savefig("$prefix\_momentum_error.png")

figure()
scatter(xk,vk,s=1, c=fk,lw=0)
# autoscale(tight="True")
# axis([0,L,minimum(vk),maximum(vk)])
colorbar();tight_layout()
xlabel("x");ylabel("v")
# savefig("$prefix\_particles.png")

#
# #Test OSDE HERMITE
# h=x->exp(-x.^2/2).*(x-0.5)+exp(-(x-2.2).^2)
# Dh=x->exp(-x.^2/2).*(x-0.5).*(-x) + exp(-x.^2/2)  -exp(-(x-2.2).^2).*2.*(x-2.2)
# Np=Int(1e7)
# xk=randn(Float64,Np)
#
# wk=h(xk)./(exp(-0.5.*xk.^2)/sqrt(2*pi) )
#
#
#
# hhat=rhs_particle_Hermite(xk,wk,40)/Np/sqrt(pi)
#
# x=collect(linspace(-15,15,2000))
# figure()
# plot(x,h(x))
# plot(x,HermiteH_series(x,hhat))
#
# figure()
# # plot(x,Dh(x))
# plot(x,gradient(h(x),x[2]-x[1] ))
#
# Dhhat=diff_Hermite(hhat)
#
#
# plot(x,HermiteH_series(x,Dhhat))
#  plot(x,HermiteHdx_series(x,hhat))


export convert, conj, real, imag, abs2, norm

## convert functions
function convert(::Type{Polynomial}, expo::Exponent)
    return Polynomial(SortedDict{Exponent, Number}(expo=>1.0))
end

function convert(::Type{Polynomial}, x::Variable)
    return Polynomial(SortedDict{Exponent, Number}(Exponent(SortedDict(x=>Degree(1,0)))=>1.0))
end

function convert(::Type{Polynomial}, λ::Number)
    return Polynomial(SortedDict{Exponent, Number}(Exponent()=>λ))
end

function convert(::Type{Point}, pt::SortedDict{Variable, T}) where T
    return Point(convert(SortedDict{Variable, Number}, pt))
end

function convert(::Type{AbstractPolynomial}, λ::Number)
    return Polynomial(SortedDict{Exponent, Number}(Exponent()=>λ))
end

## Conjugate
function conj(d::Degree)
    return Degree(d.conjvar, d.explvar)
end

function conj(x::Variable)
    if x.kind<:Complex
        return Exponent(SortedDict(x=>Degree(0,1)))
    else
        return Exponent(SortedDict(x=>Degree(1,0)))
    end
end

function conj(expo::Exponent)
    expodict = SortedDict{Variable, Degree}()
    for (var, deg) in expo.expo
        if iscomplex(var)
            expodict[var] = conj(deg)
        else
            expodict[var] = deg
        end
    end
    return Exponent(expodict)
end

function conj(p::Polynomial)
    pdict = SortedDict{Exponent, Number}()
    for (expo, λ) in p
        pdict[conj(expo)] = conj(λ)
    end
    return Polynomial(pdict)
end

## Real, imag
real(p::Polynomial) = (p + conj(p))/2
imag(p::Polynomial) = (p - conj(p))/(2im)

function real(p::T) where T<:AbstractPolynomial
    return (p + conj(p))/2
end

function imag(p::T) where T<:AbstractPolynomial
    return (p + -conj(p))/(2im)
end

## Norm, abs2
function abs2(p::T) where T<:AbstractPolynomial
    return p*conj(p)
end

function norm(pt::Point, p::Real = 2)
    p > 0 || error(LOGGER, "norm(): p should be positive ($p ≤ 0).")
    if p == Inf
        maxi = -Inf
        for (var, val) in pt
            # iscomplex(var) && warn("norm(): var $(var) is not real, splitting real and imag parts.")
            if !iscomplex(var) && (val > maxi)
                maxi = val
            elseif iscomplex(var) && max(real(val), imag(val)) > maxi
                maxi = max(real(val), imag(val))
            end
        end
        return maxi
    elseif p == 1
      s = 0
      for (var, val) in pt
          if !iscomplex(var)
            s += abs(val)
          else
            # warn("norm(): var $(var) is not real, splitting real and imag parts.")
            s += abs(real(val)) + abs(imag(val))
          end
      end
      return s
    else
      s = 0
      for (var, val) in pt
          if !iscomplex(var)
            s += val^p
          else
            # warn("norm(): var $(var) is not real, splitting real and imag parts.")
            s += real(val)^p + imag(val)^p
          end
      end
      return s^(1/p)
  end
end


using jInv.Mesh
using Multigrid
using Multigrid.DomainDecomposition
using SparseArrays
using LinearAlgebra

speye = (n) -> SparseMatrixCSC(1.0I,n,n);

domain = [0.0, 1.0, 0.0, 1.0];
n = [64,64];
Mr = getRegularMesh(domain,n)
numCores = 4;
innerKatzIter = 5;
inner = 5;
nrhs = 2;

println("************************************************* Hybrid Keczmarz for DivSigGrad 2D ******************************************************");

m  = exp.(randn(prod(n)));
Ar = getNodalDivSigGradMatrix(Mr,vec(m));
Ar = Ar + 2e-1*opnorm(Ar,1)*speye(size(Ar,2));
Ar = convert(SparseMatrixCSC{Float64,Int64},Ar)

N = size(Ar,2);
b = Ar*rand(Float64,N,nrhs); b = b/norm(b);
x = zeros(Float64,N,nrhs);

HKparam = getHybridKaczmarz(Float64,Int64, Ar,Mr,[4,4], getNodalIndicesOfCell,0.8,numCores,innerKatzIter);
prec = getHybridKaczmarzPrecond(HKparam,Ar,nrhs);
x = FGMRES_relaxation(Ar,copy(b),x,inner,prec,1e-5*norm(b),true,numCores)[1]
println("Norm is: ",norm(Ar*x - b))

println("************************************************* Hybrid Keczmarz for Elasticity 2D ******************************************************");

mu = 2.0*ones(prod(Mr.n));
lambda = mu;
Ar = GetLinearElasticityOperator(Mr,mu,lambda);
Ar = Ar + 2e-1*opnorm(Ar,1)*sparse(1.0I,size(Ar,2),size(Ar,2));
N = size(Ar,2);
b = Ar*rand(N,nrhs); b = b/norm(b);
x = zeros(N,nrhs);

HKparam = getHybridKaczmarz(Float64,Int64, Ar,Mr,[4,4], getFacesStaggeredIndicesOfCellNoPressure,0.8,numCores,innerKatzIter);
prec = getHybridKaczmarzPrecond(HKparam,Ar,nrhs);
x = FGMRES_relaxation(Ar,copy(b),x,inner,prec,1e-5*norm(b),true,numCores)[1]
println("Norm is: ",norm(Ar*x - b))


@doc raw"""
    LennardJones(; cutoff, nl_only, lorentz_mixing, weight_14, force_unit, energy_unit, skip_shortcut)

The Lennard-Jones 6-12 interaction. The potential is given by
```math
V_{ij}(r_{ij}) = 4\varepsilon_{ij} \left[\left(\frac{\sigma_{ij}}{r_{ij}}\right)^{12} - \left(\frac{\sigma_{ij}}{r_{ij}}\right)^{6}\right]
```
and the force on each atom by
```math
\begin{aligned}
\vec{F}_i &= 24\varepsilon_{ij} \left(2\frac{\sigma_{ij}^{12}}{r_{ij}^{13}} - \frac{\sigma_{ij}^6}{r_{ij}^{7}}\right) \frac{\vec{r}_{ij}}{r_{ij}} \\
&= \frac{24\varepsilon_{ij}}{r_{ij}^2} \left[2\left(\frac{\sigma_{ij}^{6}}{r_{ij}^{6}}\right)^2 -\left(\frac{\sigma_{ij}}{r_{ij}}\right)^{6}\right] \vec{r}_{ij}
\end{aligned}
```
"""
struct LennardJones{S, C, W, F, E} <: GeneralInteraction
    cutoff::C
    nl_only::Bool
    lorentz_mixing::Bool
    weight_14::W
    force_unit::F
    energy_unit::E
end

function LennardJones(;
                        cutoff=NoCutoff(),
                        nl_only=false,
                        lorentz_mixing=true,
                        weight_14=1.0,
                        force_unit=u"kJ * mol^-1 * nm^-1",
                        energy_unit=u"kJ * mol^-1",
                        skip_shortcut=false)
    return LennardJones{skip_shortcut, typeof(cutoff), typeof(weight_14), typeof(force_unit), typeof(energy_unit)}(
        cutoff, nl_only, lorentz_mixing, weight_14, force_unit, energy_unit)
end

@inline @inbounds function force(inter::LennardJones{S, C},
                                    coord_i,
                                    coord_j,
                                    atom_i,
                                    atom_j,
                                    box_size,
                                    weight_14::Bool=false) where {S, C}
    dr = vector(coord_i, coord_j, box_size)
    r2 = sum(abs2, dr)

    if !S && iszero(atom_i.σ) || iszero(atom_j.σ)
        return ustrip.(zero(coord_i)) * inter.force_unit
    end

    # Lorentz-Berthelot mixing rules use the arithmetic average for σ
    # Otherwise use the geometric average
    σ = inter.lorentz_mixing ? (atom_i.σ + atom_j.σ) / 2 : sqrt(atom_i.σ * atom_j.σ)
    ϵ = sqrt(atom_i.ϵ * atom_j.ϵ)

    cutoff = inter.cutoff
    σ2 = σ^2
    params = (σ2, ϵ)

    if cutoff_points(C) == 0
        f = force_divr_nocutoff(inter, r2, inv(r2), params)
    elseif cutoff_points(C) == 1
        r2 > cutoff.sqdist_cutoff && return ustrip.(zero(coord_i)) * inter.force_unit

        f = force_divr_cutoff(cutoff, r2, inter, params)
    elseif cutoff_points(C) == 2
        r2 > cutoff.sqdist_cutoff && return ustrip.(zero(coord_i)) * inter.force_unit

        if r2 < cutoff.activation_dist
            f = force_divr_nocutoff(inter, r2, inv(r2), params)
        else
            f = force_divr_cutoff(cutoff, r2, inter, params)
        end
    end

    if weight_14
        return f * dr * inter.weight_14
    else
        return f * dr
    end
end

@fastmath function force_divr_nocutoff(::LennardJones, r2, invr2, (σ2, ϵ))
    six_term = (σ2 * invr2) ^ 3

    return (24ϵ * invr2) * (2 * six_term ^ 2 - six_term)
end

@inline @inbounds function potential_energy(inter::LennardJones{S, C},
                                            s::System,
                                            i::Integer,
                                            j::Integer,
                                            weight_14::Bool=false) where {S, C}
    dr = vector(s.coords[i], s.coords[j], s.box_size)
    r2 = sum(abs2, dr)

    if !S && iszero(s.atoms[i].σ) || iszero(s.atoms[j].σ)
        return ustrip(zero(s.box_size[1])) * inter.energy_unit
    end

    σ = inter.lorentz_mixing ? (s.atoms[i].σ + s.atoms[j].σ) / 2 : sqrt(s.atoms[i].σ * s.atoms[j].σ)
    ϵ = sqrt(s.atoms[i].ϵ * s.atoms[j].ϵ)

    cutoff = inter.cutoff
    σ2 = σ^2
    params = (σ2, ϵ)

    if cutoff_points(C) == 0
        pe = potential(inter, r2, inv(r2), params)
    elseif cutoff_points(C) == 1
        r2 > cutoff.sqdist_cutoff && return ustrip(zero(s.box_size[1])) * inter.energy_unit

        pe = potential_cutoff(cutoff, r2, inter, params)
    elseif cutoff_points(C) == 2
        r2 > cutoff.sqdist_cutoff && return ustrip(zero(s.box_size[1])) * inter.energy_unit

        if r2 < cutoff.activation_dist
            pe = potential(inter, r2, inv(r2), params)
        else
            pe = potential_cutoff(cutoff, r2, inter, params)
        end
    end

    if weight_14
        return pe * inter.weight_14
    else
        return pe
    end
end

@fastmath function potential(::LennardJones, r2, invr2, (σ2, ϵ))
    six_term = (σ2 * invr2) ^ 3

    return 4ϵ * (six_term ^ 2 - six_term)
end


struct Vars
    t::Num
    θ::Vector{Num}
    ϕ::Vector{Num}
end

Vars(N) = Vars((@variables t, θ[1:N](t), ϕ[1:N](t))...)

struct RHS
    θ::Vector{Num}
    ϕ::Vector{Num}
end

Base.:+(x::RHS, y::RHS) = RHS(x.θ .+ y.θ, x.ϕ .+ y.ϕ)

function effectiveField(field::Array, vars::Vars, α)
    # field is Nx3 dimensional, given in cartesian coordinates
    N = length(vars.ϕ)
    ϕres = Vector{Num}(undef, N)
    θres = Vector{Num}(undef, N)
    for i in 1:N
        hx, hy, hz = field[i,:]
        θ = vars.θ[i]
        ϕ = vars.ϕ[i]
        ϕres[i] = hz + cos(ϕ) * (-(hx * cot(θ)) + hy * α * csc(θ)) - hy * cot(θ) * sin(ϕ) - hx * α * csc(θ) * sin(ϕ)
        θres[i] = (hy + hx * α * cos(θ)) * cos(ϕ) - hz * α * sin(θ) + (-hx + hy * α * cos(θ)) * sin(ϕ)
    end
    RHS(θres, ϕres)
end

function spinTorque(torque::Array, vars::Vars)
    # takes as input an array
    N = length(vars.ϕ)
    ϕres = Vector{Num}(undef, N)
    θres = Vector{Num}(undef, N)
    for i in 1:N
        ϕ = vars.ϕ[i]
        θ = vars.θ[i]
        ϕres[i] = csc(θ) * (torque[i,1] * sin(ϕ) - torque[i,2] * cos(ϕ))
        θres[i] = torque[i,3] * sin(θ) - cos(θ) * (torque[i,1] * cos(ϕ) + torque[i,2] * sin(ϕ))
    end
    return RHS(θres, ϕres)
end

function spinTorque(torquefunc::Function, vars::Vars)
    # takes as input an function with as parameter Particle and should return a 3D array
    mapping = mappingAtoms(unit_cells)
    N = length(mapping)
    res = zeros(N, 3)
    for (i, p) in mapping
        res[i, :] = torquefunc(p)
    end
    spintorque(res, vars)
end

function exchangeCoupling(coupling::Matrix, vars::Vars)
    # coupling is a NxN array which describes which sites couple to which
    # returns two arrays with θ and ϕ right hand sides of the EOM
    ϕres = Vector{Num}(undef, size(coupling)[1])
    θres = Vector{Num}(undef, size(coupling)[1])
    for i in 1:size(coupling)[1]
        tempθ = 0
        tempϕ = 0
        ϕi = vars.ϕ[i]
        θi = vars.θ[i]
        for j in 1:size(coupling)[2]
            ϕj = vars.ϕ[j]
            θj = vars.θ[j]
            tempθ += -coupling[i,j] * sin(θj) * sin(θi - θj)
            tempϕ += coupling[i,j] * (
                cos(θj) - cos(ϕi - ϕj) * cot(θi) * sin(θj)
            )
        end
        θres[i] = tempθ
        ϕres[i] = tempϕ
    end
    RHS(θres, ϕres)
end

function DMICoupling(coupling::Array, DMI::Array, vars::Vars)
    # coupling is a NxN array which describes which sites couple to which
    # DMI is a 3D vector that describes the DMI field
    # returns two arrays with θ and ϕ right hand sides of the EOM
    ϕres = Vector{Num}(undef, size(coupling)[1])
    θres = Vector{Num}(undef, size(coupling)[1])
    Dx, Dy, Dz = DMI
    for i in 1:size(coupling)[1]
        tempθ = 0
        tempϕ = 0
        ϕi = vars.ϕ[i]
        θi = vars.θ[i]
        for j in 1:size(coupling)[2]
            ϕj = vars.ϕ[j]
            θj = vars.θ[j]

            tempθ += coupling[i,j] * (
                Dz * cos(ϕi - ϕj) * sin(θj) - cos(θj) * (Dx * cos(ϕi) + Dy * sin(ϕi))
            )
            tempϕ += coupling[i,j] * (
                -(cos(θj) * cot(θi) * (Dy * cos(ϕi) - Dx * sin(ϕi))) +
                -  sin(θj) * (-(cos(ϕj) * (Dy + Dz * cot(θi) * sin(ϕi))) + (Dx + Dz * cos(ϕi) * cot(θi)) * sin(ϕj))
                )
        end
        θres[i] = tempθ
        ϕres[i] = tempϕ
    end
    RHS(θres, ϕres)
end

function setupEOM(vars::Vars, rhs::RHS)
    D = Differential(vars.t)
    [D.(vars.θ) .~ rhs.θ; D.(vars.ϕ) .~ rhs.ϕ]
end

function initBoltzmann(vars::Vars, λ)
    # boltzmann is proportional to θ (for now)
    # the angle ϕ is chosen randomly between 0 and 2pi
    N = length(vars.θ)
    dist = Exponential(λ / 2)
    init_θ = rand(dist, N)
    init_ϕ = rand(Float64, N) * 2π
    return [[vars.θ[i] => init_θ[i] for i in 1:N]; [vars.ϕ[i] => init_ϕ[i] for i in 1:N]]
end


@testset "Geometric product" begin
    @test A * (B + C) ≈ A * B + A * C
    @test (B + C) * A ≈ B * A + C * A
    @test A * 1 == A
    @test A + (-A) ≈ zero(typeof(A))

    @test 1v1 * 1v3 == 1v13
    @test 1v3 * 1v1 == -1v13
    @test 1v3 * 1v2 * 1v1 == -1v123
    @test (1v1 + 2v2 + 1v12) * (1v1 + 1v3) == 1v + 1v13 - 2v12 + 2v23 - 1v2 + 1v123

    for r ∈ 0:4
        Aᵣ = As[r+1]
        for s ∈ 0:4
            Bₛ = Bs[s+1]
            res = Aᵣ * Bₛ
            contribs = map(k -> grade_projection(res, Val(abs(r - s) + 2k)), 0:(r + s - abs(r - s)) ÷ 2)
            @test res ≈ sum(contribs)
        end
    end
end


using Plots

function gaussian(x; μ, σ)
    y= @. exp(-(x-μ)^2/2)/sqrt(2π)/sigma
end

function p(y;x,a,b)
    return gaussian(y, μ=a*sin(bx), σ=1)
end

function main()
    a=1.
    b=1.

    #function body
end


@doc raw"""
    Models.Eggbox()

Eggbox/Egg carton likelihood function

```math
z(x, y) = \left[a + \cos\frac{x}{b} \cdot \cos\frac{x}{b} \right]^5
```

# Examples
```jldoctest
julia> model, lnZ = Models.Eggbox();

julia> lnZ
235.88
```
"""
function Eggbox()
    tmax = 5π

    # uniform prior from 0, 1
    prior(X) = X
    function loglike(X)
        a = cos(tmax * (2 * first(X) - 1) / 2)
        b = cos(tmax * (2 * last(X) - 1) / 2)
        return (2 + a * b)^5
    end

    lnZ = 235.88 # where do we get this from??
    return NestedModel(loglike, prior), lnZ
end


# This file is part of the ValidatedNumerics.jl package; MIT licensed

## Definitions of special intervals and associated functions

## Empty interval:
doc"""`emptyinterval`s are represented as the interval [∞, -∞]; note
that this interval is an exception to the fact that the lower bound is
larger than the upper one."""
emptyinterval{T<:Real}(::Type{T}) = Interval{T}(Inf, -Inf)
emptyinterval{T<:Real}(x::Interval{T}) = emptyinterval(T)
emptyinterval() = emptyinterval(precision(Interval)[1])
const ∅ = emptyinterval(Float64)

isempty(x::Interval) = x.lo == Inf && x.hi == -Inf

const ∞ = Inf

## Entire interval:
doc"""`entireinterval`s represent the whole Real line: [-∞, ∞]."""
entireinterval{T<:Real}(::Type{T}) = Interval{T}(-Inf, Inf)
entireinterval{T<:Real}(x::Interval{T}) = entireinterval(T)
entireinterval() = entireinterval(precision(Interval)[1])

isentire(x::Interval) = x.lo == -Inf && x.hi == Inf
isunbounded(x::Interval) = x.lo == -Inf || x.hi == Inf

# NaI: not-an-interval
doc"""`NaI` not-an-interval: [NaN, NaN]."""
nai{T<:Real}(::Type{T}) = Interval{T}(convert(T, NaN), convert(T, NaN))
nai{T<:Real}(x::Interval{T}) = nai(T)
nai() = nai(precision(Interval)[1])

isnai(x::Interval) = isnan(x.lo) || isnan(x.hi)

isfinite(x::Interval) = isfinite(x.lo) && isfinite(x.hi)
isnan(x::Interval) = isnai(x)

doc"""`isthin(x)` corresponds to `isSingleton`, i.e. it checks if `x` is the set consisting of a single exactly representable float. Thus any float which is not exactly representable does *not* yield a thin interval."""
function isthin(x::Interval)
    # (m = mid(x); m == x.lo || m == x.hi)
    x.lo == x.hi
end

doc"`iscommon(x)` checks if `x` is a **common interval**, i.e. a non-empty, bounded, real interval."
function iscommon(a::Interval)
    (isentire(a) || isempty(a) || isnai(a) || isunbounded(a)) && return false
    true
end

doc"`widen(x)` widens the lowest and highest bounds of `x` to the previous and next representable floating-point numbers, respectively."
widen{T<:AbstractFloat}(x::Interval{T}) = Interval(prevfloat(x.lo), nextfloat(x.hi))


"""
	function build_mlp(idim::Int, hdim::Int, odim::Int, nlayers::Int; activation::String="relu", lastlayer::String="")
	Creates a chain with `nlayers` layers of `hdim` neurons with transfer function `activation`.
	input and output dimension is `idim` / `odim`
	If lastlayer is no specified, all layers use the same function.
	If lastlayer is "linear", then the last layer is forced to be Dense.
	It is also possible to specify dimensions in a vector.

```juliadoctest
julia> build_mlp(4, 11, 1, 3, activation="relu")
Chain(Dense(4, 11, relu), Dense(11, 11, relu), Dense(11, 1, relu))

julia> build_mlp([4, 11, 11, 1], activation="relu")
Chain(Dense(4, 11, relu), Dense(11, 11, relu), Dense(11, 1, relu))

julia> build_mlp(4, 11, 1, 3, activation="relu", lastlayer="tanh")
Chain(Dense(4, 11, relu), Dense(11, 11, relu), Dense(11, 1, tanh))
```
"""
build_mlp(ks::Vector{Int}, fs::Vector) = Flux.Chain(map(i -> Dense(i[2],i[3],i[1]), zip(fs,ks[1:end-1],ks[2:end]))...)

build_mlp(idim::Int, hdim::Int, odim::Int, nlayers::Int; kwargs...) =
	build_mlp(vcat(idim, fill(hdim, nlayers-1)..., odim); kwargs...)

function build_mlp(ks::Vector{Int}; activation::String = "relu", lastlayer::String = "")
	activation = (activation == "linear") ? "identity" : activation
	fs = Array{Any}(fill(eval(:($(Symbol(activation)))), length(ks) - 1))
	if !isempty(lastlayer)
		fs[end] = (lastlayer == "linear") ? identity : eval(:($(Symbol(lastlayer))))
	end
	build_mlp(ks, fs)
end

"""
	vec_size(conv_dims, scalings, channel)

Computes the size of vectorized ouput, given size of input, convolutional scalings and last channel.
"""
function vec_size(conv_dims, scalings, channel)
	ho = conv_dims[1]/(reduce(*, scalings)) # height before reshaping
	wo = conv_dims[2]/(reduce(*, scalings)) # width before reshaping
	# this ensures size compatibility, ho/wo are Ints
	(ho == floor(Int, ho)) ? ho = floor(Int, ho) : error("your input size and scaling is not compatible")
	(wo == floor(Int, wo)) ? wo = floor(Int, wo) : error("your input size and scaling is not compatible")
	ho*wo*channel, ho, wo
end

# these two functions are taken from GMExtensions.jl, but I wanted to avoid the dependency since
# it is an unregistered package
# TODO: maybe use this in GANomaly constructor?
"""
	conv_encoder(idim, zdim, kernelsizes, channels, scalings[; activation, densedims])

Constructs a convolutional encoder.

# Arguments
	- `idim`: size of input - (h,w,c)
	- `zdim`: latent space dimension
	- `kernelsizes`: kernelsizes for conv. layers (only odd numbers are allowed)
	- `channels`: channel numbers
	- `scalings`: scalings vector
	- `activation`: default relu
	- `densedims`: if set, more than one dense layers are used

# Example
```julia-repl
julia> encoder = conv_encoder((64, 48, 1), 2, (3, 5, 5), (2, 4, 8), (2, 2, 2), densedims = (256,))
Chain(Conv((3, 3), 1=>2, relu), MaxPool((2, 2), pad = (0, 0, 0, 0), stride = (2, 2)), Conv((5, 5), 2=>4, relu), MaxPool((2, 2), pad = (0, 0, 0, 0), stride = (2, 2)), Conv((5, 5), 4=>8, relu), MaxPool((2, 2), pad = (0, 0, 0, 0), stride = (2, 2)), #44, Dense(384, 2))
julia> encoder(randn(Float32, 64, 48, 1, 2))
2×2 Array{Float32,2}:
  0.247844   0.133781
 -0.605763  -0.494911
```
"""
function conv_encoder(idim::Union{Tuple, Vector}, zdim::Int, kernelsizes::Union{Tuple, Vector},
	channels::Union{Tuple, Vector}, scalings::Union{Tuple, Vector};
	activation = "relu", densedims::Union{Tuple, Vector} = [], batchnorm=false)
	nconv = length(kernelsizes)
	(nconv == length(channels) == length(scalings)) ? nothing : error("incompatible input dimensions")
	(length(idim) == 3) ? nothing : error("idim must be (h, w, c)")
	# also check that kernelsizes are all odd numbers
	(all(kernelsizes .% 2 .== 1)) ? nothing : error("not implemented for odd kernelsizes")

	# string to function
	actf = eval(:($(Symbol(activation))))

	# initialize dimensions
	cins = vcat(idim[3], channels[1:end-1]...) # channels in
	couts = channels # channels out
	din, _, _ = vec_size(idim, scalings, channels[end]) # the size of vectorized output after all convolutions

	# now build a vector of layers to be used later
	layers = Array{Any,1}()
	# first add the convolutional and maxpooling layers
	for (k, ci, co, s) in zip(kernelsizes, cins, couts, scalings)
		pad = Int((k-1)/2)
		# paddding so that input and output size are same
		push!(layers, Conv((k,k), ci=>co, actf; pad = (pad, pad)))
		push!(layers, MaxPool((s,s)))
		# generally, bn is not recommended for training vaes
		batchnorm ? push!(layers, BatchNorm(co)) : nothing
	end

	# reshape
	push!(layers, x -> reshape(x, din, :))

	# and dense layers
	ndense = length(densedims)
	dins = vcat(din, densedims...)
	douts = vcat(densedims..., zdim)
	dacts = vcat([actf for _ in 1:ndense]..., identity)
	for (_di, _do, _da) in zip(dins, douts, dacts)
		push!(layers, Dense(_di, _do, _da))
	end

	Flux.Chain(layers...)
end

"""
	conv_decoder(odim, zdim, kernelsizes, channels, scalings[; activation, densedims, vec_output,
		vec_output_dim])

Constructs a convolutional encoder.

# Arguments
- `odim`: size of outnput - (h,w,c)
- `zdim`: latent space dimension
- `kernelsizes`: kernelsizes for conv. layers (only odd numbers are allowed)
- `channels`: channel numbers
- `scalings`: scalings vector
- `activation`: default relu
- `densedims`: if set, more than one dense layers are used
- `vec_output`: output is vectorized (default false)
- `vec_output_dim`: determine what the final size of the vectorized input should be (e.g. add extra dimensions for variance estimation)

# Example
```julia-repl
julia> decoder = conv_decoder((64, 48, 1), 2, (5, 5, 3), (8, 4, 2), (2, 2, 2); densedims = (256,))
Chain(Dense(2, 256, relu), Dense(256, 384, relu), #19, ConvTranspose((2, 2), 8=>8, relu), Conv((5, 5), 8=>4, relu), ConvTranspose((2, 2), 4=>4, relu), Conv((5, 5), 4=>2, relu), ConvTranspose((2, 2), 2=>2, relu), Conv((3, 3), 2=>1))
julia> y = decoder(randn(Float32, 2, 2));
julia> size(y)
(64, 48, 1, 2)
```
"""
function conv_decoder(odim::Union{Tuple, Vector}, zdim::Int, kernelsizes::Union{Tuple, Vector},
	channels::Union{Tuple, Vector}, scalings::Union{Tuple, Vector};
	activation = relu, densedims::Union{Tuple, Vector} = [],
	vec_output = false, vec_output_dim = nothing, batchnorm=false)
	nconv = length(kernelsizes)
	(nconv == length(channels) == length(scalings)) ? nothing : error("incompatible input dimensions")
	(length(odim) == 3) ? nothing : error("odim must be (h, w, c)")
	# also check that kernelsizes are all odd numbers
	(all(kernelsizes .% 2 .== 1)) ? nothing : error("not implemented for odd kernelsizes")

	# string to function
	actf = eval(:($(Symbol(activation))))

	# initialize some stuff
	cins = channels # channels in
	couts = vcat(channels[2:end]..., odim[3]) # channels out
	dout, ho, wo = vec_size(odim, scalings, channels[1]) # size of vectorized output after convolutions

	# now build a vector of layers to be used later
	layers = Array{Any,1}()

	# start with dense layers
	ndense = length(densedims)
	dins = vcat(zdim, densedims...)
	douts = vcat(densedims..., dout)
	for (_di, _do) in zip(dins, douts)
		push!(layers, Dense(_di, _do, actf))
	end

	# reshape
	push!(layers, x -> reshape(x, ho, wo, channels[1], :))

	# add the transpose and convolutional layers
	acts = vcat([actf for _ in 1:nconv-1]..., identity)
	for (k, ci, co, s, act) in zip(kernelsizes, cins, couts, scalings, acts)
		pad = Int((k-1)/2)
		# use convtranspose for upscaling - there are other posibilities, however this seems to be ok
		push!(layers, ConvTranspose((s,s), ci=>ci, actf, stride = (s,s)))
		push!(layers, Conv((k,k), ci=>co, act; pad = (pad, pad)))
		# generally, bn is not recommended for training vaes
		batchnorm ? push!(layers, BatchNorm(co)) : nothing
	end

	# if you want vectorized output
	if vec_output
		push!(layers, x -> reshape(x, reduce(*, odim), :))
		# if you want the final vector to be a different size than the output
		# (e.g. you want extra dimensions for variance estimation)
		(vec_output_dim == nothing) ? nothing : push!(layers, Dense(reduce(*, odim), vec_output_dim))
	end

	Flux.Chain(layers...)
end


module Diagram

abstract type halfEdge end # this hack is currently necessary for definition of circular types in Julia, see https://github.com/JuliaLang/julia/issues/269

mutable struct Region
   generator::Tuple{Real, Real}
   borderHead::Union{halfEdge, Nothing} # first half edge of the border; we can traverse the border with he = he.next;
end

mutable struct HalfEdge <: halfEdge
   origin::Union{Tuple{Real, Real}, Nothing}

   isFixed::Bool

   twin::Union{HalfEdge, Nothing}
   next::Union{HalfEdge, Nothing}
   prev::Union{HalfEdge, Nothing}

   generator::Tuple{Real, Real}
end

function makeTwins(a::HalfEdge, b::HalfEdge)
   a.twin = b
   b.twin = a
end

function concat(a::HalfEdge, b::HalfEdge)
   a.next = b
   b.prev = a
end

mutable struct DCEL # Doubly-connected edge list
   regions::Array{Region}
   halfEdges::Array{HalfEdge}
end

function DCEL(points::Array{Tuple{Real, Real}, 1})
	n = size(points)[1]

   regions = Array{Region}(undef, 1, n)
   for i in 1:n
      regions[i] = Region(points[i], nothing)
   end

   halfEdges = HalfEdge[]

	return DCEL(regions, halfEdges)
end

function borderCoordinates(f::Region)
   x = []
   y = []

   if f.borderHead == nothing
      return x, y
   end

   he = f.borderHead

   if he.origin != nothing
      append!(x, he.origin[1])
      append!(y, he.origin[2])
   end
   he = he.next

   i = 0
   while he != nothing && he != f.borderHead
      if he.origin != nothing
         append!(x, he.origin[1])
         append!(y, he.origin[2])
      end
      he = he.next
      i += 1
   end

   return x, y
end

function regionBorders(V::DCEL)
   borders = []

   for region in V.regions
      append!(borders, [borderCoordinates(region)])
   end

   return borders
end

function centroidAndArea(region::Region)
   A = Cx = Cy = 0

   he = region.borderHead
   while true
      shoelace = he.origin[1]*he.next.origin[2] - he.origin[2]*he.next.origin[1]
      A += shoelace
      Cx += (he.origin[1] + he.next.origin[1]) * shoelace
      Cy += (he.origin[2] + he.next.origin[2]) * shoelace

      he = he.next
      if he == region.borderHead
         break
      end
   end

   A /= 2
   Cx /= 6*A
   Cy /= 6*A

   return ((Cx, Cy), A)
end

function centroidsAndAreas(V::DCEL)
   centroids = Array{Tuple{Real, Real}, 1}([])
   areas = Array{Real, 1}([])
   for region in V.regions
      res = centroidAndArea(region)
      push!(centroids, (res[1]))
      push!(areas, (res[2]))
   end

   return centroids, areas
end

end # module


# Graph to matrices

using Graphs
using Test
using SparseArrays

# data

global eds = [Edge(1,1,2), Edge(2,1,3), Edge(3,2,4), Edge(4,3,4)]

global a0 = fill(false, 4, 4)
global a0[1, 2] = a0[1, 3] = a0[2, 4] = a0[3, 4] = true

global a0u = copy(a0)
global a0u[2, 1] = a0u[3, 1] = a0u[4, 2] = a0u[4, 3] = true

global a0_s = sparse(a0)
global a0u_s = sparse(a0u)

# graphs

global gd_elst = simple_edgelist(4, eds)
global gu_elst = simple_edgelist(4, eds; is_directed=false)

global gd_ilst = simple_inclist(4)
global gu_ilst = simple_inclist(4, is_directed=false)

let
for ed in eds
  add_edge!(gd_ilst, ed.source, ed.target)
  add_edge!(gu_ilst, ed.source, ed.target)
end
end

# adjacency matrix

@test adjacency_matrix(gd_elst) == a0
@test adjacency_matrix(gu_elst) == a0u

@test adjacency_matrix_sparse(gd_elst) == a0_s
@test adjacency_matrix_sparse(gu_elst) == a0u_s

@test adjacency_matrix(gd_ilst) == a0
@test adjacency_matrix(gu_ilst) == a0u

@test adjacency_matrix_sparse(gd_ilst) == a0_s
@test adjacency_matrix_sparse(gu_ilst) == a0u_s

# weight matrix

global eweights = [1., 2., 3., 4.]
global wm0  = [0. 1. 2. 0.; 0. 0. 0. 3.; 0. 0. 0. 4.; 0. 0. 0. 0.]
global wm0u = [0. 1. 2. 0.; 1. 0. 0. 3.; 2. 0. 0. 4.; 0. 3. 4. 0.]
global wm0_s = sparse(wm0)
global wm0u_s = sparse(wm0u)

@test weight_matrix(gd_elst, eweights) == wm0
@test weight_matrix(gu_elst, eweights) == wm0u

@test weight_matrix_sparse(gd_elst, eweights) == wm0_s
@test weight_matrix_sparse(gu_elst, eweights) == wm0u_s

@test weight_matrix(gd_ilst, eweights) == wm0
@test weight_matrix(gu_ilst, eweights) == wm0u

@test weight_matrix_sparse(gd_ilst, eweights) == wm0_s
@test weight_matrix_sparse(gu_ilst, eweights) == wm0u_s

# distance matrix

global dm0  = [0. 1. 2. Inf; Inf 0. Inf 3.; Inf Inf 0. 4.; Inf Inf Inf 0.]
global dm0u = [0. 1. 2. Inf; 1. 0. Inf 3.; 2. Inf 0. 4.; Inf 3. 4. 0.]

@test distance_matrix(gd_elst, eweights) == dm0
@test distance_matrix(gu_elst, eweights) == dm0u

@test distance_matrix(gd_ilst, eweights) == dm0
@test distance_matrix(gu_ilst, eweights) == dm0u

# Laplacian matrix

global L0 = [2. -1. -1. 0.; -1. 2. 0. -1.; -1. 0. 2. -1.; 0. -1. -1. 2.]
global L0_s = sparse(L0)

@test laplacian_matrix(gu_elst) == L0
@test laplacian_matrix(gu_ilst) == L0

@test laplacian_matrix_sparse(gu_elst) == L0_s
@test laplacian_matrix_sparse(gu_ilst) == L0_s

global Lw = [3. -1. -2. 0.; -1. 4. 0. -3.; -2. 0. 6. -4.; 0. -3. -4. 7.]
global Lw_s = sparse(Lw)

@test laplacian_matrix(gu_elst, eweights) == Lw
@test laplacian_matrix(gu_ilst, eweights) == Lw

@test laplacian_matrix_sparse(gu_elst, eweights) == Lw_s
@test laplacian_matrix_sparse(gu_ilst, eweights) == Lw_s


"""
Internal data structure to partition a box

`domain`:       box defining the entire domain

`left`:         leftmost / bottom edge of the domain

`scale`:        1 / diameter of each box in the new partition (componentwise)

`dimsprod`:     for indexing the partition:
                key is counted up in first dimension first, then second dimension, etc... ie

                 1st dim →
                  * — * — * — *
            2nd   | 1 | 2 | 3 |
            dim   * — * — * — *
            ↓     | 4 | 5 | 6 |
                  * — * — * — *
.
"""
struct BoxPartition{N,T} <: AbstractBoxPartition{Box{N,T}}
    domain::Box{N,T}
    left::SVector{N,T}
    scale::SVector{N,T}
    dims::SVector{N,Int}
    dimsprod::SVector{N,Int}
end

function BoxPartition(domain::Box{N,T}, dims::NTuple{N,Int}) where {N,T}
    dims = SVector{N,Int}(dims)
    left = domain.center .- domain.radius
    scale = dims ./ (2*domain.radius)
    # nr. of boxes / diameter of the domain == 1 / diameter of each box
    dimsprod_ = [SVector(1); cumprod(dims)]
    dimsprod = dimsprod_[SOneTo(N)]

    return BoxPartition(domain, left, scale, dims, dimsprod)
end

function BoxPartition(domain::Box{N,T}) where {N,T}
    dims = tuple(ones(Int64,N)...)
    BoxPartition(domain, dims)
end

BoxPartition(domain::Box{1,T}, dims::Int) where {T} = BoxPartition(domain, (dims,))

dimension(partition::BoxPartition{N,T}) where {N,T} = N

function subdivide(P::BoxPartition{N,T}, dim::Int) where {N,T}
    new_dims = ntuple(i -> P.dims[i]*(i==dim ? 2 : 1), N)
    return BoxPartition(P.domain, new_dims)
end

keytype(::Type{<:BoxPartition}) = Int
keys_all(partition::BoxPartition) = 1:prod(partition.dims)
# == 1 : partition.dimsprod[end] * partition.dims[end]

Base.size(partition::BoxPartition) = partition.dims.data # .data returns as tuple

function Base.show(io::IO, partition::BoxPartition)
    print(io, "$(partition.dims[1])")
    for k = 2:length(partition.left)
        print(io, " x $(partition.dims[k])")
    end
    print(io, " BoxPartition")
end

# TODO: replace with overloaded getindex
function key_to_box(partition::BoxPartition{N,T}, key::M) where M <: Union{Int, NTuple{N, Int}} where {N,T}
    dims = size(partition)
    radius = partition.domain.radius ./ dims
    left = partition.domain.center .- partition.domain.radius
    center = left .+ radius .+ (2 .* radius) .* (CartesianIndices(dims)[key].I .- 1)
    # start at leftmost box in the partition and move $key boxes right
    return Box(center, radius)
end

function unsafe_point_to_ints(partition::BoxPartition, point)
    x = (point .- partition.left) .* partition.scale
    # counts how many boxes x is away from left (componentwise)
    return map(xi -> Base.unsafe_trunc(Int, xi), x)
end

function point_to_key(partition::BoxPartition, point)
    x_ints = unsafe_point_to_ints(partition, point)

    if any(x_ints .< zero(eltype(x_ints))) || any(x_ints .>= partition.dims)
        @debug "point does not lie in the domain" point partition.domain
        return nothing
    end

    return sum(x_ints .* partition.dimsprod) + 1
end


prepare_homotopy(H::AbstractHomotopy, alg::APCA{true}) = homogenize(H)
prepare_homotopy(H::AbstractHomotopy, alg::APCA{false}) = H

"""
    prepare_startvalue(H, startvalue, ::AbstractPredictorCorrectorAlgorithm{true})

Embeds a vector into the projective space if necessary, i.e. if it's length is one less
than the number of variables of `H`. `H` is assumed to be homogenized. After the (eventual)
embedding the value is normalized.
"""
function prepare_startvalue(H::AbstractHomotopy, startvalue, ::APCA{true})
    N = nvariables(H)
    n = length(startvalue)
    if N == n
        normalize(startvalue)
    elseif N - 1 == n
        normalize!([1; startvalue])
    else
        error("A start value has length $n. Excepted length $N or $(N-1).")
    end
end

function prepare_startvalue(H::AbstractHomotopy, startvalue, ::APCA{false})
    N = nvariables(H)
    n = length(startvalue)
    if N == n
        return startvalue
    else
        return error("A start_value has length $n. Excepted length $N.")
    end
end


using OneHot, Test, Random
using OneHot: argmax_

@testset "argmax_" begin
    @inferred argmax_(randn(3,2,4); dims=2)
    @test size(argmax_(randn(3,2,4); dims=2)) == (3,4)
    A = [3 4; 1 2]
    i = argmax_(A; dims=2)
    @test A[i] == [4, 2]
end

@testset "decode" begin
    A = randn(5,5,4)
    cl = OneHot.decode(A)
    for I in CartesianIndices(cl)
        @test maximum(A[:,I]) == A[cl[I],I]
    end
    @test size(cl) == size(A)[2:end]

    X = OneHotArray(rand(1:4,5,5), 4)
    cl = OneHot.decode(X)
    for I in CartesianIndices(cl)
        @test maximum(X[:,I]) == X[cl[I],I]
    end
end

q = 10
A = rand(1:q, 4,5,6)
X = @inferred OneHot.encode(A, 1:q)
@test size(X) == (q, size(A)...)
@test vec(X) == [X[i] for i=1:length(X)]
for i in CartesianIndices(A), a = 1:q
    @test X[a,i] == (A[i] == a)
end
@inferred OneHot.decode(X)
@test OneHot.decode(X) == A
@test OneHot.decode(Array(X)) == A
@inferred OneHot.decode(Array(X))


type ActiveSet
  indexes::Vector{Int64}
  numActive::Int64
end

function ActiveSet{T<:AbstractFloat}(
    x::StridedVector{T};
    options::LassoOptions = LassoOptions()
    )
  zero_thr = options.zerothr
  numElem = length(x)
  activeset = [1:numElem;]
  numActive = 0
  @inbounds for j = 1:numElem
    if abs(x[j]) >= zero_thr
      numActive += 1
      activeset[numActive], activeset[j] = activeset[j], activeset[numActive]
    end
  end
  ActiveSet(activeset, numActive)
end

function _add_violator!{T<:AbstractFloat}(
    activeset::ActiveSet,
    x::StridedVector{T},
    A::StridedMatrix{T},
    b::StridedVector{T},
    λ::StridedVector{T};
    options::LassoOptions   =   LassoOptions()
    )
  changed = false

  zerothr = options.zerothr
  numElem = length(x)
  numActive = activeset.numActive
  indexes = activeset.indexes
  # check for things to be removed from the active set
  i = 0
  @inbounds while i < numActive
    i = i + 1
    t = indexes[i]
    if abs(x[t]) < zerothr
      x[t] = zero(T)
      changed = true
      indexes[numActive], indexes[i] = indexes[i], indexes[numActive]
      numActive -= 1
      i = i - 1
    end
  end
  activeset.numActive = numActive

  I = 0
  V = zero(T)
  @inbounds for i=numActive+1:numElem
    t = indexes[i]
    tmp = _Axk(A, x, t, activeset) + b[t]
    nV = abs(tmp) - λ[t]
    if V < nV
      I = i
      V = nV
    end
  end
  if I > 0 && V > options.gradtol
    changed = true
    numActive += 1
    indexes[numActive], indexes[I] = indexes[I], indexes[numActive]
  end
  activeset.numActive = numActive
  changed
end

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

shrink{T<:AbstractFloat}(v::T, c::T) = v > c ? v - c : (v < -c ? v + c : zero(T))

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

# computes x'A[:, j]
function _Axk{T<:AbstractFloat}(A::StridedMatrix{T}, x::StridedVector{T}, j::Int64, activeset::ActiveSet)
  s = zero(T)
  indexes = activeset.indexes
  @inbounds for i=1:activeset.numActive
    ind = indexes[i]
    s += x[ind] * A[ind, j]
  end
  s
end

function _lasso!{T<:AbstractFloat}(
    x::StridedVector{T},
    A::StridedMatrix{T},
    b::StridedVector{T},
    λ::StridedVector{T},
    activeset::ActiveSet;
    options::LassoOptions   =   LassoOptions()
    )

  indexes = activeset.indexes
  iter = 0
  while true
    iter += 1
    maxUpdate = zero(T)
    @inbounds for i=1:activeset.numActive
      j = indexes[i]
      a = A[j, j]
      S0 = _Axk(A, x, j, activeset) - a * x[j] + b[j]
      newValue = shrink(-S0 / a, abs(λ[j] / a))
      if abs(x[j] - newValue) > maxUpdate
        maxUpdate = abs(x[j] - newValue)
      end
      x[j] = newValue
    end
    if iter > options.max_inner_iter || maxUpdate < options.xtol
      break
    end
  end
  x
end

lasso!{T<:AbstractFloat}(
    x::StridedVector{T},
    A::StridedMatrix{T},
    b::StridedVector{T},
    λ::T;
    options::LassoOptions   =   LassoOptions()
    ) = lasso!(x, A, b, fill(λ, length(x)); options=options)

function lasso!{T<:AbstractFloat}(
    x::StridedVector{T},
    A::StridedMatrix{T},
    b::StridedVector{T},
    λ::StridedVector{T};
    options::LassoOptions   =   LassoOptions()
    )

  maxoutiter = options.max_outer_iter
  activeset = ActiveSet(x; options=options)
  if activeset.numActive == 0
    _add_violator!(activeset, x, A, b, λ; options=options)
  end

  for outiter=1:maxoutiter
    _lasso!(x, A, b, λ, activeset; options=options)
    if ~_add_violator!(activeset, x, A, b, λ; options=options)
      break
    end
  end
  x, activeset
end


function bubble_sort(seq::Array{Int64})::Array{Int64}
    L = length(seq)
    for _ in (0:L-1)
        for n in (1:L-1)
            if seq[n+1] < seq[(n-1)+1]
                seq[(n-1)+1], seq[n+1] = (seq[n+1], seq[(n-1)+1])
            end
        end
    end
    return seq
end

function main()
    unsorted = [14, 11, 19, 5, 16, 10, 19, 12, 5, 12]
    expected = [5, 5, 10, 11, 12, 12, 14, 16, 19, 19]
    @assert(bubble_sort(unsorted) == expected)
    println(join(["OK"], " "))
end

main()


# %% Compute the scalar and tensor CMB power spectrum
# Call the python wrapper for CLASS, `classy`
# Reference: https://github.com/lesgourg/class_public
classy = pyimport("classy")
# Create an instance of the CLASS wrapper
cosmo = classy.Class()
## Scalar power spectrum
# Create a dictionary of the cosmological parameters
Tcmb = 2.7255e6 # in μK
deg_ncdm = true # 3 massive neutrinos with degenerate mass?
params = Dict(
    "output" => "tCl pCl lCl",
    "modes" => "s",
    "l_max_scalars" => lmax,
    "lensing" => "yes",
    "A_s" => exp(3.0448) * 1e-10,
    "n_s" => 0.96605,
    "k_pivot" => 0.05,
    "h" => 0.6732,
    "omega_b" => 0.022383,
    "omega_cdm" => 0.12011,
    "tau_reio" => 0.0543,
    "N_ncdm" => 1,
)
if deg_ncdm
    push!(params, "m_ncdm" => 0.02, "deg_ncdm" => 3, "N_ur" => 0.00641)
    mν = params["m_ncdm"] * params["deg_ncdm"]
else
    push!(params, "m_ncdm" => 0.06, "N_ur" => 2.0328)
    mν = params["m_ncdm"]
end
# Set the parameters to the cosmological code
cosmo.set(params)
# Run the whole code. Depending on your output, it will call the
# CLASS modules more or less fast. For instance, without any
# output asked, CLASS will only compute background quantities,
# thus running almost instantaneously.
# This is equivalent to the beginning of the `main` routine of CLASS,
# with all the struct_init() methods called.
cosmo.compute()
# Access the lensed scalar cl until l=lmax. It is a dictionnary that contains the fields: ell, tt, te, ee, bb, pp, tp
cls = cosmo.lensed_cl(lmax)
# If you want to change completely the cosmology, you should also
# clean the arguments, otherwise, if you are simply running on a loop
# of different values for the same parameters, this step is not needed
cosmo.empty()
## Tensor power spectrum
rclass = 0.01
params = Dict(
    "output" => "tCl pCl",
    "modes" => "t",
    "l_max_tensors" => lmax,
    "lensing" => "no",
    "r" => rclass,
    "n_t" => 0,
    "A_s" => exp(3.0448) * 1e-10,
    "k_pivot" => 0.05,
    "h" => 0.6732,
    "omega_b" => 0.022383,
    "omega_cdm" => 0.12011,
    "tau_reio" => 0.0543,
    "N_ncdm" => 1,
)
if deg_ncdm
    push!(params, "m_ncdm" => 0.02, "deg_ncdm" => 3, "N_ur" => 0.00641)
    mν = params["m_ncdm"] * params["deg_ncdm"]
else
    push!(params, "m_ncdm" => 0.06, "N_ur" => 2.0328)
    mν = params["m_ncdm"]
end
cosmo.set(params)
cosmo.compute()
clt = cosmo.raw_cl(lmax)
cosmo.empty()


@testset "Geometry" begin

    Random.seed!(5) # reproducability

    @testset "generate_point" begin
        SAMPLES = 10000

        @testset "$geom" for geom in (Box, BoxPBC)
            lengths = [1,2,3]
            b = geom(lengths)
            testpoints = hcat([XXZNumerics.generate_point(b) for _ in 1:SAMPLES]...)

            for i in 1:length(lengths)
                @test reduce(min, testpoints[i,:]) > 0
                @test reduce(max, testpoints[i,:]) < lengths[i]
                @test abs(mean(testpoints[i,:]) - lengths[i]/2) < sqrt(SAMPLES)*lengths[i]
            end
        end

        @testset "$geom" for geom in (NoisyChain, NoisyChainPBC)
            L = 5
            # no noise
            chain = geom(L, 2, 0)
            @test sample_blockaded(chain, L) ≈ 2 .* collect(1:L)

            # some noise - but no overlaps
            chain = geom(L, 2, 1)
            testpoints = hcat([sample_blockaded(chain, L) for _ in 1:SAMPLES]...)

            for i in 1:L
                @test reduce(min, testpoints[i,:]) > 2i-1
                @test reduce(max, testpoints[i,:]) < 2i+1
                @test abs(mean(testpoints[i,:]) - 2i) < sqrt(SAMPLES)
            end
        end
    end

    @testset "distance" begin

        @testset "box" begin
            # note that box size does not matter for distances...
            box = Box([10,10,10,10])
            @test distance(box, [4,0,0,0]  , [1,0,0,0]) ≈ 3 # simple
            @test distance(box, [3,4,0,0]  , [0,0,0,0]) ≈ 5
            @test distance(box, [1,2,3,4.0], [2,1,2,5]) ≈ 2 # more complicated (and float)
            @test distance(box, [2,1,2,5], [1,2,3,4.0]) ≈ 2 # check symmetry
        end

        @testset "boxpbc" begin
            boxpbc = BoxPBC([1,2,3])
            @test distance(boxpbc, [0.4, 0.4, 0.4], [0.5, 0.5, 0.5]) ≈ sqrt(3)*0.1  # normal distance
            @test distance(boxpbc, [0.1, 0  ,   0], [0.9, 0  ,   0]) ≈ 0.2          # wrap around
            @test distance(boxpbc, [0.4, 1.9,   0], [0.7, 0.3,   0]) ≈ 0.5          # mixed case
            @test distance(boxpbc, [1  ,   1,   2], [0,     0,   0]) ≈ sqrt(2)      # more complicated
            @test distance(boxpbc, [0,     0,   0], [1  ,   1,   2]) ≈ sqrt(2)      # more complicated (check symmetry)
        end

        @testset "NoisyChain" begin
            chain = NoisyChain(5,1,0)
            p1 = XXZNumerics.generate_point(chain, 1)
            p2 = XXZNumerics.generate_point(chain, 2)
            p3 = XXZNumerics.generate_point(chain, 3)
            p4 = XXZNumerics.generate_point(chain, 4)
            @test distance(chain, p1, p2) ≈ 1
            @test distance(chain, p2, p1) ≈ 1
            @test distance(chain, p1, p3) ≈ 2
            @test distance(chain, p1, p4) ≈ 3
        end

        @testset "NoisyChainPBC" begin
            chain = NoisyChainPBC(5,1,0)
            p1 = XXZNumerics.generate_point(chain, 1)
            p2 = XXZNumerics.generate_point(chain, 2)
            p3 = XXZNumerics.generate_point(chain, 3)
            p4 = XXZNumerics.generate_point(chain, 4)
            @test distance(chain, p1, p2) ≈ 1
            @test distance(chain, p2, p1) ≈ 1
            @test distance(chain, p1, p3) ≈ 2
            @test distance(chain, p1, p4) ≈ 2 # PBC!
            @test distance(chain, p4, p1) ≈ 2 # PBC! (check symmetry)
        end
    end

    @testset "blockade" begin

        @testset "Box" begin
            b = Box([2,])
            @test_throws ErrorException sample_blockaded(b, 3)
            points = sample_blockaded(b, 2)
            p1, p2 = points
            @test distance_matrix(b, points) == [0 distance(b, p1, p2); distance(b, p1, p2) 0]
            @test distance(b, p1, p2) > 1
        end

        @testset "BoxPBC" begin
            b = BoxPBC([2.2,])
            @test_throws ErrorException sample_blockaded(b, 3)
            points = sample_blockaded(b, 2)
            p1, p2 = points
            @test distance_matrix(b, points) == [0 distance(b, p1, p2); distance(b, p1, p2) 0]
            @test distance(b, p1, p2) > 1
        end
    end

    @testset "distance_matrix" begin
        N = 10
        @testset "$geom" for geom in (Box, BoxPBC)
            @testset "$dim" for dim in 1:3
                g = geom(4*ones(dim)*N^(1/dim)) # just make the volume big enough to fit all particles in 4x the blockaded volume should be enough
                points = sample_blockaded(g, N)
                dists = distance_matrix(g, points)
                @test size(dists) == (N, N)
                @test dists ≈ distance_matrix(g, hcat(points...))
                @test all(>(1), (dists[i,j] for i in 2:N for j in i+1:N))
            end
        end
        @testset "$geom" for geom in (NoisyChain, NoisyChainPBC)
            @testset "$dim" for dim in 1:3
                g = geom(N, 1.5, 0.6) # just make the volume big enough to fit all particles in 4x the blockaded volume should be enough
                points = sample_blockaded(g, N)
                dists = distance_matrix(g, points)
                @test size(dists) == (N, N)
                @test dists ≈ distance_matrix(g, hcat(points...))
                @test all(>(1), (dists[i,j] for i in 2:N for j in i+1:N))
            end
        end
    end
end


#== # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# Description
#
#   Functions to compute the precession according to IAU-76/FK5.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# References
#
#   [1] Vallado, D. A (2013). Fundamentals of Astrodynamics and Applications.
#       Microcosm Press, Hawthorn, CA, USA.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # ==#

export precession_fk5

################################################################################
#                                  Functions
################################################################################

"""
    precession_fk5(JD_TT::Number)

Compute the angles related to the precession movement in the Julian Day `JD_TT`
[Terrestrial Time] using the theory IAU-76/FK5.

# Returns

The angles (ζ, Θ, z) as described in [1, p. 226-228].

"""
function precession_fk5(JD_TT::Number)
    # Compute the Julian Centuries from `JD_TT`.
    T_TT = (JD_TT - JD_J2000)/36525

    # Compute the angles [arcsec].
    ζ = @evalpoly(T_TT, 0, +2306.2181, +0.30188, +0.017998)
    Θ = @evalpoly(T_TT, 0, +2004.3109, -0.42665, -0.041833)
    z = @evalpoly(T_TT, 0, +2306.2181, +1.09468, +0.018203)

    # Normalize the angles in the interval [0, 86400]s and convert to rad.
    s2r = π/648000

    ζ = mod(ζ*s2r, 2π)
    Θ = mod(Θ*s2r, 2π)
    z = mod(z*s2r, 2π)

    # Return the date.
    (ζ, Θ, z)
end


#import Pkg;
#Pkg.add("HypothesisTests")
#Pkg.add("Random")

using HypothesisTests, Random

Random.seed!(0); x= randn(100); y= randn(100);

println(OneSampleTTest(x,y))


function delaunay_triangulation(vertices::Vector{NTuple{2,T}}) where {T}
    n = length(vertices)
    ordered_vertices = biased_random_order(vertices)
    triangulation = Triangulation(ordered_vertices)
    triangle, w = initialize_triangulation!(triangulation)
    if w > 3
        # Add vertices skipped for initial triangulation
        for q in 3:w
            triangle = locate_vertex(triangulation, triangle, q)
            triangle = insert_vertex!(triangulation, q, triangle)
        end
    end
    for q in w:n
        triangle = locate_vertex(triangulation, triangle, q)
        triangle = insert_vertex!(triangulation, q, triangle)
    end
    return triangulation
end

function initialize_triangulation!(triangulation)
    u, v, w, reverse = initial_vertices(triangulation.vertices)
    if reverse
        # Initial triangle
        add_triangle!(triangulation, (w, v, u))
        # Ghost triangles
        add_triangle!(triangulation, (u, v, -1))
        add_triangle!(triangulation, (v, w, -1))
        add_triangle!(triangulation, (w, u, -1))
        return (w, v, u), w
    else
        # Initial triangle
        add_triangle!(triangulation, (u, v, w))
        # Ghost triangles
        add_triangle!(triangulation, (v, u, -1))
        add_triangle!(triangulation, (u, w, -1))
        add_triangle!(triangulation, (w, v, -1))
        return (u, v, w), w
    end
end

function initial_vertices(vertices)
    n = length(vertices)
    u_index, u = 1, vertices[1]
    v_index, v = 2, vertices[2]
    for w_index in 3:n
        w = vertices[w_index]
        direction = orient(u, v, w)
        if direction != 0
            reverse = direction < 0
            return u_index, v_index, w_index, reverse
        end
    end
    if collinear_input
        error("all input vertices are collinear")
        return (0, 0, 0, false)
    end
end

function locate_vertex(triangulation, start_triangle, q)
    u, v, w = start_triangle
    # If the start triangle is a ghost triangle,
    # begin by walking into the triangulation.
    if u == -1
        u, v, w = w, v, adjacent(triangulation, (w, v))
    elseif v == -1
        u, v, w = u, w, adjacent(triangulation, (u, w))
    elseif w == -1
        u, v, w = v, u, adjacent(triangulation, (v, u))
    end
    while true
        # If the walk visits a ghost triangle,
        # the ghost triangle must contain the query point.
        if u == -1 || v == -1 || w == -1
            return u, v, w
        end
        if orient(triangulation, q, u, v) < 0
            u, v, w = v, u, adjacent(triangulation, (v, u))
            continue
        end
        if orient(triangulation, q, v, w) < 0
            u, v, w = w, v, adjacent(triangulation, (w, v))
            continue
        end
        if orient(triangulation, q, w, u) < 0
            u, v, w = u, w, adjacent(triangulation, (u, w))
            continue
        end
        return u, v, w
    end
end

function insert_vertex!(triangulation::Triangulation{T}, vertex::NTuple{2,T}, triangle::NTuple{3,Int}) where {T}
    push!(triangulation.vertices, vertex)
    return insert_vertex!(triangulation, length(triangulation.vertices), triangle)
end

function insert_vertex!(triangulation::Triangulation, u::Int, triangle::NTuple{3,Int})
    delete_triangle!(triangulation, triangle)
    v, w, x = triangle
    dig_cavity!(triangulation, u, (v, w))
    dig_cavity!(triangulation, u, (w, x))
    return dig_cavity!(triangulation, u, (x, v))
end

function dig_cavity!(triangulation::Triangulation{T}, u, edge) where {T}
    v, w = edge
    x = adjacent(triangulation, (w, v))
    if x == 0 # Triangle has already been deleted
        return (0, 0, 0)
    end
    # Check for insertion in ghost triangle
    if x == -1
        u_incircle = incircle(triangulation, u, w, v)
    elseif v == -1
        u_incircle = incircle(triangulation, u, x, w)
    elseif w == -1
        u_incircle = incircle(triangulation, u, v, x)
    else
        u_incircle = incircle(triangulation, u, v, w, x)
    end
    if u_incircle > zero(T)
        # uvw and uvx are not Delaunay
        delete_triangle!(triangulation, (w, v, x))
        dig_cavity!(triangulation, u, (v, x))
        return dig_cavity!(triangulation, u, (x, w))
    end
    return add_triangle!(triangulation, (u, v, w))
end


using RenyiEntropyOfMagic
using OMEinsum
using SparseArrays
using LinearAlgebra
using Test

function naive_two_site(st)
    psi = reshape(kron(st, st, st, st), ntuple(_->2, 8))
    O = kron(Consts.O, Consts.O)
    P = ein"abcdefgh,acegbdfhijklmnop->ijklmnop"(psi,reshape(O, ntuple(_->2, 16)))
    return ein"abcdefgh,acegbdfh->"(psi, P)[]
end

function naive_three_site(st)
    psi = reshape(kron(st, st, st, st), ntuple(_->2, 12))
    O = kron(Consts.O, Consts.O, Consts.O)
    P = ein"abcdefghijkl,adgjbehkcfilmnopqrstuvwx->mnopqrstuvwx"(psi,reshape(O, ntuple(_->2, 24)))
    return ein"abcdefghijkl,adgjbehkcfil->"(psi, P)[]
end

@testset "random periodic boundary MPS" begin
    # random MPS
    A = rand(3, 3, 2)
    T = transfer_matrix(A)

    # tests
    # periodic boundary
    # 1 site
    st = ein"iij->j"(A)
    psi = kron(st, st, st, st)
    psi' * Consts.O * psi
    tr(T)
    @test psi' * Consts.O * psi ≈ tr(T)

    # 2 site
    st = vec(ein"ija,jib->ab"(A, A))
    @test naive_two_site(st) ≈ tr(T^2)

    A1 = rand(2, 3, 2)
    A2 = rand(3, 2, 2)
    T1 = transfer_matrix(A1)
    T2 = transfer_matrix(A2)
    st = vec(ein"ija,jib->ab"(A1, A2))
    @test naive_two_site(st) ≈ tr(T1 * T2)

    A1,A2,A3 = rand(2, 3, 2), rand(3, 3, 2), rand(3, 2, 2)
    st = vec(ein"ija,jkb,kic->abc"(A1, A2, A3))
    T1, T2, T3 = transfer_matrix(A1), transfer_matrix(A2), transfer_matrix(A3)
    @test naive_three_site(st) ≈ tr(T1 * T2 * T3)
end

@testset "random open boundary MPS" begin
    # 2 site
    lA, rA = rand(3, 2), rand(3, 2)
    lT, rT = boundary_vector(lA), boundary_vector(rA)

    st = vec(transpose(lA) * rA)
    expect = naive_two_site(st)
    @test expect ≈ dot(lT, rT)
    @test expect ≈ replicated_expectation([lA, rA])

    # 3 site
    lA, A, rA = rand(3, 2), rand(3, 3, 2), rand(3, 2)
    lT, T, rT = boundary_vector(lA), transfer_matrix(A), boundary_vector(rA)

    st = vec(ein"ia,ijb,jc->abc"(lA, A, rA))
    expect = naive_three_site(st)
    @test expect ≈ dot(lT, T * rT)
    @test expect ≈ replicated_expectation([lA, A, rA])
end

@testset "tucker decomposition" begin
    include("tucker.jl")
end


export HarmonicAngle

"""
    HarmonicAngle(; th0, cth)

A harmonic bond angle between three atoms.
"""
struct HarmonicAngle{D, K} <: SpecificInteraction
    th0::D
    cth::K
end

HarmonicAngle(; th0, cth) = HarmonicAngle{typeof(th0), typeof(cth)}(th0, cth)

@inline @inbounds function force(a::HarmonicAngle, coords_i, coords_j, coords_k, box_size)
    # In 2D we use then eliminate the cross product
    ba = vector_pad3D(coords_j, coords_i, box_size)
    bc = vector_pad3D(coords_j, coords_k, box_size)
    cross_ba_bc = ba × bc
    pa = normalize(trim3D( ba × cross_ba_bc, box_size))
    pc = normalize(trim3D(-bc × cross_ba_bc, box_size))
    angle_term = -a.cth * (acosbound(dot(ba, bc) / (norm(ba) * norm(bc))) - a.th0)
    fa = (angle_term / norm(ba)) * pa
    fc = (angle_term / norm(bc)) * pc
    fb = -fa - fc
    return SpecificForce3Atoms(fa, fb, fc)
end

@inline @inbounds function potential_energy(a::HarmonicAngle, coords_i, coords_j,
                                            coords_k, box_size)
    θ = bond_angle(coords_i, coords_j, coords_k, box_size)
    return (a.cth / 2) * (θ - a.th0) ^ 2
end