using Base.Test

@test 1 + 2 == 3

@test .1 + .2 == .3

@test_approx_eq (.1 + .2) .3

@test_approx_eq (1 + 2, .1 + .2) (3, .3)

is_close(x::Number, y::Number, epsilon = eps()) = 
    abs(y - x) <= max(1, abs(x), abs(y))*epsilon

is_close(x, y, epsilon = eps()) = 
    all(x -> is_close(x..., epsilon), zip(x, y)) 

function test_close(a, b, eps, astr, bstr)
    if !is_close(a, b, eps)
        println("test failed: $(astr) is not close to $(bstr);\n   $(a) vs $(b)")
    end
end

macro test_close(a, b)
    :( test_close($(a), $(b), 1e-6, $(string(a)), $(string(b))) )
end

@test_close (1 + 2, .1 + .2) (3, .3)
@test_close (1 + 2, .1 + .2) (2, .3)

# returns the (center, radius) of the circle, circumscribed around three points
function circumscribed_circle(P1, P2, P3)
    d12 = vec(P1 - P2); n12 = dot(d12, d12)
    d23 = vec(P2 - P3); n23 = dot(d23, d23)
    d31 = vec(P3 - P1); n31 = dot(d31, d31)
    d = norm(cross([d12; 0], [d23; 0]))
    r = sqrt(n12*n23*n31)/(2d)
    alpha = n23*dot(d12, d31)
    beta = n31*dot(d12, d23)
    gamma = n12*dot(d31, d23)
    Pc = -(alpha*P1 + beta*P2 + gamma*P3)/(2d^2)
    (Pc, r)
end

# the circumscribed_circle tests
@test_close ([-1, 1], 1) circumscribed_circle([-2; 1], [-1; 2], [0; 1])
@test_close ([0, 0], 2) circumscribed_circle([2; 0], [0; 2], [sqrt(2); sqrt(2)])

const EPSILON = 1e-5

# finds the orientation of three 2D points
# -1 for CW, 1 for CCW, 0 if on the same line 
function orientation(P1, P2, P3) 
    _, _, cz = cross([vec(P2 - P1); 0], [vec(P3 - P1); 0]) 
    if abs(cz) < EPSILON
        cz = 0
    end
    sign(cz)
end

@test 1 == orientation([0; 0], [1; 0], [2; 2])
@test 0 == orientation([0; 0], [1; 1], [2; 2])
@test -1 == orientation([0; 0], [2; 2], [1; 0])

circumscribed_circle([1; 1], [2; 2], [3; 3])

function offset_orthogonal(P2, P3)
    D = P2 - P3
    Dp = [-D[2]; D[1]]
    P2 + EPSILON*Dp/norm(Dp)
end

@test 0 != orientation([0; 0], offset_orthogonal([1; 1], [2; 2]), [2; 2])

function f_arc(x, c, R, convex = true) 
    cx, cy = c
    plusminus = convex ? 1 : -1
    cy + plusminus*sqrt(max(0, R^2 - (x - cx)^2))
end

@test_close 3 f_arc(1, [1; 1], 2, true)
@test_close -1 f_arc(1, [1; 1], 2, false)

@test_close 1 f_arc(3, [1; 1], 2, true)
@test_close 1 f_arc(3, [1; 1], 2, false)

type ArcSeg
    c::Array{Float64, 1}  # arc center
    R::Float64            # arc radius
    a::Float64            # segment start
    b::Float64            # segment end
    convex::Bool          # true if top part of the circle (otherwise concave)
    m::Float64            # middle point ratio
end

# makes an arc going through the segment ends
function make_arc(f, seg, middle_ratio = 0.5)
    a, b = seg   
    m = a + (b - a)*middle_ratio
    pa, pb, pm = [a; f(a)], [b; f(b)], [m; f(m)]
    # handle the case when all three points are on the same line
    porient = orientation(pa, pm, pb)
    if porient == 0
        # points are on the same line
        pm = offset_orthogonal(pm, pb)
    end
    c, R = circumscribed_circle(pa, pm, pb)
    ArcSeg(c, R, a, b, porient < 0, middle_ratio)
end

#  inscribe an arc into itself
_arc = make_arc(x -> f_arc(x, [1; 1], 5, true), (3, 4), 0.2)
@test_close [1; 1] _arc.c
@test_close (5, 3, 4, 0.2) (_arc.R, _arc.a, _arc.b, _arc.m)
@test _arc.convex

const NUM_INTEGRATION_SAMPLES = 1000
function integrate_trapezoidal(f, x1, x2, N = NUM_INTEGRATION_SAMPLES) 
    @assert(N >= 1)
    dx = (x2 - x1)/N;
    s0 = (f(x1) + f(x2))/2
    (s0 + sum([f(clamp(x1 + i*dx, x1, x2)) for i in 1:(N - 1)]))*(x2 - x1)/N
end

# integrate parabola 
@test_close 2^3/3 integrate_trapezoidal(x -> x^2, 0, 2)
@test_close 2^3/3 integrate_trapezoidal(x -> x^2, 0, 2, 10000)
@test_close 2^3/3 integrate_trapezoidal(x -> x^2, 0, 2, 1000000)

# integrate half a circle (offset by 1 upwards from X axis)
@test_close (pi*3^2/2 + 3*2) integrate_trapezoidal(x -> f_arc(x, [1; 1], 3, true), 1 - 3, 1 + 3, 10000)

function err(f, arc::ArcSeg)
    err_metric(x) = abs(f(x) - f_arc(x, arc.c, arc.R, arc.convex))
    res = integrate_trapezoidal(err_metric, arc.a, arc.b)
end

f(x) = 2
@test is_close(2 - pi/2, err(f, ArcSeg([1; 1], 1, 0, 2, true, 0.1)), 1e-4)

using Calculus
function err_old(f, arc::ArcSeg)
    err_metric(x) = abs(f(x) - f_arc(x, arc.c, arc.R, arc.convex))
    res = Calculus.monte_carlo(err_metric, arc.a, arc.b, NUM_INTEGRATION_SAMPLES)
end

@test_close (2 - pi/2) err_old(f, ArcSeg([1; 1], 1, 0, 2, true, 0.1))

@test_close (2 - pi/2) err_old(f, ArcSeg([1; 1], 1, 0, 2, true, 0.1))

function circle_segment_area(x, R)
    theta = 2acos((R-x)/R)
    R^2*(theta - sin(theta))/2
end

@test_approx_eq circle_segment_area(0, 7) 0
@test_approx_eq circle_segment_area(14, 7) pi*7^2
@test_approx_eq circle_segment_area(7, 7) pi*7^2/2
@test_approx_eq circle_segment_area(2, 7) (pi*7^2 - circle_segment_area(7*2 - 2, 7))

function err1(f, arc::ArcSeg)
    res = err(f, arc)
    # add the "ears" error
    fa, fb = f(arc.a), f(arc.b)
    cx, cy = arc.c[1], arc.c[2]
    if (fa > cy) != arc.convex
        # left "ear"
        res += circle_segment_area(arc.a - (cx - arc.R), arc.R)
    end
    if (fb > cy) != arc.convex
        # right "ear"
        res += circle_segment_area(cx + arc.R - arc.b, arc.R)
    end
    res
end

f(x) = 2
@test is_close(2 - pi/2, err1(f, ArcSeg([1; 1], 1, 0, 2, true, 0.1)), 1e-4)

arc = ArcSeg([1; 1], 1, 0.5, 1.5, true, 0.1)
d = f_arc(0.5, arc.c, arc.R)
f(x) = d
@test is_close(pi - circle_segment_area(d, 1), err1(f, arc), 1e-4)

const STEP_RATIO = 0.8 
const pick_center_pt = (f, a, b) -> 0.5

function fit_arcs(f, range, tolerance, pick_middle_pt = pick_center_pt)
    ra, rb = range
    D = rb - ra
    rel_tol = tolerance/D
    ca = ra
    arcs = []
    while ca < rb
        cb = rb
        while true
            middle_ratio = pick_middle_pt(f, ca, cb)
            arc = make_arc(f, (ca, cb), middle_ratio)
            E = err1(f, arc)
            if E <= rel_tol*(cb - ca)
                arcs = [arcs, arc]
                ca = cb
                break
            end
            cb = ca + STEP_RATIO*(cb - ca)
        end
    end
    arcs
end

using DataFrames

fmt(x::Float64) = round(x, 3)
fmt(x::Array{Float64,1}) = map(fmt, x)
fmt(x::Bool) = x ? "yes" : "no"
fmt(x) = x

function to_table(arcs)
    DataFrame(
        c = map(x->fmt(x.c), arcs), 
        R = map(x->fmt(x.R), arcs),
        a = map(x->fmt(x.a), arcs), 
        b = map(x->fmt(x.b), arcs), 
        m = map(x->fmt(x.m), arcs), 
        convex = map(x->fmt(x.convex), arcs))
end

to_table(fit_arcs(x -> x^3/27 + x/5 - 1, [-5 4], 0.1))

function make_g_code(arcs, rformat=true, split_lines=true)
    fmt(x) = (round(x, 3) == round(x, 0)) ? @sprintf("%d", x) : @sprintf("%.3f", x)
    function arc2g(arc)
        a, b = arc.a, arc.b
        fa = f_arc(a, arc.c, arc.R, arc.convex)
        fb = f_arc(b, arc.c, arc.R, arc.convex)
        cx, cy = arc.c
        res = split_lines ? "\n" : ""
        res *= (arc.convex ? "G02" : "G03") * " X$(fmt(b)) Y$(fmt(fb)) "
        res *= rformat ? "R$(fmt(arc.R))" : "I$(fmt(cx - a)) J$(fmt(cy - fa))"
    end
    a1 = arcs[1]
    fa1 = f_arc(a1.a, a1.c, a1.R, a1.convex)
    "G00 X$(fmt(a1.a)) Y$(fmt(fa1)) " * join(map(arc2g, arcs), " ")
end

arc = ArcSeg([1; 1], 1, 0, 2, true, 0.1)
@test "G00 X0 Y1 G02 X2 Y1 R1" == make_g_code([arc], true, false)

print(make_g_code(fit_arcs(x -> x^3/27 + x/5 - 1, [-5 4], 0.1)))

# angle of 2D vector to X axis (CCW direction is positive)
function vec_angle(c, p) 
    d = p - c
    atan2(d[2], d[1])
end

# normalizes two angle such that range is continuous 
# and represents the corresponding minor/major arc  
function normalize_angle_range(anga, angb, convex::Bool)
    if anga < angb
        anga += 2pi
    end
    if !convex
        angb += 2pi
    end
    anga, angb
end

using Base.Test
a, b = normalize_angle_range(-3.1, 3.0, true)
@test_approx_eq a (-3.1 + 2pi)
@test_approx_eq b 3.0

a, b = normalize_angle_range(0.2, -0.02, true)
@test_approx_eq a 0.2
@test_approx_eq b -0.02

a, b = normalize_angle_range(1.8, 0.7, false)
@test_approx_eq a 1.8
@test_approx_eq b (0.7 + 2pi)

const ARC_GRANULARITY = 100
function get_arc_points(f, arcs)
    xp, yp = [], []
    for arc in arcs
        ang(x) = vec_angle(arc.c, [x; f(x)])
        anga, angb = normalize_angle_range(ang(arc.a), ang(arc.b), arc.convex)
        t = linspace(anga, angb, ARC_GRANULARITY)
        for ct = t
            cx, cy = arc.c + arc.R*[cos(ct); sin(ct)]
            xp = [xp, cx]
            yp = [yp, cy]
        end
    end
    xp, yp
end

using PyPlot
function style_plot()
    PyPlot.figure(figsize=(5, 3))
    PyPlot.rc("axes", edgecolor="#eeeeee");
    PyPlot.rc("xtick", color="#777777")
    PyPlot.rc("ytick", color="#777777")
    PyPlot.grid(color="#aaaaaa",linewidth=1)
end

function style_lines(lines)
    PyPlot.setp(lines[1], color="#99bdee", linewidth=7)
    PyPlot.setp(lines[2], color="#aa6666", linewidth=2)
    PyPlot.setp(lines[3], color="#99bdee", marker="o", ls="", ms=4)
    PyPlot.setp(lines[4], color="#ddddff", marker="o", ls="", ms=3)
end

using PyCall
@pyimport matplotlib.patches as mpatches
@pyimport matplotlib.cm as cm

# draw the colormapped circles according to the arc list
function plot_circles(arcs)
    const narcs = length(arcs)

    # need to jump through the hoops somewhat, 
    # since PyPlot does not expose these things (yet)

    # import the colormap from Matplotlib
    cmap = PyPlot.get_cmap("Set2") 
    w = linspace(1, narcs, narcs)
    rgba = pycall(pycall(cm.ScalarMappable, PyObject, cmap=cmap)["to_rgba"], 
        PyAny, w, bytes=true)
    
    # add the circles as "circle patches"
    for i = 1:narcs
        arc = arcs[i]
        fmt(n) = @sprintf("%02x", rgba[i, n])
        clr = "#$(fmt(1))$(fmt(2))$(fmt(3))" # convert to "#rrggbb" string format
        pc = mpatches.Circle((arc.c[1], arc.c[2]), arc.R, edgecolor=clr, fill = false)
        pycall(PyPlot.gca()["add_artist"], PyAny, pc)
    end
end

style_plot()
PyPlot.xlim(-15, 15)   
PyPlot.ylim(-10, 10)  
plot_circles(fit_arcs(x -> x^3/27 + x/5 - 1, [-5 4], 0.1))

const PLOT_GRANULARITY = 10000
function plot_arc_interp(f, range, arcs)
    style_plot() 
    
    # segment junction points
    xpj = unique(hcat([[x.a x.b] for x in arcs]...))
    ypj = map(f, xpj) 

    # middle points
    xpm = [x.a + (x.b - x.a)*x.m for x in arcs]
    ypm = map(f, xpm) 

    # interpolated function points
    xpf = linspace(range[1], range[2], PLOT_GRANULARITY)
    ypf = map(f, xpf)
    
    # arcs points
    xpa, ypa = get_arc_points(f, arcs)   

    lines = PyPlot.plot(xpf, ypf, xpa, ypa, xpj, ypj, xpm, ypm)
    
    style_lines(lines)

    PyPlot.xlim(range[1], range[2])    
    plot_circles(arcs)
end

f(x) = x^3/27 + x/5 - 1
range = [-5 4]
plot_arc_interp(f, range, fit_arcs(f, range, 0.1))

err_total(f, arcs) = sum([err1(f, arc) for arc in arcs])

function test_arc_interp(f, range, tolerance = 0.1, print_g_code = true, pick_middle_pt = pick_center_pt)
    @time arcs = fit_arcs(f, range, tolerance, pick_middle_pt)
    println("Number of arcs: $(length(arcs))")
    println("Total error: $(err_total(f, arcs))")
    plot_arc_interp(f, range, arcs)
    if print_g_code
        gcode = make_g_code(arcs, true, false)
        println("G-code:\n$(gcode)")
    end
end

test_arc_interp(x-> 6x^2 + 4x - 8, [-4 4], 0.1, false)

test_arc_interp(x-> 6x^2 + 4x - 8, [-4 4], 1, false)

test_arc_interp(x-> 6x^2 + 4x - 8, [-4 4], 100.0, false)

test_arc_interp(x -> -abs(x - 1) + 2, [-3 4], 0.1, false)

test_arc_interp(x -> 2sin(x^3/3) + x - 1, [-3 6], 1.0, false)

using Optim
function pick_optim_pt(f, a, b)
    err_s = x -> err1(f, make_arc(f, (a, b), x))
    optimize(err_s, 0.0, 1.0, method=:brent).minimum
end

test_arc_interp(x -> 2sin(x^3/3) + x - 1, [-2 3], 1.0, false, pick_optim_pt)

test_arc_interp(x -> 2sin(x^3/3) + x - 1, [-2 3], 1.0, false)