"""
unfold_particle_filter(num_particles::Int, objects_observed::Matrix{Array{Detection2D}}, camera_trajectories::Matrix{Camera_Params}, num_receptive_fields::Int64)

Performs inference procedure.

# Arguments
- v_matrix::Union{Matrix{Float64}, Nothing} V matrix to condition on. If nothing, do inference over V matrix.
- num_particles::Int
- objects_observed: indexed by ``(\\mathrm{scene, frame, receptive field, detection})``
- camera_trajectories: Indexed by ``(\\mathrm{scene, frame})``
- num_receptive_fields::Int64

array for each frame and array for each receptive field and array for those detections

    # Returns
    - A trace containing the inferred visual system and world state.

    """
    function unfold_particle_filter(v_matrix::Union{Matrix{Float64}, Nothing},
        num_particles::Int64,
        mcmc_steps_outer::Int64,
        mcmc_steps_inner::Int64,
        objects_observed::Matrix{Array{Detection2D}},
        camera_trajectories::Matrix{Camera_Params},
        params::Video_Params,
        V_file::IOStream, ws_file::IOStream, order::Vector{Int64})

        lesioned = !isnothing(v_matrix)

        init_obs = Gen.choicemap()

        if lesioned
            for j = 1:params.n_possible_objects
                init_obs[:init_v_matrix => :lambda_fa => j => :fa] = v_matrix[j,1]
                init_obs[:init_v_matrix => :miss_rate => j => :miss] = v_matrix[j,2]
            end
        end

        #no video, no frames
        println("initialize pf")
        state = initialize_particle_filter(main, (lesioned, 0, 0, params), init_obs, num_particles)

        num_videos, num_frames = size(objects_observed)

        inferred_realities = Array{Any}(undef, num_videos)
        avg_v = zeros(params.n_possible_objects, 2)

        global acceptance_counter = 0
        global total = 0

        for v=1:num_videos
            @time begin

                println("v ", v)

                maybe_resample!(state)
                ess = effective_sample_size(normalize_weights(state.log_weights)[2])
                println("ess after resample ", ess)

                #want to do this for every frame at once. put a : instead of f
                #rf is for receptive field
                obs = Gen.choicemap()

                for f = 1:num_frames
                    camera_params = camera_trajectories[v, f]
                    obs[:videos => v => :frame_chain => f => :camera => :camera_location_x] = camera_params.camera_location.x
                    obs[:videos => v => :frame_chain => f => :camera => :camera_location_y] = camera_params.camera_location.y
                    obs[:videos => v => :frame_chain => f => :camera => :camera_location_z] = camera_params.camera_location.z
                    obs[:videos => v => :frame_chain => f => :camera => :camera_focus_x] = camera_params.camera_focus.x
                    obs[:videos => v => :frame_chain => f => :camera => :camera_focus_y] = camera_params.camera_focus.y
                    obs[:videos => v => :frame_chain => f => :camera => :camera_focus_z] = camera_params.camera_focus.z

                    #for rf = 1:num_receptive_fields
                    #println("objects_observed[v, f][rf] ", objects_observed[v, f][rf])
                    #println("type ", typeof(objects_observed[v, f][rf]))
                    #println(typeof(objects_observed[v, f]))
                    #println(typeof(convert(Array{Any, 1}, objects_observed[v, f])))
                    obs[:videos => v => :frame_chain => f => :observations_2D] = convert(Array{Any, 1}, objects_observed[v, f])
                    #end
                end
                #def should be using map to replace for loops here
                #point is, condition on the camera trajectory and on the observations

                if lesioned
                    for j = 1:params.n_possible_objects
                        obs[:videos => v => :v_matrix => :lambda_fa => j => :fa] = v_matrix[j,1]
                        obs[:videos => v => :v_matrix => :miss_rate => j => :miss] = v_matrix[j,2]
                    end
                end

                particle_filter_step!(state, (lesioned, v, num_frames, params), (UnknownChange(),), obs)

                max_particle_weight = maximum(map(i -> get_score(state.traces[i]), collect(1:num_particles)))
                #println("max log score ", max_particle_weight)

                min_particle_weight = minimum(map(i -> get_score(state.traces[i]), collect(1:num_particles)))
                #println("min log score ", min_particle_weight)

                #println("particles")
                # for i = 1:num_particles
                #      println("particle ", i)
                #      trace = state.traces[i]
                #     choice = get_choices(trace)
                #     open("$i.txt", "w") do f
                #         Base.show(f, "text/plain", get_choices(trace))
                #     end
                #
                #      println("score ", get_score(trace))
                # #
                #      if v==2
                #          selection = select(:videos => 2 => :frame_chain => 111 => :observations_2D)
                #          args = get_args(trace)
                #          println("args ", args)
                #          trace2, weight = regenerate(trace, args, (), selection)
                #          println("after regenerate score ", get_score(trace2))
                #          #println("after regenerate score ", weight)
                #          println("after regenerate ", trace2[:videos => 2 => :frame_chain => 111 => :observations_2D])
                #      end
                #
                #
                #     #println("choices")
                #     #println(get_choices(trace))
                #     println("init v ", trace[:init_v_matrix])
                #     println("scene 1 ", trace[:videos => 1 => :init_scene])
                #     println("at v 1 ", trace[:videos => 1 => :v_matrix])
                #     if v==2
                #         println("scene 2 ", trace[:videos => 2 => :init_scene])
                #         println("at v 2 ", trace[:videos => 2 => :v_matrix])
                #     end
                #
                #     #println(trace)
                # end

                ess = effective_sample_size(normalize_weights(state.log_weights)[2])
                println("ess after pf step ", ess)

                #optional rejuvination
                (perturb_params, n_objects_per_category) = get_probs_categories(objects_observed, params, v, num_frames)
                #println("perturb_params ", perturb_params)
                line_segments_per_category = get_line_segments_per_category(params, objects_observed, camera_trajectories, v, num_frames)
                #line_segments = get_line_segments(objects_observed, camera_trajectories, params, v, num_frames, num_receptive_fields, total_n_objects)
                #to-do: threads here
                #seeds = collect(1:num_particles)
                Threads.@threads for i = 1:num_particles
                    #Random.seed!(seeds[i])
                    #for i = 1:num_particles
                    println("score particle i ", i, " before perturbation", get_score(state.traces[i]))
                    println("trace ", state.traces[i][:videos => v => :init_scene])
                    #println("perturb particle i ", i)
                    state.traces[i] = perturb(lesioned, state.traces[i], v, perturb_params, mcmc_steps_outer, mcmc_steps_inner, line_segments_per_category, params)
                    #println("done perturbing i ", i)
                    println("score particle i ", i, " after perturbation", get_score(state.traces[i]))
                    println("trace ", state.traces[i][:videos => v => :init_scene])
                    #println("trace ", state.traces[i][:videos => v => :init_scene])
                    #println("log score of this trace ", get_score(state.traces[i]))
                    #visualize_trace(state.traces, i, camera_trajectories, v, 1, params)

                    #visualize_trace(state.traces, i, camera_trajectories, v, 2, params)
                end

                #visualize_trace_with_heatmap(state.traces, camera_trajectories, v, 1, params )

                min_particle_weight = minimum(map(i -> get_score(state.traces[i]), collect(1:num_particles)))
                #println("min log score ", min_particle_weight)

                max_particle_weight = maximum(map(i -> get_score(state.traces[i]), collect(1:num_particles)))
                #println("max log score ", max_particle_weight)

                ess = effective_sample_size(normalize_weights(state.log_weights)[2])
                println("ess after rejuvination ", ess)

                inferred_realities[v], avg_v = print_Vs_and_Rs_to_file(V_file, ws_file, state.traces, num_particles, params, v, order, v==num_videos)
                #println("avg_v ", avg_v)
                #println("time of v ", v)
            end #end timer
        end

        println(acceptance_counter)
        println(total)

        return (sample_unweighted_traces(state, num_particles), inferred_realities, avg_v)

    end

    """
    perturb(trace, v::Int64, perturb_params::Perturb_Params, line_segments_per_category::Array{Array{Line_Segment,1},1})
    Does 500 MCMC steps (with different proposal functions) on the scene and on the v matrix.

    """

    @gen function perturb(lesioned::Bool,
        trace,
        v::Int64,
        perturb_params::Perturb_Params,
        mcmc_steps_outer::Int,
        mcmc_steps_inner::Int,
        line_segments_per_category::Array{Array{Line_Segment,1},1},
        params::Video_Params)
        #acceptance_counter = 0
        #proposal_counter = 0

        #println("before perturbations")
        #println("v ", v)
        #
        # println("alpha ", get_retval(trace)[end][2][5,2])
        # println("beta ", get_retval(trace)[end][3][5,2])
        #
        # #println("miss 2 ", trace[:init_v_matrix => :miss_rate => 2 => :miss])
        # println("init miss 5 ", trace[:init_v_matrix => :miss_rate => 5 => :miss])
        # #println("miss 2 ", trace[:videos => v => :v_matrix => :miss_rate => 2 => :miss])
        # println("miss rate 5 ", trace[:videos => v => :v_matrix => :miss_rate => 5 => :miss])
        #
        # println("scene at v ", trace[:videos => v => :init_scene])
        n = length(perturb_params.probs_possible_objects)

        for iter=1:mcmc_steps_outer #try 100 MH moves
            #println("iter ", iter)
            #println("trace ", trace[:videos => v => :init_scene])

            trace = perturb_scene(trace, v, perturb_params, line_segments_per_category, params)
            if lesioned == false
                for iter2=1:mcmc_steps_inner
                    trace = perturb_whole_v_matrix_mh(trace, v, n)
                end
            end
            # println("lambda_fa 2 ", trace[:v_matrix => (:lambda_fa, 2)])
            # println("miss 2 ", trace[:v_matrix => (:miss_rate, 2)])
            # println("lambda_fa 5 ", trace[:v_matrix => (:lambda_fa, 5)])
            # println("miss 5 ", trace[:v_matrix => (:miss_rate, 5)])
        end
        #println("acceptance_counter $(acceptance_counter/proposal_counter)")

        #println("after perturbations")
        #println("v ", v)

        # println("alpha ", get_retval(trace)[end][2][5,2])
        # println("beta ", get_retval(trace)[end][3][5,2])
        #
        # #println("miss 2 ", trace[:init_v_matrix => :miss_rate => 2 => :miss])
        # println("init miss 5 ", trace[:init_v_matrix => :miss_rate => 5 => :miss])
        # #println("miss 2 ", trace[:videos => v => :v_matrix => :miss_rate => 2 => :miss])
        # println("miss rate 5 ", trace[:videos => v => :v_matrix => :miss_rate => 5 => :miss])
        #
        # println("scene at v ", trace[:videos => v => :init_scene])
        return trace
    end

    """
    perturb_scene(trace, v::Int64, perturb_params::Perturb_Params, line_segments_per_category::Array{Array{Line_Segment,1},1})
    Does 3 MCMC steps (with different proposal functions) on the scene.

    """

    function perturb_scene(trace, v::Int64, perturb_params::Perturb_Params, line_segments_per_category::Array{Array{Line_Segment,1},1}, params::Video_Params)
        #println("trace ", trace[:videos => v => :init_scene])
        trace, accepted = add_remove_kernel(trace, v, line_segments_per_category, perturb_params)

        #println("accepted? ", accepted)
        #println("trace ", trace[:videos => v => :init_scene])

        #println("trace ", trace[:videos => v => :init_scene])

        #only try changing location or category if there's at least one object in the scene
        if length(trace[:videos => v => :init_scene]) > 0
            trace, accepted = change_location_kernel(trace, v, 0.1, params, line_segments_per_category)

            #global acceptance_counter = acceptance_counter + accepted
            #global total = total + 1

            #println("accepted? ", accepted)
            #println("trace ", trace[:videos => v => :init_scene])

            #trace, accepted = change_category_kernel(trace, v, perturb_params)
            #println("accepted? ", accepted)
            #println("trace ", trace[:videos => v => :init_scene])
        end
        return trace
    end

    #perturb the whole v matrix
    function perturb_whole_v_matrix_mh(trace, v::Int64, n::Int64)
        selection = get_selection_whole(v, n)
        trace, accepted = mh(trace, selection) #proposes new trace from the prior
        return trace
    end

    """
    perturb_v_matrix(trace, perturb_params::Perturb_Params)
    Picks one element of the v matrix to perturb using metropolis hastings.

    """
    #just pick an element of the matrix to perturb
    function perturb_individual_element_v_matrix_mh(trace, v::Int64, n::Int64)
        i = categorical([0.5, 0.5])
        j = categorical(fill(1/n, n))

        selection = get_selection(v, j, i)
        trace, accepted = mh(trace, selection) #proposes new trace from the prior

        # if i == 1 #if perturbing fa
        #     trace, accepted = mh(trace, proposal_for_v_matrix_fa, (v, j))
        # else
        #     trace, accepted = mh(trace, proposal_for_v_matrix_miss, (v, j))
        # end

        #println("accepted? ", accepted)
        #println("trace ", trace[:v_matrix])
        return trace
    end

    #return a selection for one element of the matrix
    function get_selection(v::Int64, j::Int64, i::Int64)
        if i == 1 #change lambda_fa
            if v == 1
                selection = select(:videos => v => :v_matrix => :lambda_fa => j => :fa,
                :init_v_matrix => :lambda_fa => j => :fa)
            else
                selection = select(:videos => v => :v_matrix => :lambda_fa => j => :fa,
                :videos => v-1 => :v_matrix => :lambda_fa => j => :fa)
            end
        else
            if v == 1
                selection = select(:videos => v => :v_matrix => :miss_rate => j => :miss,
                :init_v_matrix => :miss_rate => j => :miss)
            else
                selection = select(:videos => v => :v_matrix => :miss_rate => j => :miss,
                :videos => v-1 => :v_matrix => :miss_rate => j => :miss)
            end
        end
        return selection
    end

    #return a selection for the whole v matrix
    function get_selection_whole(v::Int64, n::Int64)
        selection = select()

        for j = 1:n
            if v == 1
                push!(selection, :videos => v => :v_matrix => :lambda_fa => j => :fa)
                push!(selection, :init_v_matrix => :lambda_fa => j => :fa)
                push!(selection, :v_matrix => :miss_rate => j => :miss)
                push!(selection, :init_v_matrix => :miss_rate => j => :miss)
            else
                push!(selection, :videos => v => :v_matrix => :lambda_fa => j => :fa)
                push!(selection, :videos => v-1 => :v_matrix => :lambda_fa => j => :fa)
                push!(selection, :videos => v => :v_matrix => :miss_rate => j => :miss)
                push!(selection, :videos => v-1 => :v_matrix => :miss_rate => j => :miss)
            end
        end

        return selection
    end

    """
    perturb_v_matrix_hmc(trace, perturb_params::Perturb_Params)
    Perturbs an element of the v_matrix using Hamiltonian MC

    """

    #just pick an element of the matrix to perturb
    function perturb_v_matrix_hmc(trace, v::Int64, perturb_params::Perturb_Params)
        n = length(perturb_params.probs_possible_objects)
        i = categorical([0.5, 0.5])
        j = categorical(fill(1/n, n))

        if i == 1 #if perturbing fa
            trace, accepted = hmc(trace, select(:videos => v => :v_matrix => :lambda_fa => j => :fa))
        else
            trace, accepted = hmc(trace, select(:videos => v => :v_matrix => :miss_rate => j => :miss))
        end

        #println("accepted? ", accepted)
        #println("trace ", trace[:v_matrix])

        return trace
    end

    """
    proposal_for_v_matrix_fa(trace, j::Int64)
    Performs on MH step on the false alarm rate for object of category j.

        """

        @gen function proposal_for_v_matrix_fa(trace, v::Int64, j::Int64)
            std = 0.01
            choices = get_choices(trace)
            #centered on previous value
            @trace(trunc_normal(choices[:videos => v => :v_matrix => :lambda_fa => j => :fa], std, 0.0, 10000.0), :videos => v => :v_matrix => :lambda_fa => j => :fa) #had to make trunc_normal because of error otherwise
        end

        """
        proposal_for_v_matrix_miss(trace, j::Int64)
        Performs on MH step on the miss rate for object of category j.

            """

            @gen function proposal_for_v_matrix_miss(trace, v::Int64, j::Int64)
                std = 0.1
                choices = get_choices(trace)
                #centered on previous value
                @trace(trunc_normal(choices[:videos => v => :v_matrix => :miss_rate => j => :miss], std, 0.0, 1.0), :videos => v => :v_matrix => :miss_rate => j => :miss)
            end

            """
            get_probs_categories(objects_observed::Matrix{Array{Detection2D}}, params::Video_Params, v::Int64, num_frames::Int64, num_receptive_fields::Int64)

            Returns a probability distribution over the object categories to be used
            in the proposal functions.

            Derived from the objects that the visual system observes.
            """

            function get_probs_categories(objects_observed::Matrix{Array{Detection2D}}, params::Video_Params, v::Int64, num_frames::Int64)
                track_categories = zeros(length(params.possible_objects)) #each element will be the number of times that category was detected. adding 1
                probabilities = zeros(length(params.possible_objects))
                for f = 1:num_frames
                    #for rf = 1:num_receptive_fields
                    for (index, value) in enumerate(objects_observed[v, f])
                        track_categories[value[3]] = track_categories[value[3]]+1#category
                    end
                    #end
                end
                #make it so that 10% of the weight gets divided evenly among all the categories
                other_ten_percent = sum(track_categories)/9
                each = other_ten_percent/length(params.possible_objects)
                probabilities = copy(track_categories)
                probabilities = probabilities .+ each

                #in case of no observations, make uniform
                if sum(probabilities) == 0
                    probabilities .= 1
                end

                return (Perturb_Params(probs_possible_objects = (probabilities)./sum(probabilities)), track_categories)
            end

            """
            get_line_segments_per_category(params::Video_Params, objects_observed::Matrix{Array{Detection2D}}, camera_trajectories::Matrix{Camera_Params}, v::Int64, num_frames::Int64, num_receptive_fields::Int64)

            Gets all line segments in 3d space that correspond to each 2D detection
            in every frame of a scene.
            """
            function get_line_segments_per_category(params::Video_Params, objects_observed::Matrix{Array{Detection2D}}, camera_trajectories::Matrix{Camera_Params}, v::Int64, num_frames::Int64)
                line_segments = Array{Array{Line_Segment, 1}}(undef, length(params.possible_objects))
                for j = 1:length(params.possible_objects)
                    line_segments[j] = []
                end
                for f = 1:num_frames
                    camera_params = camera_trajectories[v, f]
                    #for rf = 1:num_receptive_fields
                    for (index, value) in enumerate(objects_observed[v, f])
                        #println("value ", value)
                        line_segment = get_line_segment(camera_params, params, value)
                        #println("line_segment in pf_inference ", line_segment)
                        push!(line_segments[value[3]], line_segment)
                    end
                    #end
                end
                return line_segments
            end

            """Duplicated from Gen library"""
            function effective_sample_size(log_normalized_weights::Vector{Float64})
                log_ess = -logsumexp(2. * log_normalized_weights)
                return exp(log_ess)
            end

            """Duplicated from Gen Library"""
            function normalize_weights(log_weights::Vector{Float64})
                log_total_weight = logsumexp(log_weights)
                log_normalized_weights = log_weights .- log_total_weight
                return (log_total_weight, log_normalized_weights)
            end

            export unfold_particle_filter


using Test
module constants
#MathConstants.
global const ϕ = nothing
global const ∞ = Inf
global const -∞ = -Inf

end
#end
#Ε(x) = exp(x)
#error function  ϵ (generic , cherry-picked, smooth)
#global ϵ(x) =  exp(x)^-1

#end
#end

#--- Testing area #craziness allowed
module accurateDigits
#digits accuracy
"""
```inputs:
y: error function
returns number of accurate (at least)
TODO: How to test this test?

Yeilds an error ϵ with
at least the number of Digits required (or better)

note: this is not a linear function
it exponentially goes to z

the logic:using the rule
"if an error has a digit 0, that means the digit
    (associated with this error) is accurate"

a homebrewed way, fabricated out of the spaciousness  #novel
#TODO: How to verify this is correct? #need for a verification method
"""
global ϵ(x) = exp(x)^-1 ##beging with a monotonically decreasomg (survival) funtion- that is smooth  -this enforces a constantly increasing
#=
function valuate(value)
    if value isa Inf value = ∞
    elseif value isa -Inf value = -∞
    elseif value isa nothing || Nothing ||"" value = ϕ

    return value
end
=#

using Test, Plots
#digi(n::Int64, x::Int64, y::Float64
function digi(;n = 6, x = 1) # y = exp(x))
    print(typeof(x))
    return ϵ(log2(n) * x)
end

digi(n=1)
digi(n=0)

digi(n=10^6)
digi(n=10^10)

end
#digi

#=
#diffDigi(a::Int64, b::Int64)
function diffDigi(a = 5, b = 6)
    return digi(b) - digi(a)
end
#=
    digi = 6
    _6 = ϵ(digi * exp(1))
    ϵ(12)

    digi1 = 5
    _5 = ϵ(digi1 * exp(1))

    #for getting rid of non-linearity (hopefully)
    #subtract between 2 consequtive (non-linear) numbers

    diff = _5 - _6
    #=
    as of result we'd lost a bit of accuracy compared to 6
    but 'diff' function is linear
    =#

    _7 = ϵ(7 * exp(1))
=#

function maxEuclidDigi(;a = 5, b = 6)
    _a = digi(a)
    _b = digi(b)
    #maximal #mean euclidean distance
    meanDiff = (max(_a, _b) - min(_a, _b)) / 2
    return meanDiff
end
#= TODO:UNCOMMENT
@test _ϵ = maxEuclidDigi(5, 6)
@test diffDigi(5, 6)
@test diffDigi(5, 6) > maxEuclidDigi(5, 6) #there was an error during testing
end
end

@test maxEuclidDigi(5, 6)
@test diffDigi(5, 6)
@test diffDigi(5, 6) > maxEuclidDigi(5, 6) #there was an error during testing
#=
digi=6
_6 = ϵ(digi*exp(1))
ϵ(12)

digi1 = 5
_5 = ϵ(digi1*exp(1))

#for getting rid of non-linearity
#subtract between 2 consequtive numbers

diff = _5 - _6
=#

#=
as of result we'd lost a bit of accuracy compared to 6
but 'diff' function is linear

_7 = ϵ(7*exp(1))

#maximal euclidean distance
diff = (max(_5,_6) - min(_5,_6) )/2

#@test
end
=#


#-------------------------------------------------------------------
#* EMSO Model Library (EML) Copyright (C) 2004 - 2007 ALSOC.
#*
#* This LIBRARY is free software; you can distribute it and/or modify
#* it under the therms of the ALSOC FREE LICENSE as available at
#* http://www.enq.ufrgs.br/alsoc.
#*
#* EMSO Copyright (C) 2004 - 2007 ALSOC, original code
#* from http://www.rps.eng.br Copyright (C) 2002-2004.
#* All rights reserved.
#*
#* EMSO is distributed under the therms of the ALSOC LICENSE as
#* available at http://www.enq.ufrgs.br/alsoc.
#*
#*--------------------------------------------------------------------
#* Model of basic streams
#*----------------------------------------------------------------------
#* Author: Paula B. Staudt and Rafael de P. Soares
#* $Id$
#*---------------------------------------------------------------------
type stream
	stream()=begin
		NComp=outers.NComp
		new(
			DanaInteger(Dict{Symbol,Any}(
				:Brief=>"Number of chemical components",
				:Lower=>1
			)),
			flow_mol(Dict{Symbol,Any}(
				:Brief=>"Stream Molar Flow Rate"
			)),
			temperature(Dict{Symbol,Any}(
				:Brief=>"Stream Temperature"
			)),
			pressure(Dict{Symbol,Any}(
				:Brief=>"Stream Pressure"
			)),
			enth_mol(Dict{Symbol,Any}(
				:Brief=>"Stream Enthalpy",
				:Protected=>true
			)),
			fraction(Dict{Symbol,Any}(
				:Brief=>"Vapourization fraction"
			)),
			fill(fraction(Dict{Symbol,Any}(
				:Brief=>"Stream Molar Fraction"
			)),(NComp)),
			[:NComp,],
			[:F,:T,:P,:h,:v,:z,]
		)
	end
	NComp::DanaInteger
	F::flow_mol
	T::temperature
	P::pressure
	h::enth_mol
	v::fraction
	z::Array{fraction}
	parameters::Array{Symbol,1}
	variables::Array{Symbol,1}
	attributes::Dict{Symbol,Any}
end
export stream
function atributes(in::stream,_::Dict{Symbol,Any})
	fields::Dict{Symbol,Any}=Dict{Symbol,Any}()
	fields[:Pallete]=false
	fields[:Brief]="General Material Stream"
	fields[:Info]="This is the basic building block for the EML models.
	Every model should have input and output streams derived
	from this model."
	drive!(fields,_)
	return fields
end
stream(_::Dict{Symbol,Any})=begin
	newModel=stream()
	newModel.attributes=atributes(newModel,_)
	newModel
end


# Precondition
# ============

primitive type Value 8 end

const NONE  = reinterpret(Value, 0x00)
const TRUE  = reinterpret(Value, 0x01)
const FALSE = reinterpret(Value, 0x02)
const BOTH  = reinterpret(Value, 0x03)

function Base.convert(::Type{UInt8}, v::Value)
    return reinterpret(UInt8, v)
end

function Base.convert(::Type{Value}, b::UInt8)
    return reinterpret(Value, b)
end

function Base.:|(v1::Value, v2::Value)
    return convert(Value, convert(UInt8, v1) | convert(UInt8, v2))
end

function Base.:&(v1::Value, v2::Value)
    return convert(Value, convert(UInt8, v1) & convert(UInt8, v2))
end

struct Precondition
    names::Vector{Symbol}
    values::Vector{Value}
end

function Precondition()
    return Precondition(Symbol[], Value[])
end

function Base.getindex(precond::Precondition, name::Symbol)
    i = findfirst(n -> n == name, precond.names)
    if i == 0
        return BOTH
    else
        return precond.values[i]
    end
end

function Base.push!(precond::Precondition, kv::Pair{Symbol,Value})
    name, value = kv
    i = findfirst(n -> n == name, precond.names)
    if i == nothing
        push!(precond.names, name)
        push!(precond.values, value)
    else
        precond.values[i] |= value
    end
    return precond
end

function precondition_names(precond::Precondition)
    return copy(precond.names)
end

function conflicts(precond1::Precondition, precond2::Precondition)
    for (n1, v1) in precond1
        if (v1 & precond2[n1]) == NONE
            return true
        end
    end
    return false
end

function Base.iterate(precond::Precondition, i=1)
    if i > length(precond.names)
        return nothing
    end
    return (precond.names[i], precond.values[i]), i + 1
end

function isconditioned(precond::Precondition)
    for v in precond.values
        if v != BOTH
            return true
        end
    end
    return false
end


using Microbiome
using MicrobiomePlots
using Colors
using Random
using Test

@testset "Abundances" begin
    Random.seed!(1)
    M = rand(100, 10)

    abund = abundancetable(
        M, ["sample_$x" for x in 1:10],
        ["feature_$x" for x in 1:100])

    @test typeof(abundanceplot(abund, topabund=5)) <: Plots.Plot
    @test typeof(abundanceplot(abund, srt=collect(10:-1:1))) <: Plots.Plot

    @test_skip typeof(abundanceplot(abund, srt=:feature_1)) <: Plots.Plot # Needs method feature sorting

    ann = annotationbar(parse.(Color, ["red", "white", "blue"]))

    @test typeof(plot(ann)) <: Plots.Plot
end


"The game object holds all metadata pertinent to the game and all its contents."
mutable struct Game
    turn_number::Int
    id::Int
    players::Dict{Int, Player}
    game_map::GameMap
    me::Player
end

"Initiates a game object collecting all start-state instances for the contained items for pre-game."
function init_game()
    constants = load_constants(JSON.parse(readline(stdin)))
    num_players, my_id = parse.(Int, split(readline()))
    players = Dict(i => generate_players() for i=0:num_players-1)
    me = players[my_id]
    game_map = generate_game_map()
    constants, Game(0, my_id, players, game_map, me)
end

"Sends a list of commands to the engine."
send_commands(commands::Vector) = println(join(commands, " "))

"""
Indicate that your bot is ready to play.
# Arguments
- `name::String` : The name of your bot.
"""
start_game(name::String) = send_commands([name])

"Updates the game object's state."
function update_frame!(game::Game)
    game.turn_number = parse(Int, readline())
    for _ in keys(game.players)
        player, num_ships, num_dropoffs, halite = parse.(Int, split(readline()))
        update_player!(game.players[player], num_ships, num_dropoffs, halite)
    end
    update_game_map!(game.game_map)
    for player in values(game.players)
        for ship in get_ships(player)
            mark_unsafe!(game.game_map[ship.position], ship)
        end
        game.game_map[player.shipyard.position].structure = player.shipyard
        for dropoff in get_dropoffs(player)
            game.game_map[dropoff.position].structure = dropoff
        end
    end
    game
end

"Method to send all commands to the game engine, effectively ending your turn."
end_turn(commands::Vector) = send_commands(commands)


### RandomPolicy ###
# maintained by @zsunberg
"""
a generic policy that uses the actions function to create a list of actions and then randomly samples an action from it.
"""
mutable struct RandomPolicy{RNG<:AbstractRNG, P<:Union{POMDP,MDP}, U<:Updater} <: Policy
    rng::RNG
    problem::P
    updater::U # set this to use a custom updater, by default it will be a void updater
end
# The constructor below should be used to create the policy so that the action space is initialized correctly
RandomPolicy(problem::Union{POMDP,MDP};
             rng=Base.GLOBAL_RNG,
             updater=VoidUpdater()) = RandomPolicy(rng, problem, updater)

## policy execution ##
function action(policy::RandomPolicy, s)
    return rand(policy.rng, actions(policy.problem, s))
end

function action(policy::RandomPolicy, b::Void)
    return rand(policy.rng, actions(policy.problem))
end

## convenience functions ##
updater(policy::RandomPolicy) = policy.updater

"""
solver that produces a random policy
"""
mutable struct RandomSolver <: Solver
    rng::AbstractRNG
end
RandomSolver(;rng=Base.GLOBAL_RNG) = RandomSolver(rng)
solve(solver::RandomSolver, problem::Union{POMDP,MDP}) = RandomPolicy(solver.rng, problem, VoidUpdater())


using Caesar, RoME, IncrementalInference
using RoMEPlotting, Gadfly, KernelDensityEstimatePlotting
using KernelDensityEstimate
using DocStringExtensions
using DelimitedFiles, JLD

include(joinpath(@__DIR__,"slamUtils.jl"))
include(joinpath(@__DIR__,"plotSASUtils.jl"))

expnum = 2;
if expnum == 1
    topdir = joinpath(ENV["HOME"],"data","kayaks","07_18")
    trialstr = "07_18_parsed_set1";
    datadir = joinpath(topdir,trialstr);
    ijldname = datadir * "/exp1" * ".jld"
elseif expnum==2
    topdir = joinpath(ENV["HOME"],"data","kayaks","07_18")
    trialstr = "07_18_parsed_set2";
    datadir = joinpath(topdir,trialstr);
    ijldname = datadir * "/exp1" * ".jld"
elseif expnum==3
    topdir = joinpath(ENV["HOME"],"data","kayaks","07_20")
    trialstr = "07_20_parsed_set1";
    datadir = joinpath(topdir,trialstr);
    ijldname = datadir * "/exp1" * ".jld"
end

cfgFile = joinpath(ENV["HOME"],"data","sas","SAS2D.yaml");
chirpFile = joinpath(ENV["HOME"],"data","sas","chirp250.txt");
cfgd=loadConfigFile(cfgFile)
cfgd["range_snr_floor"]=0.95
iload = load(ijldname)
nrange = iload["nrange"]
irange = iload["irange"];
itemp = readdlm(datadir*"/inav.csv",',',Float64,'\n', skipstart=1);

# total length defined here
saswindow = 6;
nsasfac = 15;

wstart = 370; wlen = saswindow*nsasfac;
dataframes = collect(nrange[wstart]:nrange[wstart]+wlen);

sasstart = 1;
sasdataframes = [zeros(Int,saswindow) for _ in 1:nsasfac];
sasposes = [];
irangenew = irange[wstart] : 5 : irange[wstart+nsasfac*saswindow]
irangewindowed = collect(irangenew)[1:saswindow:nsasfac*saswindow+1]
igt = [itemp[irangewindowed,2] itemp[irangewindowed,4]];

poses = [Symbol("x$i") for i in 1:wlen]
posData = importdata_nav(dataframes,datadir=datadir);
navchecked, errorind = sanitycheck_nav(posData)

# dposData = deepcopy(posData)
# cumulativeDrift!(dposData,[0.0;0],[0.1,0.1])

# Check the trajectory
posPl = Gadfly.plot(layer(x=posData[:,1],y=posData[:,2], Geom.point, Geom.path, Theme(default_color=colorant"green",highlight_width = 0pt)),layer(x=igt[:,1],y=igt[:,2], Geom.point, Geom.path, Theme(default_color=colorant"red" ,highlight_width = 0pt))); posPl |> PDF("/tmp/test.pdf");

for i=1:nsasfac
    framestart = nrange[wstart]+sasstart-1+(i-1)*saswindow;
    sasdataframes[i] = collect(framestart:framestart+saswindow-1)
    push!(sasposes,[Symbol("x$j") for j in sasstart+(i-1)*saswindow:sasstart+saswindow-1+(i-1)*saswindow])
end

fg = initfg();
rtkCov= Matrix(Diagonal([0.1;0.1].^2));
for i in 1:wlen
  addVariable!(fg, poses[i], Point2)
  pp = PriorPoint2(MvNormal(posData[i,:], rtkCov))
  addFactor!(fg, [poses[i];], pp, autoinit=true)
  tmpInit = approxConv(fg,Symbol("x$(i)f1"),poses[i]);
  XXkde = manikde!(tmpInit,getManifolds(fg,poses[i]));
  setValKDE!(fg,poses[i],XXkde);
end

beacons = [Symbol("l$j") for j in 1:nsasfac]

for i=1:nsasfac
    addVariable!(fg, beacons[i], Point2)

    waveformData = importdata_waveforms(sasdataframes[i],2, datadir=datadir);
    sas2d = prepareSAS2DFactor(saswindow, waveformData, rangemodel=:Correlator,
                               cfgd=cfgd, chirpFile=chirpFile)
    addFactor!(fg, [beacons[i];sasposes[i]], sas2d, autoinit=false)
    @show currfac = ls(fg,beacons[i])[1]

    tmpInit = approxConv(fg,currfac,beacons[i]);
    XXkde = kde!(tmpInit);
    setValKDE!(fg,beacons[i],XXkde);

    if i>1
        dpμ = [0.0;0.0];
        dpσ = Matrix(Diagonal([2.0;2].^2))
        vp = Point2Point2(MvNormal(dpμ,dpσ))
        addFactor!(fg, [beacons[i-1];beacons[i]], vp, autoinit=false)
    end
end

writeGraphPdf(fg,viewerapp="", engine="neato", filepath = "/tmp/test.pdf")
# getSolverParams(fg).drawtree = true
getSolverParams(fg).limititers=500
#getSolverParams(fg).showtree = true
getSolverParams(fg).maxincidence = 200

tree = solveTree!(fg)

# Some Debuggging
# @time L1 = approxConv(fg,:l1x1x2x3x4x5x6x7x8f1,:l1)
# @time L1 = approxConv(fg,:l2x9x10x11x12x13x14x15x16f1,:l2)

mfIn = importdata_waveforms(sasdataframes[3],2, datadir=datadir);
range_mfData = zeros(Complex{Float64}, 8192, saswindow)
chirpIn = readdlm(chirpFile,',',Float64,'\n')
range_mf = prepMF(chirpIn,8192,saswindow) # MF
range_mf(mfIn,range_mfData) # matched filter
freq_sampling= 37500.0; sound_speed= 1481.0 ;
ranget = [0:8000-500;]*sound_speed/freq_sampling
bss = AliasingScalarSampler(ranget,exp.(norm.(range_mfData[1:7501,3])),SNRfloor=0.95)
out = rand(bss,1000)
Gadfly.plot(layer(x=out, Geom.histogram)) |> PDF("/tmp/rangetest.pdf")

# plk = plotBeaconSolve(fg)
plk=[];
for sym in ls(fg) #plotting all landmarks labeled
    if occursin(r"l",string(sym))
        X1 = getKDEMax(getBelief(fg,sym))
        push!(plk, layer(x=[X1[1];],y=[X1[2];],label=String["$sym";], Geom.point,Geom.label), Theme(default_color=colorant"red",point_size = 1.5pt,highlight_width = 0pt))
        L1pd = predictbelief(fg,sym,ls(fg, sym))
        push!(plk,layer(x=L1pd[1,:],y=L1pd[2,:],Geom.histogram2d(xbincount=300, ybincount=300)))
    end
end

# K1 = plotKDEContour(kde!(getVal(fg,:l1)[1:2,:]),xlbl="X (m)", ylbl="Y (m)",levels=5,layers=true);
# push!(plk,K1...)
# push!(plk,Gadfly.Theme(key_position = :none));

push!(plk,layer(x=posData[:,1],y=posData[:,2], Geom.point, Geom.path, Theme(default_color=colorant"blue")), layer(x=igt[:,1],y=igt[:,2], Geom.point, Geom.path, Theme(default_color=colorant"green" )))
# push!(plk, Coord.cartesian(xmin=0, xmax=120, ymin=-80, ymax=20,fixed=true))
plkplot = Gadfly.plot(plk...); plkplot |> PDF("/tmp/test.pdf")

plk= [];

push!(plk,layer(x=posData[:,1],y=posData[:,2], Geom.path, Theme(default_color=colorant"green")), layer(x=igt[:,1],y=igt[:,2], Geom.point, Geom.path, Theme(default_color=colorant"red" )))

L1 = getVal(getVariable(fg, :l1))
push!(plk,layer(x=L1[1,:],y=L1[2,:],Geom.histogram2d(xbincount=300, ybincount=300)))

# push!(plk, Coord.cartesian(xmin=40, xmax=100, ymin=-120, ymax=-50,fixed=true))
plkplot = Gadfly.plot(plk...); plkplot |> PDF("/tmp/test.pdf");

## Plot the SAS factor pairs
fsym = :l1x1x2x3x4x5x6x7x8f1
pl = plotSASPair(fg, fsym, show=true, filepath="/tmp/testDP_2pp_3vpvp_init.pdf");


import Base: convert, promote_rule, string, print, show
import Base.Dates: value, toms

# Convenience type for working with HH:MM:SS.
immutable Time <: TimePeriod
    seconds::Int
end
const ZERO = Time(0)

function Time(hour::Integer, minute::Integer, second::Integer)
    Time(hour * 3600 + minute * 60 + second)
end

function Time(s::AbstractString)
    # "-" represents 0:00 for some DST rules
    s == "-" && return ZERO
    parsed = map(n -> parse(Int, n), split(s, ':'))

    # Only can handle up to hour, minute, second.
    length(parsed) > 3 && throw(ArgumentError("Invalid Time string"))
    any(parsed[2:end] .< 0) && throw(ArgumentError("Invalid Time string"))

    # Handle variations where minutes and seconds may be excluded.
    values = [0,0,0]
    values[1:length(parsed)] = parsed

    if values[1] < 0
        for i in 2:length(values)
            values[i] = -values[i]
        end
    end

    return Time(values...)
end

# TimePeriod methods
value(t::Time) = t.seconds
toms(t::Time) = value(t) * 1000

hour(t::Time) = div(value(t), 3600)
minute(t::Time) = rem(div(value(t), 60), 60)
second(t::Time) = rem(value(t), 60)

function hourminutesecond(t::Time)
    h, r = divrem(value(t), 3600)
    m, s = divrem(r, 60)
    return h, m, s
end

convert(::Type{Second}, t::Time) = Second(value(t))
convert(::Type{Millisecond}, t::Time) = Millisecond(value(t) * 1000)
promote_rule{P<:Union{Week,Day,Hour,Minute,Second}}(::Type{P}, ::Type{Time}) = Second
promote_rule(::Type{Millisecond}, ::Type{Time}) = Millisecond

# Should be defined in Base.Dates
Base.isless(x::Period, y::Period) = isless(promote(x,y)...)

# https://en.wikipedia.org/wiki/ISO_8601#Times
function string(t::Time)
    neg = value(t) < 0 ? "-" : ""
    h, m, s = map(abs, hourminutesecond(t))
    @sprintf("%s%02d:%02d:%02d", neg, h, m, s)
end
print(io::IO, t::Time) = print(io, string(t))
show(io::IO, t::Time) = print(io, t)

# min/max offsets across all zones and all time.
# Note: A warning is given when we find an Olson zone that exceeds these values.
const MIN_GMT_OFFSET = Time("-15:56:00")  # Asia/Manilla
const MAX_GMT_OFFSET = Time("15:13:42")   # America/Metlakatla

# min/max save across all zones/rules and all time.
# Note: A warning is given when we find an Olson rule that exceeds these values.
const MIN_SAVE = Time("00:00")
const MAX_SAVE = Time("02:00")  # France, Germany, Port, Spain

const MIN_OFFSET = MIN_GMT_OFFSET + MIN_SAVE
const MAX_OFFSET = MAX_GMT_OFFSET + MAX_SAVE
const ABS_DIFF_OFFSET = abs(MAX_OFFSET - MIN_OFFSET)


"""
file tfw
"""
function save_tfw(output::String, GSD::Float64, lx::Float64, uy::Float64)
    fname = splitext(output)[1]
    io = open(fname * ".tfw", "w")
    write(io, "$(Float64(GSD))\n")
    write(io, "0.000000000000000\n")
    write(io, "0.000000000000000\n")
    write(io, "-$(Float64(GSD))\n")
    L = @sprintf("%f", lx)
    U = @sprintf("%f", uy)
    write(io, "$L\n")
    write(io, "$U\n")
    close(io)
end

"""
Save point cloud extracted file .las.
"""
function savepointcloud(params::ParametersOrthophoto, temp::String)

    flushprintln("Point cloud: saving ...")

    # update number of points in header
    params.mainHeader.records_count = params.numPointsProcessed

    # update header bounding box
    flushprintln("Point cloud: update bbox ...")
    params.mainHeader.x_min = params.tightBB.x_min
    params.mainHeader.y_min = params.tightBB.y_min
    params.mainHeader.z_min = params.tightBB.z_min
    params.mainHeader.x_max = params.tightBB.x_max
    params.mainHeader.y_max = params.tightBB.y_max
    params.mainHeader.z_max = params.tightBB.z_max

    # write las file
    pointtype = LasIO.pointformat(params.mainHeader) # las point format

    if params.numPointsProcessed != 0 # if n == 0 nothing to save
        # in temp : list of las point records
        open(temp) do s
            # write las
            open(params.outputfile, "w") do t
                write(t, LasIO.magic(LasIO.format"LAS"))
                write(t, params.mainHeader)

                # LasIO.skiplasf(s)
                for i = 1:params.numPointsProcessed
                    p = read(s, pointtype)
                    write(t, p)
                end
            end
        end
    end

    rm(temp) # remove temp
    flushprintln("Point cloud: done ...")
end

"""
Save orthoprojection image.
"""
function saveimage(params::ParametersOrthophoto)
    flushprintln("Image: saving ...")
    # save tfw
    save_tfw(params.outputimage, params.GSD, params.refX, params.refY)

    # save image
    save(params.outputimage, Images.colorview(RGB, params.RGBtensor))

    flushprintln("Image: done ...")
end

function newPointRecord(
    point::Point,
    type::LasIO.DataType,
    header::LasIO.LasHeader;
    affineMatrix = Matrix(Common.I, 4, 4),
)

    position = Common.apply_matrix(affineMatrix, point.position)
    x = LasIO.xcoord(position[1], header)
    y = LasIO.ycoord(position[2], header)
    z = LasIO.zcoord(position[3], header)
    intensity = UInt16(0)
    flag_byte = UInt8(0)
    raw_classification = UInt8(0)
    scan_angle = Int8(0)
    user_data = UInt8(0)
    pt_src_id = UInt16(0)

    if type == LasIO.LasPoint0
        return type(
            x,
            y,
            z,
            intensity,
            flag_byte,
            raw_classification,
            scan_angle,
            user_data,
            pt_src_id,
        )

    elseif type == LasIO.LasPoint1
        gps_time = Float64(0)
        return type(
            x,
            y,
            z,
            intensity,
            flag_byte,
            raw_classification,
            scan_angle,
            user_data,
            pt_src_id,
            gps_time,
        )

    elseif type == LasIO.LasPoint2
        red = point.color[1]
        green = point.color[2]
        blue = point.color[3]
        return type(
            x,
            y,
            z,
            intensity,
            flag_byte,
            raw_classification,
            scan_angle,
            user_data,
            pt_src_id,
            red,
            green,
            blue,
        )

    elseif type == LasIO.LasPoint3
        gps_time = Float64(0)
        red = rgb[1]
        green = rgb[2]
        blue = rgb[3]
        return type(
            x,
            y,
            z,
            intensity,
            flag_byte,
            raw_classification,
            scan_angle,
            user_data,
            pt_src_id,
            gps_time,
            red,
            green,
            blue,
        )

    end

end


function update_aux!(edmf::EDMF_PrognosticTKE{N_up}, gm, grid, state, Case, param_set, TS) where {N_up}
    #####
    ##### Unpack common variables
    #####
    kc_surf = kc_surface(grid)
    kf_surf = kf_surface(grid)
    kc_toa = kc_top_of_atmos(grid)
    up = edmf.UpdVar
    en = edmf.EnvVar
    en_thermo = edmf.EnvThermo
    ρ0_f = face_ref_state(state).ρ0
    p0_c = center_ref_state(state).p0
    ρ0_c = center_ref_state(state).ρ0
    α0_c = center_ref_state(state).α0
    g = CPP.grav(param_set)
    c_m = CPEDMF.c_m(param_set)
    KM = center_aux_turbconv(state).KM
    KH = center_aux_turbconv(state).KH
    surface = Case.Sur
    obukhov_length = surface.obukhov_length
    FT = eltype(grid)
    prog_gm = center_prog_grid_mean(state)
    prog_gm_f = face_prog_grid_mean(state)
    aux_up = center_aux_updrafts(state)
    aux_up_f = face_aux_updrafts(state)
    aux_en = center_aux_environment(state)
    aux_en_f = face_aux_environment(state)
    aux_gm = center_aux_grid_mean(state)
    aux_tc_f = face_aux_turbconv(state)
    aux_tc = center_aux_turbconv(state)
    aux_bulk = center_aux_bulk(state)
    prog_en = center_prog_environment(state)
    aux_en_2m = center_aux_environment_2m(state)
    prog_up = center_prog_updrafts(state)
    prog_up_f = face_prog_updrafts(state)
    aux_en_unsat = aux_en.unsat
    aux_en_sat = aux_en.sat
    m_entr_detr = aux_tc.ϕ_temporary
    ∇m_entr_detr = aux_tc.ψ_temporary
    w_up_c = aux_tc.w_up_c
    w_en_c = aux_tc.w_en_c
    wvec = CC.Geometry.WVector
    Δt = TS.dt
    max_area = edmf.max_area
    sde_model = edmf.sde_model

    #####
    ##### center variables
    #####

    @inbounds for k in real_center_indices(grid)
        #####
        ##### Set primitive variables
        #####
        @inbounds for i in 1:N_up
            if is_surface_center(grid, k)
                if prog_up[i].ρarea[k] / ρ0_c[k] >= edmf.minimum_area
                    aux_up[i].θ_liq_ice[k] = edmf.h_surface_bc[i]
                    aux_up[i].q_tot[k] = edmf.qt_surface_bc[i]
                    aux_up[i].area[k] = edmf.area_surface_bc[i]
                else
                    aux_up[i].θ_liq_ice[k] = prog_gm.θ_liq_ice[k]
                    aux_up[i].q_tot[k] = prog_gm.q_tot[k]
                end
            else
                if prog_up[i].ρarea[k] / ρ0_c[k] >= edmf.minimum_area
                    aux_up[i].θ_liq_ice[k] = prog_up[i].ρaθ_liq_ice[k] / prog_up[i].ρarea[k]
                    aux_up[i].q_tot[k] = prog_up[i].ρaq_tot[k] / prog_up[i].ρarea[k]
                    aux_up[i].area[k] = prog_up[i].ρarea[k] / ρ0_c[k]
                else
                    aux_up[i].θ_liq_ice[k] = prog_gm.θ_liq_ice[k]
                    aux_up[i].q_tot[k] = prog_gm.q_tot[k]
                    aux_up[i].area[k] = 0
                end
            end
        end

        #####
        ##### compute bulk
        #####
        aux_bulk.q_tot[k] = 0
        aux_bulk.θ_liq_ice[k] = 0
        aux_bulk.area[k] = sum(i -> aux_up[i].area[k], 1:N_up)
        if aux_bulk.area[k] > 0
            @inbounds for i in 1:N_up
                aux_bulk.q_tot[k] += aux_up[i].area[k] * aux_up[i].q_tot[k] / aux_bulk.area[k]
                aux_bulk.θ_liq_ice[k] += aux_up[i].area[k] * aux_up[i].θ_liq_ice[k] / aux_bulk.area[k]
            end
        else
            aux_bulk.q_tot[k] = prog_gm.q_tot[k]
            aux_bulk.θ_liq_ice[k] = prog_gm.θ_liq_ice[k]
        end
        aux_en.area[k] = 1 - aux_bulk.area[k]
        aux_en.tke[k] = prog_en.ρatke[k] / (ρ0_c[k] * aux_en.area[k])
        aux_en.Hvar[k] = prog_en.ρaHvar[k] / (ρ0_c[k] * aux_en.area[k])
        aux_en.QTvar[k] = prog_en.ρaQTvar[k] / (ρ0_c[k] * aux_en.area[k])
        aux_en.HQTcov[k] = prog_en.ρaHQTcov[k] / (ρ0_c[k] * aux_en.area[k])

        #####
        ##### decompose_environment
        #####
        a_bulk_c = aux_bulk.area[k]
        val1 = 1 / (1 - a_bulk_c)
        val2 = a_bulk_c * val1
        aux_en.q_tot[k] = max(val1 * prog_gm.q_tot[k] - val2 * aux_bulk.q_tot[k], 0) #Yair - this is here to prevent negative QT
        aux_en.θ_liq_ice[k] = val1 * prog_gm.θ_liq_ice[k] - val2 * aux_bulk.θ_liq_ice[k]

        #####
        ##### saturation_adjustment and buoyancy
        #####
        ts_gm = thermo_state_pθq(param_set, p0_c[k], prog_gm.θ_liq_ice[k], prog_gm.q_tot[k])
        aux_tc.θ_virt[k] = TD.virtual_pottemp(ts_gm)

        ts_en = thermo_state_pθq(param_set, p0_c[k], aux_en.θ_liq_ice[k], aux_en.q_tot[k])

        aux_en.T[k] = TD.air_temperature(ts_en)
        aux_en.θ_virt[k] = TD.virtual_pottemp(ts_en)
        aux_en.θ_dry[k] = TD.dry_pottemp(ts_en)
        aux_en.q_liq[k] = TD.liquid_specific_humidity(ts_en)
        aux_en.q_ice[k] = TD.ice_specific_humidity(ts_en)
        rho = TD.air_density(ts_en)
        aux_en.buoy[k] = buoyancy_c(param_set, ρ0_c[k], rho)

        update_sat_unsat(en_thermo, state, k, ts_en)
        aux_en.RH[k] = TD.relative_humidity(ts_en)

        @inbounds for i in 1:N_up
            if aux_up[i].area[k] < edmf.minimum_area && k > kc_surf && aux_up[i].area[k - 1] > 0.0
                qt = aux_up[i].q_tot[k - 1]
                h = aux_up[i].θ_liq_ice[k - 1]
                ts_up = thermo_state_pθq(param_set, p0_c[k], h, qt)
            else
                ts_up = thermo_state_pθq(param_set, p0_c[k], aux_up[i].θ_liq_ice[k], aux_up[i].q_tot[k])
            end
            aux_up[i].q_liq[k] = TD.liquid_specific_humidity(ts_up)
            aux_up[i].q_ice[k] = TD.ice_specific_humidity(ts_up)
            aux_up[i].T[k] = TD.air_temperature(ts_up)
            ρ = TD.air_density(ts_up)
            aux_up[i].buoy[k] = buoyancy_c(param_set, ρ0_c[k], ρ)
            aux_up[i].RH[k] = TD.relative_humidity(ts_up)
        end
        aux_gm.buoy[k] = (1.0 - aux_bulk.area[k]) * aux_en.buoy[k]
        @inbounds for i in 1:N_up
            aux_gm.buoy[k] += aux_up[i].area[k] * aux_up[i].buoy[k]
        end
        @inbounds for i in 1:N_up
            aux_up[i].buoy[k] -= aux_gm.buoy[k]
        end
        aux_en.buoy[k] -= aux_gm.buoy[k]

        #####
        ##### compute bulk thermodynamics
        #####
        aux_bulk.q_liq[k] = 0
        aux_bulk.q_ice[k] = 0
        aux_bulk.T[k] = 0
        aux_bulk.RH[k] = 0
        aux_bulk.buoy[k] = 0
        if a_bulk_c > 0
            @inbounds for i in 1:N_up
                aux_bulk.q_liq[k] += aux_up[i].area[k] * aux_up[i].q_liq[k] / a_bulk_c
                aux_bulk.q_ice[k] += aux_up[i].area[k] * aux_up[i].q_ice[k] / a_bulk_c
                aux_bulk.T[k] += aux_up[i].area[k] * aux_up[i].T[k] / a_bulk_c
                aux_bulk.RH[k] += aux_up[i].area[k] * aux_up[i].RH[k] / a_bulk_c
                aux_bulk.buoy[k] += aux_up[i].area[k] * aux_up[i].buoy[k] / a_bulk_c
            end
        else
            aux_bulk.RH[k] = aux_en.RH[k]
            aux_bulk.T[k] = aux_en.T[k]
        end

        #####
        ##### update_GMV_diagnostics
        #####
        aux_gm.q_liq[k] = (aux_bulk.area[k] * aux_bulk.q_liq[k] + (1 - aux_bulk.area[k]) * aux_en.q_liq[k])
        aux_gm.q_ice[k] = (aux_bulk.area[k] * aux_bulk.q_ice[k] + (1 - aux_bulk.area[k]) * aux_en.q_ice[k])
        aux_gm.T[k] = (aux_bulk.area[k] * aux_bulk.T[k] + (1 - aux_bulk.area[k]) * aux_en.T[k])
        aux_gm.buoy[k] = (aux_bulk.area[k] * aux_bulk.buoy[k] + (1 - aux_bulk.area[k]) * aux_en.buoy[k])

        has_condensate = TD.has_condensate(aux_bulk.q_liq[k] + aux_bulk.q_ice[k])
        aux_bulk.cloud_fraction[k] = if has_condensate && a_bulk_c > 1e-3
            1
        else
            0
        end
    end

    #####
    ##### face variables: diagnose primitive, diagnose env and compute bulk
    #####
    # TODO: figure out why `ifelse` is allocating
    @inbounds for i in 1:N_up
        a_up_bcs = (; bottom = CCO.SetValue(edmf.area_surface_bc[i]), top = CCO.Extrapolate())
        If = CCO.InterpolateC2F(; a_up_bcs...)
        a_min = edmf.minimum_area
        a_up = aux_up[i].area
        @. aux_up_f[i].w = ifelse(If(a_up) >= a_min, max(prog_up_f[i].ρaw / (ρ0_f * If(a_up)), 0), FT(0))
    end
    @inbounds for i in 1:N_up
        aux_up_f[i].w[kf_surf] = edmf.w_surface_bc[i]
    end

    parent(aux_tc_f.bulk.w) .= 0
    a_bulk_bcs = (; bottom = CCO.SetValue(sum(edmf.area_surface_bc)), top = CCO.Extrapolate())
    Ifb = CCO.InterpolateC2F(; a_bulk_bcs...)
    @inbounds for i in 1:N_up
        a_up = aux_up[i].area
        a_up_bcs = (; bottom = CCO.SetValue(edmf.area_surface_bc[i]), top = CCO.Extrapolate())
        Ifu = CCO.InterpolateC2F(; a_up_bcs...)
        @. aux_tc_f.bulk.w += ifelse(Ifb(aux_bulk.area) > 0, Ifu(a_up) * aux_up_f[i].w / Ifb(aux_bulk.area), FT(0))
    end
    # Assuming gm.W = 0!
    @. aux_en_f.w = -Ifb(aux_bulk.area) / (1 - Ifb(aux_bulk.area)) * aux_tc_f.bulk.w

    #####
    #####  diagnose_GMV_moments
    #####
    get_GMV_CoVar(edmf, grid, state, :Hvar, :θ_liq_ice)
    get_GMV_CoVar(edmf, grid, state, :QTvar, :q_tot)
    get_GMV_CoVar(edmf, grid, state, :HQTcov, :θ_liq_ice, :q_tot)

    # TODO - use this inversion in free_convection_windspeed and not compute zi twice
    edmf.zi = get_inversion(grid, state, param_set, surface.Ri_bulk_crit)

    update_surface(Case, grid, state, gm, TS, param_set)
    update_forcing(Case, grid, state, gm, TS, param_set)
    update_radiation(Case, grid, state, gm, TS, param_set)

    compute_pressure_plume_spacing(edmf, param_set)

    #####
    ##### compute_updraft_closures
    #####

    Ic = CCO.InterpolateF2C()
    ∇c = CCO.DivergenceF2C()
    LB = CCO.LeftBiasedC2F(; bottom = CCO.SetValue(FT(0)))

    @inbounds for i in 1:N_up
        # compute ∇m at cell centers
        a_up = aux_up[i].area
        w_up = aux_up_f[i].w
        w_en = aux_en_f.w
        w_gm = prog_gm_f.w
        @. m_entr_detr = a_up * (Ic(w_up) - Ic(w_gm))
        @. ∇m_entr_detr = ∇c(wvec(LB(m_entr_detr)))
        @. w_up_c = Ic(w_up)
        @. w_en_c = Ic(w_en)
        @inbounds for k in real_center_indices(grid)
            # entrainment

            q_cond_up = TD.condensate(TD.PhasePartition(aux_up[i].q_tot[k], aux_up[i].q_liq[k], aux_up[i].q_ice[k]))
            q_cond_en = TD.condensate(TD.PhasePartition(aux_en.q_tot[k], aux_en.q_liq[k], aux_en.q_ice[k]))

            if aux_up[i].area[k] > 0.0

                εδ_model_vars = GeneralizedEntr(;
                    q_cond_up = q_cond_up,
                    q_cond_en = q_cond_en,
                    w_up = w_up_c[k],
                    w_en = w_en_c[k],
                    b_up = aux_up[i].buoy[k],
                    b_en = aux_en.buoy[k],
                    tke = aux_en.tke[k],
                    dMdz = ∇m_entr_detr[k],
                    M = m_entr_detr[k],
                    a_up = aux_up[i].area[k],
                    a_en = aux_en.area[k],
                    R_up = edmf.pressure_plume_spacing[i],
                    RH_up = aux_up[i].RH[k],
                    RH_en = aux_en.RH[k],
                    max_area = max_area,
                    zc_i = grid.zc[k].z,
                    Δt = Δt,
                )

                er = entr_detr(param_set, εδ_model_vars, edmf.entr_closure)
                aux_up[i].entr_sc[k] = er.ε_dyn
                aux_up[i].detr_sc[k] = er.δ_dyn
                # stochastic closure
                stoch_ε = stochastic_closure(param_set, sde_model, Entrainment())
                stoch_δ = stochastic_closure(param_set, sde_model, Detrainment())
                aux_up[i].entr_sc[k] *= stoch_ε
                aux_up[i].detr_sc[k] *= stoch_δ

                aux_up[i].frac_turb_entr[k] = er.ε_turb
                aux_up[i].horiz_K_eddy[k] = er.K_ε
            else
                aux_up[i].entr_sc[k] = 0.0
                aux_up[i].detr_sc[k] = 0.0
                aux_up[i].frac_turb_entr[k] = 0.0
                aux_up[i].horiz_K_eddy[k] = 0.0
            end
        end
    end

    @inbounds for i in 1:N_up

        # pressure
        b_up = aux_up[i].buoy
        a_up = aux_up[i].area
        w_up = aux_up_f[i].w
        w_en = aux_en_f.w
        updraft_top = up.updraft_top[i]
        asp_ratio = 1.0

        b_bcs = (; bottom = CCO.SetValue(b_up[kc_surf]), top = CCO.SetValue(b_up[kc_toa]))
        a_bcs = (; bottom = CCO.SetValue(edmf.area_surface_bc[i]), top = CCO.SetValue(FT(0)))
        w_bcs = (; bottom = CCO.SetValue(wvec(FT(0))), top = CCO.SetValue(wvec(FT(0))))
        bcs = (; a_up = a_bcs, b_up = b_bcs, w_up = w_bcs)

        nh_press_buoy = aux_up_f[i].nh_pressure_b
        nh_press_adv = aux_up_f[i].nh_pressure_adv
        nh_press_drag = aux_up_f[i].nh_pressure_drag

        nh_press_buoy .= nh_pressure_buoy(param_set, a_up, b_up, ρ0_f, asp_ratio, bcs)
        nh_press_adv .= nh_pressure_adv(param_set, updraft_top, a_up, ρ0_f, w_up, bcs)
        nh_press_drag .= nh_pressure_drag(param_set, updraft_top, a_up, ρ0_f, w_up, w_en, bcs)
        @. aux_up_f[i].nh_pressure = nh_press_buoy + nh_press_adv + nh_press_drag
    end

    #####
    ##### compute_eddy_diffusivities_tke
    #####

    # Subdomain exchange term
    Ic = CCO.InterpolateF2C()
    b_exch = center_aux_turbconv(state).b_exch
    parent(b_exch) .= 0
    a_en = aux_en.area
    w_en = aux_en_f.w
    tke_en = aux_en.tke
    @inbounds for i in 1:N_up
        a_up = aux_up[i].area
        w_up = aux_up_f[i].w
        δ_dyn = aux_up[i].detr_sc
        ε_turb = aux_up[i].frac_turb_entr
        @. b_exch +=
            a_up * Ic(w_up) * δ_dyn / a_en * (1 / 2 * (Ic(w_up) - Ic(w_en))^2 - tke_en) -
            a_up * Ic(w_up) * (Ic(w_up) - Ic(w_en)) * ε_turb * Ic(w_en) / a_en
    end

    Shear² = center_aux_turbconv(state).Shear²
    ∂qt∂z = center_aux_turbconv(state).∂qt∂z
    ∂θl∂z = center_aux_turbconv(state).∂θl∂z
    ∂θv∂z = center_aux_turbconv(state).∂θv∂z
    ∂qt∂z_sat = center_aux_turbconv(state).∂qt∂z_sat
    ∂θl∂z_sat = center_aux_turbconv(state).∂θl∂z_sat
    ∂θv∂z_unsat = center_aux_turbconv(state).∂θv∂z_unsat

    ∇0_bcs = (; bottom = CCO.Extrapolate(), top = CCO.Extrapolate())
    If0 = CCO.InterpolateC2F(; ∇0_bcs...)
    u_gm = prog_gm.u
    v_gm = prog_gm.v
    w_en = aux_en_f.w
    # compute shear
    @. Shear² = (∇c(wvec(If0(u_gm))))^2 + (∇c(wvec(If0(v_gm))))^2 + (∇c(wvec(w_en)))^2

    q_tot_en = aux_en.q_tot
    θ_liq_ice_en = aux_en.θ_liq_ice
    θ_virt_en = aux_en.θ_virt
    @. ∂qt∂z = ∇c(wvec(If0(q_tot_en)))
    @. ∂θl∂z = ∇c(wvec(If0(θ_liq_ice_en)))
    @. ∂θv∂z = ∇c(wvec(If0(θ_virt_en)))

    # Second order approximation: Use dry and cloudy environmental fields.
    cf = aux_en.cloud_fraction
    shm = copy(cf)
    parent(shm) .= shrink_mask(vec(cf))

    # Since NaN*0 ≠ 0, we need to conditionally replace
    # our gradients by their default values.
    @. ∂qt∂z_sat = ∇c(wvec(If0(aux_en_sat.q_tot)))
    @. ∂θl∂z_sat = ∇c(wvec(If0(aux_en_sat.θ_liq_ice)))
    @. ∂θv∂z_unsat = ∇c(wvec(If0(aux_en_unsat.θ_virt)))
    for k in real_center_indices(grid)
        if shm[k] == 0
            ∂qt∂z_sat[k] = ∂qt∂z[k]
            ∂θl∂z_sat[k] = ∂θl∂z[k]
            ∂θv∂z_unsat[k] = ∂θv∂z[k]
        end
    end

    @inbounds for k in real_center_indices(grid)

        # buoyancy_gradients
        if edmf.bg_closure == BuoyGradMean()
            # First order approximation: Use environmental mean fields.
            ts_en = TD.PhaseEquil_pTq(param_set, p0_c[k], aux_en.T[k], aux_en.q_tot[k])
            bg_kwargs = (;
                t_sat = aux_en.T[k],
                qv_sat = TD.vapor_specific_humidity(ts_en),
                qt_sat = aux_en.q_tot[k],
                θ_sat = aux_en.θ_dry[k],
                θ_liq_ice_sat = aux_en.θ_liq_ice[k],
                ∂θv∂z_unsat = ∂θv∂z[k],
                ∂qt∂z_sat = ∂qt∂z[k],
                ∂θl∂z_sat = ∂θl∂z[k],
                p0 = p0_c[k],
                en_cld_frac = aux_en.cloud_fraction[k],
                alpha0 = α0_c[k],
            )
            bg_model = EnvBuoyGrad(edmf.bg_closure; bg_kwargs...)

        elseif edmf.bg_closure == BuoyGradQuadratures()

            bg_kwargs = (;
                t_sat = aux_en_sat.T[k],
                qv_sat = aux_en_sat.q_vap[k],
                qt_sat = aux_en_sat.q_tot[k],
                θ_sat = aux_en_sat.θ_dry[k],
                θ_liq_ice_sat = aux_en_sat.θ_liq_ice[k],
                ∂θv∂z_unsat = ∂θv∂z_unsat[k],
                ∂qt∂z_sat = ∂qt∂z_sat[k],
                ∂θl∂z_sat = ∂θl∂z_sat[k],
                p0 = p0_c[k],
                en_cld_frac = aux_en.cloud_fraction[k],
                alpha0 = α0_c[k],
            )
            bg_model = EnvBuoyGrad(edmf.bg_closure; bg_kwargs...)
        end
        bg = buoyancy_gradients(param_set, bg_model)

        # Limiting stratification scale (Deardorff, 1976)
        # compute ∇Ri and Pr
        ∇_Ri = gradient_Richardson_number(bg.∂b∂z, Shear²[k], eps(0.0))
        aux_tc.prandtl_nvec[k] = turbulent_Prandtl_number(param_set, obukhov_length, ∇_Ri)

        ml_model = MinDisspLen(;
            z = grid.zc[k].z,
            obukhov_length = obukhov_length,
            tke_surf = aux_en.tke[kc_surf],
            ustar = surface.ustar,
            Pr = aux_tc.prandtl_nvec[k],
            p0 = p0_c[k],
            ∇b = bg,
            Shear² = Shear²[k],
            tke = aux_en.tke[k],
            b_exch = b_exch[k],
        )

        ml = mixing_length(param_set, ml_model)
        aux_tc.mls[k] = ml.min_len_ind
        aux_tc.mixing_length[k] = ml.mixing_length
        aux_tc.ml_ratio[k] = ml.ml_ratio

        KM[k] = c_m * ml.mixing_length * sqrt(max(aux_en.tke[k], 0.0))
        KH[k] = KM[k] / aux_tc.prandtl_nvec[k]

        aux_en_2m.tke.buoy[k] = -aux_en.area[k] * ρ0_c[k] * KH[k] * bg.∂b∂z
    end

    #####
    ##### compute covarinaces tendendies
    #####
    tke_press = aux_en_2m.tke.press
    w_en = aux_en_f.w
    parent(tke_press) .= 0
    @inbounds for i in 1:N_up
        w_up = aux_up_f[i].w
        nh_press = aux_up_f[i].nh_pressure
        @. tke_press += (Ic(w_en) - Ic(w_up)) * Ic(nh_press)
    end

    compute_covariance_entr(edmf, grid, state, Val(:tke), Val(:w))
    compute_covariance_entr(edmf, grid, state, Val(:Hvar), Val(:θ_liq_ice))
    compute_covariance_entr(edmf, grid, state, Val(:QTvar), Val(:q_tot))
    compute_covariance_entr(edmf, grid, state, Val(:HQTcov), Val(:θ_liq_ice), Val(:q_tot))
    compute_covariance_shear(edmf, grid, state, gm, Val(:tke), Val(:w))
    compute_covariance_shear(edmf, grid, state, gm, Val(:Hvar), Val(:θ_liq_ice))
    compute_covariance_shear(edmf, grid, state, gm, Val(:QTvar), Val(:q_tot))
    compute_covariance_shear(edmf, grid, state, gm, Val(:HQTcov), Val(:θ_liq_ice), Val(:q_tot))
    compute_covariance_dissipation(edmf, grid, state, :tke, param_set)
    compute_covariance_dissipation(edmf, grid, state, :Hvar, param_set)
    compute_covariance_dissipation(edmf, grid, state, :QTvar, param_set)
    compute_covariance_dissipation(edmf, grid, state, :HQTcov, param_set)

    # TODO defined again in compute_covariance_shear and compute_covaraince
    @inbounds for k in real_center_indices(grid)
        aux_en_2m.tke.rain_src[k] = 0
        aux_en_2m.Hvar.rain_src[k] = ρ0_c[k] * aux_en.area[k] * 2 * aux_en.Hvar_rain_dt[k]
        aux_en_2m.QTvar.rain_src[k] = ρ0_c[k] * aux_en.area[k] * 2 * aux_en.QTvar_rain_dt[k]
        aux_en_2m.HQTcov.rain_src[k] = ρ0_c[k] * aux_en.area[k] * aux_en.HQTcov_rain_dt[k]
    end

    get_GMV_CoVar(edmf, grid, state, :tke, :w)

    compute_diffusive_fluxes(edmf, grid, state, gm, Case, TS, param_set)

    # TODO: use dispatch
    if edmf.Precip.precipitation_model == "clima_1m"
        # helper to calculate the rain velocity
        # TODO: assuming gm.W = 0
        # TODO: verify translation
        term_vel_rain = aux_tc.term_vel_rain
        term_vel_snow = aux_tc.term_vel_snow
        prog_pr = center_prog_precipitation(state)

        @inbounds for k in real_center_indices(grid)
            term_vel_rain[k] = CM1.terminal_velocity(param_set, CM1.RainType(), ρ0_c[k], prog_pr.q_rai[k])
            term_vel_snow[k] = CM1.terminal_velocity(param_set, CM1.SnowType(), ρ0_c[k], prog_pr.q_sno[k])
        end
    end

    @inbounds for i in 1:N_up
        up.updraft_top[i] = 0.0

        @inbounds for k in real_center_indices(grid)
            if aux_up[i].area[k] > 1e-3
                up.updraft_top[i] = max(up.updraft_top[i], grid.zc[k])
            end
        end
    end

end


module ROMEO

using Statistics
using StatsBase

const NBINS = 256

include("utility.jl")
include("priorityqueue.jl")
include("weights.jl")
include("seed.jl")
include("region_handling.jl")
include("algorithm.jl")
include("unwrapping.jl")

export unwrap, unwrap!, unwrap_individual, unwrap_individual!

end # module


#!/usr/bin/env julia
using RobotOS
@rosimport geometry_msgs.msg: Point, Pose, Pose2D, PoseStamped, Vector3, Twist
@rosimport std_srvs.srv: Empty, SetBool
@rosimport nav_msgs.srv.GetPlan
@rosimport gazebo_msgs.msg: ModelState, ModelStates
@rosimport gazebo_msgs.srv: SetModelState, GetModelState, GetWorldProperties, GetLinkState, SetLinkState
@rosimport obstacle_detector.msg: Obstacles, CircleObstacle
@rosimport mavs_control.msg: Control

rostypegen()
using geometry_msgs.msg
using std_srvs.srv
using nav_msgs.srv
using nav_msgs.srv.GetPlan
using gazebo_msgs.srv
using gazebo_msgs.msg
using obstacle_detector.msg
using mavs_control.msg

using NLOptControl
using VehicleModels
using DataFrames
using MAVs
using KNITRO
using PyCall
@pyimport tf.transformations as tf

# TODO
# 1) detect crash
# 3) get the Gazebo sim time
# 5) pass these parameters to a service or something -> needs to happen on a seperate node!
# 6) update OCP based off of current gazebo model position
# 7) get the current position of Gazebo model => need all of the states!!
# 8) get time from the Gazebo world
# 9) populate obstacle field in Gazebo based off of c.o

"""

--------------------------------------------------------------------------------------\n
Author: Huckleberry Febbo, Graduate Student, University of Michigan
Date Create: 4/6/2017, Last Modified: 8/2/2017 \n
--------------------------------------------------------------------------------------\n
"""
function setObstacleData(params)

  if RobotOS.has_param("obstacle_radius")
    Q = params[2][7];           # number of obstacles the algorithm can handle

    if isnan(RobotOS.get_param("obstacle_radius"))
      for i in 1:Q
        setvalue(params[2][1][i],1.0);
        setvalue(params[2][2][i],1.0);
        setvalue(params[2][3][i],-100.0 - 3*i);
        setvalue(params[2][4][i],-100.0);
        setvalue(params[2][5][i],0.0);
        setvalue(params[2][6][i],0.0);
      end
    else
      r=deepcopy(RobotOS.get_param("obstacle_radius"))
      x=deepcopy(RobotOS.get_param("obstacle_x"))
      y=deepcopy(RobotOS.get_param("obstacle_y"))
      vx=deepcopy(RobotOS.get_param("obstacle_vx"))
      vy=deepcopy(RobotOS.get_param("obstacle_vy"))

      L = length(r)               # number of obstacles detected
      N = Q - L;
      if N < 0
        warn(" \n The number of obstacles detected exceeds the number of obstacles the algorithm was designed for! \n
                  Consider increasing the number of obstacles the algorithm can handle \n!")
      end

      for i in 1:Q
        if i <= L          # add obstacle
          setvalue(params[2][1][i],r[i]);
          setvalue(params[2][2][i],r[i]);
          setvalue(params[2][3][i],x[i]);
          setvalue(params[2][4][i],y[i]);
          setvalue(params[2][5][i],vx[i]);
          setvalue(params[2][6][i],vy[i]);
        else              # set obstacle off field
          setvalue(params[2][1][i],1.0);
          setvalue(params[2][2][i],1.0);
          setvalue(params[2][3][i],-100.0 - 3*i);
          setvalue(params[2][4][i],-100.0);
          setvalue(params[2][5][i],0.0);
          setvalue(params[2][6][i],0.0);
        end
      end
    end
  end

  return nothing
end

function main()
  init_node("rosjl_obstacles")
  #linkName = "base_footprint"  # TODO make this a parameter

  modelName = RobotOS.get_param("robotName")

  ###############################
  # set up services and messages

  # pause simulation
  const pause_physics = ServiceProxy("/gazebo/pause_physics",Empty)
  println("Waiting for '/gazebo/pause_physics' service...")
  wait_for_service("/gazebo/pause_physics")

  # unpause simulation
  const unpause_physics = ServiceProxy("/gazebo/unpause_physics",Empty)
  println("Waiting for '/gazebo/unpause_physics' service...")
  wait_for_service("/gazebo/unpause_physics")

  # Set up service to get Gazebo model state
  const get_state = ServiceProxy("/gazebo/get_model_state", GetModelState)
  println("Waiting for 'gazebo/get_model_state' service...")
  wait_for_service("gazebo/get_model_state")

  # Set up service to set Gazebo model state
  const set_state = ServiceProxy("/gazebo/set_model_state", SetModelState)
  println("Waiting for 'gazebo/set_model_state' service...")
  wait_for_service("gazebo/set_model_state")

  # message for solution to optimal control problem
  const pub = Publisher{Control}("/mavs/optimal_control", queue_size=10)

  # set up services and messages
  ###############################

  c=defineCase(;(:mode=>:autoGazebo));

  # Get the current position of the Gazebo model
  gs = GetModelStateRequest()
  gs.model_name = modelName
  gs_r = get_state(gs)

  if !gs_r.success
      error(string(" calling /gazebo/get_model_state service: ", gs_r.status_message))
  end

  # Define position to move robot
  vehPose = gs_r.pose  # use the current position
  vehPose.position.x = c.m.X0[1]
  vehPose.position.y = c.m.X0[2]
  roll = 0; pitch = 0; yaw = c.m.X0[5];
  Q = tf.quaternion_from_euler(roll, pitch, yaw)
  vehPose.orientation.x = Q[1]
  vehPose.orientation.y = Q[2]
  vehPose.orientation.z = Q[3]
  vehPose.orientation.w = Q[4]

  # Define the robot state
  ms = ModelState()
  ms.model_name = modelName
  ms.pose = vehPose

  # Set the state of the Gazebo model
  ss = SetModelStateRequest()
  ss.model_state = ms
  println("Calling 'gazebo/set_model_state' service...")
  ss_r = set_state(ss)

  n=initializeAutonomousControl(c);

  driveStraight!(n)

  loop_rate = Rate(2.0)

  # unpause physics
  up = EmptyRequest()
  unpause_physics(up)

  while !is_shutdown()

    if ((n.r.dfs_plant[end][:x][end]-c.g.x_ref)^2 + (n.r.dfs_plant[end][:y][end]-c.g.y_ref)^2)^0.5 < 2*n.XF_tol[1]
       println("Goal Attained! \n"); n.mpc.goal_reached=true;
       pp = EmptyRequest()
       pause_physics(pp)
       break;
    end

    println("Running model for the: ", n.r.eval_num," time");
    setObstacleData(n.params)
    updateAutoParams!(n,c)  # check to see if goal is in range

    # get the cur6rent position of Gazebo model
    gs = GetModelStateRequest()
    gs.model_name = modelName
    gs_r = get_state(gs)
    if !gs_r.success
        error(string(" calling /gazebo/get_model_state service: ", gs_r.status_message))
    end
    X0 = zeros(8)
    X0[1] = gs_r.pose.position.x
    X0[2] = gs_r.pose.position.y
    X0[3] = n.mpc.X0p[3]
    X0[4] = n.mpc.X0p[4]
    Q = [gs_r.pose.orientation.x,gs_r.pose.orientation.y,gs_r.pose.orientation.z,gs_r.pose.orientation.w]
    roll, pitch, yaw = tf.euler_from_quaternion(Q)
    X0[5] = yaw
    X0[6] = n.mpc.X0p[6]
    X0[7] = n.mpc.X0p[7]
    X0[8] = n.mpc.X0p[8]
    println(string(X0,"\n"))
    updateX0!(n,X0;(:userUpdate=>true))
    status=autonomousControl!(n)                # rerun optimization
  #  n.mpc.t0_actual=(n.r.eval_num-1)*n.mpc.tex  # external so that it can be updated easily in PathFollowing
    n.mpc.t0_actual = to_sec(get_rostime())
    msg = Control()
    msg.t = n.mpc.t0_actual + n.r.t_st
    msg.x = n.r.X[:,1]
    msg.y = n.r.X[:,2]
    msg.psi = n.r.X[:,5]
    msg.sa = n.r.X[:,6]
    msg.vx = n.r.X[:,7]
    # TODO consider buffering the message here..
    publish(pub,msg)

  #  spinOnce() # call the callbacks
    rossleep(loop_rate) # NOTE might not want to do this ...
  end
end

if !isinteractive()
    main()
end


"""
Training loops on PyTorch models
"""

md"""

Is it possible to do everything via callback?

Only thing user would have to do is:
- pass in a `PyObject` model
- bass in a `PyTorchBackend(; device)` callback
- (optionally) pass in `PyObject` optimiser and loss functions (these can be
converted automatically)

The callback would do:

- `Zygote.gradient(::PyTorchParams)` should be overwritten to not calculate
    gradients, but just return the loss
- `BackwardBegin` should call `state.loss.backward()`
- `BackwardEnd` should set `state.loss = state.loss.item()` so that callbacks like
    `Metrics` will just see a number and it can be GC'd on Python side.
- `Flux.Optimise.update!(::PyTorchOptimiser, ::PyTorchParams, _)` should be overloaded
    to call `optim.step()` and `optim.zero_grad()`
"""

"""
    PyTorchBackend()

Callback that allows you to train PyTorch models with FluxTraining.jl.
To use, pass this callback to `Learner` and pass a `PyObject` PyTorch `model`.
"""
struct PyTorchBackend <: FluxTraining.Callback
    device::Any
end

PyTorchBackend() = PyTorchBackend(torch.cuda.is_available() ? "cuda" : "cpu")

import FluxTraining: init!, on, stateaccess, Read, Write, resolveconflict, Unresolvable

# During `init!`, the `Learner`'s `model`, `params`, `optimizer` and `lossfn`
# are, where possible, replaced with PyTorch versions.
function FluxTraining.init!(cb::PyTorchBackend, learner)
    learner.model = convertmodel(learner.model; device = cb.device)
    learner.params = Flux.params(learner.model)
    learner.optimizer = convertoptimiser(learner.optimizer, learner.params)
    learner.lossfn = convertlossfunction(learner.lossfn)
    return nothing
end

stateaccess(::PyTorchBackend) = (;
    model = Write(),
    params = Write(),
    optimizer = Write(),
    lossfn = Write(),
    step = Write(),
)

# `PyTorchBackend` should not be used with `ToGPU`/`ToDevice`:
resolveconflict(::PyTorchBackend, ::ToDevice) = Unresolvable()
resolveconflict(::ToDevice, ::PyTorchBackend) = Unresolvable()

function FluxTraining.on(::EpochBegin, ::AbstractTrainingPhase, cb::PyTorchBackend, learner)
    Flux.trainmode!(learner.model)
    learner.params = Flux.params(learner.model)
    learner.optimizer = convertoptimiser(learner.optimizer, learner.params)
end
function FluxTraining.on(::EpochBegin, ::AbstractValidationPhase, cb::PyTorchBackend, learner)
    Flux.testmode!(learner.model)
end

# At the beginning of every step, convert data to tensors on the correct
# device:
function FluxTraining.on(::StepBegin, ::Phase, cb::PyTorchBackend, learner)
    learner.step.xs = totensor(learner.step.xs, device=cb.device)
    learner.step.ys = totensor(learner.step.ys, device=cb.device)
end

# To make PyTorch training work with the default training loop, we'll
# implement a custom method for `Zygote.gradient` that dispatches on
# `PyTorchParams`.
Zygote.gradient(f, ::PyTorchParams) = f()

# Instead, when the `BackwardBegin` event is triggered, `loss.backward()`
# is called to do the backward pass. Importantly, this is done only during
# `AbstractTrainingPhase`s.
function FluxTraining.on(
    ::BackwardBegin,
    ::AbstractTrainingPhase,
    ::PyTorchBackend,
    learner,
)
    learner.step.loss.backward()
    learner.step.loss = learner.step.loss.item()
    # no gradient object to return
    return nothing
end

# On `StepEnd` during `AbstractValidationPhase`s, `learner.state.loss` is
# replaced by its scalar value so that callbacks like `Metrics` and can
# read it and things can be garbagecollected on the Python side. For
# `AbstractTrainingPhase`s this already happens during ´BackwardBegin`.

function FluxTraining.on(
    ::StepEnd,
    ::AbstractValidationPhase,
    ::PyTorchBackend,
    learner,
)
    learner.step.loss = learner.step.loss.item()
end

# Finally, `Flux.update!` is overloaded for `PyTorchOptimiser`

function Flux.Optimise.update!(o::PyTorchOptimiser, ::PyTorchParams, gs)
    o.optim.step()
    o.optim.zero_grad()
end


using MathOptInterface
const MOI = MathOptInterface

include("scaled_psd_cone_bridge.jl")

MOI.Utilities.@product_of_sets(
    Cones,
    MOI.Zeros,
    MOI.Nonnegatives,
    MOI.SecondOrderCone,
    ScaledPSDCone,
    MOI.ExponentialCone,
    MOI.DualExponentialCone,
    MOI.PowerCone{T},
    MOI.DualPowerCone{T},
)

const OptimizerCache{T} = MOI.Utilities.GenericModel{
    Cdouble,
    MOI.Utilities.ObjectiveContainer{Cdouble},
    MOI.Utilities.VariablesContainer{Cdouble},
    MOI.Utilities.MatrixOfConstraints{
        Cdouble,
        MOI.Utilities.MutableSparseMatrixCSC{
            Cdouble,
            T,
            MOI.Utilities.ZeroBasedIndexing,
        },
        Vector{Cdouble},
        Cones{Cdouble},
    },
}

function _to_sparse(
    ::Type{T},
    A::MOI.Utilities.MutableSparseMatrixCSC{
        Cdouble,
        T,
        MOI.Utilities.ZeroBasedIndexing,
    },
) where {T}
    return -A.nzval, A.rowval, A.colptr
end

mutable struct MOISolution
    primal::Vector{Float64}
    dual::Vector{Float64}
    slack::Vector{Float64}
    ret_val::Int
    raw_status::String
    objective_value::Float64
    dual_objective_value::Float64
    objective_constant::Float64
    solve_time_sec::Float64
    iterations::Int
end

function MOISolution()
    return MOISolution(
        Float64[],
        Float64[],
        Float64[],
        0, # SCS_UNFINISHED
        "",
        NaN,
        NaN,
        NaN,
        0.0,
        0,
    )
end

mutable struct Optimizer <: MOI.AbstractOptimizer
    cones::Union{Nothing,Cones{Cdouble}}
    sol::MOISolution
    silent::Bool
    options::Dict{Symbol,Any}
    function Optimizer(; kwargs...)
        optimizer = new(nothing, MOISolution(), false, Dict{Symbol,Any}())
        for (key, value) in kwargs
            MOI.set(optimizer, MOI.RawOptimizerAttribute(String(key)), value)
        end
        return optimizer
    end
end

function MOI.get(::Optimizer, ::MOI.Bridges.ListOfNonstandardBridges)
    return [ScaledPSDConeBridge{Cdouble}]
end

MOI.get(::Optimizer, ::MOI.SolverName) = "SCS"

MOI.is_empty(optimizer::Optimizer) = optimizer.cones === nothing

function MOI.empty!(optimizer::Optimizer)
    optimizer.cones = nothing
    optimizer.sol = MOISolution()
    return
end

###
### MOI.RawOptimizerAttribute
###

function MOI.set(optimizer::Optimizer, param::MOI.RawOptimizerAttribute, value)
    return optimizer.options[Symbol(param.name)] = value
end

function MOI.get(optimizer::Optimizer, param::MOI.RawOptimizerAttribute)
    return optimizer.options[Symbol(param.name)]
end

###
### MOI.Silent
###

MOI.supports(::Optimizer, ::MOI.Silent) = true

function MOI.set(optimizer::Optimizer, ::MOI.Silent, value::Bool)
    optimizer.silent = value
    return
end

MOI.get(optimizer::Optimizer, ::MOI.Silent) = optimizer.silent

###
### MOI.AbstractModelAttribute
###

function MOI.supports(
    ::Optimizer,
    ::Union{
        MOI.ObjectiveSense,
        MOI.ObjectiveFunction{MOI.ScalarAffineFunction{Float64}},
    },
)
    return true
end

###
### MOI.AbstractVariableAttribute
###

# TODO(odow): support variable primal starts
# function MOI.supports(
#     ::Optimizer,
#     ::MOI.VariablePrimalStart,
#     ::Type{MOI.VariableIndex},
# )
#     return true
# end

###
### MOI.AbstractConstraintAttribute
###

# TODO(odow): support constraint primal and dual starts
# function MOI.supports(
#     ::Optimizer,
#     ::Union{MOI.ConstraintPrimalStart,MOI.ConstraintDualStart},
#     ::Type{<:MOI.ConstraintIndex},
# )
#     return true
# end

function MOI.supports_constraint(
    ::Optimizer,
    ::Type{MOI.VectorAffineFunction{Cdouble}},
    ::Type{
        <:Union{
            MOI.Zeros,
            MOI.Nonnegatives,
            MOI.SecondOrderCone,
            ScaledPSDCone,
            MOI.ExponentialCone,
            MOI.DualExponentialCone,
            MOI.PowerCone{Cdouble},
            MOI.DualPowerCone{Cdouble},
        },
    },
)
    return true
end

function _map_sets(f, ::Type{T}, sets, ::Type{S}) where {T,S}
    F = MOI.VectorAffineFunction{Cdouble}
    cis = MOI.get(sets, MOI.ListOfConstraintIndices{F,S}())
    return T[f(MOI.get(sets, MOI.ConstraintSet(), ci)) for ci in cis]
end

function MOI.optimize!(
    dest::Optimizer,
    src::MOI.Utilities.UniversalFallback{OptimizerCache{T}},
) where {T}
    linear_solver = get(dest.options, :linear_solver, DirectSolver)
    if T != scsint_t(linear_solver)
        cache = MOI.Utilities.UniversalFallback(
            OptimizerCache{scsint_t(linear_solver)}(),
        )
        MOI.copy_to(cache, src)
        return MOI.optimize!(dest, cache)
    end
    # The real stuff starts here.
    MOI.empty!(dest)
    index_map = MOI.Utilities.identity_index_map(src)
    Ab = src.model.constraints
    A = Ab.coefficients

    model_attributes = MOI.get(src, MOI.ListOfModelAttributesSet())
    objective_function_attr =
        MOI.ObjectiveFunction{MOI.ScalarAffineFunction{Cdouble}}()
    for attr in model_attributes
        if attr != MOI.ObjectiveSense() && attr != objective_function_attr
            throw(MOI.UnsupportedAttribute(attr))
        end
    end
    max_sense = false
    if MOI.ObjectiveSense() in model_attributes
        max_sense = MOI.get(src, MOI.ObjectiveSense()) == MOI.MAX_SENSE
    end
    objective_constant = 0.0
    c = zeros(A.n)
    if objective_function_attr in model_attributes
        obj = MOI.get(src, objective_function_attr)
        objective_constant = MOI.constant(obj)
        for term in obj.terms
            c[term.variable.value] += (max_sense ? -1 : 1) * term.coefficient
        end
    end
    # `model.primal` contains the result of the previous optimization.
    # It is used as a warm start if its length is the same, e.g.
    # probably because no variable and/or constraint has been added.
    if A.n != length(dest.sol.primal)
        resize!(dest.sol.primal, A.n)
        fill!(dest.sol.primal, 0.0)
    end
    if A.m != length(dest.sol.dual)
        resize!(dest.sol.dual, A.m)
        fill!(dest.sol.dual, 0.0)
    end
    if A.m != length(dest.sol.slack)
        resize!(dest.sol.slack, A.m)
        fill!(dest.sol.slack, 0.0)
    end
    # Set starting values and throw error for other variable attributes
    vis_src = MOI.get(src, MOI.ListOfVariableIndices())
    MOI.Utilities.pass_attributes(dest, src, index_map, vis_src)
    # Set starting values and throw error for other constraint attributes
    for (F, S) in MOI.get(src, MOI.ListOfConstraintTypesPresent())
        cis_src = MOI.get(src, MOI.ListOfConstraintIndices{F,S}())
        MOI.Utilities.pass_attributes(dest, src, index_map, cis_src)
    end
    options = copy(dest.options)
    if dest.silent
        options[:verbose] = 0
    end
    dest.cones = deepcopy(Ab.sets)
    sol = scs_solve(
        linear_solver,
        A.m,
        A.n,
        A,
        Ab.constants,
        c,
        Ab.sets.num_rows[1],
        Ab.sets.num_rows[2] - Ab.sets.num_rows[1],
        _map_sets(MOI.dimension, T, Ab, MOI.SecondOrderCone),
        _map_sets(MOI.side_dimension, T, Ab, ScaledPSDCone),
        div(Ab.sets.num_rows[5] - Ab.sets.num_rows[4], 3),
        div(Ab.sets.num_rows[6] - Ab.sets.num_rows[5], 3),
        vcat(
            _map_sets(set -> set.exponent, Float64, Ab, MOI.PowerCone{Cdouble}),
            _map_sets(
                set -> -set.exponent,
                Float64,
                Ab,
                MOI.DualPowerCone{Cdouble},
            ),
        ),
        dest.sol.primal,
        dest.sol.dual,
        dest.sol.slack;
        options...,
    )
    dest.sol = MOISolution(
        sol.x,
        sol.y,
        sol.s,
        sol.ret_val,
        raw_status(sol.info),
        (max_sense ? -1 : 1) * sol.info.pobj,
        (max_sense ? -1 : 1) * sol.info.dobj,
        objective_constant,
        (sol.info.setupTime + sol.info.solveTime) / 1000,
        sol.info.iter,
    )
    return index_map, false
end

function MOI.optimize!(dest::Optimizer, src::MOI.ModelLike)
    linear_solver = get(dest.options, :linear_solver, DirectSolver)
    T = scsint_t(linear_solver)
    cache = MOI.Utilities.UniversalFallback(OptimizerCache{T}())
    index_map = MOI.copy_to(cache, src)
    MOI.optimize!(dest, cache)
    return index_map, false
end

function MOI.get(optimizer::Optimizer, ::MOI.SolveTimeSec)
    return optimizer.sol.solve_time_sec
end

function MOI.get(optimizer::Optimizer, ::MOI.RawStatusString)
    return optimizer.sol.raw_status
end

"""
    ADMMIterations()

The number of ADMM iterations completed during the solve.
"""
struct ADMMIterations <: MOI.AbstractModelAttribute end

MOI.is_set_by_optimize(::ADMMIterations) = true

function MOI.get(optimizer::Optimizer, ::ADMMIterations)
    return optimizer.sol.iterations
end

# Implements getter for result value and statuses
# SCS returns one of the following integers:
# -7 SCS_INFEASIBLE_INACCURATE
# -6 SCS_UNBOUNDED_INACCURATE
# -5 SCS_SIGINT
# -4 SCS_FAILED
# -3 SCS_INDETERMINATE
# -2 SCS_INFEASIBLE  : primal infeasible, dual unbounded
# -1 SCS_UNBOUNDED   : primal unbounded, dual infeasible
#  0 SCS_UNFINISHED  : never returned, used as placeholder
#  1 SCS_SOLVED
#  2 SCS_SOLVED_INACCURATE
function MOI.get(optimizer::Optimizer, ::MOI.TerminationStatus)
    s = optimizer.sol.ret_val
    @assert -7 <= s <= 2
    if s == -7
        return MOI.ALMOST_INFEASIBLE
    elseif s == -6
        return MOI.ALMOST_DUAL_INFEASIBLE
    elseif s == 2
        return MOI.ALMOST_OPTIMAL
    elseif s == -5
        return MOI.INTERRUPTED
    elseif s == -4
        return MOI.NUMERICAL_ERROR
    elseif s == -3
        return MOI.SLOW_PROGRESS
    elseif s == -2
        return MOI.INFEASIBLE
    elseif s == -1
        return MOI.DUAL_INFEASIBLE
    elseif s == 1
        return MOI.OPTIMAL
    else
        @assert s == 0
        return MOI.OPTIMIZE_NOT_CALLED
    end
end

function MOI.get(optimizer::Optimizer, attr::MOI.ObjectiveValue)
    MOI.check_result_index_bounds(optimizer, attr)
    value = optimizer.sol.objective_value
    if !MOI.Utilities.is_ray(MOI.get(optimizer, MOI.PrimalStatus()))
        value += optimizer.sol.objective_constant
    end
    return value
end

function MOI.get(optimizer::Optimizer, attr::MOI.DualObjectiveValue)
    MOI.check_result_index_bounds(optimizer, attr)
    value = optimizer.sol.dual_objective_value
    if !MOI.Utilities.is_ray(MOI.get(optimizer, MOI.DualStatus()))
        value += optimizer.sol.objective_constant
    end
    return value
end

function MOI.get(optimizer::Optimizer, attr::MOI.PrimalStatus)
    if attr.result_index > MOI.get(optimizer, MOI.ResultCount())
        return MOI.NO_SOLUTION
    elseif optimizer.sol.ret_val in (-3, 1, 2)
        return MOI.FEASIBLE_POINT
    elseif optimizer.sol.ret_val in (-6, -1)
        return MOI.INFEASIBILITY_CERTIFICATE
    end
    return MOI.INFEASIBLE_POINT
end

function MOI.get(
    optimizer::Optimizer,
    attr::MOI.VariablePrimal,
    vi::MOI.VariableIndex,
)
    MOI.check_result_index_bounds(optimizer, attr)
    return optimizer.sol.primal[vi.value]
end

function MOI.get(
    optimizer::Optimizer,
    attr::MOI.ConstraintPrimal,
    ci::MOI.ConstraintIndex{F,S},
) where {F,S}
    MOI.check_result_index_bounds(optimizer, attr)
    return optimizer.sol.slack[MOI.Utilities.rows(optimizer.cones, ci)]
end

function MOI.get(optimizer::Optimizer, attr::MOI.DualStatus)
    if attr.result_index > MOI.get(optimizer, MOI.ResultCount())
        return MOI.NO_SOLUTION
    elseif optimizer.sol.ret_val in (-3, 1, 2)
        return MOI.FEASIBLE_POINT
    elseif optimizer.sol.ret_val in (-7, -2)
        return MOI.INFEASIBILITY_CERTIFICATE
    end
    return MOI.INFEASIBLE_POINT
end

function MOI.get(
    optimizer::Optimizer,
    attr::MOI.ConstraintDual,
    ci::MOI.ConstraintIndex{F,S},
) where {F,S}
    MOI.check_result_index_bounds(optimizer, attr)
    return optimizer.sol.dual[MOI.Utilities.rows(optimizer.cones, ci)]
end

MOI.get(optimizer::Optimizer, ::MOI.ResultCount) = 1


facts("Leadfield") do

    L = 1

    context("Create") do

        L = Leadfield(rand(2000, 3, 6), vec(rand(2000, 1)), vec(rand(2000, 1)), vec(rand(2000, 1)), vec(["Cz","80Hz_SWN_70dB_R","20Hz_SWN_70dB_R","40Hz_SWN_70dB_R","10Hz_SWN_70dB_R","_4Hz_SWN_70dB_R"]))

        @fact size(L.L) --> (2000, 3, 6)
    end

    context("Show") do

        show(L)
    end

    context("Match") do

        s = read_SSR(joinpath(dirname(@__FILE__), "../../data", "test_Hz19.5-testing.bdf"))

        L1 = match_leadfield(L, s)
        @fact size(L1.L) --> (2000, 3, 6)

        keep_channel!(s, ["Cz","80Hz_SWN_70dB_R","20Hz_SWN_70dB_R","40Hz_SWN_70dB_R"])
        L2 = match_leadfield(deepcopy(L), s)
        @fact size(L2.L) --> (2000, 3, 4)
        @fact L2.L[:, :, 1] --> L1.L[:, :, 1]
        @fact L2.L[:, :, 2] --> L1.L[:, :, 3]
        @fact L2.L[:, :, 4] --> L1.L[:, :, 6]
        @fact L2.sensors --> channelnames(s)

        s = merge_channels(s, "Cz", "garbage")
        @fact_throws BoundsError match_leadfield(L, s)

        L = Leadfield(rand(2000, 3, 5), vec(rand(2000, 1)), vec(rand(2000, 1)), vec(rand(2000, 1)), vec(["Cz","80Hz_SWN_70dB_R","10Hz_SWN_70dB_R","_4Hz_SWN_70dB_R"]))
        @fact_throws ErrorException  match_leadfield(L, s)

    end

    context("Operations") do
        ldf = Leadfield(rand(5, 3, 2), collect(1:5.0), collect(1:5.0), collect(1:5.0), ["ch1", "ch2"])

        context("Find location") do
            for n = 1:size(ldf.L, 1)
                @fact n --> find_location(ldf, Talairach(ldf.x[n], ldf.y[n], ldf.z[n]))
            end

            for n = 1:size(ldf.L, 1)
                @fact n --> find_location(ldf, Talairach(ldf.x[n]+0.1, ldf.y[n]-0.1, ldf.z[n]))
            end
        end

    end
end


module TFIMSampler

using BitBasis
using DelimitedFiles
using ProgressMeter
using Random
using StatsBase
using LinearAlgebra
using SparseArrays

set_zero_subnormals(true)

include("new.jl")
include("mcmc.jl")

export sample, DQMC, pairing, sweep!, fast_update!, get_probs, get_prob, get_all_probs

end # module


update!(p::Printer, problem, gradient, ξ, t) = p.verbosity ≥ 2 && @info "t = $t"

function update!(writer::VTKWriter, problem, gradient, ξ, t)
    if writer.n % writer.nupdate == 0
        (; pvd, dir, filename, ifile) = writer
        (; model) = problem

        ncompartment = length(model.mesh.points)
        npoint_cmpts = size.(model.mesh.points, 2)
        inds_cmpts = [0; cumsum(npoint_cmpts[1:end])]

        vtmfile = vtk_multiblock("$dir/$(filename)_$ifile")

        for icmpt = 1:ncompartment
            points = model.mesh.points[icmpt]
            elements = model.mesh.elements[icmpt]

            cells = [
                MeshCell(VTKCellTypes.VTK_TETRA, elements[:, i]) for i = 1:size(elements, 2)
            ]
            ξ_cmpt = ξ[1+inds_cmpts[icmpt]:inds_cmpts[icmpt+1]]

            vtkfile = vtk_grid(vtmfile, points, cells)
            vtkfile["Magnetization (real part)", VTKPointData()] = real(ξ_cmpt)
            vtkfile["Magnetization (imaginary part)", VTKPointData()] = imag(ξ_cmpt)
            vtkfile["Gradient", VTKFieldData()] = gradient(t)
        end

        pvd[t] = vtmfile
        writer.ifile += 1
    end
    writer.n += 1
end

function update!(p::Plotter, problem, gradient, ξ, t)
    if p.n % p.nupdate == 0
        femesh = problem.model.mesh
        M = problem.matrices.M
        ncompartment, nboundary = size(femesh.facets)
        ξ_cmpts = split_field(femesh, ξ)
        push!(p.t[], t)
        if gradient isa ScalarGradient
            p.f[] = push!(p.f[], gradient.profile(t))
        else
            grad = Vec3f(gradient(t))
            p.g⃗[] = [grad]
            p.g⃗_hist[] = push!(p.g⃗_hist[], grad)
        end
        p.attenuation[] = push!(p.attenuation[], sum(abs, M * ξ) / p.S₀)
        p.ξ[] = ξ
        for icmpt = 1:ncompartment, iboundary = 1:nboundary
            p.magnitude[icmpt, iboundary][] = abs.(ξ_cmpts[icmpt])
            p.phase[icmpt, iboundary][] = angle.(ξ_cmpts[icmpt])
        end

        sleep(0.01)
    end
    p.n += 1
end


using SnakeAI

const WIDTH = SnakeAI.WIDTH
const HEIGHT = SnakeAI.HEIGHT
const BACKGROUND = colorant"black"

game = SnakeAI.Game()
agent = SnakeAgent()

function draw()
    # Drawing the snake
    for p in game.snake.body
        outside = Rect(p.x, p.y, SnakeAI.BLOCK_SIZE, SnakeAI.BLOCK_SIZE)
        inside = Rect(p.x + 2, p.y + 2, SnakeAI.BLOCK_SIZE - 4, SnakeAI.BLOCK_SIZE - 4)
        draw(outside, colorant"#00262a", fill=true)
        draw(inside, colorant"#00717f", fill=true)

    end

    # Drawing the food
    f = Rect(game.food.x, game.food.y, SnakeAI.BLOCK_SIZE, SnakeAI.BLOCK_SIZE)
    draw(f, colorant"#7F0071", fill=true)
end

function update(g::GameZero.Game)
    global agent, game

    train!(agent, game)
end


module Land

using DocStringExtensions
using LinearAlgebra, StaticArrays

using CLIMAParameters
using CLIMAParameters.Planet:
    ρ_cloud_liq, ρ_cloud_ice, cp_l, cp_i, T_0, LH_f0, T_freeze, grav

using ..VariableTemplates
using ..MPIStateArrays
using ..BalanceLaws
import ..BalanceLaws:
    BalanceLaw,
    vars_state,
    flux_first_order!,
    flux_second_order!,
    source!,
    boundary_conditions,
    boundary_state!,
    compute_gradient_argument!,
    compute_gradient_flux!,
    nodal_init_state_auxiliary!,
    init_state_prognostic!,
    nodal_update_auxiliary_state!
using ..DGMethods: LocalGeometry, DGModel
export LandModel

"""
    LandModel{PS, S, SRC, IS} <: BalanceLaw

A BalanceLaw for land modeling.
Users may over-ride prescribed default values for each field.

# Usage

    LandModel(
        param_set,
        soil,
        source
        init_state_prognostic
    )

# Fields
$(DocStringExtensions.FIELDS)
"""
struct LandModel{PS, S, SRC, IS} <: BalanceLaw
    "Parameter set"
    param_set::PS
    "Soil model"
    soil::S
    "Source Terms (Problem specific source terms)"
    source::SRC
    "Initial Condition (Function to assign initial values of state variables)"
    init_state_prognostic::IS
end

"""
    LandModel(
        param_set::AbstractParameterSet,
        soil::BalanceLaw;
        source::SRC = (),
        init_state_prognostic::IS = nothing
    ) where {SRC, IS}

Constructor for the LandModel structure.
"""
function LandModel(
    param_set::AbstractParameterSet,
    soil::BalanceLaw;
    source::SRC = (),
    init_state_prognostic::IS = nothing,
) where {SRC, IS}
    @assert init_state_prognostic ≠ nothing
    land = (param_set, soil, source, init_state_prognostic)
    return LandModel{typeof.(land)...}(land...)
end

function vars_state(land::LandModel, st::Prognostic, FT)
    @vars begin
        soil::vars_state(land.soil, st, FT)
    end
end

function vars_state(land::LandModel, st::Auxiliary, FT)
    @vars begin
        z::FT
        soil::vars_state(land.soil, st, FT)
    end
end

function vars_state(land::LandModel, st::Gradient, FT)
    @vars begin
        soil::vars_state(land.soil, st, FT)
    end
end

function vars_state(land::LandModel, st::GradientFlux, FT)
    @vars begin
        soil::vars_state(land.soil, st, FT)
    end
end

function nodal_init_state_auxiliary!(
    land::LandModel,
    aux::Vars,
    tmp::Vars,
    geom::LocalGeometry,
)
    aux.z = geom.coord[3]
    land_init_aux!(land, land.soil, aux, geom)
end

function flux_first_order!(
    land::LandModel,
    flux::Grad,
    state::Vars,
    aux::Vars,
    t::Real,
    directions,
) end

function compute_gradient_argument!(
    land::LandModel,
    transform::Vars,
    state::Vars,
    aux::Vars,
    t::Real,
)

    compute_gradient_argument!(land, land.soil, transform, state, aux, t)
end

function compute_gradient_flux!(
    land::LandModel,
    diffusive::Vars,
    ∇transform::Grad,
    state::Vars,
    aux::Vars,
    t::Real,
)

    compute_gradient_flux!(
        land,
        land.soil,
        diffusive,
        ∇transform,
        state,
        aux,
        t,
    )

end

function flux_second_order!(
    land::LandModel,
    flux::Grad,
    state::Vars,
    diffusive::Vars,
    hyperdiffusive::Vars,
    aux::Vars,
    t::Real,
)
    flux_second_order!(
        land,
        land.soil,
        flux,
        state,
        diffusive,
        hyperdiffusive,
        aux,
        t,
    )

end

function nodal_update_auxiliary_state!(
    land::LandModel,
    state::Vars,
    aux::Vars,
    t::Real,
)
    land_nodal_update_auxiliary_state!(land, land.soil, state, aux, t)
end

function source!(
    land::LandModel,
    source::Vars,
    state::Vars,
    diffusive::Vars,
    aux::Vars,
    t::Real,
    direction,
)
    land_source!(land.source, land, source, state, diffusive, aux, t, direction)
end

function init_state_prognostic!(
    land::LandModel,
    state::Vars,
    aux::Vars,
    coords,
    t,
    args...,
)
    land.init_state_prognostic(land, state, aux, coords, t, args...)
end

include("land_bc.jl")
include("SoilWaterParameterizations.jl")
using .SoilWaterParameterizations
include("SoilHeatParameterizations.jl")
using .SoilHeatParameterizations
include("Runoff.jl")
using .Runoff
include("soil_model.jl")
include("soil_boundary_types.jl")
include("soil_water.jl")
include("soil_heat.jl")
include("soil_bc.jl")
include("source.jl")
end # Module