#!/usr/local/bin/ruby -Ks

$: << '~/src/eclipse/autopilot/common/ruby'

require 'ublox'

if $0 == __FILE__ then
	
	if ARGV.size < 1 then
		puts "Usage: #{__FILE__} target_log [A_modify]"
		exit(-1)
	end
	
	a_lines = []
	gpos_lines = []
	gvel_lines = []
	
	modify_time = ARGV.size == 2 ? ARGV[1].to_i : 0
	open(ARGV[0]).each{|line|
		if line =~ /^A/ then
			values = line.split(/\s+/)
			values[1] = values[1].to_i + modify_time
			result = sprintf("A %10d", values[1])
			result += values[2..-1].collect{|value| sprintf(" %8s", value)}.join
			a_lines << [values[1], result]
		elsif line =~ /^G/ then
			packet = U_blox::string2packet(line[2...(line.size - 2)])
			if !U_blox::valid_header?(packet) then next end
			if !U_blox::valid_size?(packet) then next end
			if !U_blox::valid_parity?(packet) then next end
			itow = U_blox::fetch_ITOW(packet) * 1E3
			if pllh = U_blox::fetch_longlatheight(packet) then
				gpos_lines << [itow, [pllh, U_blox::fetch_pos_acc(packet)].flatten]
			elsif vned = U_blox::fetch_velned(packet) then
				gvel_lines << [itow, [vned, U_blox::fetch_vel_acc(packet)].flatten]
			end
		else
			puts line
		end
	}
	
	g_lines = []
	while !gpos_lines.empty? or !gvel_lines.empty?
		gpos = gpos_lines.shift if !gpos
		gvel = gvel_lines.shift if !gvel
		if !gpos then
			val = 'G ' + sprintf('%10d', gvel[0]) \
			           + ' ' * 12 * 5 \
			           + gvel[1].collect{|param| sprintf('%12f', param)}.join
			#g_lines << [gvel[0], val]
			gvel = nil
		elsif gpos[0] == gvel[0] then
			val = 'G ' + sprintf('%10d', gpos[0]) \
			           + gpos[1].collect{|param| sprintf('%12f', param)}.join \
			           + gvel[1].collect{|param| sprintf('%12f', param)}.join
			g_lines << [gpos[0], val]
			gpos = gvel = nil
		elsif gpos[0] < gvel[0] then
			val = 'G ' + sprintf('%10d', gpos[0]) \
			           + gpos[1].collect{|param| sprintf('%12f', param)}.join
			#g_lines << [gpos[0], val]
			gpos = nil
		else
			val = 'G ' + sprintf('%10d', gvel[0]) \
			           + ' ' * 12 * 5 \
			           + gvel[1].collect{|param| sprintf('%12f', param)}.join
			#g_lines << [gvel[0], val]
			gvel = nil
		end
	end
	
	a_line = nil
	g_line = nil
	while !a_lines.empty? or !g_lines.empty?
		a_line = a_lines.shift if !a_line
		g_line = g_lines.shift if !g_line
		
		begin
			if a_line[0] == g_line[0] then
				puts a_line[1]
				puts g_line[1]
				a_line = g_line = nil
			elsif a_line[0] < g_line[0] then
				puts a_line[1]
				a_line = nil
			else
				puts g_line[1]
				g_line = nil
			end
		rescue
			if !a_line then
				puts g_line[1]
				g_line = nil
			else
				puts a_line[1]
				a_line = nil
			end
		end
	end
end