#!/usr/bin/ruby

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

require 'gnuplot_support'

$stderr.puts "Usage: #{$0} real_flight.csv [--options=...]"

TARGET_DIR = './CVS_IGNORE'

options = {
  :logs => 100
}
ARGV.reject!{|item|
  skip = false
  if item =~ /--([^=]+)=/ then
    options[$1.to_sym] = $'
    skip = true
  end
  skip
}
options[:logs] = options[:logs].to_i

file_true = nil
file_sim_list = []
Dir::open(TARGET_DIR){|dir|
  dir.each{|file|
    if file =~ /^flight_log_(\d+)\.csv/ then
      if $1.to_i >= options[:logs] then
        next
      end
      file_sim_list << File::join(TARGET_DIR, file)
    elsif file =~ /^flight_log_true.csv/ then
      file_true = File::join(TARGET_DIR, file)
    end
  }
}

if !file_true or file_sim_list.empty? then
  puts "File not found..."
  exit
end

def process_file(fname)
  data = []
  open(fname).each{|line|
    data << line.chop!.split(',').collect!{|item| item.to_f}
  }
  start_time = data[0][0]
  data[1..-1].each{|item|
    item[0] -= start_time
  }
  data[0][0] = 0.0
  return data.transpose
end

true_data = process_file(file_true)

sim_data = {}
file_sim_list.each{|fname|
  sim_data[fname] = process_file(fname)
}

real_data = nil
unless ARGV.empty? then
  fname = ARGV.shift
  real_data = []
  open(fname).each{|line|
    values = line.chop.split(',')
    real_data << [
      values[0].to_f,   # t
      values[16].to_f,  # de
      values[11].to_f,  # V
      values[12].to_f,  # alpha
      values[8].to_f,   # theta
      values[34].to_f,  # q
      values[37].to_f,  # qdot
      values[30].to_f,  # a_x
      values[32].to_f,  # a_z
    ]
  }
  start_time = real_data[0][0]
  real_data.each{|item|
    item[0] -= start_time
  }
  real_data = real_data.transpose
end

TARGETS = [
  # [name, y_label, index, SF]
  ['V', 'V_{wind} [m/s]', 2, 1],
  ['alpha', '{/Symbol a} [deg]', 3, 180.0 / Math::PI],
  ['theta', '{/Symbol q} [deg]', 4, 180.0 / Math::PI],
  ['q', 'q [deg/s]', 5, 180.0 / Math::PI],
  ['ax', 'a_{x} [m/s^{2}]', 7, 1],
  ['az', 'a_{z} [m/s^{2}]', 8, 1],
]

TARGETS.each{|item|
  eps_fname = "sim_#{item[0]}.eps"
  png_fname = "sim_#{item[0]}.png"
  Plotter::plot_basic(eps_fname){|plot|
    plot.xlabel "'Time [s]'"
    plot.ylabel "'#{item[1]}'"
    plot.set('size', "1, 0.4")
    plot.set('lmargin', '10')
    plot.set('bmargin', '3')
    plot.set('rmargin', '2')
    plot.set('tmargin', '1')
  
    items = []
    
    sim_data.each{|fname, data|
      items << Gnuplot::DataSet::new([
          data[0].collect{|v| sprintf("%3.2f", v)}, 
          data[item[2]].collect{|v| sprintf("%3.3f", v * item[3])}]){|ds|
        ds.with = "lines lw 1 lt 1 lc 3"
        if items.empty? then
          ds.title = 'Error simulated'
        else
          ds.notitle
        end
      }
    }
    
    items << Gnuplot::DataSet::new([
        true_data[0].collect{|v| sprintf("%3.2f", v)}, 
        true_data[item[2]].collect{|v| sprintf("%3.3f", v * item[3])}]){|ds|
      ds.with = "lines lw 3 lt 1"
      ds.title = 'Error free'
    }
    plot.xrange "[0:#{true_data[0].last.ceil}]"
    
    if real_data then
      t_data = real_data[0]
      
      # Vとalphaを移動させる(時刻が微妙におかしい)
      if options.include?(:ads_shift) and (item[2] == 2) or (item[2] == 3) then 
        ads_shift = options[:ads_shift].to_f
        t_data = t_data.collect{|t| t - ads_shift}
      end
      
      items << Gnuplot::DataSet::new([
          t_data.collect{|v| sprintf("%3.2f", v)}, 
          real_data[item[2]].collect{|v| sprintf("%3.3f", v * item[3])}]){|ds|
        ds.with = "lines lw 3 lt 1 lc 2"
        ds.title = 'Real Flight'
      }
    end
    
    plot.data = items
  }
  
  # pngに変換
  system(<<-__TEXT__)
gs -q -dSAFER -dNOPAUSE -dBATCH -sDEVICE=png256 -dEPSCrop -r288 -dTextAlphaBits=4 -dGraphicsAlphaBits=4 -sOutputFile=#{png_fname} #{eps_fname}
__TEXT__
}