module lcmtesttypes

export lcm_test_type_1, lcm_test_type_2, lcm_test_type_3, polynomial_t, polynomial_matrix_t
export hard_coded_example

using LCMCore, StaticArrays
using Random: randstring

mutable struct lcm_test_type_1 <: LCMType
    a::Int16
    blength::Int32
    b::Vector{Int64}
    c::SVector{3, Int32}
end

@lcmtypesetup(lcm_test_type_1,
    b => (blength, )
)

function Base.:(==)(x::lcm_test_type_1, y::lcm_test_type_1)
    x.a == y.a || return false
    x.blength == y.blength || return false
    x.b == y.b || return false
    x.c == y.c || return false
    true
end

function Base.rand(::Type{lcm_test_type_1})
    blength = rand(Int32(0) : Int32(10))
    lcm_test_type_1(rand(Int16), blength, rand(Int64, blength), rand(SVector{3, Int32}))
end

function hard_coded_example(::Type{lcm_test_type_1})
    ret = lcm_test_type_1()
    ret.a = 1
    ret.blength = 4
    ret.b = [4, 5, -6, 7]
    ret.c = [10, -1, 5]
    ret
end

mutable struct lcm_test_type_2 <: LCMType
    dlength::Int32
    f_inner_length::Int32
    a::Bool
    b::UInt8
    c::lcm_test_type_1
    d::Vector{lcm_test_type_1}
    e::SVector{3, lcm_test_type_1}
    f::Matrix{Int64}
end

@lcmtypesetup(lcm_test_type_2,
    d => (dlength, ),
    f => (3, f_inner_length)
)

function Base.:(==)(x::lcm_test_type_2, y::lcm_test_type_2)
    x.dlength == y.dlength || return false
    x.f_inner_length == y.f_inner_length || return false
    x.a == y.a || return false
    x.b == y.b || return false
    x.c == y.c || return false
    x.d == y.d || return false
    x.e == y.e || return false
    x.f == y.f || return false
    true
end

function Base.rand(::Type{lcm_test_type_2})
    dlength = rand(Int32(0) : Int32(10))
    f_inner_length = rand(Int32(0) : Int32(10))
    a = rand(Bool)
    b = rand(UInt8)
    c = rand(lcm_test_type_1)
    d = [rand(lcm_test_type_1) for i = 1 : dlength]
    e = SVector{3}([rand(lcm_test_type_1) for i = 1 : 3])
    f = rand(Int64, 3, f_inner_length)
    lcm_test_type_2(dlength, f_inner_length, a, b, c, d, e, f)
end

function hard_coded_example(::Type{lcm_test_type_2})
    ret = lcm_test_type_2()
    ret.dlength = 2
    ret.f_inner_length = 2
    ret.a = false
    ret.b = 6
    ret.c = hard_coded_example(lcm_test_type_1)
    ret.d = [hard_coded_example(lcm_test_type_1), hard_coded_example(lcm_test_type_1)]
    ret.d[1].a = 2
    ret.e = [hard_coded_example(lcm_test_type_1), hard_coded_example(lcm_test_type_1), hard_coded_example(lcm_test_type_1)]
    ret.e[3].c = [5, 3, 8]
    ret.f = [1 2; 3 4; 5 7]
    ret
end

mutable struct lcm_test_type_3 <: LCMType
    a::String
    blength::Int32
    b::Vector{String}
    c::SVector{2, String}
    d::Float64
end

@lcmtypesetup(lcm_test_type_3,
    b => (blength, )
)

function Base.:(==)(x::lcm_test_type_3, y::lcm_test_type_3)
    x.a == y.a || return false
    x.blength == y.blength || return false
    x.b == y.b || return false
    x.c == y.c || return false
    x.d == y.d || return false
    true
end

function Base.rand(::Type{lcm_test_type_3})
    a = randstring()
    blength = rand(Int32(0) : Int32(10))
    b = [randstring() for i = 1 : blength]
    c = SVector{2}([randstring() for i = 1 : 2])
    d = rand()
    lcm_test_type_3(a, blength, b, c, d)
end

function hard_coded_example(::Type{lcm_test_type_3})
    ret = lcm_test_type_3()
    ret.a = "abcd"
    ret.blength = 3
    ret.b = ["x*4f", "4^G32", "4"]
    ret.c = ["xyz", "wxy"]
    ret.d = 2.5
    ret
end

mutable struct polynomial_t <: LCMType
    timestamp::Int64
    num_coefficients::Int32
    coefficients::Vector{Float64}
end

@lcmtypesetup(polynomial_t,
    coefficients => (num_coefficients, )
)

function Base.:(==)(x::polynomial_t, y::polynomial_t)
    x.timestamp == y.timestamp || return false
    x.num_coefficients == y.num_coefficients || return false
    x.coefficients == y.coefficients || return false
    true
end

function Base.rand(::Type{polynomial_t})
    timestamp = rand(Int64)
    num_coefficients = rand(Int32(0) : Int32(10))
    coefficients = rand(num_coefficients)
    polynomial_t(timestamp, num_coefficients, coefficients)
end

function hard_coded_example(::Type{polynomial_t})
    polynomial_t(1234, 4, [5.0, 0.0, 1.0, 3.0])
end

mutable struct polynomial_matrix_t <: LCMType
    timestamp::Int64
    rows::Int32
    cols::Int32
    polynomials::Matrix{polynomial_t}
end

@lcmtypesetup(polynomial_matrix_t,
    polynomials => (rows, cols)
)

function Base.:(==)(x::polynomial_matrix_t, y::polynomial_matrix_t)
    x.timestamp == y.timestamp || return false
    x.rows == y.rows || return false
    x.cols == y.cols || return false
    x.polynomials == y.polynomials || return false
    true
end

function Base.rand(::Type{polynomial_matrix_t})
    timestamp = rand(Int64)
    rows = rand(Int32(0) : Int32(10))
    cols = rand(Int32(0) : Int32(10))
    polynomials = [rand(polynomial_t) for row = 1 : rows, col = 1 : cols]
    polynomial_matrix_t(timestamp, rows, cols, polynomials)
end

function hard_coded_example(::Type{polynomial_matrix_t})
    polynomials = Matrix{polynomial_t}(undef, 2, 1)
    polynomials[1] = polynomial_t(1234, 4, [5.0, 0.0, 1.0, 3.0])
    polynomials[2] = polynomial_t(1234, 4, [10.0, 0.0, 2.0, 6.0])
    polynomial_matrix_t(1234, 2, 1, polynomials)
end

end # module


using Distributed
addprocs(2)

@everywhere using Pkg
@everywhere Pkg.activate("./julia-multiprocessing/TestModule1")

@everywhere using TestModule1

#%%
print_nprocs()

func1()

func2()

test_interp(3.1415)
sin(3.1415)

TestModule1.create_interp()


##########################################################################
# Copyright 2017 Samuel Ridler.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
##########################################################################

# tools to help manipulate graphs

using JEMSS
import LightGraphs
using SparseArrays

travelModeString(i::Int) = "mode_$i"

function checkNodeIndices(nodes::Vector{Node})
	result = all(i -> i == nodes[i].index, 1:length(nodes)) # true if ok
	result || @warn("Node indices are not 1:n")
	return result
end

function checkArcIndices(arcs::Vector{Arc})
	result = all(i -> i == arcs[i].index, 1:length(arcs)) # true if ok
	result || @warn("Arc indices are not 1:n")
	return result
end

# check arc.fromNodeIndex and arc.toNodeIndex values are in expected range
function checkArcNodeIndices(numNodes::Int, arcs::Vector{Arc})
	result = all(i -> (1 <= arcs[i].fromNodeIndex <= numNodes) && (1 <= arcs[i].toNodeIndex <= numNodes), 1:length(arcs)) # true if ok
	result || @warn("Arc from/to node indices are not all within 1:numNodes")
	return result
end
checkArcNodeIndices(nodes::Vector{Node}, arcs::Vector{Arc}) = checkArcNodeIndices(length(nodes), arcs)

# get number of travel modes for an arc
function getNumTravelModes(arc::Arc)
	i = 1
	while haskey(arc.fields, travelModeString(i))
		i += 1
	end
	numModes = i-1
	@assert(numModes >= 1, "Arcs should contain at least one travel mode field, e.g. '$(travelModeString(1))'")
	return numModes
end
getNumTravelModes(arcs::Vector{Arc}) = getNumTravelModes(arcs[1]) # assume all arcs have same fields

# get travel times for each travel mode of an arc
function getArcTravelTimes(arc::Arc)
	numModes = getNumTravelModes(arcs)
	travelTimes = Vector{Float}(undef, numModes)
	for i = 1:numModes
		travelTimes[i] = arc.fields[travelModeString(i)]
	end
	return travelTimes
end

# return vector of travel times for arcs,
# first dimension is for travel modes, second is for arcs
function getArcsTravelTimes(arcs::Vector{Arc})
	numArcs = length(arcs)
	numModes = getNumTravelModes(arcs)
	travelTimes = Array{Float,2}(undef, numModes, numArcs)
	for i = 1:numModes
		modeString = travelModeString(i)
		for j = 1:numArcs
			travelTimes[i,j] = arcs[j].fields[modeString]
		end
	end
	return travelTimes
end

function nodesAddField!(nodes::Vector{Node}, field::String; default::Any = nothing, overwrite::Bool = false)
	if !overwrite && haskey(nodes[1].fields, field) return end
	for node in nodes
		node.fields[field] = default
	end
end

function arcsAddField!(arcs::Vector{Arc}, field::String; default::Any = nothing, overwrite::Bool = false)
	if !overwrite && haskey(arcs[1].fields, field) return end
	for arc in arcs
		arc.fields[field] = default
	end
end

nodesDeleteField!(nodes::Vector{Node}, field::String) = for node in nodes delete!(node.fields, field) end
arcsDeleteField!(arcs::Vector{Arc}, field::String) = for arc in arcs delete!(arc.fields, field) end

function JEMSS.writeArcsFile(arcsFilename::String, arcs::Vector{Arc}, arcForm::String)
	travelTimes = getArcsTravelTimes(arcs)
	writeArcsFile(arcsOutputFilename, arcs, travelTimes, arcForm)
end

function createLightGraph(numNodes::Int, arcs::Vector{Arc})
	lightGraph = LightGraphs.DiGraph(numNodes)
	for arc in arcs
		LightGraphs.add_edge!(lightGraph, arc.fromNodeIndex, arc.toNodeIndex)
	end
	return lightGraph
end
createLightGraph(nodes::Vector{Node}, arcs::Vector{Arc}) = createLightGraph(length(nodes), arcs)

# tag the elements (nodes and arcs) that contribute to the largest strongly connected component
# tagging is done by adding a field with name given by kwarg `lsccHeader`
function graphTagLargestComponentElts!(nodes::Vector{Node}, arcs::Vector{Arc};
	lsccHeader::String = "in_largest_component")

	# shorthand
	numNodes = length(nodes)
	numArcs = length(arcs)

	lightGraph = createLightGraph(nodes, arcs)

	# find strongly connected components; note that Graphs.strongly_connected_components() did not work correctly (the output was not what I expected), so LightGraphs.strongly_connected_components() is needed instead
	components = LightGraphs.strongly_connected_components(lightGraph)
	largestComponent = components[argmax([length(c) for c in components])] # node indices of largest strongly connected component

	# tag nodes and arcs in largest strongly connected component
	tagNode = fill(false, numNodes) # tagNode[i] = true if nodes[i] in largest component
	tagArc = fill(false, numArcs) # tagArc[i] = true if arcs[i] in largest component
	for i in largestComponent
		tagNode[i] = true
	end
	for (i,arc) in enumerate(arcs)
		if tagNode[arc.fromNodeIndex] && tagNode[arc.toNodeIndex]
			tagArc[i] = true
		end
	end

	# tag by adding fields to nodes and arcs
	for i = 1:numNodes
		nodes[i].fields[lsccHeader] = tagNode[i]
	end
	for i = 1:numArcs
		arcs[i].fields[lsccHeader] = tagArc[i]
	end

	return
end

# keep nodes and arcs where filters return true
# indices will be renumbered
# arcs with either (to/from) node removed are not automatically removed
function graphRemoveElts!(nodes::Vector{Node}, arcs::Vector{Arc};
	nodeFilter::Function = (x->true), arcFilter::Function = (x->true))

	# before filtering, create mapping from old indices to new
	keepNodeIndices = findall(nodeFilter, nodes)
	nodeNewIndex = fill(nullIndex, length(nodes))
	nodeNewIndex[keepNodeIndices] = 1:length(keepNodeIndices) # nodeNewIndex[i] gives new node index for old nodes[i]

	# filter nodes, renumber nodes from 1 to n
	filter!(nodeFilter, nodes)
	for (i,node) in enumerate(nodes)
		node.index = i
	end

	# filter arcs, renumber nodes from 1 to n
	filter!(arcFilter, arcs)
	for (i,arc) in enumerate(arcs)
		arc.index = i
	end

	# connect arcs to nodes
	# arc.fromNodeIndex or arc.toNodeIndex will become nullIndex if node does not exist
	for arc in arcs
		arc.fromNodeIndex = nodeNewIndex[arc.fromNodeIndex]
		arc.toNodeIndex = nodeNewIndex[arc.toNodeIndex]
	end

	# check that all needed nodes exist for filtered arcs
	checkArcNodeIndices(nodes, arcs)

	return
end

# remove elements outside of largest strongly connected component
function graphKeepLargestComponent!(nodes::Vector{Node}, arcs::Vector{Arc};
	lsccHeader::String = "in_largest_component")
	graphTagLargestComponentElts!(nodes, arcs; lsccHeader = lsccHeader)
	nodeFilter(node::Node) = node.fields[lsccHeader]
	arcFilter(arc::Arc) = arc.fields[lsccHeader]
	graphRemoveElts!(nodes, arcs, nodeFilter = nodeFilter, arcFilter = arcFilter)
	nodesDeleteField!(nodes, lsccHeader)
	arcsDeleteField!(arcs, lsccHeader)
	return
end

# remove arcs from graph that are connected to/from nodes outside of range
function graphRemoveDisconnectedArcs!(nodes::Vector{Node}, arcs::Vector{Arc})
	numNodes = length(nodes)
	arcFilter(arc::Arc) = (1 <= arc.fromNodeIndex <= numNodes) && (1 <= arc.toNodeIndex <= numNodes)
	graphRemoveElts!(nodes, arcs, arcFilter = arcFilter)
end

# return dict with tuple of node indices (from, to) as keys, and vector of arc indices as values
function graphNodePairArcIndices(arcs::Vector{Arc})
	nodePairArcIndices = Dict{Tuple{Int,Int}, Vector{Int}}() # nodePairArcIndices[i,j] gives indices of arcs going from node i to node j
	for arc in arcs
		i = arc.fromNodeIndex
		j = arc.toNodeIndex
		if !haskey(nodePairArcIndices, (i,j))
			nodePairArcIndices[(i,j)] = []
		end
		push!(nodePairArcIndices[(i,j)], arc.index)
	end
	return nodePairArcIndices
end

# find duplicate arcs (same 'to' and 'from' nodes),
# return vector of vector of indices of duplicate arcs
function graphFindDuplicateArcs(arcs::Vector{Arc})
	nodePairArcIndices = graphNodePairArcIndices(arcs)
	duplicateArcsSets = []
	for (nodePair, arcIndices) in nodePairArcIndices
		if length(arcIndices) > 1
			push!(duplicateArcsSets, arcIndices)
		end
	end
	return duplicateArcsSets
end

graphContainsDuplicateArcs(arcs::Vector{Arc}) = graphFindDuplicateArcs(arcs) != []
function checkGraphContainsNoDuplicateArcs(arcs::Vector{Arc})
	result = !graphContainsDuplicateArcs(arcs) # true if ok
	result || @warn("There are duplicate arcs.")
	return result
end

# merge arc2 into arc1, keeping minimum travel time (from both arcs) for each travel mode in arc1
# arc2 will need to be removed in separate step
function mergeDuplicateArcs!(arc1::Arc, arc2::Arc)
	@assert(arc1.fromNodeIndex == arc2.fromNodeIndex)
	@assert(arc1.toNodeIndex == arc2.toNodeIndex)
	numModes = getNumTravelModes(arc1)
	for i = 1:numModes
		modeString = travelModeString(i)
		arc1.fields[modeString] = min(arc1.fields[modeString], arc2.fields[modeString])
	end
	# assume that arc.distance is same for both arcs
end
mergeDuplicateArcs!(arcs::Vector{Arc}, i::Int, j::Int) = mergeDuplicateArcs!(arcs[i], arcs[j])

# merge any duplicate arcs into one
# will take the shortest travel time from any merged arcs, for each travel mode
function graphMergeDuplicateArcs!(nodes::Vector{Node}, arcs::Vector{Arc};
	mergeArcsFunction::Function = mergeDuplicateArcs!, mergeHeader::String = "merge_result")

	arcsAddField!(arcs, mergeHeader, default = "unmerged")

	# find which arcs are duplicates
	duplicateArcsSets = graphFindDuplicateArcs(arcs) # duplicateArcsSets[i] has a vector of indices of duplicate arcs
	for duplicateArcs in duplicateArcsSets
		i = duplicateArcs[1]
		arcs[i].fields[mergeHeader] = "merged"
		for j in duplicateArcs[2:end]
			mergeArcsFunction(arcs, i, j)
			arcs[j].fields[mergeHeader] = "merged_in"
		end
	end

	# remove duplicate arcs
	arcFilter(arc::Arc) = arc.fields[mergeHeader] != "merged_in"
	graphRemoveElts!(nodes, arcs, arcFilter = arcFilter)
end

# Divide an arc into even sections a given number of times, 0 divides makes no change.
# Only some nodes and arcs fields are updated (node offRoadAccess, arc travel times and osm weight), the rest are unchanged but may need changing.
function graphDivideArc!(nodes::Vector{Node}, arcs::Vector{Arc}; arcIndex::Int = 0, numDivides::Int = 0)
	if numDivides <= 0 return end
	@assert(1 <= arcIndex <= length(arcs))

	# shorthand
	numNodes = length(nodes)
	numArcs = length(arcs)
	arc = arcs[arcIndex]
	fromNode = nodes[arc.fromNodeIndex]
	toNode = nodes[arc.toNodeIndex]

	# keep track of which nodes have been added, and which arcs have been divided
	addedHeader = "added"
	divArcHeader = "divided_arc_index"
	nodesAddField!(nodes, addedHeader; default = false)
	arcsAddField!(arcs, divArcHeader; default = nullIndex)

	# change some fields of arc
	for i = 1:getNumTravelModes(arc)
		arc.fields[travelModeString(i)] /= numDivides + 1
	end
	if haskey(arc.fields, "osm_weight")
		arc.fields["osm_weight"] /= numDivides + 1
	end
	arc.distance /= numDivides + 1
	arc.fields[divArcHeader] = arc.index

	tempNode = deepcopy(fromNode) # could copy any node
	tempNode.fields[addedHeader] = true
	tempArc = deepcopy(arc)
	# some fields of these new nodes and arcs will have incorrect/irrelevant information and should be ignored

	for i = 1:numDivides
		# add node
		newNode = deepcopy(tempNode)
		newNode.index = numNodes + i
		newNode.location = linearInterpLocation(fromNode.location, toNode.location, 0.0, 1.0, i/(numDivides+1))
		newNode.offRoadAccess = fromNode.offRoadAccess && toNode.offRoadAccess
		push!(nodes, newNode)

		# add arc
		newArc = deepcopy(tempArc)
		newArc.index = numArcs + i
		newArc.fromNodeIndex = newNode.index
		newArc.toNodeIndex = newNode.index + 1
		push!(arcs, newArc)
	end
	arc.toNodeIndex = numNodes + 1
	arcs[end].toNodeIndex = toNode.index
end

# Divide arcs into multiple arcs (with added nodes) so that the the maximum arc travel time <= maxArcTravelTime.
function graphDivideArcs!(nodes::Vector{Node}, arcs::Vector{Arc}; maxArcTravelTime::Float = Inf)
	if maxArcTravelTime == Inf return end
	@assert(maxArcTravelTime > 0)

	nodePairArcIndex = Dict{Tuple{Int,Int},Int}()
	for arc in arcs
		@assert(!haskey(nodePairArcIndex, (arc.fromNodeIndex, arc.toNodeIndex)), arc) # duplicate arcs not allowed
		nodePairArcIndex[arc.fromNodeIndex, arc.toNodeIndex] = arc.index
	end

	travelTimes = getArcsTravelTimes(arcs) # values before dividing arcs, actual values will change (but not in this var)
	arcsDivided = fill(false, length(arcs))
	for (node1, node2) in keys(nodePairArcIndex)
		if arcsDivided[nodePairArcIndex[node1, node2]] continue end # arc has already been divided
		numNodes = length(nodes) # keep track of number of nodes before current divide
		if !haskey(nodePairArcIndex, (node2, node1)) # no arc in opposite direction
			i = nodePairArcIndex[node1, node2]
			numDivides = ceil(Int, maximum(travelTimes[:,i]) / maxArcTravelTime) - 1
			if numDivides < 1 continue end
			graphDivideArc!(nodes, arcs; arcIndex = i, numDivides = numDivides)
			arcsDivided[i] = true
		else
			# there are two arcs in opposite directions, need to split both simultaneously
			arc1 = nodePairArcIndex[node1, node2]
			arc2 = nodePairArcIndex[node2, node1]
			numDivides = ceil(Int, maximum(travelTimes[:,[arc1,arc2]]) / maxArcTravelTime) - 1 # look at maximum travel time of both arcs
			if numDivides < 1 continue end
			graphDivideArc!(nodes, arcs; arcIndex = arc1, numDivides = numDivides)
			graphDivideArc!(nodes, arcs; arcIndex = arc2, numDivides = numDivides)
			arcsDivided[arc1] = true
			arcsDivided[arc2] = true

			# have created extra nodes when dividing arc2; need to remove these and instead use nodes that were created when dividing arc1
			for i = 1:numDivides
				pop!(nodes)
			end
			for i = 1:numDivides
				arc = arcs[end-i+1]
				arc.fromNodeIndex = numNodes + i
				i == 1 || (arc.toNodeIndex = numNodes + i - 1)
			end
			arcs[arc2].toNodeIndex = numNodes + numDivides
		end
	end

	# check max arc travel time
	travelTimes = getArcsTravelTimes(arcs)
	@assert(maximum(travelTimes) <= maxArcTravelTime)

	checkNodeIndices(nodes)
	checkArcIndices(arcs)
	checkArcNodeIndices(nodes, arcs)
	checkGraphContainsNoDuplicateArcs(arcs)
end

# Tag all nodes and arcs that are used in any shortest path:
# from an origin node to a chosen node,
# from a chosen node to a destination node, or
# from an origin node to a destination node
# This assumes that any duplicate arcs have been removed.
# This version requires LightGraphs.dijkstra_shortest_paths function to allow distmx::Dict, for sake of reducing run time
function graphTagSpElts!(nodes::Vector{Node}, arcs::Vector{Arc};
	spHeader::String = "in_a_sp", chosenNodes::Vector{Int} = Int[], originNodes::Vector{Int} = Int[], destNodes::Vector{Int} = Int[], dijkstraSpDistmxAllowDict::Bool = false)

	@assert(!graphContainsDuplicateArcs(arcs), "Cannot handle duplicate arcs")
	if dijkstraSpDistmxAllowDict @info("Setting `dijkstraSpDistmxAllowDict` = true requires LightGraphs.dijkstra_shortest_paths function to be changed, to allow arg `distmx` to be type Dict") end

	# shorthand
	numArcs = length(arcs)
	numNodes = length(nodes)

	# keep track of all tagged nodes and arcs
	nodeTagged = fill(false, numNodes)
	arcTagged = fill(false, numArcs)

	# to be used, and reset, for each SP tree:
	tagNode = Vector{Bool}(undef, numNodes)
	tagArc = Vector{Bool}(undef, numArcs)

	nodePairArcIndex = Dict{Tuple{Int,Int},Int}()
	revNodePairArcIndex = Dict{Tuple{Int,Int},Int}()
	for arc in arcs
		nodePairArcIndex[arc.fromNodeIndex, arc.toNodeIndex] = arc.index
		revNodePairArcIndex[arc.toNodeIndex, arc.fromNodeIndex] = arc.index
	end

	# # using sparse matrices takes much longer to populate than using dict
	# nodePairArcIndex = spzeros(Int, numNodes, numNodes)
	# for arc in arcs
		# nodePairArcIndex[arc.fromNodeIndex, arc.toNodeIndex] = arc.index
	# end
	# revNodePairArcIndex = transpose(nodePairArcIndex)

	lightGraph = createLightGraph(nodes, arcs)
	revLightGraph = LightGraphs.reverse(lightGraph)

	travelTimes = getArcsTravelTimes(arcs)
	numModes = size(travelTimes,1)

	# create distance matrix, to be reused
	println("Setting arc time matrix / dict")
	if dijkstraSpDistmxAllowDict
		# using this with LightGraphs requires dijkstra_shortest_paths function distmx arg type to be changed
		arcTimes = Dict{Tuple{Int,Int},Float}()
		revArcTimes = Dict{Tuple{Int,Int},Float}()
		for arc in arcs
			if mod(arc.index, 1000) == 0 print("\r\tarc: ", arc.index) end
			arcTimes[arc.fromNodeIndex, arc.toNodeIndex] = nullTime # to be set later
			revArcTimes[arc.toNodeIndex, arc.fromNodeIndex] = nullTime # to be set later
		end
		println()
	else
		# use sparse matrix
		# this is much slower to populate than a dict, but works with unedited version of LightGraphs.dijkstra_shortest_paths
		arcTimes = spzeros(Float, numNodes, numNodes)
		revArcTimes = spzeros(Float, numNodes, numNodes)
		for arc in arcs
			if mod(arc.index, 1000) == 0 print("\r\tarc: ", arc.index) end
			arcTimes[arc.fromNodeIndex, arc.toNodeIndex] = nullTime # to be set later
			revArcTimes[arc.toNodeIndex, arc.fromNodeIndex] = nullTime # to be set later
		end
		println()
	end

	# Tag all nodes and arcs (by setting vars tagNode and tagArc) that are in a shortest path from the originNode to any node in destNodeSets
	# To work with destination node (instead of origin), provide a reversed lightGraph and transposed arcTimes and nodePairArcIndex
	function graphTagSpTreeElts!(lightGraph::LightGraphs.DiGraph{Int}, originNode::Int, destNodeSets::Vector{Vector{Int}}, arcTimes::Union{SparseMatrixCSC{Float,Int},Dict{Tuple{Int,Int},Float}}, nodePairArcIndex::Dict{Tuple{Int,Int},Int})
		# global tagNode, tagArc
		i = originNode # shorthand
		spData = LightGraphs.dijkstra_shortest_paths(lightGraph, i, arcTimes)
		spPreds = spData.parents # spPreds[j] gives index of node before j on shortest path from i to j
		tagNode[:] .= false
		tagNode[i] = true
		tagArc[:] .= false
		for nodeIndices in destNodeSets
			for j in nodeIndices
				while !tagNode[j]
					tagNode[j] = true
					k = j
					j = spPreds[j]
					tagArc[nodePairArcIndex[j,k]] = true
				end
			end
		end
		nodeTagged .|= tagNode
		arcTagged .|= tagArc
		return
	end

	println("Calculating shortest paths")
	for modeIndex in 1:numModes

		println("\tmode: $modeIndex")

		# set distance matrix values
		for arc in arcs
			arcTimes[arc.fromNodeIndex, arc.toNodeIndex] = travelTimes[modeIndex, arc.index]
			revArcTimes[arc.toNodeIndex, arc.fromNodeIndex] = travelTimes[modeIndex, arc.index]
		end

		for (i,originNode) in enumerate(originNodes)
			print("\r\torigin node: $i (of $(length(originNodes)))")
			# tag nodes in a shortest path from node originNode to any chosen/dest nodes
			graphTagSpTreeElts!(lightGraph, originNode, [chosenNodes, destNodes], arcTimes, nodePairArcIndex)
		end
		println()

		for (i,destNode) in enumerate(destNodes)
			print("\r\tdest node: $i (of $(length(destNodes)))")
			# tag nodes in a shortest path from any chosen/origin node to node destNode
			graphTagSpTreeElts!(revLightGraph, destNode, [chosenNodes, originNodes], revArcTimes, revNodePairArcIndex)
		end
		println()

	end

	# use vars `nodeTagged` and `arcTagged` to set value of new field for nodes and arcs
	for i = 1:numNodes
		nodes[i].fields[spHeader] = nodeTagged[i]
	end
	for i = 1:numArcs
		arcs[i].fields[spHeader] = arcTagged[i]
	end

	return
end


# File: soem.jl
# Project: example
# Created Date: 30/12/2020
# Author: Shun Suzuki
# -----
# Last Modified: 17/12/2021
# Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
# -----
# Copyright (c) 2020 Hapis Lab. All rights reserved.
#

using Printf

using AUTD3
using StaticArrays
using StaticArrays: size

function simple(autd::AUTD)
    set_silent_mode(autd, true)

    g = focal_point_gain(SVector(90.0, 80.0, 150.0))
    freq::Int32 = 150
    m = sine_modulation(freq)

    send(autd, m, g)

    println("press enter to exit...")
    readline()

    dispose(g)
    dispose(m)
    stop(autd)
end

function bessel(autd::AUTD)
    set_silent_mode(autd, true)

    g = bessel_beam_gain(SVector(90.0, 80.0, 150.0), SVector(0.0, 0.0, 1.0), 13.0 / 180.0 * pi)
    freq::Int32 = 150
    m = sine_modulation(freq)

    send(autd, m, g)

    println("press enter to exit...")
    readline()

    dispose(g)
    dispose(m)
    stop(autd)
end

function holo(autd::AUTD)
    set_silent_mode(autd, true)

    foci = [
        SVector(120.0, 80.0, 150.0),
        SVector(60.0, 80.0, 150.0)
    ]
    amps = [1.0, 1.0]
    g = holo_gain_sdp(foci, amps)

    freq::Int32 = 150
    m = sine_modulation(freq)

    send(autd, m, g)

    println("press enter to exit...")
    readline()

    dispose(g)
    dispose(m)
    stop(autd)
end

function stm_test(autd::AUTD)
    set_silent_mode(autd, false)

    m = static_modulation()
    send(autd, m, Nothing)

    center = SVector(90.0, 80.0, 150.0)

    radius = 30.0
    size = 200
    stm_cnt = stm(autd)
    for i = 1:size
        theta::Float64 = 2pi * i / size
        r = center + radius * SVector(cos(theta), sin(theta), 0)
        f = focal_point_gain(r)
        add_stm_gain(stm_cnt, f)
        dispose(f)
    end

    freq::Float64 = 1.0
    start_stm(stm_cnt, freq)

    println("press enter to exit...")
    readline()

    stop_stm(stm_cnt)
    finish_stm(stm_cnt)

    dispose(m)
    stop(autd)
end

function seq(autd::AUTD)
    set_silent_mode(autd, false)

    m = static_modulation()

    seq = sequence()

    center = SVector(90.0, 80.0, 150.0)
    radius = 30.0
    size::UInt64 = 200
    for i = 1:size
        theta = 2.0 * pi * i / size
        p = radius * SVector(cos(theta), sin(theta), 0.0)
        add_point(seq, center + p)
    end
    freq::Float64 = 200.0
    set_freq(seq, freq)

    send(autd, m, seq)

    println("press enter to exit...")
    readline()

    dispose(seq)
    dispose(m)
    stop(autd)
end

function seq_gain(autd::AUTD)
    set_silent_mode(autd, false)

    m = static_modulation()

    center = SVector(90.0, 80.0, 150.0)
    radius = 30.0
    size::UInt64 = 200
    seq = gain_sequence(autd, duty_phase_full)

    for i = 1:size
        theta = 2.0 * pi * i / size
        p = radius * SVector(cos(theta), sin(theta), 0.0)
        g = focal_point_gain(center + p)
        add_gain(seq, g)
    end

    freq::Float64 = 1
    set_freq(seq, freq)

    send(autd, m, seq)

    println("press enter to exit...")
    readline()

    dispose(seq)
    dispose(m)
    stop(autd)
end

function run(autd::AUTD)
    samples = [
        (simple, "Single Focal Point Sample"),
        (bessel, "Bessel beam Sample"),
        (holo, "Multiple Focal Points Sample"),
        (stm_test, "Spatio-Temporal Modulation Sample"),
        (seq, "PointSequence (Hardware STM) Sample"),
        (seq_gain, "GainSequence (Hardware STM with arbitrary Gain) Sample")
    ]

    firm_info_list = firmware_info_list(autd)
    for (i, firm_info) in enumerate(firm_info_list)
        @printf("AUTD[%d]: CPU: %s, FPGA: %s\n", i, firm_info[1], firm_info[2])
    end

    clear(autd)

    while true
        for (i, (_, name)) in enumerate(samples)
            @printf("[%d]: %s\n", i, name)
        end
        println("[Other]: finish")
        print("Choose number: ")

        idx = tryparse(Int64, readline())
        if idx === nothing || idx > length(samples) || idx < 1
            break
        end

        (fn, _) = samples[idx]
        fn(autd)
    end

    clear(autd)
    dispose(autd)
end

function get_adapter()
    adapters = enumerate_adapters()
    for (i, adapter) in enumerate(adapters)
        @printf("[%d]: %s, %s\n", i, adapter[2], adapter[1])
    end

    print("Input number: ")
    idx = tryparse(Int64, readline())
    if idx === nothing || idx > length(adapters) || idx < 1
        println("choose correct number!")
        return ""
    end

    adapters[idx][2]
end

function main()
    autd = AUTD()

    add_device(autd, SVector(0.0, 0.0, 0.0), SVector(0.0, 0.0, 0.0))

    adapter = get_adapter()
    link = soem_link(adapter, num_devices(autd))
    if !open_autd(autd, link)
        println(last_error())
        return
    end

    run(autd)
end

main()


@testset "flag" begin
    @testset "basic" begin
        @system SFlag(Controller) begin
            a => true ~ flag
            b => false ~ flag
        end
        s = instance(SFlag)
        @test s.a' == true && s.b' == false
    end

    @testset "flag vs track" begin
        @system SFlagVsTrack(Controller) begin
            a => 1 ~ accumulate
            f1(a) => a >= 1 ~ flag
            f2(a) => a >= 1 ~ track::Bool
            x1(f1) => (f1 ? 1 : 0) ~ track
            x2(f2) => (f2 ? 1 : 0) ~ track
        end
        s = instance(SFlagVsTrack)
        @test s.a' == 0
        @test s.f1' == false && s.f2' == false
        @test s.x1' == 0 && s.x2' == 0
        update!(s)
        @test s.a' == 1
        @test s.f1' == true && s.f2' == true
        @test s.x1' == 1 && s.x2' == 1
        update!(s)
        @test s.a' == 2
        @test s.f1' == true && s.f2' == true
        @test s.x1' == 1 && s.x2' == 1
    end

    @testset "flag parameter" begin
        @system SFlagParameter(Controller) begin
            a => true ~ flag(parameter)
            b => false ~ flag(parameter)
        end
        s1 = instance(SFlagParameter)
        @test s1.a' == true && s1.b' == false
        c = :SFlagParameter => (a = false, b = true)
        s2 = instance(SFlagParameter; config=c)
        @test s2.a' == false && s2.b' == true
    end
end


using Nabla
using Nabla: unbox, pos, tape, oneslike, zeroslike
using BenchmarkTools
using ChainRulesCore
using Distributions
using ForwardDiff
using LinearAlgebra
using Random
using SpecialFunctions
using Statistics
using Test

# Helper function for comparing `Ref`s, since they don't compare equal under `==`
ref_equal(a::Ref{T}, b::Ref{T}) where {T} = a[] == b[]
ref_equal(a::Ref, b::Ref) = false

# for comparing against scalar rules
derivative_via_frule(f, x) = last(frule((NoTangent(), 1.0), f, x))
# Sensiblity checkes that his is defined right
@test derivative_via_frule(cos, 0) == 0
@test derivative_via_frule(sin, 0) == 1
@test derivative_via_frule(sin, 1.2) == derivative_via_frule(sin, 2π + 1.2)

# These are the core scalar sensitives Nabla expects to have defined
# we test against them both for sensitives/scalar.jl and in sensitivities/functional.jl

const UNARY_SCALAR_SENSITIVITIES = [
    # Base:
    +, -, abs, abs2, acos, acosd, acosh, acot, acotd, acoth, acsc, acscd, acsch, asec,
    asecd, asech, asin, asind, asinh, atand, atanh, cbrt, cos, cosd, cosh, cospi, cot,cotd,
    coth, csc, cscd, csch, deg2rad, exp, exp10, exp2, expm1, inv, log, log10, log2,
    rad2deg, sec, secd, sech, sin, sind, sinh, sinpi, sqrt, tan, tand, tanh, transpose,
    # SpecialFunctions.jl:
    airyai, airyaiprime, airybi, airybiprime, besselj0, besselj1, bessely0, bessely1,
    dawson, digamma, erf, erfc, erfcinv, erfcx, erfi, erfinv, gamma, invdigamma, trigamma,
]

const DIFF_IN_FIRST_ANND_SECOND_ARG_SENSITIVITIES = [ #Base
    # Base:
    *, +, -, /, \, ^, hypot, max, min,
    # SpecialFunctions.jl:
    beta,
]
const ONLY_DIFF_IN_SECOND_ARG_SENSITIVITIES = [
    # SpecialFunctions.jl:
    besseli, besselj, besselk, bessely, polygamma,
]
const BINARY_SCALAR_SENSITIVITIES = [
    DIFF_IN_FIRST_ANND_SECOND_ARG_SENSITIVITIES;
    ONLY_DIFF_IN_SECOND_ARG_SENSITIVITIES;
]

@testset "Nabla.jl" begin

@testset "Core" begin
    include("core.jl")
    include("code_transformation/util.jl")
    include("code_transformation/differentiable.jl")
    include("sensitivity.jl")
end

@testset "Sensitivities" begin
    include("finite_differencing.jl")

    include("sensitivities/ambiguity_resolution.jl")
    # Test sensitivities for the basics.
    include("sensitivities/indexing.jl")
    include("sensitivities/scalar.jl")
    include("sensitivities/array.jl")

    # Test sensitivities for functionals.
    @testset "Functional" begin
        include("sensitivities/functional/functional.jl")
        include("sensitivities/functional/reduce.jl")
        include("sensitivities/functional/reducedim.jl")
    end

    # Test sensitivities for linear algebra optimisations.
    @testset "Linear algebra" begin
        include("sensitivities/linalg/generic.jl")
        include("sensitivities/linalg/symmetric.jl")
        include("sensitivities/linalg/uniformscaling.jl")
        include("sensitivities/linalg/diagonal.jl")
        include("sensitivities/linalg/triangular.jl")
        include("sensitivities/linalg/blas.jl")

        @testset "Factorisations" begin
            include("sensitivities/linalg/factorization/cholesky.jl")
            include("sensitivities/linalg/factorization/svd.jl")
        end
    end

    include("checkpointing.jl")
end

end


module SimulacoesTemplate

  include("Data.jl")
  export Data

  include("./initial-point.jl")
  include("./initial-velocities.jl")
  export initial_point, initial_velocities

  include("./rpbc.jl")
  include("./upair.jl")
  include("./utotal.jl")
  export utotal

  include("./linear_index.jl")
  export linear_index

  include("./utotal_parallel.jl")
  export utotal
  export utotal_parallel1
  export utotal_parallel2

  include("./wrap.jl")
  include("./forcepair.jl")
  include("./energyandforce.jl")
  export energyandforce!

  include("./simulate.jl")
  export simulate

end


function is_print_time(tm, dt_print)
  if mod(tm, dt_print) < dtm
      is_print = true
      println("--writing files, t = ", tm, "; maxTime = ", maxTime, "; dtm = ", dtm)
  else
      is_print = false
  end
  return is_print
end

function is_quiet_time!(time1, time2, tm, tm_quiet, geom, nz, nq, dt_print, dtm)
    if (time1 && tm < tm_quiet)
        if geom=="slab"
            nq = nquiet
        elseif geom=="spherical"
            nq = nz - nquiet
        end
        println("nz = ", nz, " nquiet = ", nquiet, " tm_quiet=", tm_quiet)
        println("dt_print = ", dt_print, " dtm = ", dtm)
        time1 = false
    elseif (time2 && tm >= tm_quiet)
        println("end of quiet time")
        nq = nz
        # dtm = dtm / dt_multi
        # lm = dtm / dr
        time2 = false
    end
    return nq, time1, time2
end


using DistributedFactorGraphs
struct TestInferenceVariable1 <: InferenceVariable end
a = DFGVariable(:a, TestInferenceVariable1())
b = copy(a)

d1 = getSolverData(a)
pop!(solverDataDict(a), :default)
d2 = getSolverData(b)

d1.dims = 6
d2.dims


@compat abstract type abc_plan_type end

mutable struct abc_pmc_plan_saveable_type <: abc_plan_type
   gen_data::String
   calc_summary_stats::String
   calc_dist::String
   prior::Distribution
   make_proposal_dist::String
   normalize::String
   is_valid::String
   num_part::Int64
   num_max_attempt::Int64
   num_max_attempt_valid::Int64
   num_max_times::Int64
   num_dist_per_obs::Int64
   epsilon_init::Float64
   init_epsilon_quantile::Float64
   epsilon_reduction_factor::Float64
   target_epsilon::Float64
   tau_factor::Float64
   frac_rejects_save::Float64   # Not implemented yet
   param_active::Vector{Int64}   # Not fully tested yet
   adaptive_quantiles::Bool      # Not fully tested yet
   stop_on_decreasing_efficiency::Bool # Not implemented yet
   save_params::Bool   # Not implemented yet
   save_summary_stats::Bool   # Not implemented yet
   save_distances::Bool   # Not implemented yet
   in_parallel::Bool
end

mutable struct abc_pmc_plan_type <: abc_plan_type
   gen_data::Function
   calc_summary_stats::Function
   calc_dist::Function
   prior::Distribution
   make_proposal_dist::Function
   normalize::Function
   is_valid::Function
   num_part::Int64
   num_max_attempt::Int64
   num_max_attempt_valid::Int64
   num_max_times::Int64
   num_dist_per_obs::Int64
   epsilon_init::Float64
   init_epsilon_quantile::Float64
   epsilon_reduction_factor::Float64
   target_epsilon::Float64
   tau_factor::Float64
   frac_rejects_save::Float64   # Not implemented yet
   param_active::Vector{Int64}   # Not fully tested yet
   adaptive_quantiles::Bool      # Not fully tested yet
   stop_on_decreasing_efficiency::Bool # Not implemented yet
   save_params::Bool   # Not implemented yet
   save_summary_stats::Bool   # Not implemented yet
   save_distances::Bool   # Not implemented yet
   in_parallel::Bool

"""
   abc_pmc_plan_type(gd::Function,css::Function,cd::Function,p::Distribution; <keyword arguements>)

Create a plan for an ABC simulations.

# Arguements
- `gd`:  Generates the data
- `css`: Calculates the summary statistics from a simulated dataset
- `cd`:  Computes the distance between two sets of summary statistiscs
- `p`:   Prior distribution for model parameters
"""
function abc_pmc_plan_type(gd::Function,css::Function,cd::Function,p::Distribution;
     make_proposal_dist::Function = make_proposal_dist_gaussian_full_covar, param_active::Vector{Int64} = collect(1:length(Distributions.rand(p))),
     normalize::Function = noop, is_valid::Function = noop,
     num_part::Integer = 10*length(Distributions.rand(p))^2, num_max_attempt::Integer = 1000, num_max_attempt_valid::Integer = 50, num_max_times::Integer = 100, num_dist_per_obs::Int64 = 1,
     epsilon_init::Float64 = 1.0, init_epsilon_quantile::Float64 = 0.75, epsilon_reduction_factor::Float64 = 0.9,
     target_epsilon::Float64 = 0.01, tau_factor::Float64 = 2.0,
     adaptive_quantiles::Bool = false, stop_on_decreasing_efficiency::Bool = false,
     save_params::Bool = true, save_summary_stats::Bool = false, save_distances::Bool = true, frac_rejects_save::Float64 = 0.0, in_parallel::Bool = false)
     @assert(num_part>=length(Distributions.rand(p)))
     @assert(num_max_attempt>=1)
     @assert(num_max_attempt_valid>=1)
     @assert(num_max_times>0)
     @assert(1<=num_dist_per_obs<=10000)
     @assert(epsilon_init>0.01)
     @assert(0<init_epsilon_quantile<=1.0)
     @assert(0.5<epsilon_reduction_factor<1.0)
     @assert(target_epsilon>0.0)
     @assert(1.0<=tau_factor<=4.0)
     @assert(0.0<=frac_rejects_save<=1.0)
     new(gd,css,cd,p,make_proposal_dist,normalize,is_valid, num_part,num_max_attempt,num_max_attempt_valid,num_max_times,num_dist_per_obs,epsilon_init, init_epsilon_quantile,epsilon_reduction_factor,target_epsilon,tau_factor,frac_rejects_save,param_active,adaptive_quantiles,stop_on_decreasing_efficiency,save_params,save_summary_stats,save_distances,in_parallel)
   end
end

function abc_pmc_plan_saveable_type(plan::abc_pmc_plan_type)
  abc_pmc_plan_saveable_type(string(plan.gen_data),string(plan.calc_summary_stats),string(plan.calc_dist),plan.prior,string(plan.make_proposal_dist),string(plan.normalize),string(plan.is_valid),plan.num_part,plan.num_max_attempt,plan.num_max_attempt_valid,plan.num_max_times,plan.num_dist_per_obs,plan.epsilon_init,plan.init_epsilon_quantile,plan.epsilon_reduction_factor,plan.target_epsilon,plan.tau_factor,plan.frac_rejects_save,plan.param_active,plan.adaptive_quantiles,plan.stop_on_decreasing_efficiency,plan.save_params,plan.save_summary_stats,plan.save_distances,plan.in_parallel)
end

# WARNING: This doesn't really work yet.  There seems to be some issue with the scope of the saved function and what's currently imported
function abc_pmc_plan_type(plan::abc_pmc_plan_saveable_type)
  abc_pmc_plan_saveable_type(eval(Symbol(plan.gen_data)),eval(Symbol(plan.calc_summary_stats)),eval(Symbol(plan.calc_dist)),plan.prior,eval(Symbol(plan.make_proposal_dist)),eval(Symbol(plan.normalize)),eval(Symbol(plan.is_valid)),plan.num_part,plan.num_max_attempt,plan.num_max_attempt_valid,plan.num_max_times,plan.num_dist_per_obs,plan.epsilon_init,plan.init_epsilon_quantile,plan.epsilon_reduction_factor,plan.target_epsilon,plan.tau_factor,plan.frac_rejects_save,plan.param_active,plan.adaptive_quantiles,plan.stop_on_decreasing_efficiency,plan.save_params,plan.save_summary_stats,plan.save_distances,plan.in_parallel)
end

mutable struct abc_log_type   # Not implemented yet
   theta::Array{Array{Float64,1},1}
   ss::Array{Any,1}
   dist::Array{Float64,1}
   epsilon::Array{Float64,1}
   generation_starts_at::Array{Int64,1}

   function abc_log_type(t::Array{Array{Float64,1},1}, ss::Array{Any,1}, d::Array{Float64,1}, eps::Array{Float64,1}, gsa::Array{Int64,1} = ones(Int64,1))
      @assert length(t) == length(ss) == length(d) == length(eps)
      new( t, ss, d, eps, gsa )
   end
end

function abc_log_type()
   abc_log_type( Array{Array{Float64,1}}(undef,0), Array{Any}(undef,0), Array{Float64}(undef,0), Array{Float64}(undef,0), Array{Int64}(undef,0) )
end

mutable struct abc_population_type_old # Deprecated, but kept for now, in case need to read in data from a file
   theta::Array{Float64,2}
   weights::Array{Float64,1}
   dist::Array{Float64,1}
   log::abc_log_type
   repeats::Array{Int64,1}

   function abc_population_type_old(t::Array{Float64,2}, w::Array{Float64,1}, d::Array{Float64,1}, l::abc_log_type = abc_log_type(), repeats::Array{Int64,1} = zeros(Int64,length(w)) )
      @assert(length(w)==length(d)==size(t,2)==length(repeats))
      new(t,w,d,l,repeats)
   end
end

mutable struct abc_population_type
   theta::Array{Float64,2}
   weights::Array{Float64,1}
   dist::Array{Float64,1}
   epsilon::Float64
   logpdf::Array{Float64,1}
   accept_log::abc_log_type
   reject_log::abc_log_type
   repeats::Array{Int64,1}

   function abc_population_type(t::Array{Float64,2}, w::Array{Float64,1}, d::Array{Float64,1}, eps::Float64, lp::Array{Float64,1} = ones(Float64,length(w))/length(w), la::abc_log_type = abc_log_type(), lr::abc_log_type = abc_log_type(), repeats::Array{Int64,1} = zeros(Int64,length(w)) )
      @assert(length(w)==length(d)==length(lp)==size(t,2)==length(repeats))
      new(t,w,d,eps,lp,la,lr,repeats)
   end
end

"""
   abc_population_type(num_param::Integer, num_particles::Integer; accept_log::abc_log_type = abc_log_type(), reject_log::abc_log_type = abc_log_type(), repeats::Array{Int64,1} = zeros(Int64,num_particles) )

num_param:  Number of model parameters for generating simulated data
num_particles: Number of particles for sequential importance sampler

Optional parameters:
accept_log: Log of accepted parameters/summary statistics/distances
reject_log: Log of rejected parameters/summary statistics/distances
repeats:    Array indicating which particles have been repeated from previous generation

"""
function abc_population_type(num_param::Integer, num_particles::Integer; accept_log::abc_log_type = abc_log_type(), reject_log::abc_log_type = abc_log_type(), repeats::Array{Int64,1} = zeros(Int64,num_particles) )
   abc_population_type(zeros(num_param,num_particles), zeros(num_particles), zeros(num_particles), 0.0, ones(Float64,num_particles)/num_particles, accept_log, reject_log, repeats)
end


using StatsBase

abstract StepData

immutable Outputter
	outputsFunction::Function
	outputStringHeader::AbstractString
	numOutputsFromOutputsFunction::Int64
end

immutable ResultFromOO
	resultString::AbstractString
	resultLine::Vector{Float64}
end

immutable OutputOpts
	ooString::AbstractString
	outputter::Outputter
	expIndices
	function OutputOpts(outputter::Outputter,expIndices; average::Bool=false, ooString::AbstractString="average = $average")
		new(ooString, outputter,expIndices)
	end
end

immutable Opts
	init::Matrix{Float64}
	stepSizePower::Int64
	maxG::Int64
	outputLevel::Int64
	stepOutputLevel::Int64
	optsString::AbstractString
	function Opts(init::Matrix{Float64}; stepSizePower::Int64=1, maxG::Int64=typemax(Int64)-35329, outputLevel::Int64=1, optsString::AbstractString="stepSizePower = $stepSizePower, maxG = $maxG", stepOutputLevel::Int64=0)
		outputLevel>2 && println("outputLevel is $outputLevel")
		(maxG>typemax(Int64)-35329 || maxG<0) && error("maxG is $maxG, and is out of range")
		new(init, stepSizePower, maxG, outputLevel, stepOutputLevel, optsString)
	end
end

immutable Problem
	L2reg::Bool
	name::AbstractString
	lossFunctionString::AbstractString
	numTP::Int64 # Don't need for EGR
	getNextSampleFunction::Task
	getSampleFunctionAt::Function # Don't need for EGR
	function Problem(a,b,c,d,e,f)
		new(a,b,c,d,e,f)
	end
end

function alg(problem::Problem, opts::Opts, sd::StepData, oo::OutputOpts, writeFunction::Function)

	srand(1)

	opts.outputLevel>0 && println("Function to minimize: $(problem.name) with loss function $(problem.lossFunctionString), L2reg = $(problem.L2reg)")
	opts.outputLevel>0 && println("Step: $(sd.stepString) with stepSizePower = $(opts.stepSizePower)")
	opts.outputLevel>0 && println("Opts: $(opts.optsString)")
	opts.outputLevel>0 && println("OutputOpts: $(oo.ooString)")

	maxG = min(opts.maxG, oo.expIndices[end]);

	results_k = Int64[]
	results_gnum = Int64[]
	results_fromOutputsFunction = zeros(0,oo.outputter.numOutputsFromOutputsFunction)

	kOutputs=1
	k=0
	gnum=0
	x=opts.init

	xSum=copy(x)
	xToTest = zeros(size(x))

	opts.outputLevel>1 && println("        k     gnum"*oo.outputter.outputStringHeader)

	while true

		if gnum >= oo.expIndices[kOutputs]
			xToTest =x
			fromOutputsFunction::ResultFromOO = oo.outputter.outputsFunction(xToTest)
			if opts.outputLevel>1
				@printf("%2.i %8.i %8.i ", kOutputs, k, gnum)
				println(fromOutputsFunction.resultString)
			end
			writeFunction(problem, sd, opts, k, gnum, oo.expIndices[kOutputs], fromOutputsFunction)
			if isnan(fromOutputsFunction.resultLine[end])
				return ("NaN found", results_k, results_gnum,results_fromOutputsFunction,xToTest)
			end
			push!(results_k,k)
			push!(results_gnum,gnum)
			results_fromOutputsFunction = [results_fromOutputsFunction ; fromOutputsFunction.resultLine']
			kOutputs += 1
		end

		gnum >= maxG && break

		(g, gnum) = sd.getStep(x, k, gnum, sd, problem; outputLevel = opts.stepOutputLevel)

		opts.outputLevel>2 && println("norm(g) before step = $(norm(g))")
		opts.outputLevel>2 && println("norm(x) before step = $(norm(x))")

		x-=(2.0^opts.stepSizePower)*g

		opts.outputLevel>2 && println("norm(x) after step = $(norm(x))")

		xSum += x;

		k+=1

	end

	# exit()

	opts.outputLevel>0 && println("Finished alg: $(sd.stepString)")

	## Write answer!
	("Finished nicely", results_k, results_gnum, results_fromOutputsFunction, xToTest)
end


function visualizeDensityMesh!(vc,
                               fgl::FactorGraph,
                               lbl::Symbol;
                               levels=3,
                               meshid::Int=2  )

  pl1 = marginal(getVertKDE(fgl,lbl),[1;2;3])

  gg = (x, a=0.0) -> evaluateDualTree(pl1, [x[1] x[2] x[3]]')[1]-a  #([x[1];x[2];x[3]]')'

  x = getKDEMax(pl1)
  maxval = gg(x)

  vv = getKDERange(pl1)
  lower_bound = Vec(vec(vv[:,1])...)
  upper_bound = Vec(vec(vv[:,2])...)

  levels = linspace(0.0,maxval,levels+2)

  # MD = []
  for val in levels[2:(end-1)]
    meshdata = GeometryData(contour_mesh(x -> gg(x,val), lower_bound, upper_bound))
    meshdata.color = RGBA( val/(1.5*maxval),0.0,1.0,val/(1.5*maxval))
    # push!(MD, meshdata)
    setgeometry!(vc[:meshes][lbl][Symbol("lev$(val)")], meshdata)
  end
  # mptr = Any(MD)
  # vc.meshes[lbl] = mptr
  # Visualizer(mptr, meshid) # meshdata
  nothing
end

function drawgt!(vc,
                 sym::Symbol,
                 gtval::Tuple{Symbol, Vector{Float64}};
                 session::AbstractString="NA"  )
  #
  if gtval[1] == :XYZ
    drawpoint!(vc, sym, tf=Translation(gtval[2][1],gtval[2][2],gtval[2][3]),
          session=session,
          color=RGBA(1.0,0,0,0.5),
          collection=:gt_landm  )
  elseif gtval[1] == :XYZqWXYZ
    drawpose!(vc, sym,
          tf = Translation(gtval[2][1],gtval[2][2],gtval[2][3]) ∘
               LinearMap(CoordinateTransformations.Quat(gtval[2][4],gtval[2][5],gtval[2][6],gtval[2][7])),
          session=session,
          collection=:gt_poses  )
  else
    warn("unknown ground truth drawing type $(gtval[1])")
  end

  nothing
end

# TODO -- maybe we need RemoteFactorGraph type
function visualizeallposes!(vc,
                            fgl::FactorGraph;
                            drawlandms::Bool=true,
                            drawtype::Symbol=:max,
                            gt::Dict{Symbol, Tuple{Symbol,Vector{Float64}}}=Dict{Symbol, Tuple{Symbol,Vector{Float64}}}(),
                            api::DataLayerAPI=localapi  )
  #
  session = fgl.sessionname
  topoint = gettopoint(drawtype)

  dotwo = false
  dothree = false
  po,ll = ls(fgl)
  if length(po) > 0
    sym = po[1]
    X = getVal(fgl, sym, api=api )
    dotwo, dothree = getdotwothree(sym, X)
  end

  # TODO -- move calls higher in abstraction to be more similar to drawdbdirector()
  for p in po
    vert = getVert(fgl, p, api=api )
    drawpose!(vc, vert, topoint, dotwo, dothree, session=session)
    if haskey(gt, p)
      drawgt!(vc, p, gt[p], session=session)
    end
  end
  if drawlandms
    for l in ll
      den = getVertKDE(fgl, l, api=api)
      pointval = topoint(den)
      drawpoint!(vc, l, tf=Translation(pointval[1:3]...), session=session)
      if haskey(gt, l)
        drawgt!(vc, l, gt[l], session=session)
      end
    end
  end

  nothing
end

function drawposepoints!(vis,
                         vert::Graphs.ExVertex;
                         session::AbstractString="NA"  )
  #
  vsym = Symbol(vert.label)
  X = getVal(vert)

  dotwo, dothree = getdotwothree(vsym, X)
  makefromX = (X::Array{Float64,2}, i::Int) -> X[1:3,i]
  if dotwo
    makefromX = (X::Array{Float64,2}, i::Int) -> Float64[X[1:2,i];0.0]
  end

  XX = Vector{Vector{Float64}}()
  for i in 1:size(X,2)
    push!(XX, makefromX(X,i))
  end
  pointcloud = PointCloud(XX)
  if string(vsym)[1] == 'l'
    pointcloud.channels[:rgb] = [RGB(1.0, 1.0, 0) for i in 1:length(XX)]
  elseif string(vsym)[1] == 'x'
    pointcloud.channels[:rgb] = [colorwheel(vert.index) for i in 1:length(XX)]
  end
  setgeometry!(vis[Symbol(session)][:posepts][vsym], pointcloud)
  nothing
end

function drawposepoints!(vis,
      fgl::FactorGraph,
      sym::Symbol;
      session::AbstractString="NA",
      api::DataLayerAPI=dlapi  )
  #
  vert = getVert(fgl, sym, api=api)
  drawposepoints!(vis, vert, session=session, api=localapi) # definitely use localapi
  nothing
end

function deletemeshes!(vc)
  delete!(vc[:meshes])
end

"""
    $(SIGNATURES)

Draw a line segment between to vertices.
"""
function drawLineBetween!(vis,
                          session::AbstractString,
                          fr::Graphs.ExVertex,
                          to::Graphs.ExVertex;
                          scale=0.01,
                          name::Symbol=:edges,
                          subname::Union{Nothing,Symbol}=nothing,
                          color=RGBA(0,1.0,0,0.5)   )
  #
  dotwo, dothree = getdotwothree(Symbol(fr.label), getVal(fr))

  xipt = zeros(3); xjpt = zeros(3);
  if dothree
    xi = marginal(getVertKDE( fr ),[1;2;3] )
    xj = marginal(getVertKDE( to ),[1;2;3] )
    xipt[1:3] = getKDEMax(xi)
    xjpt[1:3] = getKDEMax(xj)
  elseif dotwo
    xi = marginal(getVertKDE( fr ),[1;2] )
    xj = marginal(getVertKDE( to ),[1;2] )
    xipt[1:2] = getKDEMax(xi)
    xjpt[1:2] = getKDEMax(xj)
  end

  lbl = Symbol(string(fr.label,to.label))
  place = vis[Symbol(session)][name][lbl]
  if subname != nothing
    place = vis[Symbol(session)][name][subname][lbl]
  end
  drawLine!(place, xipt, xjpt, color=color, scale=scale )
  nothing
end

"""
    $(SIGNATURES)

Draw a line segment between to nodes in the factor graph.
"""
function drawLineBetween!(vis,
                          fgl::FactorGraph,
                          fr::Symbol,
                          to::Symbol;
                          scale=0.01,
                          name::Symbol=:edges,
                          subname::Union{Nothing,Symbol}=nothing,
                          color=RGBA(0,1.0,0,0.5),
                          api::DataLayerAPI=dlapi  )
  #
  v1 = getVert(fgl, fr, api=api)
  v2 = getVert(fgl, to, api=api)

  drawLineBetween!(vis,fgl.sessionname, v1,v2,scale=scale,name=name,subname=subname,color=color   )
end

"""
    $(SIGNATURES)

Assume odometry chain and draw edges between subsequent poses. Use keyword arguments to change colors, etc.
"""
function drawAllOdometryEdges!(vis,
                               fgl::FactorGraph;
                               scale=0.01,
                               name::Symbol=:edges,
                               color=RGBA(0,1.0,0,0.5),
                               api::DataLayerAPI=dlapi  )
  #
  xx, ll = ls(fgl)

  for i in 1:(length(xx)-1)
    drawLineBetween!(vis, fgl, xx[i],xx[i+1], api=api , color=color, scale=scale, name=name )
  end

  nothing
end

function drawAllBinaryFactorEdges!(vis,
                                   fgl::FactorGraph;
                                   scale=0.01,
                                   api::DataLayerAPI=dlapi  )
  #
  sloth = findAllBinaryFactors(fgl, api=api)

  for (teeth, toe) in sloth
    color = pointToColor(toe[3])
    drawLineBetween!(vis, fgl, toe[1], toe[2], subname=toe[3], scale=scale, color=color)
  end
  nothing
end

"""
    $(SIGNATURES)

Update all triads listed in poseswithdepth[Symbol(vert.label)] with wTb. Prevents cycles in
remote tree viewer of Visualizer.
"""
function updateparallelposes!(vis,
                              vert::Graphs.ExVertex,
                              poseswithdepth::Dict;
                              wTb::CoordinateTransformations.AbstractAffineMap=
                                    Translation(0,0,0.0) ∘ LinearMap(
                                    CoordinateTransformations.Quat(1.0, 0, 0, 0))    )
  #

  if haskey(poseswithdepth, Symbol(vert.label))
    for cp in poseswithdepth[Symbol(vert.label)]
      settransform!(cp, wTb)
    end
  end
  nothing
end

function drawdbdirector(;addrdict::NothingUnion{Dict{AbstractString, AbstractString}}=nothing)
  # Uncomment out for command line operation
  cloudGraph, addrdict = standardcloudgraphsetup(addrdict=addrdict,drawdepth=true, drawedges=true)
  session = addrdict["session"]

  poseswithdepth = Dict()
  # poseswithdepth[:x1] = 0 # skip this pose -- there is no big data before ICRA

  vis = startDefaultVisualization()
  sleep(1.0)

  param = Dict()
  try
    param = robotsetup(cloudGraph, session)
  catch
    warn("No robot parameters found for the session, continuing with basic visualization only")
  end

  drawloop = Bool[true]
  println("Starting draw loop...")
  while drawloop[1]
    drawdbsession(vis, cloudGraph, addrdict, param, poseswithdepth) #,  db[collection]
    println(".")
    sleep(0.5)
  end

  println("Finishing askdrawdirectordb.")
  nothing
end

#syntax support lost in 0.5, but see conversation at
# function (dmdl::DrawObject)(vc::VisualizationContainer)
# issue  https://github.com/JuliaLang/julia/issues/14919


using Mill: AlignedBags, ScatteredBags, bags, length2bags, remapbag

@testset "Constructors" begin
    @test AlignedBags().bags == UnitRange{Int}[]
    @test AlignedBags([1]).bags == [1:1]
    @test AlignedBags([1, 1]).bags == [1:2]
    @test AlignedBags([1, 1, 2, 2, 2, 3]).bags == [1:2, 3:5, 6:6]
    @test AlignedBags([3, 3, 1, 1, 1, 2]).bags == [1:2, 3:5, 6:6]
    @test AlignedBags([2, 2, 3, 3, 3, 1]).bags == [1:2, 3:5, 6:6]
    @test AlignedBags(UInt64.([2, 2, 3, 3, 3, 1])).bags == [1:2, 3:5, 6:6]
    @test_throws ErrorException AlignedBags([1, 1, 1, 2, 2, 1])
    @test_throws ErrorException AlignedBags([2, 1, 1, 1, 1, 2])

    @test ScatteredBags().bags == Int[]
    @test ScatteredBags([1]).bags == [[1]]
    @test ScatteredBags([1, 2, 1]).bags == [[1, 3], [2]]
    @test ScatteredBags([1, 2, 3, 1, 2, 3]).bags == [[1,4], [2,5], [3,6]]
    @test ScatteredBags([1, 1, 2, 2, 2, 3]).bags == [[1,2], [3,4,5], [6]]
    @test ScatteredBags([3, 3, 1, 1, 1, 2]).bags == [[3,4,5], [6], [1,2]]
    @test ScatteredBags(UInt64.([3, 3, 1, 1, 1, 2])).bags == [[3,4,5], [6], [1,2]]
    @test ScatteredBags([2, 2, 3, 3, 3, 1]).bags == [[6], [1,2], [3,4,5]]
end

@testset "bags()" begin
    @test typeof(Mill.bags(Int[])) == AlignedBags
    @test typeof(Mill.bags([1, 1, 1, 2, 2, 3, 3])) == AlignedBags
    @test typeof(Mill.bags([2, 2, 2, 1, 1, 3, 3])) == AlignedBags
    @test typeof(Mill.bags([1, 2, 1])) == ScatteredBags
end

@testset "length2bags" begin
    @test length2bags([1,3,2]).bags == [1:1,2:4,5:6]
    @test length2bags([1,3,0,2]).bags == [1:1,2:4,0:-1,5:6]
    @test length2bags([2]).bags == [1:2]
    @test length2bags([1]).bags == [1:1]
    @test length2bags([0]).bags == [0:-1]
end

@testset "aligned hcat" begin
    b1 = AlignedBags([1:1, 2:5])
    b2 = AlignedBags([1:2, 3:3])
    b3 = AlignedBags([0:-1, 1:3, 0:-1, 4:6])
    b4 = AlignedBags([])

    @test vcat(b1, b1).bags == [1:1, 2:5, 6:6, 7:10]
    @test vcat(b1, b2).bags == [1:1, 2:5, 6:7, 8:8]
    @test vcat(b1, b3).bags == [1:1, 2:5, 0:-1, 6:8, 0:-1, 9:11]
    @test vcat(b1, b4).bags == [1:1, 2:5]
    @test vcat(b2, b1).bags == [1:2, 3:3, 4:4, 5:8]
    @test vcat(b2, b2).bags == [1:2, 3:3, 4:5, 6:6]
    @test vcat(b2, b3).bags == [1:2, 3:3, 0:-1, 4:6, 0:-1, 7:9]
    @test vcat(b2, b4).bags == [1:2, 3:3]
    @test vcat(b3, b1).bags == [0:-1, 1:3, 0:-1, 4:6, 7:7, 8:11]
    @test vcat(b3, b2).bags == [0:-1, 1:3, 0:-1, 4:6, 7:8, 9:9]
    @test vcat(b3, b3).bags == [0:-1, 1:3, 0:-1, 4:6, 0:-1, 7:9, 0:-1, 10:12]
    @test vcat(b3, b4).bags == [0:-1, 1:3, 0:-1, 4:6]
    @test vcat(b4, b1).bags == b1.bags
    @test vcat(b4, b2).bags == b2.bags
    @test vcat(b4, b3).bags == b3.bags
    @test vcat(b4, b4).bags == b4.bags
    @test vcat(b1, b2, b3, b4).bags == vcat(vcat(b1, b2), vcat(b3, b4)).bags
end

@testset "scattered hcat" begin
    b1 = ScatteredBags([[1], [1, 3, 2], []])
    b2 = ScatteredBags([[], [3, 2, 1], [4, 1, 2]])
    b3 = ScatteredBags([[1], [2], [3]])
    b4 = ScatteredBags([])

    @test vcat(b1, b1).bags == [[1], [1,3,2], [], [4], [4,6,5], []]
    @test vcat(b1, b2).bags == [[1], [1,3,2], [], [], [6,5,4], [7,4,5]]
    @test vcat(b1, b3).bags == [[1], [1,3,2], [], [4], [5], [6]]
    @test vcat(b1, b4).bags == [[1], [1,3,2], []]
    @test vcat(b2, b1).bags == [[], [3,2,1], [4,1,2], [5], [5,7,6], []]
    @test vcat(b2, b2).bags == [[], [3,2,1], [4,1,2], [], [7,6,5], [8,5,6]]
    @test vcat(b2, b3).bags == [[], [3,2,1], [4,1,2], [5], [6], [7]]
    @test vcat(b2, b4).bags == [[], [3,2,1], [4,1,2]]
    @test vcat(b3, b1).bags == [[1], [2], [3], [4], [4,6,5], []]
    @test vcat(b3, b2).bags == [[1], [2], [3], [], [6,5,4], [7,4,5]]
    @test vcat(b3, b3).bags == [[1], [2], [3], [4], [5], [6]]
    @test vcat(b3, b4).bags == [[1], [2], [3]]
    @test vcat(b4, b1).bags == b1.bags
    @test vcat(b4, b2).bags == b2.bags
    @test vcat(b4, b3).bags == b3.bags
    @test vcat(b4, b4).bags == b4.bags
    @test vcat(b1, b2, b3, b4).bags == vcat(vcat(b1, b2), vcat(b3, b4)).bags
end

@testset "testing remapping for aligned bags" begin
    b = AlignedBags([1:1,2:3,4:5])
    @test remapbag(b, [2,3])[1].bags == [1:2,3:4]
    @test remapbag(b, 2:3)[1].bags == [1:2,3:4]
    @test remapbag(b, [2,3])[2] == [2,3,4,5]
    @test remapbag(b, 2:3)[2] == [2,3,4,5]

    b = AlignedBags([1:2,3:3,4:5])
    @test remapbag(b, [1,3])[1].bags == [1:2,3:4]
    @test remapbag(b, [1,3])[2] == [1,2,4,5]
    @test remapbag(b, [2,3])[1].bags == [1:1,2:3]
    @test remapbag(b, [2,3])[2] == [3,4,5]

    b = AlignedBags([1:2,0:-1,3:4])
    @test remapbag(b, [2,3])[1].bags == [0:-1,1:2]
    @test remapbag(b, [2,3])[2] == [3,4]
end

@testset "testing remapping for scattered bags" begin
    b = ScatteredBags([[1], [], [1,5], [4,3,5]])
    @test remapbag(b, [1])[1].bags == remapbag(b, 1:1)[1].bags
    @test remapbag(b, [1])[2] == remapbag(b, 1:1)[2]
    @test remapbag(b, [1, 2, 3])[1].bags == remapbag(b, 1:3)[1].bags
    @test remapbag(b, [1, 2, 3])[2] == remapbag(b, 1:3)[2]

    @test remapbag(b, [1,2,3,4])[1].bags == [[1], [], [1,2], [3,4,2]]
    @test remapbag(b, [1,2,3,4])[2] == [1,5,4,3]
    @test remapbag(b, [1,1])[1].bags == [[1], [1]]
    @test remapbag(b, [1,1])[2] == [1]
    @test remapbag(b, [1,2])[1].bags == [[1], []]
    @test remapbag(b, [1,2])[2] == [1]
    @test remapbag(b, [1,3])[1].bags == [[1], [1,2]]
    @test remapbag(b, [1,3])[2] == [1,5]
    @test remapbag(b, [1,4])[1].bags == [[1], [2,3,4]]
    @test remapbag(b, [1,4])[2] == [1,4,3,5]
    @test remapbag(b, [2,1])[1].bags == [[], [1]]
    @test remapbag(b, [2,1])[2] == [1]
    @test remapbag(b, [2,2])[1].bags == [[], []]
    @test remapbag(b, [2,2])[2] == []
    @test remapbag(b, [2,3])[1].bags == [[], [1,2]]
    @test remapbag(b, [2,3])[2] == [1,5]
    @test remapbag(b, [2,4])[1].bags == [[], [1,2,3]]
    @test remapbag(b, [2,4])[2] == [4,3,5]
end


#Ost - "E6" - восток, Nord - "N8" - север, West - "W4" - запад, Sud - "S2" - юг
function main_function7!(r)
    arr=[]
    exit_to_the_lower_left_cell!(r, arr)
    moves!(r, Nord)
    counter_Nord=moves!(r, Sud)
    side=Ost
    mark_line!(r, side)
    for _ in 1:counter_Nord
        if !ismarker(r)
            move!(r, Nord)
            side=inverse(side)
            mark_line!(r, side)
        elseif ismarked(r)
            ismarked(r)
            move!(r, Nord)
            side=inverse(side)
            move!(r, side)
            mark_line!(r, side)
        end
    end
    moves!(r, West)
    moves!(r, Sud)
    return_to_starting_point!(r, arr)
    putmarker!(r)
end
function mark_line!(r, side)
    while !isborder(r, side)
        putmarker!(r)
        move!(r, side)
        if !isborder(r, side)
            move!(r, side)
        end
    end
end
function exit_to_the_lower_left_cell!(r, arr)
    while !(isborder(r, West)&&isborder(r, Sud))
        if !isborder(r, West)
            move!(r, West)
            push!(arr, West)
        end
        if !isborder(r, West)
            move!(r, West)
            push!(arr, West)
        end
    end
end
function return_to_starting_point!(r, arr)
    while length(arr)>0
        side=pop!(arr)
        move!(r, inverse(side))
    end
end
function inverse(side)
    HorizonSide(mod(Int(side)+2, 4))
end
function moves!(r, side)
    counter=0
    while !isborder(r, side)
        move!(r, side)
        counter +=1
    end
    return counter
end


using Test
using Revise
using Pkg;
Pkg.activate(".")

using dlgo
using dlgo.Agent

function main()
    board_size = 5
    game = new_game(board_size)
    bot = RandomBot()

    while !is_over(game)
        println("\u1b[2J")
        print_board(game)

        if game.next_player == black
            print("-- ")
            human_move = readline(stdin)
            point = point_from_coords(strip(human_move))
            while !isvoid(game.board[point])
                println(game.board[point])
                human_move = readline(stdin)
                point = point_from_coords(strip(human_move))
            end
            move = play(point)
        else
            move = select_move(bot, game)
        end
        print_move(game.next_player, move)
        game = apply_move(game, move)
    end
end

main()


using Winston

println("-----------------------------------")
println("  Welcome to G(t) Plot.jl  ")
println("-----------------------------------")

mt_int = ARGS[1]
mt_float = ARGS[2]
xi = ARGS[3]

Data = readcsv("Data/Gauge_$(mt_int)_$(mt_float)_$(xi).csv")

t = Data[:,1];
# λ = Data[:,2];
# yt = Data[:,3];
# g1 = Data[:,4];
# g2 = Data[:,5];
# g3 = Data[:,6];
G = Data[:,7];

# Gauge Plot
p = FramedPlot(
    title="G(t) Plots",
    xlabel="t",
    ylabel="Gauge");
C = Curve(t, G)
setattr(C, "label", "G(t)")
lgnd = Legend(.9, .9, [C])
add(p, C, lgnd)
savefig(p, "Fig/G_$(mt_int)_$(mt_float)_$(xi).svg", (1000, 600))
run(`inkscape -z Fig/G_$(mt_int)_$(mt_float)_$(xi).svg -e Fig/G_$(mt_int)_$(mt_float)_$(xi).png -d 300 --export-background=WHITE`)
run(`rm Fig/G_$(mt_int)_$(mt_float)_$(xi).svg`)


# This file includes all functions that serve to create data structures that
# record partition information and relevant network parameters

# NOTE: this file is updated to incorporate the case when soem lines are inactivated while parsed by PowerModels.jl
#       however, we are still assuming that all buses are active (which might cause errors if not true)

function find_neighbor_buses(data::Dict{String, Any}, N_g::Set{Int64})::Set{Int64}
    neighbors = Set{Int64}()
    for line in keys(data["branch"])
        if data["branch"][line]["br_status"] != PM.pm_component_status_inactive["branch"] &&
                (data["branch"][line]["f_bus"] in N_g || data["branch"][line]["t_bus"] in N_g)
            push!(neighbors, data["branch"][line]["f_bus"], data["branch"][line]["t_bus"])
        end
    end
    return setdiff!(neighbors, N_g)
end

function generate_subnet_data(data::Dict{String, Any}, N_g::Vector{Int64})
    return generate_subnet_data(data, Set(N_g))
end

function generate_subnet_data(data::Dict{String, Any}, N_g::Set{Int64})
    sub_data = deepcopy(data)
    nbus = find_neighbor_buses(data, N_g)
    N = Set(parse.(Int, collect(keys(data["bus"]))))
    # deactivate unrelated buses
    for i in setdiff!(N, N_g, nbus)
        sub_data["bus"]["$(i)"]["bus_type"] = PM.pm_component_status_inactive["bus"]
    end
    # deactivate lines between neighbor buses
    for line in keys(data["branch"])
        if sub_data["branch"][line]["f_bus"] in nbus && data["branch"][line]["t_bus"] in nbus
            sub_data["branch"][line]["br_status"] = PM.pm_component_status_inactive["branch"]
        end
    end
    # deactivate unrelated generators (for those at neighbor nodes)
    for (_, gen) in sub_data["gen"]
        if !(gen["gen_bus"] in N_g)
            gen["gen_status"] = PM.pm_component_status_inactive["gen"]
        end
    end
    sub_data["cut_bus"] = nbus
    return sub_data
end

function ref_add_cut_bus!(ref::Dict{Symbol, Any}, data::Dict{String, Any})
    for (nw, nw_ref) in ref[:it][:pm][:nw]
        nw_ref[:cut_bus] = Dict(i => data["bus"]["$(i)"] for i in data["cut_bus"])
    end
end

function ref_add_cut_branch!(ref::Dict{Symbol, Any}, data::Dict{String, Any})
    for (nw, nw_ref) in ref[:it][:pm][:nw]
        # set up cut branches and arcs
        nw_ref[:cut_branch] = Dict(parse(Int, x.first) => x.second for x in data["branch"] if
            x.second["br_status"] != PM.pm_component_status_inactive["branch"] &&
            (x.second["f_bus"] in keys(nw_ref[:bus])) + (x.second["t_bus"] in keys(nw_ref[:bus])) +
            (x.second["f_bus"] in keys(nw_ref[:cut_bus])) + (x.second["t_bus"] in keys(nw_ref[:cut_bus])) == 3)
        nw_ref[:cut_arcs_from] = [(i,branch["f_bus"],branch["t_bus"]) for (i,branch) in nw_ref[:cut_branch]]
        nw_ref[:cut_arcs_to]   = [(i,branch["t_bus"],branch["f_bus"]) for (i,branch) in nw_ref[:cut_branch]]
        nw_ref[:cut_arcs] = [nw_ref[:cut_arcs_from]; nw_ref[:cut_arcs_to]]
    end
end

#=
function ref_add_cut_bus_arcs_refs!(ref::Dict{Symbol, Any}, data::Dict{String, Any})
    for (nw, nw_ref) in ref[:it][:pm][:nw]
        cut_bus_arcs = Dict((i, Tuple{Int,Int,Int}[]) for (i,bus) in merge(nw_ref[:bus], nw_ref[:cut_bus]))
        for (l,i,j) in nw_ref[:cut_arcs]
            push!(cut_bus_arcs[i], (l,i,j))
        end
        nw_ref[:cut_bus_arcs] = cut_bus_arcs
    end
end
=#

function ref_add_global_bus!(ref::Dict{Symbol, Any}, data::Dict{String, Any})
    function calc_all_buspair_parameters(buses, branches, conductor_ids, ismulticondcutor)
        bus_lookup = Dict(bus["index"] => bus for (i,bus) in buses)
        branch_lookup = Dict(branch["index"] => branch for (i,branch) in branches)
        buspair_indexes = Set((branch["f_bus"], branch["t_bus"]) for (i,branch) in branch_lookup)
        bp_branch = Dict((bp, typemax(Int64)) for bp in buspair_indexes)

        if ismulticondcutor
            bp_angmin = Dict((bp, [-Inf for c in conductor_ids]) for bp in buspair_indexes)
            bp_angmax = Dict((bp, [ Inf for c in conductor_ids]) for bp in buspair_indexes)
        else
            @assert(length(conductor_ids) == 1)
            bp_angmin = Dict((bp, -Inf) for bp in buspair_indexes)
            bp_angmax = Dict((bp,  Inf) for bp in buspair_indexes)
        end

        for (l,branch) in branch_lookup
            i = branch["f_bus"]
            j = branch["t_bus"]

            if ismulticondcutor
                for c in conductor_ids
                    bp_angmin[(i,j)][c] = max(bp_angmin[(i,j)][c], branch["angmin"][c])
                    bp_angmax[(i,j)][c] = min(bp_angmax[(i,j)][c], branch["angmax"][c])
                end
            else
                bp_angmin[(i,j)] = max(bp_angmin[(i,j)], branch["angmin"])
                bp_angmax[(i,j)] = min(bp_angmax[(i,j)], branch["angmax"])
            end

            bp_branch[(i,j)] = min(bp_branch[(i,j)], l)
        end

        buspairs = Dict((i,j) => Dict(
            "branch"=>bp_branch[(i,j)],
            "angmin"=>bp_angmin[(i,j)],
            "angmax"=>bp_angmax[(i,j)],
            "tap"=>branch_lookup[bp_branch[(i,j)]]["tap"],
            "vm_fr_min"=>bus_lookup[i]["vmin"],
            "vm_fr_max"=>bus_lookup[i]["vmax"],
            "vm_to_min"=>bus_lookup[j]["vmin"],
            "vm_to_max"=>bus_lookup[j]["vmax"]
            ) for (i,j) in buspair_indexes
        )

        # add optional parameters
        for bp in buspair_indexes
            branch = branch_lookup[bp_branch[bp]]
            if haskey(branch, "rate_a")
                buspairs[bp]["rate_a"] = branch["rate_a"]
            end
            if haskey(branch, "c_rating_a")
                buspairs[bp]["c_rating_a"] = branch["c_rating_a"]
            end
        end

        return buspairs
    end
    for (nw, nw_ref) in ref[:it][:pm][:nw]
        nw_ref[:all_bus] = Dict(j["bus_i"] => j for (_, j) in data["bus"])
        nw_ref[:all_branch] = Dict{Int, Any}(parse(Int, k) => v for (k,v) in data["branch"] if v["br_status"] != PM.pm_component_status_inactive["branch"])
        nw_ref[:all_buspairs] = calc_all_buspair_parameters(nw_ref[:all_bus], nw_ref[:all_branch], nw_ref[:conductor_ids], haskey(nw_ref, :conductors))
    end
end


# ------------------------------------------------------------------
# Licensed under the ISC License. See LICENSE in the project root.
# ------------------------------------------------------------------

"""
    AbstractShape

A geometrical shape in a `N`-dimensional space with coordinates of type `T`.
"""
abstract type AbstractShape{T,N} end

#------------------
# IMPLEMENTATIONS
#------------------
include("shapes/rectangle.jl")


module SmallExample

using FlatBuffers

@enum(Color, Red = 1, Green = 2, Blue = 8)
@DEFAULT Color Red

@STRUCT struct Test
	a::Int16
	b::UInt8
	# _1::UInt8 # padding
end

mutable struct TestSimpleTableWithEnum
	color::Color
end

@DEFAULT TestSimpleTableWithEnum color=Green

@STRUCT struct Vec3
	x::Float32
	y::Float32
	z::Float32
	# _::UInt32 # padding
	test1::Float64
	test2::Color
	# __::UInt8 # padding
	test3::Test
	# ___::UInt16 # padding
end

@ALIGN Vec3 16

mutable struct Stat
	id::Union{Nothing, String}
	val::Int64
	count::UInt16
end

# Julia doesn't support forward referencing of types
# @union Any_ Union{Monster, TestSimpleTableWithEnum}

mutable struct Monster
	pos::Vec3
	mana::Int16
	hp::Int16
	name::Union{Nothing, String}
	friendly::Bool # deprecated
	inventory::Union{Nothing, Vector{UInt8}}
	color::Color
	# test_type::Any_
	# test::Union{Nothing, Vector{UInt8}}
	test4::Union{Nothing, Vector{Test}}
	testarrayofstring::Union{Nothing, Vector{String}}
	testarrayoftables::Union{Nothing, Vector{Monster}}
	# don't support nested circulr reference objects yet
	# enemy::Monster
	testnestedflatbuffer::Union{Nothing, Vector{UInt8}}
	testempty::Union{Nothing, Stat}
	testbool::Bool
	testhashs32_fnv1::Int32
	testhashu32_fnv1::UInt32
	testhashs64_fnv1::Int64
	testhashu64_fnv1::UInt64
	testhashs32_fnv1a::Int32
	testhashu32_fnv1a::UInt32
	testhashs64_fnv1a::Int64
	testhashu64_fnv1a::UInt64
	testarrayofbools::Union{Nothing, Vector{Bool}}
	testf::Float32
	testf2::Float32
	testf3::Float32
end

@DEFAULT Monster hp=Int16(100) mana=Int16(150) color=convert(UInt8, Blue) friendly=false testf=Float32(3.14159) testf2=Float32(3)

end # module


"""
    shuffleobs(data, [obsdim], [rng])

Return a "subset" of `data` that spans all observations, but
has the order of the observations shuffled.

The values of `data` itself are not copied. Instead only the
indices are shuffled. This function calls [`datasubset`](@ref) to
accomplish that, which means that the return value is likely of a
different type than `data`.

```julia
# For Arrays the subset will be of type SubArray
@assert typeof(shuffleobs(rand(4,10))) <: SubArray

# Iterate through all observations in random order
for (x) in eachobs(shuffleobs(X))
    ...
end
```

The optional (keyword) parameter `obsdim` allows one to specify
which dimension denotes the observations. see `LearnBase.ObsDim`
for more detail.

The optional (keyword) parameter `rng` allows one to specify the
random number generator used for shuffling. This is useful when
reproducible results are desired. By default, uses the global RNG.
See `Random` in Julia's standard library for more info.

For this function to work, the type of `data` must implement
[`nobs`](@ref) and [`getobs`](@ref). See [`DataSubset`](@ref)
for more information.
"""
shuffleobs(data; obsdim = default_obsdim(data), rng::AbstractRNG = Random.GLOBAL_RNG) =
    shuffleobs(data, convert(LearnBase.ObsDimension,obsdim), rng)

function shuffleobs(data, obsdim, rng::AbstractRNG = Random.GLOBAL_RNG)
    allowcontainer(shuffleobs, data) || throw(MethodError(shuffleobs, (data,obsdim)))
    datasubset(data, randperm(rng, nobs(data, obsdim)), obsdim)
end


using StableRNGs
using MLJScikitLearnInterface
using Test
import MLJBase
import ScikitLearn
import MLJBase: target_scitype, input_scitype, output_scitype

const MB = MLJBase

include("testutils.jl")
include("macros.jl")

println("\nlinear-regressors");       include("models/linear-regressors.jl")
println("\nlinear-regressors-multi"); include("models/linear-regressors-multi.jl")

println("\nlinear-classifiers");      include("models/linear-classifiers.jl")
println("\ndiscriminant-analysis");   include("models/discriminant-analysis.jl")
println("\ngaussian-process");        include("models/gaussian-process.jl")
println("\nensemble");                include("models/ensemble.jl")
println("\nclustering");              include("models/clustering.jl")