#!/usr/bin/env ruby

require 'gnuplot'

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

require 'WGS84'

# こっちはINS_GPSをのバイアス推定を比較するため用

PERIOD_1R = 120
FULL_TIME = 600
LABELS = [
	'[SEC]',
	'東経[秒](真値)',
	'北緯[秒](真値)',
	'高度[m](真値)',
	'北方向速度[m/s](真値)',
	'東方向速度[m/s](真値)',
	'重力方向速度[m/s](真値)',
	'Ψ[deg](真値)',
	'Θ[deg](真値)',
	'Φ[deg](真値)',
	'東経[秒](INSのみ(モデル1))',
	'北緯[秒](INSのみ(モデル1))',
	'高度[m](INSのみ(モデル1))',
	'北方向速度[m/s](INSのみ(モデル1))',
	'東方向速度[m/s](INSのみ(モデル1))',
	'重力方向速度[m/s](INSのみ(モデル1))',
	'Ψ[deg](INSのみ(モデル1))',
	'Θ[deg](INSのみ(モデル1))',
	'Φ[deg](INSのみ(モデル1))',
	'東経[秒](INS/GPS(モデル1))',
	'北緯[秒](INS/GPS(モデル1))',
	'高度[m](INS/GPS(モデル1))',
	'北方向速度[m/s](INS/GPS(モデル1))',
	'東方向速度[m/s](INS/GPS(モデル1))',
	'重力方向速度[m/s](INS/GPS(モデル1))',
	'Ψ[deg](INS/GPS(モデル1))',
	'Θ[deg](INS/GPS(モデル1))',
	'Φ[deg](INS/GPS(モデル1))',
	'東経[秒](INSのみ(モデル2))',
	'北緯[秒](INSのみ(モデル2))',
	'高度[m](INSのみ(モデル2))',
	'北方向速度[m/s](INSのみ(モデル2))',
	'東方向速度[m/s](INSのみ(モデル2))',
	'重力方向速度[m/s](INSのみ(モデル2))',
	'Ψ[deg](INSのみ(モデル2))',
	'Θ[deg](INSのみ(モデル2))',
	'Φ[deg](INSのみ(モデル2))',
	'東経[秒](INS/GPS(モデル2))',
	'北緯[秒](INS/GPS(モデル2))',
	'高度[m](INS/GPS(モデル2))',
	'北方向速度[m/s](INS/GPS(モデル2))',
	'東方向速度[m/s](INS/GPS(モデル2))',
	'重力方向速度[m/s](INS/GPS(モデル2))',
	'Ψ[deg](INS/GPS(モデル2))',
	'Θ[deg](INS/GPS(モデル2))',
	'Φ[deg](INS/GPS(モデル2))',
	'GPS東経',
	'GPS北緯',
	'GPS高度',
	'GPS速度',
	'GPS真方位',
	'加速度(真値)[X^b]',
	'加速度計[X^b]',
	'加速度(真値)[Y^b]',
	'加速度計[Y^b]',
	'加速度(真値)[Z^b]',
	'加速度計[Z^b]',
	'角速度(真値)[ω_X^b]',
	'ジャイロ1[ω_X^b]',
	'ジャイロ2[ω_X^b]',
	'角速度(真値)[ω_Y^b]',
	'ジャイロ1[ω_Y^b]',
	'ジャイロ2[ω_Y^b]',
	'角速度(真値)[ω_Z^b]',
	'ジャイロ1[ω_Z^b]',
	'ジャイロ2[ω_Z^b]'
]

class Plotter
	def Plotter::plot 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_1R(file_prefix,target,data)
		Plotter::plot("#{file_prefix}_1R.eps"){|plot|
			time = data.collect{|moment| moment[0] <= PERIOD_1R ? moment[LABELS.index('[SEC]')] : nil}
			time.delete(nil)
			
			targets = [
					["data_true",		"#{target}(真値)"],
					["data_INS_1",		"#{target}(INSのみ(モデル1))"],
					["data_INS_GPS_1",	"#{target}(INS/GPS(モデル1))"],
					["data_INS_2",		"#{target}(INSのみ(モデル2))"],
					["data_INS_GPS_2",	"#{target}(INS/GPS(モデル2))"]
				]
			
			b = binding
			targets.each{|item|
				eval(<<-EVAL_STRING, b)
					#{item[0]} = data.collect{|moment|
							moment[0] <= PERIOD_1R ? moment[LABELS.index("#{item[1]}")] : nil
						}
					#{item[0]}.delete(nil)
				EVAL_STRING
			}
			
			plot.xlabel 'Time [sec]'
			plot.xrange "[0:#{PERIOD_1R}]"
			yield plot if block_given?
		
			plot.data = [
					Gnuplot::DataSet::new([time, eval(targets[0][0])]){|ds|
						ds.with = "lines lw 3"
						ds.title = "True"
					},
					Gnuplot::DataSet::new([time, eval(targets[2][0])]){|ds|
						ds.with = "lines lw 2 lt 5"
						ds.title = "INS/GPS(Bias est. off)"
					},
					Gnuplot::DataSet::new([time, eval(targets[4][0])]){|ds|
						ds.with = "lines lw 2 lt 8"
						ds.title = "INS/GPS(Bias est. on)"
					}
				]
		}
	end
	
	def Plotter::plot_all(file_prefix,target,data)
		Plotter::plot("#{file_prefix}_all.eps"){|plot|
			time = data.collect{|moment| moment[LABELS.index('[SEC]')]}
			time.delete(nil)
			
			targets = [
					["data_true",		"#{target}(真値)"],
					["data_INS_1",		"#{target}(INSのみ(モデル1))"],
					["data_INS_GPS_1",	"#{target}(INS/GPS(モデル1))"],
					["data_INS_2",		"#{target}(INSのみ(モデル2))"],
					["data_INS_GPS_2",	"#{target}(INS/GPS(モデル2))"]
				]
			
			b = binding
			targets.each{|item|
				eval(<<-EVAL_STRING, b)
					#{item[0]} = data.collect{|moment| moment[LABELS.index("#{item[1]}")]}
				EVAL_STRING
			}
			
			plot.xlabel 'Time [sec]'
			plot.xrange "[0:#{FULL_TIME}]"
			yield plot if block_given?
		
			plot.data = [
					Gnuplot::DataSet::new([time, eval(targets[0][0])]){|ds|
						ds.with = "lines lw 3"
						ds.title = "True"
					},
					Gnuplot::DataSet::new([time, eval(targets[2][0])]){|ds|
						ds.with = "lines lw 2 lt 5"
						ds.title = "INS/GPS(Bias est. off)"
					},
					Gnuplot::DataSet::new([time, eval(targets[4][0])]){|ds|
						ds.with = "lines lw 2 lt 8"
						ds.title = "INS/GPS(Bias est. on)"
					}
				]
		}
	end
end

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

# 2D 1周
Plotter::plot('2D_1R.eps'){|plot|
	long_true = data.collect{|moment|
			moment[0] <= PERIOD_1R ? moment[LABELS.index('東経[秒](真値)')] : nil
		}
	long_true.delete(nil)
	lat_true = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('北緯[秒](真値)')] : nil
		}
	lat_true.delete(nil)

	long_INS_1 = data.collect{|moment|
			moment[0] <= PERIOD_1R ? moment[LABELS.index('東経[秒](INSのみ(モデル1))')] : nil
		}
	long_INS_1.delete(nil)
	lat_INS_1 = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('北緯[秒](INSのみ(モデル1))')] : nil
		}
	lat_INS_1.delete(nil)
	
	long_INS_GPS_1 = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('東経[秒](INS/GPS(モデル1))')] : nil
		}
	long_INS_GPS_1.delete(nil)
	lat_INS_GPS_1 = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('北緯[秒](INS/GPS(モデル1))')] : nil
		}
	lat_INS_GPS_1.delete(nil)
	
	long_INS_2 = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('東経[秒](INSのみ(モデル2))')] : nil
		}
	long_INS_2.delete(nil)
	lat_INS_2 = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('北緯[秒](INSのみ(モデル2))')] : nil
		}
	lat_INS_2.delete(nil)
	
	long_INS_GPS_2 = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('東経[秒](INS/GPS(モデル2))')] : nil
		}
	long_INS_GPS_2.delete(nil)
	lat_INS_GPS_2 = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('北緯[秒](INS/GPS(モデル2))')] : nil
		}
	lat_INS_GPS_2.delete(nil)
	
	long_gps = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('GPS東経')] : nil
		}
	long_gps.delete(nil)
	lat_gps = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('GPS北緯')] : nil
		}
	lat_gps.delete(nil)

	#long_true.each{|item| puts item}
	#lat_true.each{|item| puts item}
	
	plot.xlabel 'longitude [139. sec]'
	plot.ylabel 'latitude [35. sec]'
	plot.xrange '[2736:2754]'
	plot.yrange '[2565:2577]'

	plot.data = [
			Gnuplot::DataSet::new([long_true, lat_true]){|ds|
				ds.with = "lines lw 3"
				ds.title = "True"
			},
			Gnuplot::DataSet::new([long_INS_GPS_1, lat_INS_GPS_1]){|ds|
				ds.with = "lines lw 2 lt 5"
				ds.title = "INS/GPS(Bias est. off)"
			},
			Gnuplot::DataSet::new([long_INS_GPS_2, lat_INS_GPS_2]){|ds|
				ds.with = "lines lw 2 lt 8"
				ds.title = "INS/GPS(Bias est. on)"
			},
			Gnuplot::DataSet::new([[long_true[0]], [lat_true[0]]]){|ds|
				ds.with = "points lt -1 pt 2"
				ds.notitle
			},
			Gnuplot::DataSet::new([[long_INS_GPS_1[0]], [lat_INS_GPS_1[0]]]){|ds|
				ds.with = "points lt -1 pt 2"
				ds.notitle
			},
		]
  
	init_lat_deg = (35.0  + 42.0 / 60 + 50.818 / 3600)
	init_lat = Math::PI / 180 * init_lat_deg
  sec_100m = (100.0 / (WGS84::r_normal(init_lat) * Math::cos(init_lat)) ) / Math::PI * 180 * 3600;
  
  plot.set('label', "\"100m\" at #{long_true[0] - sec_100m / 2}, #{lat_true[0] + 0.2} center")
  plot.set('arrow', "from #{long_true[0] - sec_100m}, #{lat_true[0]} to #{long_true[0]}, #{lat_true[0]} lt -1 lw 1")
  
  plot.set('label', "\"True Start\" at #{long_true[0] + 0.2}, #{lat_true[0]}")
  plot.set('label', "\"Sim Start\" at #{long_INS_GPS_1[0] + 0.2}, #{lat_INS_GPS_1[0]}")
  
  plot.set('label', "\"Unclockwise turning\" at graph 0.5, graph 0.1 center")
}

# 2D 全時間
Plotter::plot('2D_all.eps'){|plot|
	long_true = data.collect{|moment|
			moment[0] <= PERIOD_1R ? moment[LABELS.index('東経[秒](真値)')] : nil
		}
	long_true.delete(nil)
	lat_true = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('北緯[秒](真値)')] : nil
		}
	lat_true.delete(nil)
	
	long_INS_GPS_1 = data.collect{|moment| moment[LABELS.index('東経[秒](INS/GPS(モデル1))')]}
	lat_INS_GPS_1 = data.collect{|moment| moment[LABELS.index('北緯[秒](INS/GPS(モデル1))')]}
	
	long_INS_GPS_2 = data.collect{|moment| moment[LABELS.index('東経[秒](INS/GPS(モデル2))')]}
	lat_INS_GPS_2 = data.collect{|moment| moment[LABELS.index('北緯[秒](INS/GPS(モデル2))')]}
	
	plot.xlabel 'longitude [139. sec]'
	plot.ylabel 'latitude [35. sec]'
	plot.xrange '[2736:2754]'
	plot.yrange '[2565:2577]'

	plot.data = [
			Gnuplot::DataSet::new([long_true, lat_true]){|ds|
				ds.with = "lines lw 3"
				ds.title = "True"
			},
			Gnuplot::DataSet::new([long_INS_GPS_1, lat_INS_GPS_1]){|ds|
				ds.with = "lines lw 1 lt 5 "
				ds.title = "INS/GPS(Bias est. off)"
			},
			Gnuplot::DataSet::new([long_INS_GPS_2, lat_INS_GPS_2]){|ds|
				ds.with = "lines lw 1 lt 8"
				ds.title = "INS/GPS(Bias est. on)"
			},
			Gnuplot::DataSet::new([[long_true[0]], [lat_true[0]]]){|ds|
				ds.with = "points lt -1 pt 2"
				ds.notitle
			},
			Gnuplot::DataSet::new([[long_INS_GPS_1[0]], [lat_INS_GPS_1[0]]]){|ds|
				ds.with = "points lt -1 pt 2"
				ds.notitle
			},
		]
	
  init_lat_deg = (35.0  + 42.0 / 60 + 50.818 / 3600)
	init_lat = Math::PI / 180 * init_lat_deg
  sec_100m = (100.0 / (WGS84::r_normal(init_lat) * Math::cos(init_lat)) ) / Math::PI * 180 * 3600;
  
  plot.set('label', "\"100m\" at #{long_true[0] - sec_100m / 2}, #{lat_true[0] + 0.2} center")
  plot.set('arrow', "from #{long_true[0] - sec_100m}, #{lat_true[0]} to #{long_true[0]}, #{lat_true[0]} lt -1 lw 1")
  
  plot.set('label', "\"True Start\" at #{long_true[0] + 0.2}, #{lat_true[0]}")
  plot.set('label', "\"Sim Start\" at #{long_INS_GPS_1[0] + 0.2}, #{lat_INS_GPS_1[0]}")
  
  plot.set('label', "\"Unclockwise turning\" at graph 0.5, graph 0.1 center")
}

# 高度 1周
Plotter::plot('Alt_1R.eps'){|plot|
	time = data.collect{|moment|
			moment[0] <= PERIOD_1R ? moment[LABELS.index('[SEC]')] : nil
		}
	time.delete(nil)
	
	alt_true = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('高度[m](真値)')] : nil
		}
	alt_true.delete(nil)

	alt_INS_1 = data.collect{|moment|
			moment[0] <= PERIOD_1R ? moment[LABELS.index('高度[m](INSのみ(モデル1))')] : nil
		}
	alt_INS_1.delete(nil)
	
	alt_INS_GPS_1 = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('高度[m](INS/GPS(モデル1))')] : nil
		}
	alt_INS_GPS_1.delete(nil)
	
	alt_INS_2 = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('高度[m](INSのみ(モデル2))')] : nil
		}
	alt_INS_2.delete(nil)
	
	alt_INS_GPS_2 = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('高度[m](INS/GPS(モデル2))')] : nil
		}
	alt_INS_GPS_2.delete(nil)
	
	time_gps = []
	(1..PERIOD_1R).step(1){|i| time_gps << i}
	alt_gps = data.collect{|moment| 
			moment[0] <= PERIOD_1R ? moment[LABELS.index('GPS高度')] : nil
		}
	alt_gps.delete(nil)

	#long_true.each{|item| puts item}
	#lat_true.each{|item| puts item}
	
	plot.xlabel 'Time [sec]'
	plot.ylabel 'Altitude [m]'
	plot.xrange "[0:#{PERIOD_1R}]"
	plot.yrange '[-100:100]'

	plot.data = [
			Gnuplot::DataSet::new([time, alt_true]){|ds|
				ds.with = "lines lw 3"
				ds.title = "True"
			},
			Gnuplot::DataSet::new([time, alt_INS_GPS_1]){|ds|
				ds.with = "lines lw 2 lt 5"
				ds.title = "INS/GPS(Bias est. off)"
			},
			Gnuplot::DataSet::new([time, alt_INS_GPS_2]){|ds|
				ds.with = "lines lw 2 lt 8"
				ds.title = "INS/GPS(Bias est. on)"
			}
		]
}

# 高度 全時間
Plotter::plot('Alt_all.eps'){|plot|
	time = data.collect{|moment| moment[LABELS.index('[SEC]')]}
	
	alt_true = data.collect{|moment| moment[LABELS.index('高度[m](真値)')]}
	alt_INS_1 = data.collect{|moment|moment[LABELS.index('高度[m](INSのみ(モデル1))')]}
	alt_INS_GPS_1 = data.collect{|moment| moment[LABELS.index('高度[m](INS/GPS(モデル1))')]}
	alt_INS_2 = data.collect{|moment| moment[LABELS.index('高度[m](INSのみ(モデル2))')]}
	alt_INS_GPS_2 = data.collect{|moment| moment[LABELS.index('高度[m](INS/GPS(モデル2))')]}
	
	time_gps = []
	(0..FULL_TIME).step(1){|i| time_gps << i}
	alt_gps = data.collect{|moment| moment[LABELS.index('GPS高度')]}

	#long_true.each{|item| puts item}
	#lat_true.each{|item| puts item}
	
	plot.xlabel 'Time [sec]'
	plot.ylabel 'Altitude [m]'
	plot.xrange "[0:600]"
	plot.yrange '[-100:100]'

	plot.data = [
			Gnuplot::DataSet::new([time, alt_true]){|ds|
				ds.with = "lines lw 3"
				ds.title = "True"
			},
			Gnuplot::DataSet::new([time, alt_INS_GPS_1]){|ds|
				ds.with = "lines lw 2 lt 5"
				ds.title = "INS/GPS(Bias est. off)"
			},
			Gnuplot::DataSet::new([time, alt_INS_GPS_2]){|ds|
				ds.with = "lines lw 2 lt 8"
				ds.title = "INS/GPS(Bias est. on)"
			}
		]
}

# その他
Plotter::plot_1R('V_N', '北方向速度[m/s]', data){|plot|
	plot.ylabel('Velocity (north) [m/s]')
	plot.yrange '[-15:15]'
}
Plotter::plot_all('V_N', '北方向速度[m/s]', data){|plot|
	plot.ylabel('Velocity (north) [m/s]')
	plot.yrange '[-15:15]'
}

Plotter::plot_1R('V_E', '東方向速度[m/s]', data){|plot|
	plot.ylabel('Velocity (east) [m/s]')
	plot.yrange '[-15:15]'
}
Plotter::plot_all('V_E', '東方向速度[m/s]', data){|plot|
	plot.ylabel('Velocity (east) [m/s]')
	plot.yrange '[-15:15]'
}

Plotter::plot_1R('V_D', '重力方向速度[m/s]', data){|plot|
	plot.ylabel('Velocity (down) [m/s]')
	plot.yrange '[-10:10]'
}
Plotter::plot_all('V_D', '重力方向速度[m/s]', data){|plot|
	plot.ylabel('Velocity (down) [m/s]')
	plot.yrange '[-10:10]'
}

Plotter::plot_1R('Yaw', 'Ψ[deg]', data){|plot|
	plot.ylabel('Yaw [deg]')
	plot.yrange '[-180:180]'
}
Plotter::plot_all('Yaw', 'Ψ[deg]', data){|plot|
	plot.ylabel('Yaw [deg]')
	plot.yrange '[-180:180]'
}

Plotter::plot_1R('Pitch', 'Θ[deg]', data){|plot|
	plot.ylabel('Pitch [deg]')
	plot.yrange '[-5:5]'
}
Plotter::plot_all('Pitch', 'Θ[deg]', data){|plot|
	plot.ylabel('Pitch [deg]')
	plot.yrange '[-5:5]'
}

Plotter::plot_1R('Roll', 'Φ[deg]', data){|plot|
	plot.ylabel('Roll [deg]')
	plot.yrange '[-10:10]'
}
Plotter::plot_all('Roll', 'Φ[deg]', data){|plot|
	plot.ylabel('Roll [deg]')
	plot.yrange '[-10:10]'
}