#!/usr/bin/ruby

$: << File::dirname(__FILE__)
$: << File::join(File::dirname(__FILE__), '..', '..', 'common', 'ruby')

require 'gnuplot_support'
require 'flight_log_reader.rb'

def plot1(log, log_property, options = {:fname_base => File::basename($0, ".*")})
  item, ylabel, title, sf = *log_property
  Plotter::plot_basic("#{options[:fname_base]}.#{item}.eps"){|plot|
    plot.noxlabel
    plot.ylabel "'#{ylabel}'"
    plot.set('size', options[:size])
    plot.set('lmargin', '10')
    plot.set('bmargin', '2')
    plot.set('rmargin', '2')
    plot.set('tmargin', '1')
    time_shift = options[:time_shift] || 0
    if options[:time_range]
      time_range = options[:time_range]
      time_range = eval(time_range) if time_range.kind_of?(String)
      plot.xrange <<-__STRING__
[#{(time_range.first - time_shift).floor}:#{(time_range.last - time_shift).ceil}]
__STRING__
    end
    
    ydata = log.get(item)
    
    if options[:ylimit]
      ymin = ydata.min * sf
      ymax = ydata.max * sf
      ymiddle = (ymax + ymin) / 2
      half_width = (ymax - ymin) * options[:ylimit] / 2 
      plot.yrange <<-__STRING__
[#{ymiddle - half_width}:#{ymiddle + half_width}]
__STRING__
    end
    
    temp = [
      Gnuplot::DataSet::new([
          log.get(:Time).collect{|v| sprintf("%3.2f", v  - time_shift)}, 
          ydata.collect{|v| sprintf("%3.3f", v * sf)}]){|ds|
        ds.with = "lines lw 3 lt 1"
        ds.title = title
      },
    ]
    
    count = 1
    if options[:additional_logs] then
      options[:additional_logs].each{|data, suffix|
        target = data.get(item)
        next unless target
        temp << Gnuplot::DataSet::new([
            data.get(:Time).collect{|v| sprintf("%3.2f", v  - time_shift)}, 
            target.collect{|v| sprintf("%3.3f", v * sf)}]){|ds|
          ds.with = "lines lw 2 #{"lc 9" if count == 5}"  # 黄色を回避
          ds.title = "#{title} #{suffix}"
        }
        count += 1
      }
    end
    
    if options[:grey_backs] then # グラフの一部の背景を灰色にするオプション、配列を想定
      options[:grey_backs].each{|range|
        plot.set('object', \
            "rect from #{range.first}, graph 0 to #{range.last}, graph 1 fc lt -1 fs solid 0.15 noborder")
      }
    end
    
    plot.data = temp
  }
end

def plot2(log, property, options = {:fname_base => File::basename($0, ".*")})
  input_log = log.get(:Input).transpose + log.get(:Output).transpose
  item, title = property
  name = "d#{item}"
  index = options["#{name}".to_sym]
  sf = options["#{name}sf".to_sym]
  trim = options["#{name}trim".to_sym]
  Plotter::plot_basic("#{options[:fname_base]}.#{name}.eps"){|plot|
    plot.xlabel "'GPS Time [s]'"
    plot.ylabel "'{/Symbol d}_{#{item}} [deg]'"
    plot.set('size', options[:size])
    plot.set('lmargin', '10')
    plot.set('bmargin', '3')
    plot.set('rmargin', '2')
    plot.set('tmargin', '1')
    time_shift = options[:time_shift] || 0
    if options[:time_range]
      time_range = options[:time_range]
      plot.xrange <<-__STRING__
[#{(time_range.first - time_shift).floor}:#{(time_range.last - time_shift).ceil}]
__STRING__
    end
    
    plot.data = [
      Gnuplot::DataSet::new([
          log.get(:Time).collect{|v| sprintf("%3.2f", v  - time_shift)}, 
          input_log[index].collect{|v| sprintf("%3.3f", (v - trim) * sf)}]){|ds|
        ds.with = "lines lw 3 lt 1"
        ds.title = title
      }
    ]
  }
end

$options = {
    :fname_base => File::basename($0, '.*'),
    :size => "1,0.3",
    :additional_logs => []}

def opt_check(args)
  return args.delete_if{|item|
    hit = true
    case item
    when /^--trange=/ then
      obj = eval($')
      $options[:time_range] = obj if obj.kind_of? Range
    when /^--tshift=/ then
      $options[:time_shift] = $'.to_f
    when /^--prefix=/ then
      $options[:fname_base] = $'
    when /^--ylimit(?:=([\d.]+))?/ then
      $options[:ylimit] = $1.to_f || 1.5 # 1.5倍にする
    when /^--size=/ then
      $options[:size] = $'
    when /^--verbose=/ then
      Gnuplot.verbose if $' == 'on'
    else
      hit = false
    end
    hit
  }
end

def prologue
  str = "Usage: #{$0} log.input.csv [--trange=] [--tshift=] [--prefix=] [--add=(f,suffix[,format])] ..."
  #$options.each{|k, v|
  #  str += " [--#{k.to_s}=]"
  #}
  $stderr.puts str
  
  if ARGV.size < 1 then
    raise "Require log.input.csv!!"
    exit(-1)
  end
end

if $0 == __FILE__ then

class SI_in_FormatData
  
  LABELS = FlightLogReader::FLIGHT_LOG_LABELS
  
  def initialize(file, options = {})
    @data = FlightLogReader::read(file, 1)
    if range = options[:time_range] then
      time_index = LABELS.index(:Time)
      @data.reject!{|item| !range.include?(item[time_index])}
    end
    @data= @data.transpose
  end
  
  def get(target)
    if i = LABELS.index(target) then
      return @data[i]
    end
    return []
  end
end

class ObsFormatData
  LABELS = [
      :Time,           # GPS時刻(秒)
      :TAS,
      :AoAttack,
      :Pitch,
      :PitchRate,
      :PitchAccel,
      :AccX,          # 加速度(m/s^2) x軸(前)
      :AccZ,          #               z軸(下)
    ]
  
  def initialize(file, options = {})
    @data = []
    line_no = 0
    denom = options[:denom] || 1
    range = options[:time_range]
    time_index = LABELS.index(:Time)
    open(file).each{|line|
      temp = line.chop.split(/,/)
      if temp.size >= LABELS::size then
        temp = temp[0...LABELS::size].collect{|item| item.to_f}
        if range then
          next unless range.include?(temp[time_index])
        end
        line_no += 1
        next if line_no % denom != 0 # データを間引く
        @data << temp
      end
    }
    @data = @data.transpose
  end
  
  def get(target)
    if i = LABELS::index(target) then
      return @data[i]
    end
    return nil
  end
end

$options.merge!({
    :de => 0,
    :dt => 1,
    :desf => 1,
    :dtsf => 1,
    :detrim => 0,
    :dttrim => 0})

prologue
log_file = ARGV.shift
$options[:fname_base] = File::basename(log_file, ".*")
if log_file == '-' then
  log_file = $stdin
end
additional_log_files = []

opt_check(ARGV).each{|item|
  case item
  when /^--(d[et])=(\d+)/ then
    $options[$1.to_sym] = $2.to_i
  when /^--(d[et](?:sf|trim))=([+-.\d]+)/ then  # 舵面SFの単位[deg]
    $options[$1.to_sym] = $2.to_f
  when /^--add=/ then # 追加プロットファイル(ファイル名,suffix[,ファイル形式、デフォルトは観測量形式])
    f, suffix, format = $'.split(',')
    format ||= 'obs'
    case format
    when 'si_in' # システム同定入力ファイル
      $options[:additional_logs] << [SI_in_FormatData::new(f, $options), suffix]
    else # 'obs'形式
      $options[:additional_logs] << [ObsFormatData::new(f, $options), suffix]
    end
  else
    raise "Unknown option #{item}"
  end
}

log = SI_in_FormatData::new(log_file, $options)

[
  # [name, y_label, title, sf]
  [:TAS, 'V_{wind} [m/s]', 'True Airspeed', 1.0],
  [:AoAttack, '{/Symbol a} [deg]', 'Angle of Attack', 180.0 / Math::PI],
  [:Pitch, '{/Symbol q} [deg]', 'Pitch', 180.0 / Math::PI],
  [:PitchRate, 'q [deg/s]', 'Pitch Rate', 180.0 / Math::PI],
  [:AccX, 'a_{x} [m/s^{2}]', 'Acceleration X', 1],
  [:AccZ, 'a_{z} [m/s^{2}]', 'Acceleration Z', 1]
].each{|item| plot1(log, item, $options)}

[[:e, 'Elevator'], [:t, 'Throttle']].each{|item| plot2(log, item, $options)}

end