include("packages/TLV.jl")

#const DATA_DIR = "/data/Data/181283340_06_06_2022_HIGHWAY"
const DATA_DIR = "/data/Data/166543251_10_27_2021_OVAL"

@time points_east, points_north = TLV.cache("serialization.jld2"; force=false) do 
	east = []
	north = []
	function callback(header, msg)
		pma = msg["s_Data"]["s_PMA_LocalizationPose"]
		pose = pma["s_PoseLocation"]
		push!(east, pose["e_l_east"])
		push!(north, pose["e_l_north"])
		return false
	end

	topics = TLV.log_create_reader_config(
		[
	#		["UC_SYSTEM", "GPS_DATA_SERVICE"]
	#  		["UC_SYSTEM", "PERCEIVED_ROAD_GEOMETRY"]
	  		["UC_LOC_OUT", "LOC_ASSOCIATED_ROAD_GEOMETRY"]
	    ]
	)

	TLV.log_read(DATA_DIR, topics, callback)
	return east, north
end


using Plots
display(scatter(points_east, points_north; title="Oval Track", 
	legend=false, mc=:blue, ms=.05))
readline()
#gui()