# arcs, circles, ellipses, curves, pie, sector, bezier

"""
    circle(x, y, r, action=:none)

Make a circle of radius `r` centered at `x`/`y`.

`action` is one of the actions applied by `do_action`, defaulting to `:none`. You can
also use `ellipse()` to draw circles and place them by their centerpoint.
"""
function circle(x, y, r, action=:none)
    if action != :path
        newpath()
    end
    Cairo.arc(get_current_cr(), x, y, r, 0, 2pi)
    do_action(action)
end

"""
    circle(pt, r, action=:none)

Make a circle centered at `pt`.
"""
circle(centerpoint::Point, r, action=:none) =
    circle(centerpoint.x, centerpoint.y, r, action)

"""
    circle(pt1::Point, pt2::Point, action=:none)

Make a circle that passes through two points that define the diameter:
"""
function circle(pt1::Point, pt2::Point, action=:none)
    center = midpoint(pt1, pt2)
    radius = distance(pt1, pt2)/2
    circle(center, radius, action)
end

"""
    circle(pt1::Point, pt2::Point, pt3::Point, action=:none)

Make a circle that passes through three points.
"""
function circle(pt1::Point, pt2::Point, pt3::Point, action=:none)
    center = midpoint(pt1, pt2)
    radius = distance(pt1, pt2)/2
    circle(center3pts(pt1, pt2, pt3)..., action)
end

"""
    center3pts(a::Point, b::Point, c::Point)

Find the radius and center point for three points lying on a circle.

returns `(centerpoint, radius)` of a circle.

If there's no such circle, the function returns `(Point(0, 0), 0)`.

If two of the points are the same, use `circle(pt1, pt2)` instead.
"""
function center3pts(p1::Point, p2::Point, p3::Point)
    norm2(p::Point) = (p.x)^2+(p.y)^2

    α1 = norm2(p3-p2)*(norm2(p2-p1)+norm2(p1-p3)-norm2(p3-p2))
    α2 = norm2(p1-p3)*(norm2(p3-p2)+norm2(p2-p1)-norm2(p1-p3))
    α3 = norm2(p2-p1)*(norm2(p1-p3)+norm2(p3-p2)-norm2(p2-p1))

    if α1+α2+α3 ≠ 0.0
        c = (α1*p1+α2*p2+α3*p3)/(α1+α2+α3)
        r = √(norm2(p1-c))
        return c, r
    else
        @warn "center3pts(): There are no circles which pass through $p1, $p2 and $p3."
        return (Point(0, 0), 0)
    end
end

"""
    ellipse(xc, yc, w, h, action=:none)

Make an ellipse, centered at `xc/yc`, fitting in a box of width `w` and height `h`.
"""
function ellipse(xc, yc, w, h, action=:none)
    x  = xc - w/2
    y  = yc - h/2
    # kappa = 4.0 * (sqrt(2.0) - 1.0) / 3.0
    kappa = 0.5522847498307936
    ox = (w / 2) * kappa  # control point offset horizontal
    oy = (h / 2) * kappa  # control point offset vertical
    xe = x + w            # x-end
    ye = y + h            # y-end
    xm = x + w / 2        # x-middle
    ym = y + h / 2        # y-middle
    move(x, ym)
    curve(x, ym - oy, xm - ox, y, xm, y)
    curve(xm + ox, y, xe, ym - oy, xe, ym)
    curve(xe, ym + oy, xm + ox, ye, xm, ye)
    curve(xm - ox, ye, x, ym + oy, x, ym)
    do_action(action)
end

"""
    ellipse(cpt, w, h, action=:none)

Make an ellipse, centered at point `c`, with width `w`, and height `h`.
"""
ellipse(c::Point, w, h, action=:none) = ellipse(c.x, c.y, w, h, action)

"""
    squircle(center::Point, hradius, vradius, action=:none;
        rt = 0.5, stepby = pi/40, vertices=false)

Make a squircle or superellipse (basically a rectangle with rounded corners).
Specify the center position, horizontal radius (distance from center to a side),
and vertical radius (distance from center to top or bottom):

The root (`rt`) option defaults to 0.5, and gives an intermediate shape. Values
less than 0.5 make the shape more rectangular. Values above make the shape more
round. The horizontal and vertical radii can be different.
"""
function squircle(center::Point, hradius, vradius, action=:none;
                  rt = 0.5,
                  vertices=false,
                  stepby = pi/40,
                  reversepath=false)
    points = Point[]
    for theta in 0:stepby:2pi
        xpos = center.x + ^(abs(cos(theta)), rt) * hradius * sign(cos(theta))
        ypos = center.y + ^(abs(sin(theta)), rt) * vradius * sign(sin(theta))
        push!(points, Point(xpos, ypos))
    end
    result = reversepath ? reverse(points) : points
    if !vertices
        poly(points, action, close=true, reversepath=reversepath)
    end
    return result
end

"""
    arc(xc, yc, radius, angle1, angle2, action=:none)

Add an arc to the current path from `angle1` to `angle2` going clockwise, centered
at xc, yc.

Angles are defined relative to the x-axis, positive clockwise.
"""
function arc(xc, yc, radius, angle1, angle2, action=:none)
    Cairo.arc(get_current_cr(), xc, yc, radius, angle1, angle2)
    do_action(action)
end

"""
    arc(centerpoint::Point, radius, angle1, angle2, action=:none)

Add an arc to the current path from `angle1` to `angle2` going clockwise, centered
at `centerpoint`.
"""
arc(centerpoint::Point, radius, angle1, angle2, action=:none) =
    arc(centerpoint.x, centerpoint.y, radius, angle1, angle2, action)

"""
    carc(xc, yc, radius, angle1, angle2, action=:none)

Add an arc to the current path from `angle1` to `angle2` going counterclockwise,
centered at `xc`/`yc`.

Angles are defined relative to the x-axis, positive clockwise.
"""
function carc(xc, yc, radius, angle1, angle2, action=:none)
    Cairo.arc_negative(get_current_cr(), xc, yc, radius, angle1, angle2)
    do_action(action)
end

"""
    carc(centerpoint::Point, radius, angle1, angle2, action=:none)

Add an arc centered at `centerpoint` to the current path from `angle1` to
`angle2`, going counterclockwise.
"""
carc(centerpoint::Point, radius, angle1, angle2, action=:none) =
    carc(centerpoint.x, centerpoint.y, radius, angle1, angle2, action)

"""
      arc2r(c1::Point, p2::Point, p3::Point, action=:none)

Add a circular arc centered at `c1` that starts at `p2` and ends at `p3`, going clockwise,
to the current path.

`c1`-`p2` really determines the radius. If `p3` doesn't lie on the circular path,
 it will be used only as an indication of the arc's length, rather than its position.
"""
function arc2r(c1::Point, p2::Point, p3::Point, action=:none)
    r = distance(c1, p2)
    startangle = atan(p2.y - c1.y, p2.x - c1.x)
    endangle   = atan(p3.y - c1.y, p3.x - c1.x)
    if endangle < startangle
        endangle = mod2pi(endangle + 2pi)
    end
    arc(c1, r, startangle, endangle, action)
end

"""
    carc2r(c1::Point, p2::Point, p3::Point, action=:none)

Add a circular arc centered at `c1` that starts at `p2` and ends at `p3`,
going counterclockwise, to the current path.

`c1`-`p2` really determines the radius. If `p3` doesn't lie on the circular
path, it will be used only as an indication of the arc's length, rather than its position.
"""
function carc2r(c1::Point, p2::Point, p3::Point, action=:none)
    r = distance(c1, p2)
    startangle = atan(p2.y - c1.y, p2.x - c1.x)
    endangle   = atan(p3.y - c1.y, p3.x - c1.x)
    if startangle < endangle
        startangle = mod2pi(startangle + 2pi)
    end
    carc(c1, r, startangle, endangle, action)
end

"""
    isarcclockwise(c::Point, A::Point, B::Point)

Return `true` if an arc centered at `c` going from `A` to `B` is clockwise.

If `c`, `A`, and `B` are collinear, then a hemispherical arc could be
either clockwise or not.
"""
function isarcclockwise(c::Point, A::Point, B::Point)
    a = A - c
    b = B - c
    return crossproduct(a, b) > 0
end

"""
    sector(centerpoint::Point, innerradius, outerradius, startangle, endangle, action:none)

Draw an annular sector centered at `centerpoint`.

"""
function sector(centerpoint::Point, innerradius::Real, outerradius::Real, startangle::Real,
                endangle::Real, action::Symbol=:none)
    (innerradius > outerradius) && throw(DomainError(outerradius, "outer radius must be larger than inner radius $(innerradius)"))
    gsave()
    translate(centerpoint)
    newpath()
    move(innerradius * cos(startangle), innerradius * sin(startangle))
    line(outerradius * cos(startangle), outerradius * sin(startangle))
    arc(0, 0, outerradius, startangle, endangle, :none)
    line(innerradius * cos(endangle), innerradius * sin(endangle))
    carc(0, 0, innerradius, endangle, startangle, :none)
    closepath()
    grestore()
    do_action(action)
end

"""
    sector(innerradius::Real, outerradius::Real, startangle::Real, endangle::Real,
       action::Symbol=:none)

Draw an annular sector centered at the origin.
"""
sector(innerradius::Real, outerradius::Real, startangle::Real, endangle::Real,
       action::Symbol=:none) =
    sector(O, innerradius, outerradius, startangle, endangle, action)

"""
    sector(centerpoint::Point, innerradius, outerradius, startangle, endangle,
        cornerradius, action:none)

Draw an annular sector with rounded corners, basically a bent sausage shape,
centered at `centerpoint`.

TODO: The results aren't 100% accurate at the moment. There are small
discontinuities where the curves join.

The cornerradius is reduced from the supplied value if neceesary to prevent overshoots.
"""
function sector(centerpoint::Point, innerradius::Real, outerradius::Real, startangle::Real,
                endangle::Real, cornerradius::Real, action::Symbol=:none)
    (innerradius > outerradius) && throw(DomainError(outerradius, "outer radius must be larger than inner radius $(innerradius)"))
    gsave()
    translate(centerpoint)
    # some work is done using polar coords to calculate the points

    # attempts to prevent pathological cases
    cornerradius = min(cornerradius, abs(outerradius-innerradius)/2)
    if endangle < startangle
        endangle = mod2pi(endangle + 2pi)
    end

    # TODO reduce given corner radius to prevent messes when spanning angle too small
    # 4 is a magic number
    while abs(endangle - startangle) < 4.0(atan(cornerradius, innerradius))
        cornerradius *= 0.75
    end

    # first inner corner
    alpha1 = asin(cornerradius/(innerradius+cornerradius))
    p1p2center = (innerradius + cornerradius, startangle + alpha1)
    p1 = (innerradius, startangle + alpha1)
    p2 = (innerradius + cornerradius, startangle)
    # first outer
    alpha2 = asin(cornerradius/(outerradius-cornerradius))
    p3p4center = (outerradius - cornerradius, startangle + alpha2)
    p3 = (outerradius - cornerradius, startangle)
    p4 = (outerradius, startangle + alpha2)
    # last outer
    p5p6center = (outerradius - cornerradius, endangle - alpha2)
    p5 = (outerradius, endangle - alpha2)
    p6 = (outerradius - cornerradius, endangle)
    # last inner
    p7p8center = (innerradius + cornerradius, endangle - alpha1)
    p7 = (innerradius + cornerradius, endangle)
    p8 = (innerradius, endangle - alpha1)

    # make path
    move(@polar(p1))
    newpath()
    # inner corner
    arc(@polar(p1p2center), cornerradius, slope(@polar(p1p2center), @polar(p1)),
        slope(@polar(p1p2center), @polar(p2)), :none)
    line(@polar(p3))
    # outer corner
    arc(@polar(p3p4center), cornerradius, slope(@polar(p3p4center), @polar(p3)),
        slope(@polar(p3p4center), @polar(p4)), :none)
    # outside arc
    arc(O, outerradius, slope(O, @polar(p4)), slope(O, @polar(p5)), :none)
    # last outside corner
    arc(@polar(p5p6center), cornerradius, slope(@polar(p5p6center), @polar(p5)),
        slope(@polar(p5p6center), @polar(p6)), :none)
    line(@polar(p7))
    # last inner corner
    arc(@polar(p7p8center), cornerradius, slope(@polar(p7p8center), @polar(p7)),
        slope(@polar(p7p8center), @polar(p8)), :none)
    s1, s2 = slope(O, @polar(p8)), slope(O, @polar(p1))
    if s1 < s2
        s2 = mod2pi(s2 + 2pi)
    end
    carc(O, innerradius, s1, s2, :none)
    closepath()
    do_action(action)
    grestore()
end

"""
    sector(innerradius::Real, outerradius::Real, startangle::Real, endangle::Real,
       cornerradius::Real, action::Symbol=:none)

Draw an annular sector with rounded corners, centered at the current origin.
"""
sector(innerradius::Real, outerradius::Real, startangle::Real, endangle::Real,
       cornerradius::Real, action::Symbol=:none) =
    sector(O, innerradius, outerradius, startangle, endangle, cornerradius, action)

"""
    pie(x, y, radius, startangle, endangle, action=:none)

Draw a pie shape centered at `x`/`y`. Angles start at the positive x-axis and
are measured clockwise.
"""
function pie(x, y, radius, startangle, endangle, action=:none)
    gsave()
    translate(x, y)
    newpath()
    move(0, 0)
    line(radius * cos(startangle), radius * sin(startangle))
    arc(0, 0, radius, startangle, endangle, :none)
    closepath()
    grestore()
    do_action(action)
end

"""
    pie(centerpoint, radius, startangle, endangle, action=:none)

Draw a pie shape centered at `centerpoint`.

Angles start at the positive x-axis and are measured clockwise.
"""
pie(centerpoint::Point, radius, startangle, endangle, action) =
    pie(centerpoint.x, centerpoint.y, radius, startangle, endangle, action)

"""
    pie(radius, startangle, endangle, action=:none)

Draw a pie shape centered at the origin

"""
pie(radius, startangle, endangle, action=:none) = pie(O, radius, startangle, endangle, action)

"""
    curve(x1, y1, x2, y2, x3, y3)
    curve(p1, p2, p3)

Add a Bézier curve.

The spline starts at the current position, finishing at `x3/y3` (`p3`),
following two control points `x1/y1` (`p1`) and `x2/y2` (`p2`).
"""
curve(x1, y1, x2, y2, x3, y3) = Cairo.curve_to(get_current_cr(), x1, y1, x2, y2, x3, y3)
curve(pt1, pt2, pt3)          = curve(pt1.x, pt1.y, pt2.x, pt2.y, pt3.x, pt3.y)

"""
    circlepath(center::Point, radius, action=:none;
        reversepath=false,
        kappa = 0.5522847498307936)

Draw a circle using Bézier curves.

The magic value, `kappa`, is `4.0 * (sqrt(2.0) - 1.0) / 3.0`.
"""
function circlepath(center::Point, radius, action=:none;
    reversepath=false,
    kappa = 0.5522847498307936)
    function northtoeast(center::Point, radius, kappa)
        curve(center.x + (radius * kappa), center.y + radius, center.x + radius,
        center.y + (radius * kappa), center.x + radius, center.y )
    end

    function easttosouth(center::Point, radius, kappa)
        curve(center.x + radius, center.y - (radius * kappa), center.x + (radius * kappa),
        center.y - radius, center.x, center.y - radius)
    end

    function southtowest(center::Point, radius, kappa)
        curve(center.x - (radius * kappa), center.y - radius, center.x - radius,
        center.y - (radius * kappa), center.x - radius, center.y)
    end

    function westtonorth(center::Point, radius, kappa)
        curve(center.x - radius, center.y + (radius * kappa), center.x - (radius * kappa),
        center.y + radius, center.x, center.y + radius)
    end

    function northtowest(center::Point, radius, kappa)
        curve(center.x - (radius * kappa), center.y + radius, center.x - radius,
        center.y + (radius * kappa), center.x - radius, center.y )
    end

    function westtosouth(center::Point, radius, kappa)
        curve(center.x - radius, center.y - (radius * kappa), center.x - (radius * kappa),
        center.y - radius, center.x, center.y - radius )
    end

    function southtoeast(center::Point, radius, kappa)
        curve(center.x + (radius * kappa), center.y - radius, center.x + radius,
        center.y - (radius * kappa), center.x + radius, center.y)
    end

    function easttonorth(center::Point, radius, kappa)
        curve(center.x + radius, center.y + (radius * kappa), center.x + (radius * kappa),
        center.y + radius, center.x, center.y + radius )
    end

    move(center.x, center.y + radius)
    if !reversepath
        northtoeast(center, radius, kappa)
        easttosouth(center, radius, kappa)
        southtowest(center, radius, kappa)
        westtonorth(center, radius, kappa)
    else
        northtowest(center, radius, kappa)
        westtosouth(center, radius, kappa)
        southtoeast(center, radius, kappa)
        easttonorth(center, radius, kappa)
    end
    do_action(action)
end

"""
    ellipse(focus1::Point, focus2::Point, k, action=:none;
            stepvalue=pi/100,
            vertices=false,
            reversepath=false)

Build a polygon approximation to an ellipse, given two points and a distance, `k`, which is
the sum of the distances to the focii of any points on the ellipse (or the shortest length
of string required to go from one focus to the perimeter and on to the other focus).
"""
function ellipse(focus1::Point, focus2::Point, k, action=:none;
                 stepvalue=pi/100,
                 vertices=false,
                 reversepath=false)
    a = k/2  # a = ellipse's major axis, the widest part
    cpoint = midpoint(focus1, focus2)
    dc = distance(focus1, cpoint)
    b = sqrt(abs(a^2 - dc^2)) # minor axis, hopefuly not 0
    phi = slope(focus1, focus2) # angle between the major axis and the x-axis
    points = Point[]
    drawing = false
    for t in 0:stepvalue:2pi
        xt = cpoint.x + a * cos(t) * cos(phi) - b * sin(t) * sin(phi)
        yt = cpoint.y + a * cos(t) * sin(phi) + b * sin(t) * cos(phi)
        push!(points, Point(xt, yt))
    end
    vertices ? points : poly(points, action, close=true, reversepath=reversepath)
end

"""
    ellipse(focus1::Point, focus2::Point, pt::Point, action=:none;
            stepvalue=pi/100,
            vertices=false,
            reversepath=false)

Build a polygon approximation to an ellipse, given two points and a point somewhere on the
ellipse.
"""
function ellipse(focus1::Point, focus2::Point, pt::Point, action=:none;
                 stepvalue=pi/100,
                 vertices=false,
                 reversepath=false)
    k = distance(focus1, pt) + distance(focus2, pt)
    ellipse(focus1, focus2, k, action, stepvalue=stepvalue,
        vertices=vertices, reversepath=reversepath)
end

"""
    hypotrochoid(R, r, d, action=:none;
            stepby=0.01,
            period=0.0,
            vertices=false)

Make a hypotrochoid with short line segments. (Like a Spirograph.) The curve is traced by a
point attached to a circle of radius `r` rolling around the inside  of a fixed circle of
radius `R`, where the point is a distance `d` from  the center of the interior circle.
Things get interesting if you supply non-integral values.

Special cases include the hypocycloid, if `d` = `r`, and an ellipse, if `R` = `2r`.

`stepby`, the angular step value, controls the amount of detail, ie the smoothness of the
polygon,

If `period` is not supplied, or 0, the lowest period is calculated for you.

The function can return a polygon (a list of points), or draw the points directly using
the supplied `action`. If the points are drawn, the function returns a tuple showing how
many points were drawn and what the period was (as a multiple of `pi`).
"""
function hypotrochoid(R, r, d, action=:none;
                      close    = true,
                      stepby   = 0.01,
                      period   = 0.0,
                      vertices = false)
    function nextposition(t)
        x = (R - r) * cos(t) + (d * cos(((R - r)/r) * t))
        y = (R - r) * sin(t) - (d * sin(((R - r)/r) * t))
        return Point(x, y)
    end
    # try to calculate the period exactly
    if isapprox(period, 0)
        period = 2pi * (r/gcd(convert(Int, floor(R)), convert(Int, floor(r))))
    end
    counter=1
    points=Point[]
    for t = 0:stepby:period
        push!(points, nextposition(t))
    end
    # don't repeat end point if it's more or less the same as the start point
    isapprox(points[1], points[end]) && pop!(points)
    vertices ? points : (poly(points, action, close=close); (length(points), period/pi))
end

"""
    epitrochoid(R, r, d, action=:none;
            stepby=0.01,
            period=0,
            vertices=false)

Make a epitrochoid with short line segments. (Like a Spirograph.) The curve is traced by a
point attached to a circle of radius `r` rolling around the outside of a fixed circle of
radius `R`, where the point is a distance `d` from the center of the circle.
Things get interesting if you supply non-integral values.

`stepby`, the angular step value, controls the amount of detail, ie the smoothness of the
polygon.

If `period` is not supplied, or 0, the lowest period is calculated for you.

The function can return a polygon (a list of points), or draw the points directly using
the supplied `action`. If the points are drawn, the function returns a tuple showing how
many points were drawn and what the period was (as a multiple of `pi`).
"""
function epitrochoid(R, r, d, action=:none;
                     close    = true,
                     stepby   = 0.01,
                     period   = 0,
                     vertices = false)
    function nextposition(t)
        x = (R + r) * cos(t) - (d * cos(((R - r)/r) * t))
        y = (R + r) * sin(t) - (d * sin(((R - r)/r) * t))
        return Point(x, y)
    end
    # try to calculate the period exactly
    if isapprox(period, 0)
        period = 2pi * (r/gcd(convert(Int, floor(R)), convert(Int, floor(r))))
    end
    counter=1
    points=Point[]
    for t = 0:stepby:period
        push!(points, nextposition(t))
    end
    # don't repeat end point if it's more or less the same as the start point
    isapprox(points[1], points[end]) && pop!(points)
    vertices ? points : (poly(points, action, close=close); (length(points), period/pi))
end

"""
    spiral(a, b, action::Symbol=:none;
                     stepby = 0.01,
                     period = 4pi,
                     vertices = false,
                     log=false)

Make a spiral. The two primary parameters `a` and `b` determine the start radius, and the
tightness.

For linear spirals (`log=false`), `b` values are:

    lituus: -2

    hyperbolic spiral: -1

    Archimedes' spiral: 1

    Fermat's spiral: 2

For logarithmic spirals (`log=true`):

    golden spiral: b = ln(phi)/ (pi/2) (about 0.30)

Values of `b` around 0.1 produce tighter, staircase-like spirals.
"""
function spiral(a, b, action::Symbol=:none;
                 stepby = 0.01,
                 period = 4pi,
                 vertices = false,
                 log=false)
    function nextpositionlog(t)
        ebt = exp(t * b)
        if abs(ebt) < 10^8 # arbitrary cutoff to avoid NaNs and Infs
            x = a * ebt * cos(t)
            y = a * ebt * sin(t)
            return Point(x, y)
        else
            return nothing
        end
    end
    function nextpositionlin(t)
        tpn = t ^ (1/b)
        if tpn < 10^8
            x = a * tpn * cos(t)
            y = a * tpn * sin(t)
            return Point(x, y)
        else
            return nothing
        end
    end
    log ? nextpos=nextpositionlog : nextpos = nextpositionlin
    points = Point[]
    for t = 0:stepby:period
        pt = nextpos(t)
        if isa(nextpos(t), Point)
            push!(points, pt)
        end
    end
    if vertices == false
        poly(points, action) # no close by default :)
    end
    return points
end

"""
    intersection2circles(pt1, r1, pt2, r2)

Find the area of intersection between two circles, the first centered at `pt1` with radius
`r1`, the second centered at `pt2` with radius `r2`.

If one circle is entirely within another, that circle's area is returned.
"""
function intersection2circles(pt1, rad1, pt2, rad2)
    # via casey and jùlio on slack
    # squared radii
    rr1, rr2 = rad1 * rad1, rad2 * rad2
    d = distance(pt1, pt2)

    # trivial cases
    d ≥ rad1 + rad2      && return 0.0
    d ≤ abs(rad2 - rad1) && return π * min(rr1, rr2)

    # First center point to the middle line
    a_distancecenterfirst = (rr1 - rr2 + (d^2)) / (2d)

    # Second centre point to the middle line
    b_distancecentersecond = d - a_distancecenterfirst

    # Half of the middle line
    h_height = sqrt(rr1 - a_distancecenterfirst^2)

    # central angle for the first circle
    alpha = mod2pi(atan(h_height, a_distancecenterfirst) * 2.0 + 2π)

    # Central angle for the second circle
    beta = mod2pi(atan(h_height, b_distancecentersecond) * 2.0 + 2π)

    #  Area of the first circular segment
    A1 = rr1 / 2.0 * (alpha - sin(alpha))
    # Area of the second circular segment
    A2 = rr2 / 2.0 * (beta - sin(beta))
    return A1 + A2
end

"""
    intersectioncirclecircle(cp1, r1, cp2, r2)

Find the two points where two circles intersect, if they do. The first circle is centered
at `cp1` with radius `r1`, and the second is centered at `cp1` with radius `r1`.

Returns

    (flag, ip1, ip2)

where `flag` is a Boolean `true` if the circles intersect at the points `ip1` and `ip2`. If
the circles don't intersect at all, or one is completely inside the other, `flag` is `false`
and the points are both Point(0, 0).

Use `intersection2circles()` to find the area of two overlapping circles.

In the pure world of maths, it must be possible that two circles 'kissing' only have a
single intersection point. At present, this unromantic function reports that two kissing
circles have no intersection points.
"""
function intersectioncirclecircle(cp1, r1, cp2, r2)
    r1squared = r1^2
    r2squared = r2^2
    d = distance(cp1, cp2)
    if d > (r2 + r1) # circles do not overlap
        return (false, O, O)
    elseif (d <= abs(r1 - r2)) && (r1 >= r2)
        # second circle is completely inside first circle
        return (false, O, O)
    elseif (d <= abs(r1 - r2)) && (r1 < r2)
        # first circle is completely inside second circle
        return (false, O, O)
    end
    a = (r1squared - r2squared + d^2) / 2d
    h = sqrt(r1squared - a^2)
    p0 = cp1 + a * (cp2 - cp1)/d
    p3 = Point(
            p0.x + h * (cp2.y - cp1.y )/d,
            p0.y - h * (cp2.x - cp1.x )/d)
    p4 = Point(
            p0.x - h * (cp2.y - cp1.y )/d,
            p0.y + h * (cp2.x - cp1.x )/d)
    return (true, p3, p4)
end

"""
    circlepointtangent(through::Point, radius, targetcenter::Point, targetradius)

Find the centers of up to two circles of radius `radius` that pass through point
`through` and are tangential to a circle that has radius `targetradius` and
center `targetcenter`.

This function returns a tuple:

* (0, O, O)      - no circles exist

* (1, pt1, O)    - 1 circle exists, centered at pt1

* (2, pt1, pt2)  - 2 circles exist, with centers at pt1 and pt2

(The O are just dummy points so that three values are always returned.)
"""
function circlepointtangent(through::Point, radius, targetcenter::Point, targetradius)
    distx = targetcenter.x - through.x
    disty = targetcenter.y - through.y
    dsq = distance(through, targetcenter)^2
    if isless(dsq, 10e-6) # coincident
        return (0, O, O)
    else
        sqinv=0.5/dsq
        s = dsq - ((2radius + targetradius) * targetradius)
        root = 4(radius^2) * dsq - s^2
        s *= sqinv
        if isless(dsq, 0.0) # no center possible
            return (0, O, O)
        else
            if isless(root, 10e-6) # only one circle possible
                x = through.x + distx * s
                y = through.y + disty * s
                if isless(abs(distance(through, Point(x, y)) - radius), 10e-6)
                    return (1, Point(x, y), O)
                else
                    return (0, O, O)
                end
            else # two circles are possible
                root = sqrt(root) * sqinv
                xconst = through.x + distx * s
                yconst = through.y + disty * s
                xvar = disty * root
                yvar = distx * root
            return (2, Point(xconst - xvar, yconst + yvar), Point(xconst + xvar, yconst - yvar))
            end
        end
    end
end

"""
    circletangent2circles(radius, circle1center::Point, circle1radius, circle2center::Point, circle2radius)

Find the centers of up to two circles of radius `radius` that are tangent to the
two circles defined by `circle1...` and `circle2...`. These two circles can
overlap, but one can't be inside the other.

* (0, O, O)      - no such circles exist

* (1, pt1, O)    - 1 circle exists, centered at pt1

* (2, pt1, pt2)  - 2 circles exist, with centers at pt1 and pt2

(The O are just dummy points so that three values are always returned.)
"""
function circletangent2circles(radius, circle1center::Point, circle1radius, circle2center::Point, circle2radius)
    modradius1 = radius + circle1radius
    modradius2 = circle2radius - circle1radius
    return circlepointtangent(circle1center, modradius1, circle2center, modradius2)
end

"""
    arc2sagitta(p1::Point, p2::Point, s, action=:none)

Make a clockwise arc starting at `p1` and ending at `p2` that reaches a height of `s`, the sagitta, at the middle. Might append to current path...

Return tuple of the center point and the radius of the arc.
"""
function arc2sagitta(p1::Point, p2::Point, s, action=:none)
    if isapprox(s, 0.0)
        throw(error("Height of arc $s should be greater than 0.0"))
    end
    l = distance(p1, p2)/2
    r = (s^2 + l^2) / 2s
    flag, ip1, ip2 = intersectioncirclecircle(p1, r, p2, r)
    if flag
        if r <= s
            arc2r(ip1, p1, p2, action)
            result = (ip1, r)
        else
            arc2r(ip2, p1, p2, action)
            result = (ip2, r)
        end
    else
        result = (O, 0.0)
    end
    return result
end

"""
    carc2sagitta(p1::Point, p2::Point, s, action=:none)

Make a counterclockwise arc starting at `p1` and ending at `p2` that reaches a height of `s`, the sagitta, at the middle. Might append to current path...

Return tuple of center point and radius of arc.
"""
function carc2sagitta(p1::Point, p2::Point, s, action=:none)
    if isapprox(s, 0.0)
        throw(error("Height of arc $s should be greater than 0.0"))
    end
    l = distance(p1, p2)/2
    r = (s^2 + l^2) / 2s
    flag, ip1, ip2 = intersectioncirclecircle(p1, r, p2, r)
    if flag
        if r <= s
            carc2r(ip2, p1, p2, action)
            result = (ip1, r)
        else
            carc2r(ip1, p1, p2, action)
            result = (ip2, r)
        end
    else
        result = (O, 0.0)
    end
    return result
end

"""
    circlecircleoutertangents(cpt1::Point, r1, cpt2::Point, r2)

Return four points, `p1`, `p2, `p3`, `p4`, where a line
through `p1` and `p2`, and a line through `p3` and `p4`, form the outer
tangents to the circles defined by `cpt1/r1` and `cpt2/r2`.

Returns four identical points (`O`) if one of the circles lies inside the other.
"""
function circlecircleoutertangents(cpt1::Point, r1, cpt2::Point, r2)
    dx = cpt2.x - cpt1.x
    dy = cpt2.y - cpt1.y
    dist = distance(cpt1, cpt2)

    if dist <= abs(r2 - r1)
        # the circles overlap, no tangents
        @debug "circlecircleoutertangents: circles overlap"
        return O, O, O, O
    end

    # rotation from the x-axis.
    angle1 = atan(dy, dx)

    # relative angle of the normals
    angle2 = acos((r1 - r2)/dist)

    pt1 = Point(cpt1.x + r1 * cos(angle1 + angle2),
        cpt1.y + r1 * sin(angle1 + angle2))

    pt2 = Point(cpt2.x + r2 * cos(angle1 + angle2),
        cpt2.y + r2 * sin(angle1 + angle2))

    pt3 = Point(cpt1.x + r1 * cos(angle1 - angle2),
        cpt1.y + r1 * sin(angle1 - angle2))

    pt4 = Point(cpt2.x + r2 * cos(angle1 - angle2),
        cpt2.y + r2 * sin(angle1 - angle2))

    return pt1, pt2, pt3, pt4
end

"""
    pointcircletangent(point::Point, circlecenter::Point, circleradius)

Find the two points on a circle that lie on tangent lines
passing through an external point.

If both points are O, the external point is inside the
circle, and the result is `(O, O)`.
"""
function pointcircletangent(point::Point, circlecenter::Point, circleradius)
    d = distance(circlecenter, point)
    if d <= circleradius # isinside
        @debug "pointcircletangent: external point is inside circle"
        # for type stability, return two points anyway?
        return O, O
    end
    dx, dy = point.x - circlecenter.x, point.y - circlecenter.y
    dxr, dyr = -dy, dx # rotate
    ρ = circleradius/d
    ad = ρ^2
    bd = ρ * sqrt(1 - ρ^2)
    tangentpoint1x = circlecenter.x + (ad * dx) + (bd * dxr)
    tangentpoint1y = circlecenter.y + (ad * dy) + (bd * dyr)
    tangentpoint2x = circlecenter.x + (ad * dx) - (bd * dxr)
    tangentpoint2y = circlecenter.y + (ad * dy) - (bd * dyr)
    return Point(tangentpoint1x, tangentpoint1y), Point(tangentpoint2x, tangentpoint2y)
end

"""
circlecircleinnertangents(circle1center::Point, circle1radius, circle2center::Point, circle2radius)

Find the inner tangents of two circles. These are tangent lines that
cross as they skim past one circle and touch the other.

Returns the four points: tangentpoint1 on circle 1,
tangentpoint1 on circle2, tangentpoint2 on circle 1,
tangentpoint2 on circle2.

Returns `(O, O, O, O)` if inner tangents can't be found (eg when the circles overlap).

Use `circlecircleoutertangents()` to find the outer tangents.
"""
function circlecircleinnertangents(circle1center::Point, circle1radius, circle2center::Point, circle2radius)
    # point where the inner tangents will cross
    ip = (circle2center * circle1radius + circle1center * circle2radius) / (circle1radius + circle2radius)
    d = distance(circle1center, circle2center)
    # circles overlap?
    d <= (circle1radius + circle2radius) && return O, O, O, O
    tp1, tp2 = pointcircletangent(ip, circle1center, circle1radius)
    tp3, tp4 = pointcircletangent(ip, circle2center, circle2radius)
    # double check tp1 and tp3 are collinear, just in case the order was wrong
    if slope(tp1, ip) ≈ slope(ip, tp3)
        return tp1, tp3, tp2, tp4
    elseif slope(tp1, ip) ≈ slope(ip, tp4)
        return tp1, tp4, tp2, tp3
    end
end

"""
    ellipseinquad(qgon, action=:none)

Calculate a Bézier-based ellipse that fits inside the
quadrilateral `qgon`, an array of with at least four Points,
then apply `action`.

Returns `ellipsecenter, ellipsesemimajor, ellipsesemiminor,
ellipseangle`:

`ellipsecenter` the ellipse center

`ellipsesemimajor` ellipse semimajor axis

`ellipsesemiminor` ellipse semiminor axis

`ellipseangle` ellipse rotation

The function returns `O, 0, 0, 0` if a suitable ellipse
can't be found. (The qgon is probably not a convex polygon.)

### References

http://faculty.mae.carleton.ca/John_Hayes/Papers/InscribingEllipse.pdf
"""
function ellipseinquad(qgon, action=:none)
    p1, p2, p3, p4 = qgon
    length(unique([p1, p2, p3, p4])) != 4 && throw(error("ellipseinquad() the 4 points must be different: $(qgon)"))
    if !ispolyclockwise(qgon)
        reverse!(qgon)
    end
    if !ispolyconvex(qgon)
        return O, 0, 0, 0
    end
    r = [p1.x, p1.y, 1]
    s = [p2.x, p2.y, 1]
    t = [p3.x, p3.y, 1]
    u = [p4.x, p4.y, 1]

    # Create the 'projective collineation' matrix
    # whatever that is
    e1 = s[1] * t[1] * u[2] - r[1] * t[1] * u[2] - s[1] * t[2] * u[1] + r[1] * t[2] * u[1] - r[1] * s[2] * u[1] + r[2] * s[1] * u[1] + r[1] * s[2] * t[1] - r[2] * s[1] * t[1]
    e2 = r[1] * t[1] * u[2] - r[1] * s[1] * u[2] - s[1] * t[2] * u[1] + s[2] * t[1] * u[1] - r[2] * t[1] * u[1] + r[2] * s[1] * u[1] + r[1] * s[1] * t[2] - r[1] * s[2] * t[1]
    e3 = s[1] * t[1] * u[2] - r[1] * s[1] * u[2] - r[1] * t[2] * u[1] - s[2] * t[1] * u[1] + r[2] * t[1] * u[1] + r[1] * s[2] * u[1] + r[1] * s[1] * t[2] - r[2] * s[1] * t[1]
    e4 = s[2] * t[1] * u[2] - r[2] * t[1] * u[2] - r[1] * s[2] * u[2] + r[2] * s[1] * u[2] - s[2] * t[2] * u[1] + r[2] * t[2] * u[1] + r[1] * s[2] * t[2] - r[2] * s[1] * t[2]
    e5 = -s[1] * t[2] * u[2] + r[1] * t[2] * u[2] + s[2] * t[1] * u[2] - r[1] * s[2] * u[2] - r[2] * t[2] * u[1] + r[2] * s[2] * u[1] + r[2] * s[1] * t[2] - r[2] * s[2] * t[1]
    e6 = s[1] * t[2] * u[2] - r[1] * t[2] * u[2] + r[2] * t[1] * u[2] - r[2] * s[1] * u[2] - s[2] * t[2] * u[1] + r[2] * s[2] * u[1] + r[1] * s[2] * t[2] - r[2] * s[2] * t[1]
    e7 = s[1] * u[2] - r[1] * u[2] - s[2] * u[1] + r[2] * u[1] - s[1] * t[2] + r[1] * t[2] + s[2] * t[1] - r[2] * t[1]
    e8 = t[1] * u[2] - s[1] * u[2] - t[2] * u[1] + s[2] * u[1] + r[1] * t[2] - r[2] * t[1] - r[1] * s[2] + r[2] * s[1]
    e9 = t[1] * u[2] - r[1] * u[2] - t[2] * u[1] + r[2] * u[1] + s[1] * t[2] - s[2] * t[1] + r[1] * s[2] - r[2] * s[1]
    mat = [e1 e2 e3 ; e4 e5 e6 ; e7 e8 e9]
    matinv = inv(mat)
    #  coefficients of the ellipse equation ax2 + 2bxy + cy2 + 2dx + 2fy + g = 0
    a = matinv[1,1] * matinv[1,1] + matinv[2,1] * matinv[2,1] - matinv[3,1] * matinv[3,1]
    b = matinv[1,1] * matinv[1,2] + matinv[2,1] * matinv[2,2] - matinv[3,1] * matinv[3,2]
    c = matinv[1,2] * matinv[1,2] + matinv[2,2] * matinv[2,2] - matinv[3,2] * matinv[3,2]
    d = matinv[1,1] * matinv[1,3] + matinv[2,1] * matinv[2,3] - matinv[3,1] * matinv[3,3]
    f = matinv[1,2] * matinv[1,3] + matinv[2,2] * matinv[2,3] - matinv[3,2] * matinv[3,3]
    g = matinv[1,3] * matinv[1,3] + matinv[2,3] * matinv[2,3] - matinv[3,3] * matinv[3,3]

    #  ellipse centre
    b2ac = b*b - a*c
    ellipsecenter = Point((c*d - b*f) / b2ac, (a*f - b*d) / b2ac)

    #  axes
    k = 2(a*f*f + c*d*d + g*b*b - 2*b*d*f - a*c*g) / b2ac
    ac24b2 = sqrt((a - c) * (a - c) + 4b*b)
    ellipsesemimajor = sqrt(abs(k / abs( ac24b2 - a - c)))
    ellipsesemiminor = sqrt(abs(k / abs(-ac24b2 - a - c)))

    #  rotation angle
    if b == 0
        ellipseangle = (a < c) ? 0 : π/2
    else
        ellipseangle = (a < c) ? 0.5 * atan(2b / (a-c)) : π/2 + 0.5 * atan(2b / (a-c))
    end

    # finally
    @layer begin
        translate(ellipsecenter)
        rotate(ellipseangle)
        ellipse(O, 2ellipsesemimajor, 2ellipsesemiminor, action)
    end
    return ellipsecenter, ellipsesemimajor, ellipsesemiminor, ellipseangle
end

"""
    crescent(pos, innerradius, outeradius, action=nothing;
        vertices=false,
        reversepath=false,
        steps = 30)

Create a crescent-shaped polygon, aligned with the current x-axis.
If the inner radius is 0, you'll get a semicircle.

See also ```crescent(pos1, innerradius, pos2, outeradius...)```.

## Examples

Create a filled crescent shape with outer radius of 200, inner radius of 130.
```
crescent(O, 130, 200, :fill)
```

Create a stroked crescent shape; the inner radius of 0 produces a semicircle.

```
crescent(O, 0, 200, :stroke)
```
"""
function crescent(pos::Point, innerradius::Real, outerradius::Real, action=nothing;
        vertices=false,
        reversepath=false,
        steps = 30)
    Δ = π/steps
    ptlist = Point[]
    move(pos + Point(innerradius * sin(0), outerradius * cos(0)))
    push!(ptlist, currentpoint())
    for i in 1:steps
        pt = pos + Point(innerradius * sin(Δ * i), outerradius * cos(Δ * i))
        push!(ptlist, pt)
    end
    for i in 1:steps
        pt = pos + Point(outerradius * sin(π - (Δ * i)), outerradius * cos(π - (Δ * i)))
        push!(ptlist, pt)
    end
    if !vertices
        poly(ptlist, action, close=true, reversepath=reversepath)
    end
    return ptlist
end

"""
crescent(cp1, r1, cp2, r2, action=nothing;
            vertices=false,
            reversepath=false)

Create a crescent-shaped polygon, aligned with the current
x-axis, by finding the intersection of two circles.
The two center positions should be different.

See also ```crescent(point, innerradius, outeradius...)```.

## Examples

Create a filled crescent shape from two circles.

```
crescent(O, 100, O + (60, 0), 150, :fill)
```
"""
function crescent(cp1::Point, r1::Real, cp2::Point, r2::Real, action=nothing;
        vertices=false,
        reversepath=false)
    flag, ip1, ip2 = intersectioncirclecircle(cp1, r1, cp2, r2)
    if flag
        move(ip2)
        carc(cp2, r2, slope(cp2, ip2), slope(cp2, ip1), :path)
        arc(cp1, r1, slope(cp1, ip1), slope(cp1, ip2), :path)
        ptlist = pathtopoly()[1]
        if !vertices
            poly(ptlist, action, close=true, reversepath=reversepath)
        end
        return ptlist
    else
        @info "crescent(): the two arcs won't intersect "
    end
end
# eof


# Adapted from https://github.com/bensadeghi/DecisionTree.jl
__precompile__()

module Estimators
import Base: length, convert, promote_rule, show, start, next, done
export Estimator, Leaf, Node,
       depth,
       fit_regression_tree,
       predict, assign_leaves

float(x) = map(Float64, x)
neg(arr) = map(!, arr) # `.!arr` is invalid in 0.5, and `!arr` triggers a warning in 0.6.
const NO_BEST=(0,0)

# converts a boolean vector into an integer by summing the appropriate powers of 2
boolvec2int(v::Vector{Bool}) = sum(b*v for (b,v) in zip(2.^(0:(length(v)-1)), v))

abstract type Estimator
end

immutable Leaf <: Estimator
    id::Integer
    value::Real
end
    length(leaf::Leaf) = 1
    depth(leaf::Leaf) = 0

Leaf(id::Vector{Bool}, value::Real) = Leaf(boolvec2int(id), value)
Leaf(value::Real) = Leaf(0,value)

immutable Node <: Estimator
    node_id::Integer
    featid::Integer
    featval::Any
    left::Union{Leaf,Node}
    right::Union{Leaf,Node}
end
    length(tree::Node) = length(tree.left) + length(tree.right)
    depth(tree::Node) = 1 + max(depth(tree.left), depth(tree.right))

function Node(id::Vector{Bool}, featid::Integer, featval::Any, left::Union{Leaf,Node}, right::Union{Leaf,Node})
    return Node(boolvec2int(id), featid, featval, left, right)
end

convert(::Type{Node}, x::Leaf) = Node(0, nothing, x, Leaf(nothing,[nothing])) #makes a node with that leaf as it's left node
promote_rule(::Type{Node}, ::Type{Leaf}) = Node # this tells julia to call convert(::node, leaf) on the leaf when this happens
promote_rule(::Type{Leaf}, ::Type{Node}) = Node

function fit_regression_tree{T<:Float64, U<:Real}(X::Matrix{U}, Y::Vector{T}; min_samples_leaf=5, max_depth=-1, node_id::Vector{Bool}=[true])
    if max_depth < -1
        error("Unexpected value for max_depth: $(max_depth) (expected: max_depth >= 0, or max_depth = -1 for infinite depth)")
    end
    if length(Y) <= min_samples_leaf || max_depth==0
        return Leaf(node_id, mean(Y))

    end
    S = _split_mse(X, Y)
    if S == NO_BEST
        return Leaf(node_id, mean(Y))
    end
    feat_id, thresh = S
    split = X[:,feat_id] .< thresh
    return Node(node_id, feat_id, thresh,
                fit_regression_tree(X[split,:], Y[split],
                    min_samples_leaf=min_samples_leaf, max_depth=max(max_depth-1, -1), node_id=vcat(true,node_id)),
                fit_regression_tree(X[neg(split),:], Y[neg(split)],
                    min_samples_leaf=min_samples_leaf, max_depth=max(max_depth-1, -1), node_id=vcat(false,node_id))
                )
end

function _split_mse{T<:Float64, U<:Real}(X::Matrix{U}, Y::Vector{T})
    N, p = size(X)

    best = NO_BEST
    best_val = -Inf

    for i in 1:p
        ord = sortperm(X[:,i])
        X_i = X[ord,i]
        Y_i = Y[ord]
        if N > 100
            if VERSION >= v"0.4.0-dev"
                domain_i = quantile(X_i, linspace(0.01, 0.99, 99);
                                    sorted=true)
            else  # sorted=true isn't supported on StatsBase's Julia 0.3 version
                domain_i = quantile(X_i, linspace(0.01, 0.99, 99))
            end
        else
            domain_i = X_i
        end
        value, thresh = _best_mse_loss(X_i, Y_i, domain_i)
        if value > best_val
            best_val = value
            best = (i, thresh)
        end
    end

    return best
end

function _best_mse_loss{T<:Float64, U<:Real}(X::Vector{U}, Y::Vector{T}, domain)
    # True, but costly assert. However, see
    # https://github.com/JuliaStats/StatsBase.jl/issues/164
    # @assert issorted(X) && issorted(domain)
    best_val = -Inf
    best_thresh = 0.0
    s_l = s2_l = zero(T)
    su = sum(Y)::T
    su2 = zero(T); for l in Y su2 += l*l end  # sum of squares
    nl = 0
    n = length(Y)
    i = 1
    # Because the `X` are sorted, below is an O(N) algorithm for finding
    # the optimal threshold amongst `domain`. We simply iterate through the
    # array and update s_l and s_r (= sum(Y) - s_l) as we go. - @cstjean
    @inbounds for thresh in domain
        while i <= length(Y) && X[i] < thresh
            l = Y[i]

            s_l += l
            s2_l += l*l
            nl += 1

            i += 1
        end
        s_r = su - s_l
        s2_r = su2 - s2_l
        nr = n - nl
        # This check is necessary I think because in theory all Y could
        # be the same, then either nl or nr would be 0. - @cstjean
        if nr > 0 && nl > 0
            loss = s2_l - s_l^2/nl + s2_r - s_r^2/nr
            if -loss > best_val
                best_val = -loss
                best_thresh = thresh
            end
        end
    end
    return best_val, best_thresh
end

predict(leaf::Leaf, feature::Vector) = leaf.value

function predict(tree::Node, X::Vector)
    if tree.featval == nothing
        return predict(tree.left, X)
    elseif X[tree.featid] < tree.featval
        return predict(tree.left, X)
    else
        return predict(tree.right, X)
    end
end

function predict(tree::Union{Leaf,Node}, X::Matrix)
    N = size(X,1)
    predictions = Array{Any}(N)
    for i in 1:N
        predictions[i] = predict(tree, X[i,:])
    end
    println("predictions ->")
    println(predictions)
    if typeof(predictions[1]) <: Float64
        return float(predictions)
    else
        return predictions
    end
end

function predict(estimator_dict::Dict, X::Matrix)
    return Dict(k=>predict(v,X) for (k,v) in estimator_dict)
end

assign_leaves(leaf::Leaf, feature::Vector) = leaf.id

function assign_leaves(tree::Node, X::Vector)
    if tree.featval == nothing
        return assign_leaves(tree.left, X)
    elseif X[tree.featid] < tree.featval
        return assign_leaves(tree.left, X)
    else
        return assign_leaves(tree.right, X)
    end
end

function assign_leaves(tree::Union{Leaf,Node}, X::Matrix)
    N = size(X,1)
    assignments = Array{Int}(N)
    for i in 1:N
        assignments[i] = assign_leaves(tree, X[i,:])
    end
    return assignments
end

function assign_leaves(estimator_dict::Dict, X::Matrix)
    return Dict(k=>assign_leaves(v,X) for (k,v) in estimator_dict)
end

end #module


# This example predicts the temporal evolution of a field U,
# which has to be represented as vectors of matrices.
# Where the field comes from does not matter, but to make the example
# runnable we load one of the test systems of TimeseriesPrediction.
#
# This example uses light cone embedding and a nonlinear Barkley model.
#
# Importantly, the results are compared with the "real" evolution of the
# system.

# ### Simulate a test system
using PyPlot
using TimeseriesPrediction

testdir = dirname(dirname(pathof(TimeseriesPrediction)))*"/test"
@assert isdir(testdir)
include(testdir*"/system_defs.jl")

Ttrain = 300
Ttest = 5
T = Ttrain + Ttest

init = [ 0.6241    0.589685  0.668221   0.194882    0.687645
         0.656243  0.702544  0.476963   0.00236098  0.636111
         0.821854  0.868514  0.242682   0.2588      0.30552
         0.580972  0.355305  0.0805268  0.501724    0.728142
         0.297559  0.708676  0.583552   0.65363     0.555639]

U, V = barkley(T; tskip=100, ssize=(50,50), init = init)
summary(U)

# ### Temporal prediction of field U
γ = 2
τ = 1
r = 1
c = 1
bc = PeriodicBoundary()

pool = U[1 : Ttrain]
test  = U[ Ttrain : T]

em = light_cone_embedding(pool, γ,τ,r,c,bc)
pcaem = PCAEmbedding(pool, em; maxoutdim=5) # PCA speeds things up!

@time pred = temporalprediction(pool, em, Ttest; progress = false)

err = [abs.(test[i]-pred[i]) for i=1:Ttest+1]

println("Maximum error: ", maximum(maximum(e) for e in err))

# ### Plot prediction

# Deduce field maximum values
vmax = max(maximum(maximum(s) for s in test),
           maximum(maximum(s) for s in pred))
vmin = min(minimum(minimum(s) for s in test),
           minimum(minimum(s) for s in pred))

# plot plot plot
for i in [1, length(err)÷2, length(err)]

    fig = figure(figsize=(10,3))
    ax1 = subplot2grid((1,3), (0,0))
    ax2 = subplot2grid((1,3), (0,1))
    ax3 = subplot2grid((1,3), (0,2))

    im1 = ax1[:imshow](pred[i], cmap="viridis", vmin = vmin, vmax = vmax)
    im2 = ax2[:imshow](test[i], cmap="viridis", vmin = vmin, vmax = vmax)
    im3 = ax3[:imshow](err[i], cmap="inferno", vmin = 0, vmax = vmax-vmin)
    for (im, ax) in zip([im1,im2,im3], [ax1,ax2,ax3])
        ax[:get_xaxis]()[:set_ticks]([])
        ax[:get_yaxis]()[:set_ticks]([])
        colorbar(im, ax = ax, fraction=0.046, pad=0.04)#, format="%.1f")
    end
    ax1[:set_title]("Prediction")
    ax2[:set_title]("Real evolution")
    ax3[:set_title]("Absolute error")
    suptitle("frame $i")
    tight_layout(w_pad=0.6, h_pad=0.00001)
    subplots_adjust(top=0.75)
end
#md savefig("barkley_tempo.png"); nothing # hide
#md # ![barkley_tempo](barkley_tempo.png)


using LinearAlgebra

# If i, i+1 are conjugate pair, then they need to be either both in I or both not in I.
# If one of them is in I and the other is not then LAPACK will consider that both of them are in I.
function conditionnumber(sf::Schur, I)
    n = length(sf.values)
    select = zeros(LinearAlgebra.BlasInt, n)
    for i in I
        select[i] = 1
    end
    LinearAlgebra.LAPACK.trsen!('E', 'N', select, copy(sf.T), copy(sf.Z))[4]
end

# Manocha, D. & Demmel, J. Algorithms for intersecting parametric and algebraic curves II: multiple intersections
# Graphical Models and Image Processing, Elsevier, 1995, 57, 81-100
function clusterordschur(M::AbstractMatrix{<:Real}, ɛ)
    if isempty(M)
        # See bug JuliaLang/julia#...
        return Matrix{float(eltype(M))}(undef, 0, 0), Vector{Int}[]
    else
        _clusterordschur(M, ɛ)
    end
end
function _clusterordschur(M::AbstractMatrix{<:Real}, ɛ)
    # M = Z * T * Z' and "values" gives the eigenvalues
    if VERSION >= v"0.7-"
        sf = schur(M)
        Z = sf.Z
        v = sf.values
    else
        sf = schurfact(M)
        Z = sf[:Z]
        v = sf[:values]
    end
    # documentation says that the error on the eigenvalues is ɛ * norm(T) / conditionnumber
    nT = norm(sf.T)

    _atol(I) = ɛ * nT / conditionnumber(sf, I)

    A = typeof(_atol(1))
    V = real(eltype(v))

    ONE = one(one(V) / one(A))

    clusters = Vector{Int}[]
    λ = V[]
    atol = A[]
    # conditionnumber requires that conjugate pair need to be treated together so we first need to handle them
    # If they are in the same cluster then pair them, otherwise it is complex solution so we reject them
    i = 1
    while i <= lastindex(v)
        if isreal(v[i])
            push!(clusters, [i])
            push!(λ, v[i])
            push!(atol, _atol(i))
            i += 1
        else
            @assert i < lastindex(v) && !isreal(v[i+1])
            pairatol = _atol([i, i+1])
            if abs(v[i] - v[i+1]) / pairatol < ONE
                # Pair conjugate pairs into a real eigenvalue
                push!(clusters, [i, i+1])
                push!(λ, real((v[i] + v[i+1]) / 2)) # The imaginary part should be zero anyway
                push!(atol, pairatol)
            end
            i += 2
        end
    end
    σ = sortperm(λ)

    # For eigenvalues not clustered yet, their eigenvalues is quite large.
    # Therefore, if we cluster all i, j close enough at once we migth cluster too much
    # The technique used here is to cluster only the closest pair.
    # Once they are matched, a new atol is computed and if the cluster is complete,
    # this atol will be small which will avoid addition of new eigenvalues.
    while true
        I = 0
        J = 0
        best = ONE
        for i in eachindex(clusters)
            for j in 1:(i-1)
                d = abs(λ[i] - λ[j]) / min(atol[i], atol[j])
                if d < best
                    I = i
                    J = j
                    best = d
                end
            end
        end
        if best < ONE
            # merge I with J
            nI = length(clusters[I])
            nJ = length(clusters[J])
            λ[I] = (λ[I] * nI + λ[J] * nJ) / (nI + nJ)
            append!(clusters[I], clusters[J])
            atol[I] = _atol(clusters[I])
            deleteat!(λ, J)
            deleteat!(clusters, J)
            deleteat!(atol, J)
        else
            break
        end
    end
    Z, clusters
end


module TestCatElements
using Test
using Catlab, Catlab.Theories, Catlab.Graphs, Catlab.CategoricalAlgebra

arr = @acset Graph begin
  V = 2
  E = 1
  src=[1]
  tgt=[2]
end
elarr = elements(arr)

@testset "Arrow Elements" begin
  @test nparts(elarr, :El)  == 3
  @test nparts(elarr, :Arr) == 2
  @test nparts(elarr, :Ob)  == 2
  @test nparts(elarr, :Hom) == 2
  @test elarr[:, :πₑ]    == [1,1,2]
  @test elarr[:, :πₐ]    == [1,2]
  @test elarr[:, :src]   == [3,3]
  @test elarr[:, :tgt]   == [1,2]
  @test elarr[:, :nameo] == [:V, :E]
  @test elarr[:, :nameh] == [:src, :tgt]
  @test elarr[:, :dom]   == [2, 2]
  @test elarr[:, :cod]   == [1, 1]
  @test inverse_elements(elarr, Graph()) == arr
end

b = @acset Graph begin
  V = 2
  E = 1
  src = [1]
  tgt = [2]
end
elb = elements(b)
ThBPG, obmap, hommap = CatElements.presentation(elb)
@test Symbol.(generators(ThBPG)) == [:V_1, :V_2, :E_1, :src_E_1, :tgt_E_1]
@test inverse_elements(elb, Graph()) == b

p₀ = @acset Graph begin
  V = 2
  E = 2
  src=[1,2]
  tgt=[2,1]
end
elpₒ = elements(p₀)
ThPetri, obmap, hommap = CatElements.presentation(elpₒ)
@test Symbol.(generators(ThPetri)) ==
  [:V_1, :V_2, :E_1, :E_2, :src_E_1, :src_E_2, :tgt_E_1, :tgt_E_2]

# Shuffle element-specific indices around, but leave fundamental data unchanged
elpₒ2 = @acset Elements{Symbol} begin
  El=4; Arr=4; Ob=2; Hom=2
  πₑ = [2,1,2,1] # we've swapped indices 2/3 and V/E and src/tgt
  src = [2,4,2,4]; tgt = [1,3,3,1]; πₐ=[2,2,1,1]
  nameo = [:E, :V]
  dom = [1,1]; cod=[2,2]; nameh = [:tgt, :src]
end

@test inverse_elements(elpₒ2, Graph()) == p₀ #
elpₒ3 = deepcopy(elpₒ)  # swap the two arrows in the category of elements
set_subpart!(elpₒ3, :src, [4,3,4,3]) # instead of 3,4,3,4
@test inverse_elements(elpₒ3, Graph()) != p₀
@test is_isomorphic(inverse_elements(elpₒ3, Graph()), p₀)

@acset_type Petri(ThPetri)

sir_eltsch = @acset Petri begin
  V_1 = 3
  V_2 = 2
  E_1 = 3
  E_2 = 3

  src_E_1 = [1,2,2]
  tgt_E_1 = [1,1,2]

  src_E_2 = [1,1,2]
  tgt_E_2 = [2,2,3]
end

@testset "DiBipartite Elements" begin
  @test length(generators(ThPetri)) == 8
  @test nparts(sir_eltsch, :V_1) == 3
  @test nparts(sir_eltsch, :V_2) == 2
  @test nparts(sir_eltsch, :E_1) == 3
  @test nparts(sir_eltsch, :E_2) == 3
  @test sir_eltsch[:, :src_E_2] == [1,1,2]
  @test inverse_elements(elements(sir_eltsch), sir_eltsch) == sir_eltsch
end

# Assign two tgt's to an edge
bad = @acset Elements{Symbol} begin
  El=2; Arr=3; Ob=2; Hom=2
  πₑ = [1,2]
  src = [2,2,1]; tgt = [1,1,1]; πₐ=[1,2,2]
  nameo = [:V, :E]
  dom = [2,2]; cod=[1,1]; nameh = [:src, :tgt]
end
@test_throws ErrorException inverse_elements(bad, Graph())

# Morphisms
###########
e1 = @acset Graph begin V=1;E=2; src=[1,1]; tgt=[1,1] end
swap = ACSetTransformation(e1,e1; V=[1],E=[2,1])
eswap = elements(swap)

@testset "Elements of graph homomorphism" begin
  @test collect(eswap[:El]) == [1,3,2]
  @test collect(eswap[:Arr]) == [2,1,4,3]
end
@testset "inverse of elements of homomorphism" begin
  @test inverse_elements(eswap, e1) == swap
  for h in homomorphisms(sir_eltsch, sir_eltsch) # 24
    @test inverse_elements(elements(h), sir_eltsch) == h
  end
end
end # module


function plot_example_track_results(trackresults; f_if=0, M=10,
                                    save_to_file=missing,
                                    code_length=1023,
                                    top=0.9,
                                    bottom=0.1,
                                    left=0.1,
                                    right=0.9,
                                    wspace=0.1,
                                    hspace=0.1,
                                    marker_size=1,
                                    line_width=1,
                                    use_ylims=true,
                                    truth_snr=missing,
                                    truth_doppler=missing,
                                    truth_doppler_rate=missing,
                                    truth_t=missing,
                                    truth_phi=missing,
                                    truth_code_phase=missing)
    t = trackresults.t
    ZP = trackresults.ZP
    SNR = trackresults.SNR
    x = trackresults.x
    P = trackresults.P
    ϕ = x[1,:] ./ 2π #.* (180/π)
    σ_ϕ = sqrt.(P[1,:]) ./ 2π # .* (180/π)
    f_d = x[2,:] ./ (2π) .- f_if
    σ_f_d = sqrt.(P[2,:]) ./ (2π)
    f_dd = x[3,:] ./ (2π)
    σ_f_dd = sqrt.(P[3,:]) ./ (2π)
    dphi_meas = trackresults.dphi_meas
    dphi_filt = trackresults.dphi_filt
    code_err_meas = trackresults.code_err_meas
    code_err_filt = trackresults.code_err_filt
    code_phase_meas = trackresults.code_phase_meas
    code_phase_filt = trackresults.code_phase_filt
    N_over_2 = floor(Int, length(t)/2)
    fig = figure(figsize=(8, 7.5))
    ax1 = fig.add_subplot(4,2,1)
    ax1.plot(t, real.(ZP), label="real(ZP)", linewidth=line_width)
    ax1.plot(t, imag.(ZP), label="imag(ZP)", linewidth=line_width)
    xlabel("Time (seconds)")
    ylabel("ZP")
    legend()
    title("(a)")
    ax2 = fig.add_subplot(4,2,2)
    if ~ismissing(truth_snr)
        ax2.plot(t, SNR, "k.", markersize=marker_size, label="Measured SNR")
        ax2.axhline(y=truth_snr, c="blue", linestyle="-", label="Truth SNR",
                    linewidth=line_width)
        legend()
    else
        ax2.plot(t, SNR, "k.", markersize=marker_size)
    end
    xlabel("Time (seconds)")
    ylabel("SNR (dB)")
    title("(b)")
    ax3 = fig.add_subplot(4,2,3)
    if ~ismissing(truth_phi)
        phi_diff = truth_phi./2π .- ϕ
        phi_diff = phi_diff .- mean(phi_diff)
        ax3.plot(t,  phi_diff, "k-", label=L"ϕ - \hat{\phi}", linewidth=line_width)
        ax3.plot(t, -σ_ϕ, label=L"\pm\sigma_{\phi}",
                 color="grey", linestyle=":", linewidth=line_width)
        ax3.plot(t, σ_ϕ, color="grey", linestyle=":", linewidth=line_width)
        ylabel(string(L"ϕ - \hat{\phi}", " (cycles)"))
    else
        ax3.plot(t, ϕ, "k-", label=L"\hat{\phi}", linewidth=line_width)
        ax3.plot(t, (ϕ .- σ_ϕ), label=L"\pm\sigma_{\phi}",
                 color="grey", linestyle=":", linewidth=line_width)
        ax3.plot(t, (ϕ .+ σ_ϕ), color="grey", linestyle=":", linewidth=line_width)
        ylabel(string(L"\hat{\phi}", " (cycles)"))
    end
    xlabel("Time (seconds)")
    legend()
    title("(c)")
    ax4 = fig.add_subplot(4,2,4)
    ax4.plot(t, dphi_meas .* (180/π), "k.", label=L"\Delta\phi_{measured}", markersize=marker_size)
    ax4.plot(t, dphi_filt .* (180/π), "b-", label=L"\Delta\phi_{filtered}", linewidth=line_width)
    xlabel("Time (seconds)")
    ylabel(string(L"\Delta\phi", " (degrees)"))
    legend()
    title("(d)")
    ax5 = fig.add_subplot(4,2,5)
    ax5.plot(t, f_d, "k-", label=L"\hat{f}_d", linewidth=line_width)
    ax5.plot(t, f_d .- σ_f_d, label=L"\pm\sigma_{f_d}",
             color="grey", linestyle=":", linewidth=line_width)
    ax5.plot(t, f_d .+ σ_f_d, color="grey", linestyle=":", linewidth=line_width)
    if ~ismissing(truth_doppler)
        ax5.plot(truth_t, truth_doppler, "b-", linewidth=line_width,
                 label=string("Truth ", L"f_d"))
    end
    if use_ylims
        ylim([median(f_d .- σ_f_d.*8), median(f_d .+ σ_f_d.*8)])
    end
    xlabel("Time (seconds)")
    ylabel(string(L"\hat{f}_d", " (Hz)"))
    legend()
    title("(e)")
    ax6 = fig.add_subplot(4,2,6)
    ax6.plot(t, code_err_meas, "k.", label=L"\Delta n_{measured}", markersize=marker_size)
    ax6.plot(t, code_err_filt, "b-", label=L"\Delta n_{filtered}", linewidth=line_width)
    xlabel("Time (seconds)")
    ylabel("Code Phase Error (chips)")
    legend()
    title("(f)")
    ax7 = fig.add_subplot(4,2,7)
    ax7.plot(t, f_dd, "k-", label=L"\hat{\dot{f}}_d", linewidth=line_width)
    ax7.plot(t, f_dd .- σ_f_dd, label=L"\pm\sigma_{\dot{f}_d}",
             color="grey", linestyle=":", linewidth=line_width)
    ax7.plot(t, f_dd .+ σ_f_dd, color="grey", linestyle=":", linewidth=line_width)
    if ~ismissing(truth_doppler_rate)
        ax7.plot(truth_t, truth_doppler_rate, "b-", linewidth=line_width,
                 label=string("Truth ", L"\dot{f}_d"))
    end
    if use_ylims
        ylim([median(f_dd .- σ_f_dd.*4), median(f_dd .+ σ_f_dd.*4)])
    end
    # ylim([-408, 88])
    xlabel("Time (seconds)")
    ylabel(string(L"\hat{\dot{f}}_d", " (Hz/s)"))
    legend()
    title("(g)")
    ax8 = fig.add_subplot(4,2,8)
    ax8.plot(t, code_phase_filt .% code_length, "k-", label=L"\hat{n}", linewidth=line_width)
    intervals = floor(Int, length(t)/M)
    t_sigma_n = Array{Float64}(undef, intervals+1)
    sigma_n = Array{Float64}(undef, intervals+1)
    n = Array{Float64}(undef, intervals+1)
    for i in 1:intervals
        sigma_n[i+1] = std(code_err_meas[(i-1)*M+1:i*M])
        # n[i] = mean(code_phase_filt[(i-1)*M+1:i*M])
        # n[i] = code_phase_filt[(i-1)*M+floor(Int, M/2)]
        n[i+1] = code_phase_filt[i*M]
        t_sigma_n[i+1] = t[i*M]
        if i == 1
            t_sigma_n[1] = t[1]
            n[1] = code_phase_filt[1]
            sigma_n[1] = sigma_n[i+1]
        end
    end
    ax8.plot(t_sigma_n, (n .- sigma_n) .% code_length, label=L"\pm\sigma_n",
             color="grey", linestyle=":", linewidth=line_width)
    ax8.plot(t_sigma_n, (n .+ sigma_n) .% code_length, color="grey", linestyle=":",
             linewidth=line_width)
    if ~ismissing(truth_code_phase)
        ax8.plot(truth_t, truth_code_phase .% code_length, "b-", linewidth=line_width,
                 label=string("Truth ", L"n"))
    end
    xlabel("Time (seconds)")
    ylabel("Code Phase (chips)")
    legend()
    title("(h)")
    subplots_adjust(hspace=hspace, wspace=wspace,  top=top, left=left, right=right,
                    bottom=bottom)
    if ~ismissing(save_to_file)
        savefig(save_to_file, dpi=300)
    end
end


#RSDeltaSigmaPort: Optimization functions
#-------------------------------------------------------------------------------

"values for optimal zeros when opt==1"
const _VALUES_OPTZEROS_OPT1 = [ #Currently not type stable
	[0.0], # n = 1
	[sqrt(1/3)],
	[sqrt(3/5), 0],
	sqrt.([3/7+sqrt(9/49-3/35), 3/7-sqrt(9/49-3/35)]),
	sqrt.([5/9+sqrt(25/81-5/21), 5/9-sqrt(25/81-5/21), 0]), # n = 5
	[0.23862059, 0.66120988, 0.9324696],
	[0, 0.40584371, 0.74153078, 0.94910785],
	[0.18343709, 0.52553345, 0.79666684, 0.96028993],
	[0, 0.32425101, 0.61337056, 0.83603082, 0.9681602],
	[0.1834370913, 0.5255334458, 0.7966668433, 0.9602899327], # n = 10
	[0, 0.26953955, 0.51909468, 0.73015137, 0.88706238, 0.97822864],
	[0.12523875, 0.36783403, 0.58731921, 0.7699033, 0.90411753, 0.9815607],
	[0, 0.23045331, 0.44849063, 0.64234828, 0.8015776, 0.91759824, 0.98418306],
	[0.10806212, 0.31911586, 0.51525046, 0.68729392, 0.82720185, 0.92843513, 0.98628389], # n = 14
]

"values for optimal zeros when opt==2" #or other?; is it called in other cases?
const _VALUES_OPTZEROS_OPT2 = [
	_VALUES_OPTZEROS_OPT1[1], # n = 1
	[0.0],
	_VALUES_OPTZEROS_OPT1[3],
	[0, sqrt(5/7)],
	_VALUES_OPTZEROS_OPT1[5], # n = 5
	sqrt.([0, 7/11+sqrt(56)/33, 7/11-sqrt(56)/33]),
	_VALUES_OPTZEROS_OPT1[7],
	[0, 0.50563161, 0.79017286, 0.95914731],
	_VALUES_OPTZEROS_OPT1[9],
	[0, 0.41572267, 0.67208682, 0.86238894, 0.97342121], # n = 10
	_VALUES_OPTZEROS_OPT1[11],
	[0, 0.35222363, 0.58006251, 0.76647993, 0.90281326, 0.98132047],
	_VALUES_OPTZEROS_OPT1[13],
	[0, 0.30524384, 0.50836649, 0.6836066, 0.82537239, 0.92772336, 0.98615167], # n = 14
]
@assert(length(_VALUES_OPTZEROS_OPT1) == length(_VALUES_OPTZEROS_OPT2))

"""`optZeros = ds_optzeros( n, opt=1 )`
A helper function for the synthesizeNTF function of the Delta-Sigma Toolbox.
Returns the zeros which minimize the in-band noise power of
a delta-sigma modulator's NTF.
"""
function ds_optzeros(n, opt=1)
	norig = n; n = array_round(n) #VERIFYME
	local optZeros

	if opt==0
		optZeros = zeros(ceil(Int, norig/2))
	elseif n > length(_VALUES_OPTZEROS_OPT1)
		throw(ErrorException("Optimized zeros for n>14 are not available."))
	elseif opt==1
		optZeros = _VALUES_OPTZEROS_OPT1[n]
	else
		optZeros = _VALUES_OPTZEROS_OPT2[n]
	end

	# Sort the zeros and replicate them.
	z = sort(optZeros)
	optZeros = zeros(n)
	m=1

	if mod(n,2)==1
		optZeros[1] = z[1]
		z = z[2:length(z)]
		m=m+1
	end
	for i=1:length(z)
		optZeros[m]   =  z[i]
		optZeros[m+1] = -z[i]
		m = m+2
	end
	return optZeros
end

#Last line


import Base.Math: +, -, *, /
import Base: abs2

#generic case to apply a binary operation to two splines
function apply_binary_op{T}(f::Spline{T}, g::Spline{T}, op::Function)
	fdomain = domain(f)
	gdomain = domain(g)

	newdomain_low = max(fdomain[1], gdomain[1])
	newdomain_high = min(fdomain[end], gdomain[end])
	newdomain = (newdomain_low, newdomain_high)
	newknots = Array{T, 1}(0)

	for knot in union(f.knots, g.knots)
		if knot >= newdomain_low && knot <= newdomain_high
			push!(newknots, knot)
		end
	end

	newknots = sort(unique(newknots))
	@assert !any(isnan(newknots))

	newvalues = similar(newknots)

	for (i, knot) in enumerate(newknots)
		newvalues[i] = op(f(knot), g(knot))
	end

	return Spline(newknots, newvalues)
end

#Arithmetic for splines is defined just as for functions, since they are really approximations that can live in a finite computer to functions that cannot. Therefore, addition and scalar multiplication are defined just as you would expect

#f(x)+g(x)
function +{T}(f::Spline{T}, g::Spline{T})
	return apply_binary_op(f, g, (x, y)->x+y)
end

function -{T}(f::Spline{T}, g::Spline{T})
	return apply_binary_op(f, g, (x, y)->x-y)
end

#f(x)*g(x)
function *{T}(f::Spline{T}, g::Spline{T})
	return apply_binary_op(f, g, (x, y)->x*y)
end

#f(x)/g(x)
function /{T}(f::Spline{T}, g::Spline{T})
	return apply_binary_op(f,  g, (x, y)->x/y)
end

#f(x)+c
function +{T}(f::Spline{T}, c::Number)
	f_new = deepcopy(f)
	f_new.values += c
	f_new.a += c

	return f_new
end

#c+f(x)
function +{T}(c::Number, f::Spline{T})
	return f+c
end

#c*f(x)
function *{T}(c::Number, f::Spline{T})
	f_new = deepcopy(f)
	f_new.values *= c
	f_new.a *= c
	f_new.b *= c
	f_new.c *= c
	f_new.d *= c

	return f_new
end

#f(x)*c
function *{T}(f::Spline{T}, c::Number)
	return c*f
end

#f(x)/c
function /{T}(f::Spline{T}, c::Number)
	f_new = deepcopy(f)
	f_new.values /= c
	f_new.a /= c
	f_new.b /= c
	f_new.c /= c
	f_new.d /= c

	return f_new
end

function abs2{T<:Real}(f::Spline{T})
	return f^2
end

###   Now we define function composition for splines and functions, introducing the ∘ operator that does this   ###
function compose{T}(f::Function, g::Spline{T})
	newknots = adaptive_discretize_mesh(g)
	newvalues = similar(newknots)

	for (i, knot) in enumerate(newknots)
		newvalues[i] = f(g(knot))
	end

	return Spline(newknots, newvalues)
end

function ∘{T}(f::Function, g::Spline{T})
	return compose(f, g)
end

export compose
export ∘


#======================================
Methods using semidefinite programming
======================================#
using .SumOfSquares

"""
    new_sos(backend; kwargs...)

Return a new (empty) sum-of-squares optimization problem for the given backend.

### Input

- `backend` -- the backend (also called JuMP `solver`)
- `kwargs`  -- additional keyword arguments

### Output

An instance of `SOSModel` for the given backend and options.
"""
@inline function new_sos(backend, kwargs...)
    𝑂 = Dict(kwargs)

    if :QUIET ∈ keys(𝑂)
        # for mosek solver
        SOSModel(backend, QUIET=𝑂[:QUIET])
    else
        SOSModel(backend)
    end
end

function _enclose(sose::SumOfSquaresEnclosure, f::Function, dom::Interval_or_IntervalBox;
                  kwargs...)

    _enclose_SumOfSquares(f, dom, sose.order, sose.backend; kwargs...)
end

function _enclose_SumOfSquares(f::Function, dom::Interval, order::Int,
                               backend; kwargs...)

    # polynomial variables
    @polyvar x
    p = f(x)

    # box constraints
    B = @set inf(dom) <= x && x <= sup(dom)

    # ============
    # Upper bound
    # ============
    model = new_sos(backend, kwargs...)
    @variable(model, γ) # JuMP decision variable
    @constraint(model, p <= γ, domain=B, maxdegree=order)
    @objective(model, Min, γ)
    optimize!(model)
    upper_bound = objective_value(model)

    # ============
    # Lower bound
    # ============
    model = new_sos(backend, kwargs...)
    @variable(model, γ) # JuMP decision variable
    @constraint(model, p >= γ, domain=B, maxdegree=order)
    @objective(model, Max, γ)
    optimize!(model)
    lower_bound = objective_value(model)

    return Interval(lower_bound, upper_bound)
end

function _enclose_SumOfSquares(f::Function, dom::IntervalBox{N}, order::Int,
                               backend; kwargs...) where {N}

    # polynomial variables
    @polyvar x[1:N]
    p = f(x...)

    # box constraints
    Bi =[@set inf(dom[i]) <= x[i] && x[i] <= sup(dom[i]) for i in 1:N]
    B = reduce(intersect, Bi)

    # ============
    # Upper bound
    # ============
    model = new_sos(backend, kwargs...)
    @variable(model, γ) # JuMP decision variable
    @constraint(model, p <= γ, domain=B, maxdegree=order)
    @objective(model, Min, γ)
    optimize!(model)
    upper_bound = objective_value(model)

    # ============
    # Lower bound
    # ============
    model = new_sos(backend, kwargs...)
    @variable(model, γ) # JuMP decision variable
    @constraint(model, p >= γ, domain=B, maxdegree=order)
    @objective(model, Max, γ)
    optimize!(model)
    lower_bound = objective_value(model)

    return Interval(lower_bound, upper_bound)
end


using KernelDensitySJ
using Distributions
using StableRNGs
using Test

import KernelDensitySJ: ϕ4,ϕ6,ϕ4bounds,ϕ6bounds

include("reference.jl")

@testset "Bandwidth" begin
	include("test_bw.jl")
end
@testset "Smoothing" begin
	include("test_smooth.jl")
end


# import packages
using RoME, Distributions

# [OPTIONAL] add more julia processes to speed up inference
using Distributed
nprocs() < 3 ? addprocs(4-nprocs()) : nothing

@everywhere using RoME

# start with an empty factor graph object
fg = initfg()

# Add the first pose :x0
addVariable!(fg, :x0, Pose2)

# Add at a fixed location PriorPose2 to pin :x0 to a starting location
addFactor!(fg, [:x0], PriorPose2(MvNormal(zeros(3), diagm( [0.1;0.1;0.1].^2 ))))

# Drive around in a hexagon a number of times
for i in 0:12
    psym = Symbol("x$i")
    nsym = Symbol("x$(i+1)")
    addVariable!(fg, nsym, Pose2)
    pp = Pose2Pose2(MvNormal([10.0;0;pi/3], diagm( [0.1;0.1;0.1].^2 ) ))
    addFactor!(fg, [psym;nsym], pp )
end

# perform inference, and remember first runs are slower owing to Julia's just-in-time compiling
solveTree!(fg);

## Inter-operating visualization packages for Caesar/RoME/IncrementalInference exist
using Cairo, RoMEPlotting
Gadfly.set_default_plot_size(35cm,20cm)

# For Juno/Jupyter style use
pl = plotSLAM2D(fg, contour=false)
# For scripting use-cases you can export the image
pl |> Gadfly.PDF("/tmp/test1.pdf", 20cm, 10cm)  # or PNG(...)

# Add a landmark l1
addVariable!(fg, :l1, Point2, tags=[:LANDMARK;])

bear1 = atan(10,20)
bear2 = atan(10,10) - pi/3

# Add landmarks with Bearing range measurements at x0, x6, x12, x18, x24, x30...
# Get variables
vars = ls(fg, r"x") |> sortDFG
for xIndex in 1:6:length(vars)
    @info("Creating factor between $(vars[xIndex]) and l1...")
    p2br = Pose2Point2Bearing(Normal(bear1,0.05))
    addFactor!(fg, [vars[xIndex]; :l1], p2br)
    p2br = Pose2Point2Bearing(Normal(bear2,0.05))
    addFactor!(fg, [vars[xIndex+1]; :l1], p2br)
end

# plotKDE(fg, [:x0;:x6;:x12], dims=[1;2])
# plotKDE(fg, [:x0;:x6;:x12], dims=[3])
# plotKDE(fg, [:x1;:x7;:x13], dims=[1;2])

# drawGraph(fg, show=true)

# Initialize :l1 numerical values but do not rerun solver
ensureAllInitialized!(fg)
pl = plotSLAM2D(fg, contour=false)
pl |> Gadfly.PDF("/tmp/test2.pdf", 20cm, 10cm)  # or PNG(...)

# solve, at time of writing compute time should less than 45s
solveTree!(fg);

# redraw to see that additional information aids in refining the results
pl = plotSLAM2D(fg, contour=false)
pl |> Gadfly.PDF("/tmp/test3.pdf", 20cm, 10cm)  # or PNG(...)


include("../utils.jl")
include("group_utils.jl")

@testset "Product group" begin
    SOn = SpecialOrthogonal(3)
    Tn = TranslationGroup(2)
    Rn = Rotations(3)
    M = ProductManifold(SOn, Tn)
    G = ProductGroup(M)
    @test G isa ProductGroup
    @test base_manifold(G) === M
    @test repr(G) == "ProductGroup($(SOn), $(Tn))"
    x = Matrix{Float64}(I, 3, 3)

    t = Vector{Float64}.([1:2, 2:3, 3:4])
    ω = [[1.0, 2.0, 3.0], [3.0, 2.0, 1.0], [1.0, 3.0, 2.0]]
    tuple_pts = [(exp(Rn, x, hat(Rn, x, ωi)), ti) for (ωi, ti) in zip(ω, t)]
    tuple_v = (hat(Rn, x, [1.0, 0.5, -0.5]), [-1.0, 2.0])

    @testset "identity specializations" begin
        @test inv(G, Identity(G)) === Identity(G)
        @test identity(G, Identity(G)) === Identity(G)
        @test submanifold_component(G, Identity(G), Val(1)) === Identity(SOn)
        @test submanifold_component(G, Identity(G), Val(2)) === Identity(Tn)
        @test submanifold_components(G, Identity(G)) == (Identity(SOn), Identity(Tn))
        @test compose(G, Identity(G), Identity(G)) === Identity(G)
    end

    @testset "product point" begin
        reshapers = (Manifolds.ArrayReshaper(), Manifolds.StaticReshaper())
        for reshaper in reshapers
            shape_se = Manifolds.ShapeSpecification(reshaper, M.manifolds...)
            pts = [Manifolds.prod_point(shape_se, tp...) for tp in tuple_pts]
            v_pts = [Manifolds.prod_point(shape_se, tuple_v...)]
            @test compose(G, pts[1], Identity(G)) == pts[1]
            @test compose(G, Identity(G), pts[1]) == pts[1]
            test_group(G, pts, v_pts; test_diff = true)
        end
    end

    @testset "product repr" begin
        pts = [ProductRepr(tp...) for tp in tuple_pts]
        v_pts = [ProductRepr(tuple_v...)]
        @test compose(G, pts[1], Identity(G)) == pts[1]
        @test compose(G, Identity(G), pts[1]) == pts[1]
        test_group(G, pts, v_pts; test_diff = true, test_mutating = false)
    end
end


import LinearAlgebra: ×
import Base: ^, getindex, ndims

export Circle, S¹, Torus
export ×, ndims, info

struct Circle{ℱ}
    a::ℱ
    b::ℱ
end

function Circle(b::FT) where {FT}
    return Circle(FT(0), b)
end

function Circle()
    return Circle(2π)
end

S¹ = Circle

function Base.show(io::IO, Ω::Circle)
    printstyled(io, "[", color = 226)
    a = @sprintf("%.3f", Ω.a)
    b = @sprintf("%.3f", Ω.b)
    printstyled("$a, $b", color = 7)
    printstyled(io, ")", color = 226)
end

struct Torus{DT}
    domains::DT
end

function Base.show(io::IO, Ω::Torus)
    for (i, domain) in enumerate(Ω.domains)
        print(domain)
        if i != length(Ω.domains)
            printstyled(io, "×", color = 118)
        end
    end
end

function ndims(Ω::Circle)
    return 1
end

function ndims(Ω::Torus)
    return length(Ω.domains)
end

getindex(t::Torus, i) = t.domains[i]

# Algebra
×(arg1::Circle, arg2::Circle) = Torus((arg1, arg2))
×(args::Torus, arg2::Circle) = Torus((args.domains..., arg2))
×(arg1::Circle, args::Torus) = Torus((arg1, args.domains...))
×(arg1::Torus, args::Torus) = Torus((arg1.domains..., args.domains...))
×(args::Torus) = Torus(args...)
# Exponentiation
^(circle::Circle, n::Int) = ^(circle, Val(n))
^(circle::Circle, ::Val{1}) = circle
^(circle::Circle, ::Val{2}) = circle × circle
^(circle::Circle, ::Val{n}) where {n} = circle × ^(circle, Val(n - 1))

function info(Ω::Torus)
    println("This is a ", ndims(Ω), "-dimensional Torus.")
    print("The domain is ")
    println(Ω, ".")
    for (i, domain) in enumerate(Ω.domains)
        domain_string = "periodic"
        length = @sprintf("%.2f ", domain.b - domain.a)
        println("The dimension $i domain is ", domain_string, " with length ≈ ", length)

    end
    return nothing
end


using Plots

using FourierGPE
## Initialize simulation
L = (18.0,18.0)
N = (128,128)
sim = Sim(L,N)
@unpack_Sim sim

## set simulation parameters
μ = 12.0

# Time dependent potential function (here trivial t dep)
import FourierGPE.V
V(x,y,t) = 0.5*(x^2 + y^2)
ψ0(x,y,μ,g) = sqrt(μ/g)*sqrt(max(1.0-V(x,y,0.0)/μ,0.0)+im*0.0)

## make initial state
x,y = X
ψi = ψ0.(x,y',μ,g)
ϕi = kspace(ψi,sim)
@pack_Sim! sim

## evolve
@time sol = runsim(sim)

## ground state
ϕg = sol[end]
ψg = xspace(ϕg,sim)
showpsi(x,y,ψg)

## set simulation parameters
γ = 0.0
t = LinRange(ti,tf,Nt)
ϕi = kspace(ψg,sim)
# reltol = 1e-7
# alg = DP5()

## vortex
using VortexDistributions
R(w) = sqrt(2*μ/w^2)
R(1)
rv = 3.
healinglength(x,y,μ,g) = 1/sqrt(g*abs2(ψ0(x,y,μ,g)))
ξ0 = healinglength.(0.,0.,μ,g)
ξ = healinglength(rv,0.,μ,g)

pv = PointVortex(rv,0.,1)
vi = ScalarVortex(ξ,pv)

psi = Torus(copy(ψg),x,y)
vortex!(psi,vi)

ψi .= psi.ψ
ϕi = kspace(ψi,sim)
showpsi(x,y,psi.ψ)
## compare precession with Fetter JLTP 2010
ξ = 1/sqrt(μ)
Rtf = R(1)
Ωm = 3*log(Rtf/ξ/sqrt(2))/2/Rtf^2
Ωv = Ωm/(1-rv^2/Rtf^2)

# or a precession period
Tv = 2*pi/Ωv

ti = 0.; tf = Tv
t = LinRange(ti,tf,Nt)

@pack_Sim! sim
@time solv = runsim(sim)

## analyse
ϕf = solv[end]
ψf = xspace(ϕf,sim)

showpsi(x,y,ψf)

anim = @animate for i in eachindex(t)
    ψ = xspace(solv[i],sim)
    showpsi(x,y,ψ)
end

saveto = joinpath(@__DIR__,"vortex_precession.gif")
gif(anim,saveto,fps=25)


# ====================================================================
# Component structure
#
mutable struct Lorentzian_1D <: AbstractComponent
    norm::Parameter
    center::Parameter
    fwhm::Parameter

    function Lorentzian_1D(norm::Number, center::Number, fwhm::Number)
        @assert norm  > 0
        @assert fwhm > 0

        out = new(Parameter(norm), Parameter(center), Parameter(fwhm))
        out.norm.low = 0
        out.fwhm.low = 0
        return out
    end
end

mutable struct Lorentzian_2D <: AbstractComponent
    norm::Parameter
    centerX::Parameter
    centerY::Parameter
    fwhmX::Parameter
    fwhmY::Parameter

    function Lorentzian_2D(norm::Number, centerX::Number, centerY::Number, fwhmX::Number, fwhmY::Number)
        @assert norm   > 0
        @assert fwhmX > 0
        @assert fwhmY > 0

        out = new(Parameter(norm), Parameter(centerX), Parameter(centerY), Parameter(fwhmX), Parameter(fwhmY))
        out.norm.low = 0
        out.fwhmX.low = 0
        out.fwhmY.low = 0

        return out
    end
end

Lorentzian(norm, center, fwhm) = Lorentzian_1D(norm, center, fwhm)
Lorentzian(norm, centerX, centerY, fwhmX, fwhmY) = Lorentzian_2D(norm, centerX, centerY, fwhmX, fwhmY)

# ====================================================================
# Allocate the output array and the component `cdata`
struct Lorentzian_cdata <: AbstractComponentData; end

cdata(comp::Lorentzian_1D, domain::Domain_1D) = Lorentzian_cdata()
cdata(comp::Lorentzian_2D, domain::Domain_2D) = Lorentzian_cdata()

# ====================================================================
# Evaluate component
function evaluate!(cdata::Lorentzian_cdata, output::Vector{Float64}, domain::Domain_1D,
                   norm, center, fwhm)
    x = domain[1]

    @. (output = norm /
        (1. +
         ((x - center) / fwhm)^2.
        ))
    return output
end

function evaluate!(cdata::Lorentzian_cdata, output::Vector{Float64}, domain::Domain_2D,
                   norm, centerX, centerY, fwhmX, fwhmY)
    x = domain[1]
    y = domain[2]

    @. (output = norm /
        (1. +
         ((x - centerX) / fwhmX)^2. +
         ((y - centerY) / fwhmY)^2.
         ))

    return output
end


"""
    stalta(s::SACtr, t, ws, wl) -> ::SACFloat

Return the ratio of the short-term average (window length `ws` s) to long-term
average (length `wl` s) at time `t` s.

    stalta(s::SACtr, t1, t2, ws, wl) -> ::Vector{SACFloat}

Return STA/LTA for each sample between times `t1` and `t2` s.

    stalta(s::SACtr, ws, wl) -> ::SACtr

Return STA/LTA for whole trace as a `SACtr` object.

#### Note on window positions

In this implemetation, the ratio refers to that between a long time window *before*
time `t` to a short window *after* `t`, and here the short window includes the sample
*at* `t` (or the nearest thereto).
"""
stalta(s::SACtr, t, ws, wl) = window_average(s, t, ws)/window_average(s, t-wl-s.delta, wl)

function stalta(s::SACtr, t1, t2, ws, wl)
   n = round(Int, (t2 - t1)/s.delta) + 1
   ratio = Array{eltype(s.t)}(undef, n)
   for i in 1:n
       t = t1 + (i-1)*s.delta
       ratio[i] = stalta(s, t, ws, wl)
   end
   ratio
end

function stalta(s::SACtr, ws, wl)
    t1 = s.b + wl + 2s.delta
    t2 = s.e - ws - s.delta
    t = stalta(s, t1, t2, ws, wl)
    s2 = deepcopy(s)
    s2.t = t
    s2.npts = length(t)
    s2.b = t1
    update_headers!(s2)
end

"""
    window_average(s::SACtr, ref_time, window_length, centred=false)

Return the mean value of the absolute amplitude within a window
`window_length` s long.  `ref_time` in s may be the start of the window (default)
or the centre if `centred` is `true`.

Note that window times down to 0 are allowed, in which case the amplitude of
the nearest point is returned.
"""
function window_average(s::SACtr, time, window_length, centred=false)
    window_length >= 0 || error("`window_length` ($window_length) must be positive")
    n = round(Int, window_length/s.delta) + 1
    it1 = round(Int, (time - s.b - (centred ? window_length/2 : 0.0))/s.delta) + 1
    it2 = it1 + n - 1
    it1 > 0 && it2 <= s.npts ||
        error("Window of times $time - $(time+window_length) s lies outside " *
              "trace (trace indices $it1 - $it2; start time $(s.b) s; end time $(s.e)) s")
    ave = zero(SACFloat)
    @inbounds for i in it1:it2
        ave += abs(s.t[i])
    end
    ave/n
end


function matmul!(A, B, R)
    for i = 1:size(A)[1], j = 1:size(B)[2], k = 1:size(B)[1]
        R[i, j] += A[i, k] * B[k, j]
    end
end


# implements sparsity-exploiting versions of the main functions that
# take advantage of neighborhoods in the data

# computes a sparse approximation to the conditional neighbor matrix based on
# neighborhood graph "neighbors" computed with a fast nearsest neighbor algorithm (see util)
function conditional_neighbor(X::AbstractMatrix, σ²::AbstractVector{<:Real},
        neighbors::AbstractVecOfVec{Int}, kernel = gaussian, distance = euclidean)
    x = [c for c in eachcol(X)]
    n = length(x)
    K = spzeros(n, n)
    for i in 1:n
        xn, sn = @views x[neighbors[i]], σ²[neighbors[i]]
        @. K[neighbors[i], i] = kernel(distance(xn, (x[i],)) / sqrt(σ²[i]))
    end
    sumK = sum(K, dims = 1) # this is now a sparse operation!
    # @. K ./= sumK # this yields a dense array!
    for (i, k) in enumerate(eachcol(K))
        k ./= sumK[i]
    end
    return K
end

function optimize_variances(X::AbstractMatrix, u::Real, neighbors::AbstractVecOfVec{Int}, kernel = gaussian, distance = euclidean;
                            tol::Real = 1e-6, max_iter::Int = 32)
    n = size(X, 2)
    σ² = fill(5one(eltype(X)), n) # if this is too small initially, Newton has problems
    @threads for i in 1:n
        σ²[i] = optimize_variances(X, u, i, σ²[i], kernel, distance, tol = tol, max_iter = max_iter)
    end
    any(isnan, σ²) && throw(DomainError("Warning: variance optimization yielded NaN: ", σ²))
    return σ²
end

# optimizing variances using sparse operations
# neighbors_i are the indices of the neighbors of x[i]
function optimize_variances(X::AbstractMatrix, u::Real, i::Int, σ²::Real,
                            neighbors_i::AbstractVector{Int},
                            kernel = gaussian, distance = euclidean;
                            tol::Real = 1e-6, max_iter::Int = 32)
    x = [c for c in eachcol(X)]
    n = length(x)
    xn = @view x[neighbors_i]
    function objective(log_σ²::Real) # allocating version, is faster for high d, equal for low
        local σ² = exp(log_σ²)
        p = @. kernel(distance((x[i],), xn)/σ²) # only evaluate kernel function on neighbors
        p ./= sum(p)
        p = SparseVector(n, neighbors_i, p) # create sparse vectors
        entropy(p) - log2(u)
    end
    log_σ² = newtons_method(objective, log(σ²), tol = tol, max_iter = max_iter)
    return exp(log_σ²)
end


#imports

using JuMP, Gurobi, DelimitedFiles, Random

function atualize_lowerBound(A,c,x_res,indCost)

    (m,n) = size(A)
    I = (1:m)
    J = (1:n)
    x_resLower = zeros(n,1)
    global x_res_temp = zeros(n,1)

    for indice_j in indCost
        indice_j = floor(Int,indice_j)
        if x_res[indice_j] == 1
            x_res_temp[indice_j] = 1
            value_mult = A*x_res_temp

            flag = 0
            for i in I
                if value_mult[i] > 1

                    flag = 1
                    #break
                end
            end

            if flag == 0
                x_resLower[indice_j] = 1
            end

            global x_res_temp = copy(x_resLower)

        end
    end

    return c'*x_resLower

end

function RelaxLagrang(A, c, maxiter = 50, tol = 1e-1)
    spp   = Model(Gurobi.Optimizer);
    (m,n) = size(A)

    I = (1:m)
    J = (1:n)

    @variable(spp, x[1:n], Bin);
    u     = 1*ones(m,1);
    zs = []
    irep    = 0;
    iter    = 0;
    lower_b = 0;
    upper_b = Inf;

    step_size = 0
    grad      = zeros(m,1);
    indCost   = zeros(1,n)

    for j1 in J
        maior_valor = -Inf
        for j2 in J
            if (c[j2] >=  maior_valor) && ((j2 in indCost) == false)
                global maior_indice = j2
                 maior_valor = c[j2]
            end
        end
        indCost[j1] = maior_indice
    end

    while iter <= maxiter && abs(upper_b - lower_b) >= tol

        iter += 1
        u = max.(0, u - step_size*grad)

        @objective(spp, Max, sum(c.*x) - sum(u[i]* (sum(A[i,j]*x[j] for j in (1:n)) - 1) for i in (1:m)))
        optimize!(spp)

        x_res    = value.(x)
        soma_res = A*x_res
        z        = objective_value(spp);
        append!(zs, z)

        best_ub_old = upper_b;
        upper_b = min(upper_b, z);

        if best_ub_old == upper_b
            irep += 1;
        else
            irep = 0;
            lower_b = atualize_lowerBound(A,c,x_res,indCost)[1];

        end

        grad = ones(m,1) - soma_res + ones(m,1)*rand(1)/10
        sum_g2 = sum(grad[i]^2 for i in I)
        #step_size = (z - lower_b)/sum_g2
        step_size = 1/iter
        #step_size  = 0.008

    end

    return zs, iter
end

function loadSPP(fname)
    f=open(fname)

    m, n = parse.(Int, split(readline(f)))
    C = parse.(Int, split(readline(f)))
    A=zeros(Int, m, n)
    for i=1:m
        readline(f)
        for valor in split(readline(f))
          j = parse(Int, valor)
          A[i,j]=1
        end
    end
    close(f)
    return C, A
end

function setSPP(solverSelected, C, A)
  m, n = size(A)
  ip = Model(solverSelected)
  @variable(ip, x[1:n], Bin)
  @objective(ip, Max, sum(C.*x))
  @constraint(ip , cte[i=1:m], sum(A[i,j] * x[j] for j=1:n) <= 1)
  return ip, x
end

function setSPP_relaxlin(solverSelected, C, A)
  m, n = size(A)
  ip = Model(solverSelected)
  @variable(ip, 0 <= x[1:n] <= 1)
  @objective(ip, Max, sum(C.*x))
  @constraint(ip , cte[i=1:m], sum(A[i,j] * x[j] for j=1:n) <= 1)
  return ip, x
end

function integer_test(x)
    if floor(x)^2 - x^2 == 0
        return 1
    end
    return 0
end

function branch_ceil(solverSelected, C, A, idxs = [])
  m, n = size(A)
  ip = Model(solverSelected)
  @variable(ip, 0 <= x[1:n] <= 1)
  for idx in idxs
    @constraint(ip, x[idx] == 1)
  end
  @objective(ip, Max, sum(C.*x))
  @constraint(ip , cte[i=1:m], sum(A[i,j] * x[j] for j=1:n) <= 1)
  return ip, x
end

function branch_floor(solverSelected, C, A, idxs = [])
  m, n = size(A)
  ip = Model(solverSelected)
  @variable(ip, 0 <= x[1:n] <= 1)
  for idx in idxs
    @constraint(ip, x[idx] == 0)
  end
  @objective(ip, Max, sum(C.*x))
  @constraint(ip , cte[i=1:m], sum(A[i,j] * x[j] for j=1:n) <= 1)
  return ip, x
end

function BranchNBound(A, c, maxiter = 50, tol = 1e-1)

    rp, rp_x = setSPP_relaxlin(Gurobi.Optimizer, c, A)
    nodes    = [[rp,[]]]
    s,       = size(nodes)
    values   = []
    idxs     = []

    while s >= 0

        if s == 0
            break
        end

        p,idxs   = nodes[1]

        if s == 1
            nodes = []
            s     = 0
        else
            nodes = nodes[2:s]
            s,     = size(nodes)
        end

        try
           optimize!(p)
        catch
           println(" Modelo Infeasible. Cortem as árvores! ")
           ss,       = size(idxs)
           idxs      = idxs[1:ss-1]

        end
        append!(values, objective_value(p))

        for (i,xs) in enumerate(value.((all_variables(p))))
            if integer_test(xs) == 0

                push!(idxs, i)
                print("----------> ", idxs, " <---------")
                print(" BRANCHING ")

                floor_prob, f_x = branch_floor(Gurobi.Optimizer, c, A,idxs)
                push!(nodes, floor_prob)

                ceil_prob, c_x  =  branch_ceil(Gurobi.Optimizer, c, A,idxs)
                push!(nodes, ceil_prob)

                s += 2
                print("--> ", values, " <---")
                break
            end
        end
    end

    return objective_value(rp), value.(rp_x), values
end

function Branch_Bound(A,c)

    rp, rp_x = setSPP(Gurobi.Optimizer, c, A)

    println("Solving...");
    set_optimizer_attribute(rp, "Presolve", 0)
    set_optimizer_attribute(rp, "Cuts", 0)
    set_optimizer_attribute(rp, "Heuristics", 0)
    optimize!(rp)
end

function Branch_Cut(A,c)

    p, p_x = setSPP(Gurobi.Optimizer, c, A)

    println("Solving...");
    set_optimizer_attribute(p, "Presolve", 0)
    set_optimizer_attribute(p, "Cuts", 0)
    set_optimizer_attribute(p, "Heuristics", 0)
    set_optimizer_attribute(p, "CliqueCuts", 1)
    optimize!(p)

end


# ------------------------------------------------------------------
# Copyright (c) 2018, Júlio Hoffimann Mendes <juliohm@stanford.edu>
# Licensed under the ISC License. See LICENCE in the project root.
# ------------------------------------------------------------------

@userplot ParetoPlot

@recipe function f(p::ParetoPlot)
  # get user input
  data = p.args[1]

  x = sort(data, rev=true)
  n = length(x)
  logp = log.([i/(n+1) for i in 1:n])
  logx = log.(x)

  seriestype --> :scatter
  xlabel --> "-log(i/(n+1))"
  ylabel --> "log(x*)"

  -logp, logx
end


using Random, Distributions, StatsPlots
Random.seed!(0)

data = randn(10^2)
histogram(data, bins=8)


## Aircraft analysis benchmarking
using BenchmarkTools
using StaticArrays

# All subsequent analyses use no symmetry tricks for performance as AeroMDAO hasn't implemented it and apples must be compared to apples.

## AeroMDAO tests: https://github.com/GodotMisogi/AeroMDAO.jl
#=======================================================#
using AeroMDAO

println("AeroMDAO Aircraft Functional -")
@benchmark begin
    # Wing
    wing = Wing(foils     = Foil.(fill(naca4((0,0,1,2)), 2)),
                chords    = [1.0, 0.6],
                twists    = [2.0, 2.0],
                spans     = [5.0],
                dihedrals = [11.31],
                sweep_LEs = [2.29]);

    # Horizontal tail
    htail = Wing(foils     = Foil.(fill(naca4((0,0,1,2)), 2)),
                 chords    = [0.7, 0.42],
                 twists    = [0.0, 0.0],
                 spans     = [1.25],
                 dihedrals = [0.],
                 sweep_LEs = [6.39])

    # Vertical tail
    vtail = HalfWing(foils     = Foil.(fill(naca4((0,0,0,9)), 2)),
                     chords    = [0.7, 0.42],
                     twists    = [0.0, 0.0],
                     spans     = [1.0],
                     dihedrals = [0.],
                     sweep_LEs = [7.97])

    wing_panels  = panel_wing(wing, 20, 20, spacing = "cosine")

    htail_panels = panel_wing(htail, 12, 12;
                              position = [4., 0, 0],
                              angle    = deg2rad(0.),
                              axis 	   = [0., 1., 0.],
                              spacing  = "cosine"
                             )

    vtail_panels = panel_wing(vtail, 12, 10;
                              position = [4., 0, 0],
                              angle    = π/2,
                              axis 	   = [1., 0., 0.],
                              spacing  = "cosine"
                             )

    aircraft = Dict("Wing" 			  	=> wing_panels,
                    "Horizontal Tail" 	=> htail_panels,
                    "Vertical Tail"   	=> vtail_panels)

    # display(size.([ wing_panels[1], htail_panels[1], vtail_panels[1] ])) # Checking sizes

    ρ       = 1.225
    x_ref   = [0.5, 0., 0.]
    V, α, β = 1.0, 5.0, 0.0
    Ω       = [0.0, 0.0, 0.0]
    fs      = AeroMDAO.Freestream(V, α, β, Ω)
    S, b, c = 9.0, 10.0, 0.9

    data = solve_case(aircraft, fs;
                      rho_ref   = ρ,
                      r_ref     = x_ref,
                      area_ref  = S,
                      span_ref  = b,
                      chord_ref = c)

    nf, ff = data["Aircraft"][1:2]

    nf[1:3], nf[4:6], ff[1]
end

## BYU FLOW Lab tests: https://github.com/byuflowlab/VortexLattice.jl
#=======================================================#

# Example taken from: https://flow.byu.edu/VortexLattice.jl/dev/examples/

# Pulled these out to avoid compilation for proper benchmarking
fc   = fill((xc) -> 0, 2) # camberline function for each section
fc_h = fill((xc) -> 0, 2) # camberline function for each section
fc_v = fill((xc) -> 0, 2) # camberline function for each section

##
using VortexLattice

println("BYU FLOW Lab VortexLattice.jl - ")
@benchmark begin
    # wing
    xle = [0.0, 0.2]
    yle = [0.0, 5.0]
    zle = [0.0, 1.0]
    chord = [1.0, 0.6]
    theta = [2.0*pi/180, 2.0*pi/180]
    phi = [0.0, 0.0]
    ns = 20
    nc = 20
    spacing_s = Cosine()
    spacing_c = Cosine()
    mirror = true

    # horizontal stabilizer
    xle_h = [0.0, 0.14]
    yle_h = [0.0, 1.25]
    zle_h = [0.0, 0.0]
    chord_h = [0.7, 0.42]
    theta_h = [0.0, 0.0]
    phi_h = [0.0, 0.0]
    ns_h = 12
    nc_h = 12
    spacing_s_h = Cosine()
    spacing_c_h = Cosine()
    mirror_h = true

    # vertical stabilizer
    xle_v = [0.0, 0.14]
    yle_v = [0.0, 0.0]
    zle_v = [0.0, 1.0]
    chord_v = [0.7, 0.42]
    theta_v = [0.0, 0.0]
    phi_v = [0.0, 0.0]
    ns_v = 12
    nc_v = 10
    spacing_s_v = Cosine()
    spacing_c_v = Cosine()
    mirror_v = false

    Sref = 9.0
    cref = 0.9
    bref = 10.0
    rref = [0.5, 0.0, 0.0]
    Vinf = 1.0
    ref = Reference(Sref, cref, bref, rref, Vinf)

    alpha = 5.0*pi/180
    beta = 0.0
    Omega = [0.0; 0.0; 0.0]
    fs = VortexLattice.Freestream(Vinf, alpha, beta, Omega)

    symmetric = [false, false, false]

    # generate surface panels for wing
    wgrid, wing = wing_to_surface_panels(xle, yle, zle, chord, theta, phi, ns, nc;
        mirror=mirror, fc = fc, spacing_s=spacing_s, spacing_c=spacing_c)

    # generate surface panels for horizontal tail
    hgrid, htail = wing_to_surface_panels(xle_h, yle_h, zle_h, chord_h, theta_h, phi_h, ns_h, nc_h;
        mirror=mirror_h, fc=fc_h, spacing_s=spacing_s_h, spacing_c=spacing_c_h)
    translate!(hgrid, [4.0, 0.0, 0.0])
    translate!(htail, [4.0, 0.0, 0.0])

    # generate surface panels for vertical tail
    vgrid, vtail = wing_to_surface_panels(xle_v, yle_v, zle_v, chord_v, theta_v, phi_v, ns_v, nc_v;
        mirror=mirror_v, fc=fc_v, spacing_s=spacing_s_v, spacing_c=spacing_c_v)
    translate!(vgrid, [4.0, 0.0, 0.0])
    translate!(vtail, [4.0, 0.0, 0.0])

    # display(size.([ wing, htail, vtail ])) # Checking sizes

    grids = [wgrid, hgrid, vgrid]
    surfaces = [wing, htail, vtail]
    surface_id = [1, 2, 3]

    system = steady_analysis(surfaces, ref, fs; symmetric=symmetric, surface_id=surface_id)

    CF, CM = body_forces(system; frame=Wind())

    CDiff = far_field_drag(system)

    CF, CM, CDiff
end


using BenchmarkTools, McCormick, Random
using Statistics: std

# Note: Run time is still random so results are't strictly reproducable...
Random.seed!(1234)

ex_softplus(x) = log(1 + exp(x))
ex_maxsig(x) =  max(x, 1/(1 + exp(-(x))))
ex_maxtanh(x) = max(x,tanh(x))
ex_softsign(x) = (x)/(1 + x)
ex_sigmoid(x) = 1/(1 + exp(-(x)))
ex_silu(x) = (x)/(1 + exp(-(x)))
ex_gelu(x) = (x)*(1 + erf((x)/sqrt(2)))/2

const TRIAL_REPEAT = 10000

function loop(func, x)
    foreach(i -> func(x), 1:TRIAL_REPEAT)
end
function summarize!(n, f_trial, ex_trial)
    env_min = median(f_trial.times)/TRIAL_REPEAT
    expr_min = median(ex_trial.times)/TRIAL_REPEAT
    ratio_percent = 100*(env_min/expr_min - 1)
    println("$n | env = $(env_min) | exp = $(expr_min) | ratio = $(ratio_percent)")
end
#start_mc() = MC{1,NS}(0.0, Interval(-3,3), 1)
start_mc() = MC{1,NS}(6*rand()-3, Interval(-3,3), 1)

f_trial = @benchmark loop(softplus,x) setup=(x=start_mc())
ex_trial = @benchmark loop(ex_softplus,x) setup=(x=start_mc())
summarize!("Softplus", f_trial, ex_trial)

f_trial = @benchmark loop(maxsig,x) setup=(x=start_mc())
ex_trial = @benchmark loop(ex_maxsig,x) setup=(x=start_mc())
summarize!("Maxsig", f_trial, ex_trial)

f_trial = @benchmark loop(maxtanh,x) setup=(x=start_mc())
ex_trial = @benchmark loop(ex_maxtanh,x) setup=(x=start_mc())
summarize!("MaxTanh", f_trial, ex_trial)

f_trial = @benchmark loop(softsign,x) setup=(x=start_mc())
ex_trial = @benchmark loop(ex_softsign,x) setup=(x=start_mc())
summarize!("Softsign", f_trial, ex_trial)

f_trial = @benchmark loop(sigmoid,x) setup=(x=start_mc())
ex_trial = @benchmark loop(ex_sigmoid,x) setup=(x=start_mc())
summarize!("Sigmoid", f_trial, ex_trial)

f_trial = @benchmark loop(swish,x) setup=(x=start_mc())
ex_trial = @benchmark loop(ex_silu,x) setup=(x=start_mc())
summarize!("Silu", f_trial, ex_trial)

f_trial = @benchmark loop(gelu,x) setup=(x=start_mc())
ex_trial = @benchmark loop(ex_gelu,x) setup=(x=start_mc())
summarize!("Gelu", f_trial, ex_trial)