#!/usr/bin/env ruby

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

require 'gnuplot'
require 'gpstime'

CONFIG_FILE = 'plot_config.rb'

config = {
	:LAT_RANGE		=> '[35.55:35.73]',
	:LONG_RANGE		=> '[139.51:139.65]',
	:ALT_RANGE		=> '[0:600]',
	:VIEW_ANGLE_3D	=> '40,330',
	:TICS_3D		=> '0'
}
if FileTest::exist?(CONFIG_FILE) then
	puts "CONFIG_FILE ON!"
	eval(open(CONFIG_FILE).read)
end
p config


CB_LABELS = [
		'Time',
		'Roll',
		'Pitch',
		'Yaw/Hdg',
		'Roll rate',
		'Pitch rate',
		'Yaw rate',
		'Velocity North',
		'Velocity East',
		'Velocity Down',
		'Longitude',
		'Latitude',
		'Altitude',
		'Status BIT',
		'ITOW'
	]

EZ_LABELS = [
		'Type',
		'ITOW',
		'longitude',
		'latitude',
		'height',
		'v_north',
		'v_east',
		'v_down',
		'Yaw',
		'Pitch',
		'Roll'
	]
GN_LABELS = [
		'GPSTime',
		'Week',
		'Latitude',
		'Longitude',
		'H-Ell',
		'Easting',
		'Northing',
		'X-LL',
		'Y-LL',
		'Z-LL',
		'AccEast',
		'AccNrth',
		'AccUp',
		'VEast',
		'VNorth',
		'VDown',
		'SDEast',
		'SDNorth',
		'SDHeight',
		'SD-VE',
		'SD-VN',
		'Q',
		'PDOP',
		'ClkCorr',
		'SolType'
	]

class Plotter3
	def Plotter3::plot_basic file_name
		Gnuplot.open{|gp|
			Gnuplot::SPlot::new(gp){|plot|
				puts "drawing #{file_name} ..."
				plot.terminal "postscript eps color"
				plot.output file_name
				yield plot
			}
		}
	end
end

# 3Dプロット
=begin
Plotter3::plot_basic('3D.eps'){|plot|
	data_x = [1,2,3]
	data_y = [2,3,4]
	data_z = [4,6,8]
		
	items = [
			Gnuplot::DataSet::new([data_x, data_y, data_z]){|ds|
				ds.with = "lines lw 3"
				ds.title = "Original"
			}
		]
		
	plot.data = items
}
exit
=end

if ARGV.size < 2 then
	puts "Usage: #{__FILE__} cb(log) ez(processed) [cb_ITOW_shift(ms)] [grafnav]"
	exit
end

cb_data = []
line_no = 0
start_time = Time::now
open(ARGV[0]).each{|line|
	if line =~ /Date\t(\d+)\/(\d+)\/(\d+)/ then
		start_time = Time::local($3.to_i, $1.to_i, $2.to_i)
		next
	elsif line =~ /Time\t(\d+):(\d+):(\d+)/ then
		start_time += (($1.to_i * 60) + $2.to_i) * 60 + $3.to_i
		if ARGV.size > 2 then
			start_time += (ARGV[2].to_i / 1000)
		end
		p start_time
		next
	end

	temp = line.chop.split(/\t/)
	if temp.size >= CB_LABELS.size then
		if (line_no += 1) == 1 then next end #タイトル行削除
		if line_no % 10 == 2 then	# データを間引く
			temp = temp.collect{|item| item.to_f}
			temp[CB_LABELS.index('ITOW')] = (GPSTime::itow(start_time + temp[CB_LABELS.index('Time')])[2] * 1000).to_i
			#p temp
			cb_data << temp
		end
	end
}

ez_data = []
line_no = 0
open(ARGV[1]).each{|line|
	if (line_no += 1) == 1 then next end
	temp = line.chop.split(/\t/)
	if temp.size >= EZ_LABELS.size then
		ez_data << temp.collect{|item| item.to_f}
	end
}

gn_data = nil
if ARGV.size > 3 then
	gn_data = []
	line_no = 0
	open(ARGV[3]).each{|line|
		if (line_no += 1) < 21 then next end
		temp = []
		temp << (line[0...9].to_f * 1000).to_i
		temp << line[10...20].to_f
		temp << line[21...25].to_i + (line[26...28].to_i * 60 + line[29...37].to_f) / 3600
		temp << line[38...42].to_i + (line[43...45].to_i * 60 + line[46...54].to_f) / 3600
		temp << line[55...67].to_f
		temp << line[68...80].to_f
		temp << line[81...93].to_f
		temp << line[94...106].to_f
		temp << line[107...119].to_f
		temp << line[120...132].to_f
		temp << line[133...140].to_f
		temp << line[141...148].to_f
		temp << line[149...156].to_f
		temp << line[157...166].to_f
		temp << line[167...176].to_f
		temp << -(line[177...186].to_f)
		temp << line[187...199].to_f
		temp << line[200...212].to_f
		temp << line[213...225].to_f
		temp << line[226...235].to_f
		temp << line[236...245].to_f
		temp << line[246...247].to_i
		temp << line[248...254].to_f
		temp << line[255...262].to_f
		temp << line[263..-1]
		gn_data << temp
	}
end

class Array
  def to_gsplot
    f = ""
    
    x = self[0]
    y = self[1]
    z = self[2]
    
    (0...([x.size, y.size, z.size].min{|a, b| a <=> b})).each{|i|
	  f << [ x[i], y[i], z[i] ].join(" ") << "\n"
	}
	
	f
  end
end

# 3Dプロット
Plotter3::plot_basic('3D.eps'){|plot|
	cb_lat = cb_data.collect{|moment| moment[CB_LABELS.index('Latitude')]}
	cb_long = cb_data.collect{|moment| moment[CB_LABELS.index('Longitude')]}
	cb_alt = cb_data.collect{|moment| moment[CB_LABELS.index('Altitude')]}
	
	ez_lat = ez_data.collect{|moment| moment[EZ_LABELS.index('latitude')]}
	ez_long = ez_data.collect{|moment| moment[EZ_LABELS.index('longitude')]}
	ez_alt = ez_data.collect{|moment| moment[EZ_LABELS.index('height')]}
	
	plot.xlabel "'longitude [deg]'"
	plot.ylabel "'latitude [deg]'"
	plot.set('zlabel', "'altitude [m]'")
	plot.xrange config[:LONG_RANGE]
	plot.yrange config[:LAT_RANGE]
	plot.set('zrange', config[:ALT_RANGE])
	plot.set('view', config[:VIEW_ANGLE_3D])
	plot.set('ticslevel', config[:TICS_3D])

	items = [
			Gnuplot::DataSet::new([ez_long, ez_lat, ez_alt]){|ds|
				ds.with = "lines lw 3"
				ds.title = "Prototype"
			},
			Gnuplot::DataSet::new([cb_long, cb_lat, cb_alt]){|ds|
				ds.with = "lines lw 2"
				ds.title = "Crossbow NAV420"
			},
			Gnuplot::DataSet::new([ez_long, ez_lat, Array::new(ez_alt.size, 0)]){|ds|
				ds.with = "lines lw 1 lt 1"
				ds.notitle
			},
			Gnuplot::DataSet::new([cb_long, cb_lat, Array::new(cb_alt.size, 0)]){|ds|
				ds.with = "lines lw 1 lt 2"
				ds.notitle
			}
		]

	if gn_data then
		gn_lat = gn_data.collect{|moment| moment[GN_LABELS.index('Latitude')]}
		gn_long = gn_data.collect{|moment| moment[GN_LABELS.index('Longitude')]}
		gn_alt = gn_data.collect{|moment| moment[GN_LABELS.index('H-Ell')]}
		items << Gnuplot::DataSet::new([gn_long, gn_lat, gn_alt]){|ds|
				ds.with = "lines lw 2"
				ds.title = "GPS Postprocess"
			}
	end
	plot.data = items
}