#!/usr/bin/env ruby

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

require 'gnuplot'
require 'gpstime'

CONFIG_FILE = 'plot_config.rb'

config = {
	:LAT_RANGE		=> '[35.54:35.72]',
	:LONG_RANGE		=> '[139.5:139.65]',
	:ALT_RANGE		=> '[0:600]',
	:VIEW_ANGLE_3D	=> '40,330',
	:TICS_3D		=> '0',
	:YAW_RANGE		=> '[-180:180]',
	:ROLL_RANGE		=> '[-30:30]',
	:PITCH_RANGE	=> '[-20:20]',
	:VN_RANGE		=> '[-80:80]',
	:VE_RANGE		=> '[-80:80]',
	:VD_RANGE		=> '[-20:20]'
}
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 Plotter
	def Plotter::plot_basic file_name
		Gnuplot.open{|gp|
			Gnuplot::Plot::new(gp){|plot|
				puts "drawing #{file_name} ..."
				plot.terminal "postscript eps color"
				plot.output file_name
				yield plot
			}
		}
	end
	
	def Plotter::plot(file_prefix, cb, ez, gn = nil)
		Plotter::plot_basic("#{file_prefix}.eps"){|plot|
			cb_time = cb[0].collect{|moment| moment[CB_LABELS.index('ITOW')]}
			ez_time = ez[0].collect{|moment| moment[EZ_LABELS.index('ITOW')]}
			
			cb_y = cb[0].collect{|moment| moment[CB_LABELS.index(cb[1])]}
			ez_y = ez[0].collect{|moment| moment[EZ_LABELS.index(ez[1])]}
			
			plot.xlabel "'GPS Time [msec]'"
			plot.xrange <<-__STRING__
[#{(ez[0][0][EZ_LABELS.index('ITOW')] / 10000).to_i * 10000}:#{((ez[0][-1][EZ_LABELS.index('ITOW')] / 10000).to_i + 1) * 10000}]
			__STRING__
			
			yield plot if block_given?
		
			items = [
					Gnuplot::DataSet::new([ez_time, ez_y]){|ds|
						ds.with = "lines lw 3"
						ds.title = "Prototype"
					},
					Gnuplot::DataSet::new([cb_time, cb_y]){|ds|
						ds.with = "lines lw 2"
						ds.title = "Crossbow NAV420"
					}
				]
			
			if gn then
				gn_time = gn[0].collect{|moment| moment[GN_LABELS.index('GPSTime')]}
				gn_y = gn[0].collect{|moment| moment[GN_LABELS.index(gn[1])]}
				items << Gnuplot::DataSet::new([gn_time, gn_y]){|ds|
						ds.with = "lines lw 2"
						ds.title = "GPS Postprocess"
					}
			end
			
			plot.data = items
		}
	end
end

if ARGV.size < 2 then
	puts "Usage: #{__FILE__} cb(log) ez(processed) [cb_ITOW_shift = 0(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

# 2Dプロット
Plotter::plot_basic('2D.eps'){|plot|
	cb_lat = cb_data.collect{|moment| moment[CB_LABELS.index('Latitude')]}
	cb_long = cb_data.collect{|moment| moment[CB_LABELS.index('Longitude')]}
	
	ez_lat = ez_data.collect{|moment| moment[EZ_LABELS.index('latitude')]}
	ez_long = ez_data.collect{|moment| moment[EZ_LABELS.index('longitude')]}
	
	plot.xlabel "'longitude [deg]'"
	plot.ylabel "'latitude [deg]'"
	plot.xrange config[:LONG_RANGE]
	plot.yrange config[:LAT_RANGE]

	items = [
			Gnuplot::DataSet::new([ez_long, ez_lat]){|ds|
				ds.with = "lines lw 3"
				ds.title = "Prototype"
			},
			Gnuplot::DataSet::new([cb_long, cb_lat]){|ds|
				ds.with = "lines lw 2"
				ds.title = "Crossbow NAV420"
			}
		]

	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')]}
		items << Gnuplot::DataSet::new([gn_long, gn_lat]){|ds|
				ds.with = "lines lw 2"
				ds.title = "GPS Postprocess"
			}
	end
	plot.data = items
}

# Yaw
Plotter::plot_basic('Yaw.eps'){|plot|	
	cb_time = cb_data.collect{|moment| moment[CB_LABELS.index('ITOW')]}
	ez_time = ez_data.collect{|moment| moment[EZ_LABELS.index('ITOW')]}
		
	cb_y = cb_data.collect{|moment| moment[CB_LABELS.index('Yaw/Hdg')]}
	ez_y = ez_data.collect{|moment| moment[EZ_LABELS.index('Yaw')]}
	
	vh_time = []
	vh_y = []
	
	ez_data.each{|moment|
		v_e = moment[EZ_LABELS.index('v_east')]
		v_n = moment[EZ_LABELS.index('v_north')]
		if ((v_e ** 2) + (v_n ** 2)) >= (5 ** 2) then	#リミッタ,5m/s以上
			vh_time << moment[EZ_LABELS.index('ITOW')]
			vh_y << (Math::atan2(v_e, v_n) / Math::PI * 180)
		end
	}
		
	plot.xlabel "'GPS Time [msec]'"
	plot.xrange <<-__STRING__
[#{(ez_data[0][EZ_LABELS.index('ITOW')] / 10000).to_i * 10000}:#{((ez_data[-1][EZ_LABELS.index('ITOW')] / 10000).to_i + 1) * 10000}]
	__STRING__
	
	plot.ylabel "'Yaw [deg]'"
	plot.yrange config[:YAW_RANGE]
	plot.set('ytics', 60)
	plot.set('key', 'right bottom')
	
	items = [
			Gnuplot::DataSet::new([ez_time, ez_y]){|ds|
				ds.with = "lines lw 3"
				ds.title = "Prototype"
			},
			Gnuplot::DataSet::new([cb_time, cb_y]){|ds|
				ds.with = "lines lw 2"
				ds.title = "Crossbow NAV420"
			},
			Gnuplot::DataSet::new([vh_time, vh_y]){|ds|
				ds.with = "lines lw 2 lt 5"
				ds.title = "V heading (V > 5 m/s)"
			}
		]
		
	plot.data = items
}

# プロットする
#Plotter::plot(file_prefix, [cb_data, cb_target], [ez_data, ez_target])
=begin
Plotter::plot('Yaw', [cb_data, 'Yaw/Hdg'], [ez_data, 'Yaw']){|plot|
	plot.ylabel "'Yaw [deg]'"
	plot.yrange '[-180:180]'
	plot.set('ytics', 60)
}
=end
Plotter::plot('Roll', [cb_data, 'Roll'], [ez_data, 'Roll']){|plot|
	plot.ylabel "'Roll [deg]'"
	plot.yrange config[:ROLL_RANGE]
}
Plotter::plot('Pitch', [cb_data, 'Pitch'], [ez_data, 'Pitch']){|plot|
	plot.ylabel "'Pitch [deg]'"
	plot.yrange config[:PITCH_RANGE]
}
Plotter::plot('V_N', [cb_data, 'Velocity North'], [ez_data, 'v_north'], (gn_data ? [gn_data, 'VNorth'] : nil)){|plot|
	plot.ylabel "'Velocity (north) [m/s]'"
	plot.yrange config[:VN_RANGE]
}
Plotter::plot('V_E', [cb_data, 'Velocity East'], [ez_data, 'v_east'], (gn_data ? [gn_data, 'VEast'] : nil)){|plot|
	plot.ylabel "'Velocity (east) [m/s]'"
	plot.yrange config[:VE_RANGE]
}
Plotter::plot('V_D', [cb_data, 'Velocity Down'], [ez_data, 'v_down'], (gn_data ? [gn_data, 'VDown'] : nil)){|plot|
	plot.ylabel "'Velocity (down) [m/s]'"
	plot.yrange config[:VD_RANGE]
}
Plotter::plot('Alt', [cb_data, 'Altitude'], [ez_data, 'height'], (gn_data ? [gn_data, 'H-Ell'] : nil)){|plot|
	plot.ylabel "'Altitude [m]'"
	plot.yrange config[:ALT_RANGE]
}