Julia precision issues

using Statistics

# Run in command line as
# > julia icp_precision.jl n precision
# Example:
# > julia icp_precision.jl 9 3000

struct Region
    A::Vector{BigFloat} # point set A
    B::Vector{BigFloat} # point set B
    width::BigFloat     # largest (a, b) distance

    function Region(A::Vector{BigFloat}, B::Vector{BigFloat})
        width::BigFloat = maximum([abs(a-b) for a ∈ A, b ∈ B])
        return new(sort(A), sort(B), width)
    end
end

struct ICPConstruction
    regions::Vector{Region}
    A::Vector{BigFloat} # point set A
    B::Vector{BigFloat} # point set B
    δ::BigFloat         # largest (a, b) distance within any region

    "Assembles full ICP construction with A and B point sets (vectors)."
    function ICPConstruction(regions::Vector{Region})
        # extract, concatenate, and sort overall A and B from regions
        A = reduce(vcat, map(r -> r.A, regions)) |> sort
        B = reduce(vcat, map(r -> r.B, regions)) |> sort
        δ = reduce(vcat, map(r -> r.width, regions)) |> maximum
        return new(regions, A, B, δ)
    end
end

function add_region(c::ICPConstruction, new::Region)::ICPConstruction
    δ::BigFloat = maximum([c.δ, new.width])
    min_old::BigFloat = minimum(vcat(c.A, c.B))
    max_new::BigFloat = maximum(vcat(new.A, new.B))
    shift::BigFloat = abs(min_old - max_new) + 3δ + 1

    A′ = new.A .- shift
    B′ = new.B .- shift
    regions = vcat(c.regions, Region(A′, B′))
    return ICPConstruction(regions)
end

function AugmentedLinearShifter(n::BigInt)::ICPConstruction
    # width of augmented linear shifter is (2n + 1)ℓ - 1 / k^n
    k::BigInt = 3n + 2
    ℓ::BigFloat = sum([1 / k^j for j ∈ 0:n])

    # Define A
    A::Vector{BigFloat} = [-2i * ℓ for i ∈ 0:n]

    # Define B
    b₀::BigFloat = 0.0
    b(i) = sum([1 / k^j for j ∈ 0:i-1])
    bᵢ::Vector{BigFloat} = [b(i) for i ∈ 1:n]
    B::Vector{BigFloat} = vcat(b₀, bᵢ)

    # Define Linear Shifter
    linear_shifter = Region(A, B)
    return ICPConstruction([linear_shifter])
end

struct Redirector
    regions::Tuple{Region, Region}
    A::Vector{BigFloat} # A₁ ∪ A₂
    B::Vector{BigFloat} # B₁ ∪ B₂
    v::BigFloat
    y::BigFloat

    function Redirector(v::BigFloat, y::BigFloat, k::BigInt)
        # Region 1
        a::BigFloat = 0.5k * v - y # a is y to the left of the (b₁ b₂) midpoint
        b₁::BigFloat = 0.0
        b₂::BigFloat = k * v
        A₁ = [a]
        B₁ = [b₁, b₂]
        R₁ = Region(A₁, B₁)

        # Region 2
        a′::BigFloat = 0.0
        A₂ = [a′]
        B₂ = [a′ + 0.5]
        R₂ = Region(A₂, B₂)

        regions = (R₁, R₂)
        A = reduce(vcat, map(r -> r.A, regions)) |> sort
        B = reduce(vcat, map(r -> r.B, regions)) |> sort
        return new((R₁, R₂), A, B, v, y)
    end
end

function icp1d(construction::ICPConstruction; k = big"0")
    A::Vector{BigFloat} = copy(construction.A)
    B::Vector{BigFloat} = copy(construction.B)

    t::BigFloat = 0.0
    correspondences::Vector{Int64} = []
    for a ∈ A
        neighbor = argmin(abs.(B .- a))
        push!(correspondences, neighbor)
    end

    # Remember transformations for visualization
    ts::Vector{BigFloat} = []
    cs::Vector{Vector{Int64}} = []

    while true
        # remember iteration state for visualization
        push!(ts, t)
        push!(cs, copy(correspondences))

        # find nearest neighbours
        BNeighbours = @view B[correspondences]

        # calculate mean for each point cloud
        a0::BigFloat = k == 0 ? mean(A) : sum(A) / k
        b0::BigFloat = k == 0 ? mean(BNeighbours) : sum(BNeighbours) / k

        # compute transformations
        t = b0 - a0
        A .+= t

        # compute new correspondences
        correspondences = []
        for a ∈ A
            neighbor = argmin(abs.(B .- a))
            push!(correspondences, neighbor)
        end

        # terminate if nearest neighbours don't change
        if correspondences == cs[end]
            push!(ts, t)
            break
        end
    end

    return ts, cs
end

function run_full_construction(n::BigInt)
    k::BigInt = 3n + 2
    ℓ::BigFloat = sum([1 / k^j for j ∈ 0:n])

    construction = AugmentedLinearShifter(n);
    redirectors = [Redirector(ℓ + 1, (2i + 1)ℓ, k) for i ∈ 0:n-1]
    redirector_regions = map(r -> collect(r.regions), redirectors) |> Iterators.flatten |> collect

    for region ∈ redirector_regions
        construction = add_region(construction, region)
    end

    # determine kickoff translation
    x₁::BigFloat = icp1d(construction, k=k)[1][2]

    a′::BigFloat = big"0.0"
    b′::BigFloat = a′ + (1.0 - x₁)k
    kicker_region = Region([a′], [b′])

    # complete construction with kicker region
    construction = add_region(construction, kicker_region)
    @assert length(construction.A) == k "k must equal |A|"

    # run ICP on fill construction
    translations, correspondences = icp1d(construction)
    return construction, translations, correspondences
end

if length(ARGS) == 2
    n = parse(BigInt, ARGS[1])
    setprecision(parse(Int, ARGS[2]))
else
    n = big"8"
    setprecision(2000)
end

println("\n🌟 Running one-dimensional ICP for n = $n with precision $(precision(BigFloat))...")

construction, t, c = run_full_construction(n)
iterations_expected = (n+1)^2 + 1

println("🌟 ICP took $(length(t)) iterations. $iterations_expected were expected. $(length(t) == iterations_expected ? "✅" : "❌")\n")