using DataFrames
using CSV
using JSON
using LightGraphs
using NearestNeighbors
using Statistics
using StatsBase
using BenchmarkTools
using LinearAlgebra
using PlotlyJS
import PlotlyJS: plot, scatter
using Colors
using Plots


struct material
    α::Number
    E::Number
    σ̄::AbstractArray{Number, 2} # Yield stress \sigma\bar
    T_cutoff::Number
end


mutable struct contour
    pos
    time
end


struct contourdata
    contours::Vector{contour}
    G::SimpleDiGraph
    layers::Vector
    travel_dists::Dict
    layer_height::Number
end


struct voxmap
    seglen::Vector{Float64}
    segcontours::Vector{Int}
    c::Number
end


struct voxdata
    voxels::DataFrame
    maps::Vector{voxmap}
    below::Vector{Int}
    width::Number
end


struct tempdecay
    extrusion::Number
    ambient::Number
    decay_rate::Number
end


function visualise_tempdecay(td::tempdecay; tmax=200)
    Temp(t::Number) = td.ambient + (td.extrusion-td.ambient)*ℯ^(-td.decay_rate*t)
    plot(Temp, 0, tmax)
end


function vecvec_to_matrix(vecvec)
    # convert vector of vectors int a matrix
    dim1 = length(vecvec)
    dim2 = length(vecvec[1])
    my_array = zeros(Float32, dim1, dim2)
    for i in 1:dim1
        for j in 1:dim2
            my_array[i,j] = vecvec[i][j]
        end
    end
    return my_array
end


function contour(d::Dict)
    return contour(vecvec_to_matrix(d["pos"]), d["time"])
end


function contourdata(cons::Vector{contour}, max_layers::Int, min_dist::Number)
    G = LightGraphs.SimpleDiGraph(0)

    # separate contours into layers
    layer_heights = sort(collect(Set([c.pos[end,3] for c in cons])))
    layers = [[] for i in 1:length(layer_heights)]
    clayeri = []
    contour_trees = []

    # place contours in layers and construct KDTree for each contour
    for i in 1:length(cons)
        l = searchsorted(layer_heights, cons[i].pos[1,3])[1]
        push!(layers[l], i)
        push!(clayeri, l)
        add_vertex!(G)
        push!(contour_trees, KDTree(transpose(cons[i].pos)))
    end

    # loop through contours from previous layer and compare waypoints
    for i in 1:length(cons)
        l = clayeri[i]

        # add contours from max_layers below
        if l > max_layers
            for c in layers[l-max_layers]
                add_edge!(G, c, i)
            end
        end

        if l == 1 || max_layers == 1
            continue
        end

        for c in layers[l-1]
            # if any points in contour i within min_dist of any points in contour c
            if any([length(b) > 0 for b in inrange(contour_trees[c], transpose(cons[i].pos), min_dist)])
                add_edge!(G, c, i) # mark i dependent on c
            end
        end
    end

    return contourdata(cons, G, layers, Dict(), layer_heights[1])
end


function seg_helper_orientation(p,q,r)
    val = (q[2]-p[2]) * (r[1]-q[1]) - (q[1]-p[1]) * (r[2]-q[2])
    if val > 0
        return 1 # clockwise
    elseif val < 0
        return 2 # anticlockwise
    else
        return 0 # colinear
    end
end


function onseg(p,q,r)
    # check if q lies on segment pr assuming 3 points are colinear
    return ((q[1] <= max(p[1], r[1])) && (q[1] >= min(p[1], r[1])) &&
    (q[2] <= max(p[2], r[2])) && (q[2] >= min(p[2], r[2])))
end


function seg_intersect(p1,q1,p2,q2)

    o1 = seg_helper_orientation(p1, q1, p2)
    o2 = seg_helper_orientation(p1, q1, q2)
    o3 = seg_helper_orientation(p2, q2, p1)
    o4 = seg_helper_orientation(p2, q2, q1)

    if (o1 ≠ o2) && (o3 ≠ o4) ||
        o1==0 && onseg(p1, p2, q1) ||
        o2==0 && onseg(p1, q2, q1) ||
        o3==0 && onseg(p2, p1, q2) ||
        o4==0 && onseg(p2, q1, q2)
        return true
    end

    return false
end


dist(p1, p2) = √sum((p1 -p2).^2)
interpolate(p1, p2, xi, axis) = p1 + (p2-p1)*(xi-p1[axis])/(p2[axis]-p1[axis])
interpolate(p1, p2, x1, xi, x2) = p1 + (p2-p1)*(xi-x1)/(x2-x1)


function voxmap(vox::Vector{Float64}, vox_d::Number, cdata::contourdata)

    # for one vox, get all contours which pass through it
    # only need to search contours in its layer
    l = Int(round((vox[3] + cdata.layer_height/2)/cdata.layer_height))
    voxx1 = vox[1] + vox_d/2
    voxx2 = vox[1] - vox_d/2
    voxy1 = vox[2] + vox_d/2
    voxy2 = vox[2] - vox_d/2

    seg_now = false
    seglen = Vector{Number}()
    segoffset = Vector{Number}()
    segcontours = Vector{Int}()
    seglen_sofar = 0
    t_start = 0

    if l > length(cdata.layers)
        return voxmap(seglen, segcontours, 0)
    end

    for cid in cdata.layers[l]

        c = cdata.contours[cid]

        # check if contour passes thorough this vox
        for i in 2:size(c.pos)[1]

            # make sure it is a line segment, not a point
            if c.pos[i-1,1:2] == c.pos[i,1:2]
                continue
            end

            # is this line segment completely outside vox?
            if c.pos[i, 1] > voxx1 && c.pos[i-1, 1] > voxx1 ||
                c.pos[i,1] < voxx2 && c.pos[i-1, 1] < voxx2 ||
                c.pos[i,2] < voxy2 && c.pos[i-1, 2] < voxy2 ||
                c.pos[i,2] > voxy1 && c.pos[i-1, 2] > voxy1

                # segment outside vox entirely
                if seg_now
                    println("Something's gone wrong: segment entirely outside voxel, but last segment inside")
                end
                continue
            end

            p1inside = c.pos[i-1, 1] < voxx1 && c.pos[i-1, 1] > voxx2 && c.pos[i-1, 2] > voxy2 && c.pos[i-1, 2] < voxy1
            p2inside = c.pos[i, 1] < voxx1 && c.pos[i,1] > voxx2 && c.pos[i,2] > voxy2 && c.pos[i,2] < voxy1
            # is this line segment completely inside vox?
            if p1inside && p2inside

                seglen_sofar += dist(c.pos[i], c.pos[i-1]) # append to existing contour
                if !seg_now # start new seg
                    t_start = 0 #  0 bc contour must be starting for this case
                    seg_now = true

                    if i!=2
                        println("Whole segment inside but something wrong")
                    end
                    continue
                end
            end

            cross_side1 = seg_intersect(c.pos[i-1,:], c.pos[i,:], [voxx1, voxy1], [voxx1, voxy2])
            cross_side2 = seg_intersect(c.pos[i-1,:], c.pos[i,:], [voxx1, voxy1], [voxx2, voxy1])
            cross_side3 = seg_intersect(c.pos[i-1,:], c.pos[i,:], [voxx2, voxy1], [voxx2, voxy2])
            cross_side4 = seg_intersect(c.pos[i-1,:], c.pos[i,:], [voxx2, voxy2], [voxx1, voxy2])

            # does this line segment intersect with vox only once
            if p1inside ⊻ p2inside

                # find intersection point
                if cross_side1 || cross_side3
                    # intersection with x
                    xi = [voxx1, voxx2][[cross_side1, cross_side3]][1]
                    p_i = interpolate(c.pos[i-1,:], c.pos[i,:], xi, 1)
                    t_i = interpolate(c.time[i-1], c.time[i], c.pos[i-1,1], xi, c.pos[i,1])
                elseif cross_side2 || cross_side4
                    # intersection with y
                    yi = [voxy1, voxy2][[cross_side2, cross_side4]][1]
                    p_i = interpolate(c.pos[i-1,:], c.pos[i,:], yi, 2)
                    t_i = interpolate(c.time[i-1], c.time[i], c.pos[i-1,2], yi, c.pos[i,2])
                end

                if p1inside
                    # end existing segment
                    if !seg_now
                        # contour end on the first segment
                        t_start = 0
                        seglen_sofar = 0
                    end
                    seglen_sofar += dist(c.pos[i-1, :], p_i)
                    push!(segcontours, cid)
                    push!(seglen, seglen_sofar)
                    push!(segoffset, (t_i + t_start)/2)
                    seglen_sofar = 0
                    seg_now = false
                else
                    # start new contour
                    t_start = t_i
                    seglen_sofar = dist(p_i, c.pos[i, :])
                    seg_now = true
                end
                continue

            elseif sum([cross_side1, cross_side2, cross_side3, cross_side4]) >= 2
                # intersects twice
                p_is = []
                t_is = []
                if cross_side1
                    p = interpolate(c.pos[i-1,:], c.pos[i,:], voxx1, 1)
                    if !isnan(p[1])
                        push!(p_is, p)
                        push!(t_is, interpolate(c.time[i-1], c.time[i], c.pos[i-1,1], voxx1, c.pos[i,1]))
                    end
                end
                if cross_side2
                    p = interpolate(c.pos[i-1,:], c.pos[i,:], voxy1, 2)
                    if !isnan(p[1])
                        push!(p_is,p)
                        push!(t_is, interpolate(c.time[i-1], c.time[i], c.pos[i-1,2], voxy1, c.pos[i,2]))
                    end
                end
                if cross_side3
                    p = interpolate(c.pos[i-1,:], c.pos[i,:], voxx2, 1)
                    if !isnan(p[1])
                        push!(p_is,p)
                        push!(t_is, interpolate(c.time[i-1], c.time[i], c.pos[i-1,1], voxx2, c.pos[i,1]))
                    end
                end
                if cross_side4
                    p = interpolate(c.pos[i-1,:], c.pos[i,:], voxy2, 2)
                    if !isnan(p[1])
                        push!(p_is, p)
                        push!(t_is, interpolate(c.time[i-1], c.time[i], c.pos[i-1,2], voxy2, c.pos[i,2]))
                    end
                end

                if seg_now
                    print("Something's wrong")
                end

                if length(p_is) >= 2
                    push!(segoffset, mean(t_is))
                    push!(segcontours, cid)
                    if length(p_is) == 2
                        push!(seglen, dist(p_is[1], p_is[2]))
                    else
                        push!(seglen, dist(p_is[1], p_is[3]))
                    end
                else
                    p1inside = c.pos[i-1, 1] <= voxx1 && c.pos[i-1, 1] >= voxx2 &&
                            c.pos[i-1, 2] >= voxy2 && c.pos[i-1, 2] <= voxy1
                    p = p1inside ? c.pos[i-1,:] : c.pos[i,:]
                    t = p1inside ? c.time[i-1] : c.time[i]

                    push!(segcontours, cid)
                    push!(segoffset, (t + t_is[1])/2)
                    push!(seglen, dist(p_is[1], p))
                end
            end
        end

        # if contour ends inside the voxel
        if seg_now
            # end segment
            push!(segcontours, cid)
            push!(seglen, seglen_sofar)
            push!(segoffset, (t_start + last(c.time))/2)
            seglen_sofar = 0
            t_start = 0
            seg_now = false
        end

        # for those contours find exact segments
    end
    c = Float64(segoffset ⋅ seglen)/sum(seglen) # constant used for cost calc
    new_seglen = Vector{Float64}()
    new_segcontours = Vector{Int64}()
    for i in 1:length(segcontours)
        if !(segcontours[i] in new_segcontours)
            push!(new_segcontours, segcontours[i])
            push!(new_seglen, sum(seglen[segcontours.==segcontours[i]]))
        end
    end
    return voxmap(new_seglen./sum(new_seglen), new_segcontours, c)
end


function voxdata(fname::String, cdata::contourdata)
    voxels = DataFrames.DataFrame(CSV.File(fname))
    w = dist(Vector(voxels[1, ["x","y","z"]]), Vector(voxels[2, ["x","y","z"]]))
    println("Assumed width ", w)
    vpos = [[v.x, v.y, v.z] for v in eachrow(voxels)]
    voxms = [voxmap(v, w, cdata) for v in vpos]
    below = indexin([v - [0,0,cdata.layer_height] for v in vpos], vpos)
    replace!(below, nothing=>0)
    return voxdata(voxels, voxms, below, w)
end


function random_rollout(cdata::contourdata)
    done_contours = Set{Int}()
    avail_contours = Set(cdata.layers[1])
    todo_contours = Set(1:length(cdata.contours))
    rollout = Vector{Int}()

    while length(avail_contours) > 0
        c = rand(avail_contours)
        push!(rollout, c)

        # remove selected contour from todo and avail, add to done
        delete!(avail_contours, c)
        delete!(todo_contours, c)
        push!(done_contours, c)

        # update available contours
        for i in todo_contours
            if i in avail_contours
                continue
            elseif length(inneighbors(cdata.G, i)) == 0
                push!(avail_contours, i)
                continue
            end

            add = true
            for j in inneighbors(cdata.G, i)
                if !(j in done_contours)
                    add = false
                    break
                end
            end

            if add
                push!(avail_contours, i)
            end
        end
    end

    return rollout
end


function greedy_rollout(cdata::contourdata)
    done_contours = Set{Int}()
    avail_contours = Set(cdata.layers[1])
    todo_contours = Set(1:length(cdata.contours))
    rollout = Vector{Int}()
    contour_order = zeros(length(cdata.contours))
    dep_times = zeros(length(cdata.contours))

    while length(avail_contours) > 0
        # get average times of dependency completion
        for c in avail_contours
            deps = inneighbors(cdata.G, c)
            if length(deps) == 0
                dep_times[c] = 1
                continue
            end
            dep_times[c] = mean(contour_order[deps])
        end
        temp_avail_list = collect(avail_contours)
        _,i = findmax(dep_times[temp_avail_list])
        c = temp_avail_list[i] # contour with deps printed most recently
        push!(rollout, c)
        contour_order[c] = length(rollout)

        # remove selected contour from todo and avail, add to done
        delete!(avail_contours, c)
        delete!(todo_contours, c)
        push!(done_contours, c)

        # update available contours
        for i in todo_contours
            if i in avail_contours
                continue
            elseif length(inneighbors(cdata.G, i)) == 0
                push!(avail_contours, i)
                continue
            end

            add = true
            for j in inneighbors(cdata.G, i)
                if !(j in done_contours)
                    add = false
                    break
                end
            end

            if add
                push!(avail_contours, i)
            end
        end
    end
    return rollout
end


function greedish_rollout(cdata::contourdata; α=0.5)
    # α=1 is totally greedy
    # α=0 is totally random
    done_contours = Set{Int}()
    avail_contours = Set(cdata.layers[1])
    todo_contours = Set(1:length(cdata.contours))
    rollout = Vector{Int}()
    contour_order = zeros(length(cdata.contours))
    dep_times = zeros(length(cdata.contours))

    while length(avail_contours) > 0
        # get average times of dependency completion
        for c in avail_contours
            deps = inneighbors(cdata.G, c)
            if length(deps) == 0
                dep_times[c] = 1
                continue
            end
            dep_times[c] = mean(contour_order[deps])
        end
        temp_avail_list = collect(avail_contours)
        w = dep_times[temp_avail_list]
        top_choices = w .>= minimum(w) + α*(maximum(w) - minimum(w))
        c = rand(temp_avail_list[top_choices])
        push!(rollout, c)
        contour_order[c] = length(rollout)

        # remove selected contour from todo and avail, add to done
        delete!(avail_contours, c)
        delete!(todo_contours, c)
        push!(done_contours, c)

        # update available contours
        for i in todo_contours
            if i in avail_contours
                continue
            elseif length(inneighbors(cdata.G, i)) == 0
                push!(avail_contours, i)
                continue
            end

            add = true
            for j in inneighbors(cdata.G, i)
                if !(j in done_contours)
                    add = false
                    break
                end
            end

            if add
                push!(avail_contours, i)
            end
        end
    end
    return rollout
end


function valid_swap(rollout::Vector{Int}, i::Int, j::Int, cdata::contourdata)
    # would swapping indices i and j in rollout result in another valid rollout?
    # NOTE THIS FUNCTION DOESNT WORK
    # IT ONLY CHECKS DEPENDENCIES TO A DEPTH OF 1
    # TODO, leave for now, use check_validity to double check at the end

    if i>j
        i,j = j,i
    elseif i==j
        return true
    end

    c1 = rollout[i]
    c2 = rollout[j]
    c2_dependson = inneighbors(cdata.G, c2)

    if c1 in c2_dependson
        return false
    end

    c1_dependents = outneighbors(cdata.G, c1)
    c_between = rollout[i+1:j-1]

    for c in c_between
        if c ∈ c1_dependents || c ∈ c2_dependson
            return false
        end
    end

    return true
end


function valid_swap(rollout::Vector{Int},
        multirange::Tuple{Int, Int},
        new_pos::Int,
        cdata::contourdata)
    s, e = multirange
    c1s = rollout[s:e]
    c2s = rollout[e+1:new_pos]

    for c in c1s
        c_dependents = outneighbors(cdata.G, c)
        if any([c ∈ c_dependents for c in c2s])
            return false
        end
    end
    return true
end


function check_validity(rollout::Vector{Int}, cdata::contourdata)
    # make sure a given rollout is valid
    done_contours = Set{Int}()

    for c in rollout
        c_dependson = inneighbors(cdata.G, c)

        if !issubset(c_dependson, done_contours)
            return false
        end

        push!(done_contours, c)
    end
    return true
end


function swap!(rollout::Vector{Int}, i::Int, j::Int)
    # swap values at ith and jth indices
    rollout[i], rollout[j] = rollout[j], rollout[i]
end


function swap!(rollout::Vector{Int},
        multirange::Tuple{Int, Int},
        new_pos::Int
        )
    s, e = multirange
    c1s = rollout[s:e]
    c2s = rollout[e+1:new_pos]

    rollout[s:new_pos] = [c2s;c1s]
end

function test_voxmap()
    # create vox
    vox = [0,0,0.5]
    vox_d = 2
    pos1 = [[0.5, -0.5] ones(2)*0.5 ones(2)]
    time1 = [0,1]
    pos2 = [[1.5, 0.5, -0.5, -1.5] ones(4)*0.5 ones(4)]
    time2 = Vector(0:3)
    pos3 = [[-0.5, -0.5] [2, -2] ones(2)]
    time3 = [0, 2.5]
    pos4 = [[0.5, 2.5] [-1.5, -0.5] ones(2)]
    time4 = [0,1]
    pos5 = [[-2,2] [-2,2] ones(2)]
    time5 = [0,1]
    contour1 = contour(pos1, time1)
    contour2 = contour(pos2,time2)
    contour3 = contour(pos3,time3)
    contour4 = contour(pos4,time4)
    contour5 = contour(pos5,time5)

    contours = [contour1, contour2, contour3, contour4, contour5]
    cdata = contourdata(contours, 1, 1)
    vm = voxmap(vox, vox_d, cdata)
    return vm
end


function clean_contour(c::contour)
    # remove first element of array if second element is the same
    while c.pos[1,:] == c.pos[2,:]
        c.pos = c.pos[2:end, :]
    end
    return c
end


function stress_multiplier!(a::DataFrame, mul::Number)
    a.Sx = a.Sx*mul
    a.Sy = a.Sy*mul
    a.Sz = a.Sz*mul
    a.Txy = a.Txy*mul
    a.Tyz = a.Tyz*mul
    a.Txz = a.Txz*mul
    return
end


function construct_cost(cdata::contourdata, vd::voxdata, mat::material, td::tempdecay, fname::Symbol=:cost_f)
    contour_times = [cdata.contours[c].time[end] for c in 1:length(cdata.contours)]

    # considered voxels
    not_empty_voxels = length.([m.seglen for m in vd.maps]) .>0
    valid_voxels = (1:length(vd.below))[(vd.below.!=0) .& not_empty_voxels]
    valid_voxels = valid_voxels[not_empty_voxels[vd.below[valid_voxels]]]

    considered_voxels = valid_voxels
    relbelows = vd.below[valid_voxels]
    rel_voxels = vd.voxels[considered_voxels,:]
    relmaps = vd.maps
    vox_area_scaling = min.(abs.(1 ./ rel_voxels.AreaRatio),1)

    # voxtimes vectorize
    max_contours_per_voxel = maximum([length(r.seglen) for r in relmaps])
    vox_contour_id = ones(Int64, length(relmaps), max_contours_per_voxel)
    for i in 1:length(relmaps)
        vox_contour_id[i, 1:length(relmaps[i].segcontours)] = relmaps[i].segcontours
    end
    vox_c = [Float64(v.c) for v in relmaps]
    vox_seglen = zeros(length(relmaps), max_contours_per_voxel)
    for i in 1:length(relmaps)
        vox_seglen[i, 1:length(relmaps[i].seglen)] = relmaps[i].seglen
    end

    # precompute constant values
    F = (2/mat.σ̄[1,1]^2 - 1/mat.σ̄[3,3]^2)/2
    G = 1/(2*mat.σ̄[3,3]^2)
    L = 1/(2*mat.σ̄[1,2]^2)
    M = 1/(2*mat.σ̄[1,3]^2)

    a = quote
        function $fname(rl::Vector{Int})
            # voxel times
            timestart = cumsum([$contour_times[c] for c in rl])
            voxtimes = sum($vox_seglen .* timestart[$vox_contour_id], dims=2) .+ $vox_c

            # voxel temps
            Δt = voxtimes[$considered_voxels] - voxtimes[$relbelows]
            ΔT = $(mat.T_cutoff.-td.ambient) .- $(td.extrusion-td.ambient).*ℯ.^(-$td.decay_rate.*Δt)
            replace!(x-> x<0 ? 0 : x, ΔT)

            # voxel stresses
            rel_v= $rel_voxels
            σ11 = rel_v.Sx + $(mat.E*mat.α)*ΔT
            σ22 = rel_v.Sy + $(mat.E*mat.α)*ΔT
            σ33 = rel_v.Sz
            σ12 = rel_v.Txy
            σ23 = rel_v.Tyz + $((cdata.layer_height/vd.width)*mat.E*mat.α)*ΔT
            σ31 = rel_v.Txy + $((cdata.layer_height/vd.width)*mat.E*mat.α)*ΔT
            return sum(($vox_area_scaling .* ($F * (σ11 - σ22).^2 +
                $G * ((σ33 - σ11).^2 + (σ33 - σ22).^2) +
                $(2 * L) * (σ12).^2 +
                $(2 * M) * (σ23 + σ31).^2)).^2)
        end
    end
    return eval(a)
end


function construct_cost_hist(cdata::contourdata, vd::voxdata, mat::material, td::tempdecay, fname::Symbol=:cost_hist)
    contour_times = [cdata.contours[c].time[end] for c in 1:length(cdata.contours)]

    # considered voxels
    println("n total voxels:")
    println(length(vd.maps))
    not_empty_voxels = length.([m.seglen for m in vd.maps]) .>0
    valid_voxels = (1:length(vd.below))[(vd.below.!=0) .& not_empty_voxels]
    valid_voxels = valid_voxels[not_empty_voxels[vd.below[valid_voxels]]]
    println("n non-empty voxels:")
    println(sum(not_empty_voxels))

    considered_voxels = valid_voxels
    relbelows = vd.below[valid_voxels]
    rel_voxels = vd.voxels[considered_voxels,:]
    println("n valid voxels:")
    println(size(rel_voxels)[1])
    relmaps = vd.maps
    vox_area_scaling = min.(abs.(1 ./ rel_voxels.AreaRatio),1)
    #vox_area_scaling = ones(length(1 ./ rel_voxels.AreaRatio))

    # voxtimes vectorize
    max_contours_per_voxel = maximum([length(r.seglen) for r in relmaps])
    vox_contour_id = ones(Int64, length(relmaps), max_contours_per_voxel)
    for i in 1:length(relmaps)
        vox_contour_id[i, 1:length(relmaps[i].segcontours)] = relmaps[i].segcontours
    end
    vox_c = [Float64(v.c) for v in relmaps]
    vox_seglen = zeros(length(relmaps), max_contours_per_voxel)
    for i in 1:length(relmaps)
        vox_seglen[i, 1:length(relmaps[i].seglen)] = relmaps[i].seglen
    end

    # precompute constant values
    F = (2/mat.σ̄[1,1]^2 - 1/mat.σ̄[3,3]^2)/2
    G = 1/(2*mat.σ̄[3,3]^2)
    L = 1/(2*mat.σ̄[1,2]^2)
    M = 1/(2*mat.σ̄[1,3]^2)

    a = quote
        function $fname(rl::Vector{Int})
            # voxel times
            timestart = cumsum([$contour_times[c] for c in rl])
            voxtimes = sum($vox_seglen .* timestart[$vox_contour_id], dims=2) .+ $vox_c

            # voxel temps
            Δt = voxtimes[$considered_voxels] - voxtimes[$relbelows]
            ΔT = $(mat.T_cutoff.-td.ambient) .- $(td.extrusion-td.ambient).*ℯ.^(-$td.decay_rate.*Δt)
            replace!(x-> x<0 ? 0 : x, ΔT)

            # voxel stresses
            rel_v= $rel_voxels
            σ11 = rel_v.Sx + $(mat.E*mat.α)*ΔT
            σ22 = rel_v.Sy + $(mat.E*mat.α)*ΔT
            σ33 = rel_v.Sz
            σ12 = rel_v.Txy
            σ23 = rel_v.Tyz + $((cdata.layer_height/vd.width)*mat.E*mat.α)*ΔT
            σ31 = rel_v.Txy + $((cdata.layer_height/vd.width)*mat.E*mat.α)*ΔT
            return ($vox_area_scaling .* ($F * (σ11 - σ22).^2 +
                $G * ((σ33 - σ11).^2 + (σ33 - σ22).^2) +
                $(2 * L) * (σ12).^2 +
                $(2 * M) * (σ23 + σ31).^2)).^2
        end
    end
    return eval(a)
end

function construct_best_neighbor(cdata::contourdata, cost_f::Function, k::Int=50)
    n_contours=length(cdata.contours)
    bn_func = quote
        function best_neighbor!(rl::Vector{Int}, current_cost::Number)
            costs = Dict()
            for i in 1:1:$n_contours-1
                for j in i+1:min(i+$k,$n_contours)
                    if valid_swap(rl, i, j, $cdata)
                        swap!(rl, i, j)
                        costs[i,j] = $cost_f(rl)
                        swap!(rl, i, j)
                    end
                end
            end
            v, (i,j) = findmin(costs)
            if abs(v - current_cost) < current_cost/1e6
                return 0
            elseif v < current_cost
                swap!(rl, i, j)
                return v
            end
            return 0
        end
    end
    return eval(bn_func)
end


function construct_best_neighbor_multi(cdata::contourdata,
        cost_f::Function,
        k::Int=10,
        multi_len::Int=5)
    n_contours=length(cdata.contours)
    bn_func = quote
        function best_neighbor!(rl::Vector{Int}, current_cost::Number)
            costs = Dict()
            for i in 1:$n_contours-1
                for j in i+$(multi_len+1):min(i+$(multi_len + k),$n_contours)
                    if valid_swap(rl, (i, i + $multi_len), j, $cdata)
                        swap!(rl, (i, i + $multi_len), j)
                        costs[i,j] = $cost_f(rl)
                        swap!(rl, (i, j - $multi_len - 1), j)
                    end
                end
            end
            v, (i,j) = findmin(costs)
            if abs(v - current_cost) < current_cost/1e6
                return 0
            elseif v < current_cost
                swap!(rl, (i, i + $multi_len), j)
                return v
            end
            return 0
        end
    end
    return eval(bn_func)
end


function local_search!(rl::Vector{Int}, max_iter::Int)
    cost_val = Inf
    for l in 1:max_iter
        c = best_neighbor!(rl, cost_val)
        if c ≠ 0
            cost_val = c
        else
            break
        end
    end
    return cost_val
end


function plot(vd::voxdata, i::Int, cdata::contourdata)
    vm = vd.maps[i]
    loc = Array(vd.voxels[i, ["x", "y", "z"]])
    w = vd.width/2

    sq_x = [loc[1]-w, loc[1]-w, loc[1]+w, loc[1]+w, loc[1]-w]
    sq_y = [loc[2]+w, loc[2]-w, loc[2]-w, loc[2]+w, loc[2]+w]

    traces = Vector{GenericTrace}()
    push!(traces, scatter(x=sq_x, y=sq_y,
        mode="lines",
        name="Voxel",
        line=attr(color="black", width=4)
        ))
    for (c,l) in zip(vm.segcontours, vm.seglen)
        push!(traces, scatter(
            x=cdata.contours[c].pos[:,1],
            y=cdata.contours[c].pos[:,2],
            mode="lines",
            name=round(l;digits=2)))
    end
    layout = Layout(
        yaxis=attr(scaleanchor="x", scaleratio=1)
    )
    plot(traces, layout)
end


function plot(rl::Vector{Int}, cdata::contourdata)
    n = length(rl)
    cols = range(HSV(180,1,1), stop=HSV(-180,1,1), length=n)
    traces = Vector{GenericTrace}()
    layout = Layout(
        scene_aspect_ratio="data",
        showlegend=false
    )

    for i in 1:n
        push!(traces, scatter(
            x=cdata.contours[rl[i]].pos[:,1],
            y=cdata.contours[rl[i]].pos[:,2],
            z=cdata.contours[rl[i]].pos[:,3],
            mode="lines",
            type="scatter3d",
            line=attr(color=cols[i])
            ))
    end
    plot(traces, layout)
end


function plot_animate(rl::Vector{Int}, cdata::contourdata; cam=(45,45), rate=1)
    i=1
    p=Plots.plot3d(cdata.contours[rl[i]].pos[:,1],
            cdata.contours[rl[i]].pos[:,2],
            cdata.contours[rl[i]].pos[:,3],
            camera=cam
    )

    return @gif for i in 2:length(rl)
        plot3d!(p,
            cdata.contours[rl[i]].pos[:,1],
            cdata.contours[rl[i]].pos[:,2],
            cdata.contours[rl[i]].pos[:,3],
            legend=false,
            camera=cam
        )
    end every rate
end


function plot(results::Dict)
    rkey = keys(results)
    default_costs = [results[k]["cost_default"] for k in rkey]
    random_costs = [results[k]["cost_random"] for k in rkey]
    greedy_costs = [results[k]["cost_greedy"] for k in rkey]
    local_costs = [results[k]["cost_local"] for k in rkey]
    trace2 = scatter(x=rkey,y=random_costs./default_costs, name="Random")
    trace3 = scatter(x=rkey,y=local_costs./default_costs, name="Local Search")
    trace4 = scatter(x=rkey,y=greedy_costs./default_costs, name="Greedy")
    traces = [trace2, trace3, trace4]
    plot(traces)
end


function update_result(
    results::Dict,
    obj::String,
    rl::Vector{Int},
    cost::Number,
    type::Symbol
    )

    if obj ∉ keys(results)
        results[obj] = Dict(
            "best_rollout" => Vector{Number}(),
            "cost_default"=>Inf,
            "cost_random"=>Inf,
            "cost_local"=>Inf
            # TODO: also save parameters, save all costs
            )
    end
    if type==:local && results[obj]["cost_local"]>cost
        results[obj]["cost_local"] = cost
        results[obj]["best_rollout"] = rl
    else
        if !("cost_" * string(type) in keys(results[obj]))
            results[obj]["cost_" * string(type)] = Inf
        end
        if results[obj]["cost_" * string(type)]>cost
            results[obj]["cost_" * string(type)] = cost
        end
    end
end


function save_result(results::Dict, fname::String)
    stringdata = JSON.json(results)
    open(fname, "w") do f
        write(f, stringdata)
    end
end