#!/usr/bin/env ruby

=begin
if ARGV.size < 4 then
	puts "Usage: #{__FILE__} ez(3.1) ez(3.2) ez(3.3) gaia"
	exit
end
=end
if ARGV.size < 3 then
	puts "Usage: #{__FILE__} ez(3.1) ez(3.3) gaia"
	exit
end

$: << '~/src/eclipse/autopilot/common/ruby/'
$: << '~/src/eclipse/autopilot/note/060609/'
$: << '~/src/eclipse/autopilot/note/060609/analysis'

require 'gnuplot_support'
require 'gpstime'

begin
  $: << $0[0..($0.rindex('/'))]
rescue
  $: << '.'
end

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]',
	:BETA_RANGE  => '[-25:25]',
	:LABEL_FORMAT_E => false
}
if FileTest::exist?(CONFIG_FILE) then
	puts "CONFIG_FILE ON!"
	eval(open(CONFIG_FILE).read)
end
p config
$config = config

require 'ez_reader'
EZ_LABELS = EzReader::EZ_LABELS

context = binding
#[:ez_3_1, :ez_3_2, :ez_3_3].each{|data|
[:ez_3_1, :ez_3_3].each{|data|
  ez_data = eval("#{data} = EzReader::read(ARGV.shift)", context)
  EzReader::yawsimple(ez_data)
  vh_time = eval("#{data}_vh_time = []", context)
  vh_y = eval("#{data}_vh_y = []", context)
  beta = eval("#{data}_beta = []", context)
  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)
      temp_beta = (moment[EZ_LABELS.index('Yaw')] - vh_y.last)
      if temp_beta >= 180 then
        temp_beta -= 360
      elsif temp_beta <= -180 then
        temp_beta += 360
      end
      beta << temp_beta
  	end
  }
}

require 'gaia_reader'
GAIA_LABELS = GaiaReader::GAIA_LABELS

gaia_data = GaiaReader::read(ARGV.shift)
GaiaReader.yawsimple(gaia_data)

gaia_vh_time = []
gaia_vh_y = []
gaia_beta = []
gaia_data.each{|moment|
	v_e = moment[GAIA_LABELS.index('Velocity East')]
	v_n = moment[GAIA_LABELS.index('Velocity North')]
	if ((v_e ** 2) + (v_n ** 2)) >= (5 ** 2) then	#リミッタ,5m/s以上
		gaia_vh_time << moment[GAIA_LABELS.index('ITOW')]
		gaia_vh_y << (Math::atan2(v_e, v_n) / Math::PI * 180)
    beta = (moment[GAIA_LABELS.index('Yaw')] - gaia_vh_y.last)
    if beta >= 180 then
      beta -= 360
    elsif beta <= -180 then
      beta += 360
    end
    gaia_beta << beta
	end
}

# beta
Plotter::plot_basic('beta_compare.eps'){|plot|
  start_t = eval("ez_3_1")[0][EZ_LABELS.index('ITOW')]
  end_t = eval("ez_3_1")[-1][EZ_LABELS.index('ITOW')]
  
	plot.xrange <<-__STRING__
[#{(start_t / 10000).to_i * 10000}:#{((end_t / 10000).to_i + 1) * 10000}]
	__STRING__
  	
	plot.ylabel "'Sideslip angle [deg]'"
	plot.yrange config[:BETA_RANGE]
	plot.set('ytics', 10)
	plot.set('key', 'right bottom')
	if config[:LABEL_FORMAT_E]
    plot.xlabel "'GPS time [msec]'"
  else
    plot.xlabel "'GPS time [{/Symbol \264} 10^{5} sec]'"
    plot.set('format x "%4.2t"')
  end
	
	plot.set('size', '0.8,0.8')
	
	items = []
	#[:ez_3_1, :ez_3_2, :ez_3_3].each{|data|
	[:ez_3_1, :ez_3_3].each_with_index{|data, i|
	  items << Gnuplot::DataSet::new([eval("#{data}_vh_time"), eval("#{data}_beta")]){|ds|
				ds.with = "lines lw 2"
				#data.to_s =~ /ez_(.*)/
				#ds.title = "Prototype (#{$1.gsub(/_/, '.')})"
				ds.title = "Prototype (#{i == 0 ? 'not improved' : 'improved'})"
			}
  }
			
  items << Gnuplot::DataSet::new([gaia_vh_time, gaia_beta]){|ds|
			ds.with = "lines lw 2"
			ds.title = "GAIA"
		}
		
	plot.data = items
}