-- Python implementation of
--     Algorithm for Automatically Fitting Digitized Curves
--     by Philip J. Schneider
--     "Graphics Gems", Academic Press, 1990
-- by Volker Poplawski
-- translated to lua by me



local function distance(a,b)
    return math.dist(a[1],a[2],b[1],b[2])
end

local function dot(a,b)
    return a[1] * b[1] + a[2] * b[2]
end

local function zeros(a,b,c)
    local t = {}
    for i=1,a,1 do 
        t[i]={}
        for j=1,b,1 do 
            t[i][j]={}
            for k=1,c,1 do 
                t[i][j][k]=0
            end
        end
    end
    return t 
end

local function add (a,b)
    local A = lume.clone(a)
    A[1]=A[1]+b.x
    A[2]=A[2]+b.y
    return A
end


local function sub (a,b)
    local A = lume.clone(a)
    A[1]=A[1]+b.x
    A[2]=A[2]+b.y
    return A
end

local function lerp1(v,a,b,t)
    v.x = a[1] + (b[1] - a[1]) * t
    v.y = a[2] + (b[2] - a[2]) * t
end

local function lerp2(v,a,b,t)
    v.x = a.x + (b.x - a.x) * t
    v.y = a.y + (b.y - a.y) * t
end


local c1,c2,c3,c4 = vec2(),vec2(),vec2(),vec2()
local function q(ctrlPoly, t)
    c1:set(ctrlPoly[1][1],ctrlPoly[1][2])
    c2:set(ctrlPoly[2][1],ctrlPoly[2][2])
    c3:set(ctrlPoly[3][1],ctrlPoly[3][2])
    c4:set(ctrlPoly[4][1],ctrlPoly[4][2])
    return 
          c1 * (1.0-t)^3  
        + c2 * 3*(1.0-t)^2 * t 
        + c3 * 3*(1.0-t)* t^2 
        + c4 * t^3 
end


local function qprime(ctrlPoly, t)
    c1:set(ctrlPoly[1][1],ctrlPoly[1][2])
    c2:set(ctrlPoly[2][1],ctrlPoly[2][2])
    c3:set(ctrlPoly[3][1],ctrlPoly[3][2])
    c4:set(ctrlPoly[4][1],ctrlPoly[4][2])
    return 
          (c2-c1) * 3*(1.0-t)^2    
        + (c3-c2) * 6*(1.0-t) * t  
        + (c4-c3) * 3*t^2         
end

local function qprimeprime(ctrlPoly, t)
    c1:set(ctrlPoly[1][1],ctrlPoly[1][2])
    c2:set(ctrlPoly[2][1],ctrlPoly[2][2])
    c3:set(ctrlPoly[3][1],ctrlPoly[3][2])
    c4:set(ctrlPoly[4][1],ctrlPoly[4][2])
    return 
          (c3-c2*2+c1) * 6*(1.0-t)  
        + (c4-c3*2+c2) * 6*(t)     
end

local function chordLengthParameterize(points)
    local u = {0.0}
    for i=2, #points,1 do
        table.insert(u,u[i-1] + distance(points[i] , points[i-1]))
    end

    for i=1,#u,1 do
        u[i] = u[i] / u[#u]
    end

    return u
end
local tmp = vec2()
local function generateBezier(points, parameters, leftTangent, rightTangent)
    local bezCurve = {points[1], nil, nil, points[#points]}

    -- compute the A's
    local A = zeros(#parameters, 2, 2)
    for i,u in ipairs(parameters) do 
        A[i][1] = leftTangent  * 3*((1-u)^2) * u
        A[i][2] = rightTangent * 3*(1-u)    * (u^2)
    end

    -- Create the C and X matrices
    local C = {{0,0},{0,0}}
    local X = {0,0}

    for i,u in ipairs(parameters) do
        local point = points[i] 
        C[1][1] = C[1][1] + vec2.dot(A[i][1], A[i][1])
        C[1][2] = C[1][2] + vec2.dot(A[i][1], A[i][2])
        C[2][1] = C[2][1] + vec2.dot(A[i][1], A[i][2])
        C[2][2] = C[2][2] + vec2.dot(A[i][2], A[i][2])

        local qb = q({points[1], points[1], points[#points], points[#points]},u)
        tmp:set(point[1]-qb.x,point[2]-qb.y  )

        X[1] = X[1] + vec2.dot(A[i][1], tmp)
        X[2] = X[2] + vec2.dot(A[i][2], tmp)
    end

    -- Compute the determinants of C and X
    local det_C0_C1 = C[1][1] * C[2][2] - C[2][1] * C[1][2]
    local det_C0_X  = C[1][1] * X[2] - C[2][1] * X[1]
    local det_X_C1  = X[1] * C[2][2] - X[2] * C[1][2]

    -- Finally, derive alpha values
    local alpha_l = (det_C0_C1 == 0) and 0.0 or det_X_C1 / det_C0_C1
    local alpha_r = (det_C0_C1 == 0) and 0.0 or det_C0_X / det_C0_C1

    -- If alpha negative, use the Wu/Barsky heuristic (see text) */
    -- (if alpha is 0, you get coincident control points that lead to
    -- divide by zero in any subsequent NewtonRaphsonRootFind() call. */
    local segLength = distance(points[1] , points[#points])
    local epsilon = 1.0e-6 * segLength
    if alpha_l < epsilon or alpha_r < epsilon then 
        -- fall back on standard (probably inaccurate) formula, and subdivide further if needed.
        bezCurve[2] = add(bezCurve[1] , leftTangent * (segLength / 3.0))
        bezCurve[3] = add(bezCurve[4] , rightTangent * (segLength / 3.0))
    else
        -- First and last control points of the Bezier curve are
        -- positioned exactly at the first and last data points
        -- Control points 1 and 2 are positioned an alpha distance out
        -- on the tangent vectors, left and right, respectively
        bezCurve[2] = add(bezCurve[1] , leftTangent * alpha_l)
        bezCurve[3] = add(bezCurve[4] , rightTangent * alpha_r)
    end
    return bezCurve
end

local function newtonRaphsonRootFind(bez, point, u)
    local d = q(bez, u)
    d.x,d.y = d.x-point[1],d.y-point[2]

    local num = d * qprime(bez, u)
    local prime = qprime(bez, u)
    local den = prime*prime + d * qprimeprime(bez, u)
    local numerator = num.x+num.y
    local denominator = den.x+den.y

    if denominator == 0.0 then
        return u
    else
        return u - numerator/denominator
    end
end


local function reparameterize(bezier, points, parameters)
    local t = {}
    for i,u in ipairs(parameters) do
        local point = points[i] 
        local root = newtonRaphsonRootFind(bezier, point, u)
        table.insert(t,root)
    end
    return t
end


local function computeMaxError(points, bez, parameters)
    local maxDist = 0.0
    local splitPoint = #points/2
    for i,u in ipairs(parameters) do
        local point = points[i] 
        local q = q(bez,u)
        local dist = math.sdist(q.x,q.y,point[1],point[2])
        if dist > maxDist then 
            maxDist = dist
            splitPoint = i
        end
    end

    return maxDist, splitPoint
end

local _maxbeziertime = 2/60
local function fitCubic(points, leftTangent, rightTangent, error)
    if ltimer.getTime()>_beziertime+_maxbeziertime then return false end
    -- Use heuristic if region only has two points in it
    if #points == 2 then 
        local dist = distance(points[1] ,points[2]) / 3.0
        local bezCurve = {points[1], add(points[1] , leftTangent * dist), add(points[2] , rightTangent * dist), points[2]}
        return {bezCurve}
    end

    -- Parameterize points, and attempt to fit curve
    local u = chordLengthParameterize(points)
    local bezCurve = generateBezier(points, u, leftTangent, rightTangent)
    -- Find max deviation of points to fitted curve
    local maxError, splitPoint = computeMaxError(points, bezCurve, u)
    if maxError < error then
        return {bezCurve}
    end

    -- If error not too large, try some reparameterization and iteration
    if maxError < error^2 then
        for i=1 ,20,1 do 
            local uPrime = reparameterize(bezCurve, points, u)
            bezCurve = generateBezier(points, uPrime, leftTangent, rightTangent)
            maxError, splitPoint = computeMaxError(points, bezCurve, uPrime)
            if maxError < error then
                return {bezCurve}
            end
            u = uPrime
            if ltimer.getTime()>_beziertime+_maxbeziertime then return false end
        end
    end

    -- Fitting failed -- split at max error point and fit recursively
    local beziers = {}
    local centerTangent = vec2.normalize(vec2(points[splitPoint-1]) - vec2(points[splitPoint+1]))
    classicConcat(beziers, fitCubic(splice(points,1,splitPoint+1), leftTangent, centerTangent, error))
    classicConcat(beziers, fitCubic(splice(points,splitPoint), -centerTangent, rightTangent, error))

    return beziers
end



-- Fit one (ore more) Bezier curves to a set of points
function fitCurve(points, maxError)
    local leftTangent = vec2.normalize(vec2(points[2]) - vec2(points[1]))
    local rightTangent = vec2.normalize(vec2(points[#points-1]) - vec2(points[#points]))
    return fitCubic(points, leftTangent, rightTangent, maxError)
end
