#!/usr/bin/ruby

=begin
テストケースのドライブスクリプト
=end

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

require 'fileutils'
require 'gnuplot_support'

$global_options = {}

ARGV.reject!{|arg|
  next unless arg =~ /--([^=]+)=/
  $global_options[$1.intern] = $'
  true
}

EXE_FILE = "#{File::dirname(__FILE__)}/build_VC9/test.exe"

def deg2rad(deg)
  return Math::PI / 180 * deg
end

DEFAULT_OPTIONS = {
  :deltaT => 0.02,
  :final_t => 50,
  :rand_seed => 0,
  :model_k => 30, :model_a => 5, :model_m => 10, 
  :model_F0 => 50, :model_omega => deg2rad(100),
  :omega_sigma => deg2rad(0),
  :pos_sigma => 5e-4,
  :ftr_freq_min => 0.2,
  :ftr_freq_max => 5,
  :ftr_freq_step => 25,
  :wfr_threshold => 1e2,
  :wfr_ifading => 0,
  :filter => :ls, #:ls, :wls :rwls #ftr #wfr2 #wfr
  :input_lpf => 'off',
  :ffactor => 1.0,
  :model_scenario => nil,
}

DATA_LABELS = [
    :time, # 時刻
    :vel, # 真値(速度)
    :pos, # 真値(位置)
    :u,   # 入力(加速度) 
    :z,   # 観測量(位置)
    :A_00, :A_01, :B_00, :A_10, :A_11, :B_10 # 係数行列A(状態量側), B(観測量)
    ]

plot_proc = proc{|options, fname_prefix|

# オプションの優先順位は
#   DEFAULT_OPTIONS 
#     < (スクリプト内で指定) 
#     < (スクリプトの引数として指定、ただしそれまでに指定されているものに限る)
options.update(DEFAULT_OPTIONS){|key, self_val, other_val| self_val}
$global_options.each{|k, v|
  options[k] = v if options.include?(k)
}
options.reject!{|k, v| v ? false : true}
[:deltaT, :final_t, 
    :model_k, :model_a, :model_m, :model_F0, :model_omega,
    :omega_sigma, :pos_sigma,
    :ftr_freq_min, :ftr_freq_max,
    :wfr_threshold, :wfr_ifading,
    :ffactor].each{|key|
  options[key] = options[key].to_f
}

# 特殊な入力の処理
input_fetch = nil
if v = options.delete(:input_fetch)
  
  [
      [:modal, 
          proc{|t| 
              v = options[:model_F0] / options[:model_m] \
                  * Math::sin((deg2rad(200) * t / options[:final_t]) * t)
              "#{v} #{v}"
          }], # 角速度を0=>200[deg/s]まで変化させる
      [:modal_with_noise, 
          proc{|t|
              omega0 = (deg2rad(200) * t / options[:final_t]) * t
              omega = omega0 + (deg2rad(5) * rand)
              f0_div_m = options[:model_F0] / options[:model_m]
              v0 = f0_div_m * Math::sin(omega0)
              v = f0_div_m * Math::sin(omega)
              "#{v0} #{v}"
          }], # 角速度を0=>200[deg/s]まで変化させる(ノイズ付)
      [:rand, 
          proc{|t| v = options[:model_F0] / options[:model_m] * 10 * rand; 
              "#{v} #{v}"}],
      ].each{|item, procedure|
    next unless v == item
    input_fetch = procedure
    break
  }
  options[:input_fetch] = '-' if input_fetch
end

# WFRのときは入力を解析する
options[:input_analyze] = :on if :wfr == options[:filter]

options_str = options.to_a.collect{|k, v| (v ? "--#{k}=#{v}" : "")}.join(' ')
cmd_str = "#{EXE_FILE} #{options_str}"

$stderr.puts cmd_str

#break

data = []
IO::popen(cmd_str, "r+"){|io|
  if input_fetch then
    srand(0)
    io.sync = true
    i = 0
    #while !IO::select([io], [], [], 0) # 結果がでてくるまで入力を続ける
    (options[:final_t] / options[:deltaT]).to_i.times{|i|
      io.puts input_fetch.call(options[:deltaT] * i)
      i += 1
    }
    #end
  end
  io.each{|line|
    line.chop!
    data << line.split(/\s*,/).collect{|item| item.to_f} 
  }
}

[:ls, :wls].each{|item| 
  if item == options[:filter] then
    data = [data[-1]]
    break
  end
}

# 入力解析ファイルの待避
if :wfr == options[:filter] then
  FileUtils.mv("input_col_0B.eps", "#{fname_prefix}.input_without_noise.eps")
  FileUtils.mv("input_col_0C.eps", "#{fname_prefix}.input_with_noise.eps")
end

data_trans = data.transpose

# 固有値解析を行う(2次系だから振動減衰解の1組しかでてこない)
require 'gsl'

matrices = []
data.size.times{|i|
  matrices << GSL::Matrix::zeros(2) 
}

[:A_00, :A_01, :A_10, :A_11].each{|item|
  next unless item.to_s =~ /(\d)(\d)/
  i, j = $1.to_i, $2.to_i
  data_trans[DATA_LABELS::index(item)].each_with_index{|v, k|
    matrices[k][i, j] = v
  }
}

ref_mat = GSL::Matrix[
    [-options[:model_a]/options[:model_m], -options[:model_k]/options[:model_m]], 
    [1, 0]]

matrices_eig = matrices.collect{|mat|
  eig_vals, eig_vecs = GSL::Eigen::nonsymmv(mat)
  [eig_vals, eig_vecs]
}

Plotter::plot_basic("#{fname_prefix}.route.eps"){|plot|
  plot.xlabel 'Real'
  plot.ylabel 'Imaginary'
  plot.set('size', "1,0.6")
  plot.set('lmargin', '10')
  plot.set('bmargin', '3')
  plot.set('rmargin', '2')
  plot.set('tmargin', '1')
  plot.set('pointsize', '1')
  plot.set('xzeroaxis')
  plot.set('yzeroaxis')
  plot.set('xrange', $global_options[:xrange]) if $global_options[:xrange]
  plot.set('yrange', $global_options[:yrange]) if $global_options[:yrange]
  
  plot_target = []
  
  [matrices_eig].each_with_index{|mats_analyzed, j|
    x = []
    y = []
    color = []
    
    whole_size = mats_analyzed.size
    mats_analyzed.each_with_index{|val_vecs, i|
      r = 0xFF - (0xFF * (i.to_f / whole_size)).to_i
      g = (0xFF * (i.to_f / whole_size)).to_i
      b = 0
      if j > 0 then
        g, b = b, g # gとbを入れ替える
      end
      temp = []
      val_vecs[0].each{|val|
        temp << [val.real, val.imag, ((r << 16) + (g << 8) + b)]
      }
      _x, _y, _color = temp.transpose 
      x << _x
      y << _y
      color << _color
    }
    
    if color.size == 1 then
      if j > 0 then
        color[0] = [0x0000FF] * color[0].size
      else
        color[0] = [0x00FF00] * color[0].size  
      end
    end
    
    title_prefix = ""
    if data.size > 1 then
      if j == 0 then
        plot_target << Gnuplot::DataSet::new([x[0], y[0], color[0]]){|ds|
          ds.with = "points ps 2 pt 5 lt rgbcolor variable"
          ds.title = "#{title_prefix}Time = #{data.first[DATA_LABELS::index(:time)]} [s]"
        } # 初期値は共通なので一つしか書かない
      end
      if data.size > 2 then
        plot_target << Gnuplot::DataSet::new([ \
            x[1..-1].flatten, y[1..-1].flatten, color[1..-1].flatten]){|ds|
          ds.with = "points lt rgbcolor variable"
          ds.notitle
        }
      end
    end
    plot_target << Gnuplot::DataSet::new([x[-1], y[-1], color[-1]]){|ds|
      ds.with = "points ps 2 pt 7 lt rgbcolor variable"
      # 最終推定が得られる時刻(前者) と 最終推定の時刻(後者)
      last_t = options[:final_t] #data.last[DATA_LABELS::index(:time)]
      ds.title = "#{title_prefix}Time = #{last_t} [s]"
    }
    # 見やすくなるように黒○で囲む
    plot_target << Gnuplot::DataSet::new([x[-1], y[-1]]){|ds|
      ds.with = "points ps 2 pt 65 lt 0"
      ds.notitle
    }
  }

  if ref_mat then
    a_eigen_vals, a_eigen_vecs = GSL::Eigen::nonsymmv(ref_mat)
    ref_x = []
    ref_y = []
    a_eigen_vals.each{|v|
      ref_x << v.real
      ref_y << v.imag
    }
    plot_target << Gnuplot::DataSet::new([ref_x, ref_y]){|ds|
      ds.with = "points ps 2 pt 3 lt 7"
      ds.title = "Reference"
    }
  end
      
  plot.data = plot_target 
}

}

prefix = ''
challenges = {}

try = proc{|selector|
  if selector.size == challenges.size then
    options = {}
    fname = []
    selector.each{|i, j|
      options[challenges[i][0]] = challenges[i][1][j]
      fname << "#{challenges[i][0].to_s[0].chr}#{j}"
    }
    fname_prefix = "#{prefix}#{fname.join('_')}"
    $stderr.puts options.inspect
    $stderr.puts fname_prefix
    plot_proc.call(options, fname_prefix)
  else
    challenges[selector.size][1].size.times{|i|
      try.call([[selector.size, i]] + selector)
    }
  end
}


=begin
prefix = 'test_'
challenges = {
  :filter => [:ls],
  :pos_sigma => [1e-4],
  :omega_sigma => [deg2rad(0)],
  :model_omega => [deg2rad(100)],
}.to_a
try.call([]) # 再帰的にオプションを足していく
exit
=end

# 組み込みのサイン波によるシミュレーション
prefix = 'builtin_'
challenges = {
  :filter => [:rwls, :ftr, :wfr],
  :pos_sigma => [1e-4, 1e-2, 1e-0],
  :omega_sigma => [deg2rad(0), deg2rad(5)],
  :model_omega => [deg2rad(100), deg2rad(50), deg2rad(200)], # ちなみにデフォルトだと固有振動数は1.714[rad] => 98[deg]
}.to_a
try.call([]) # 再帰的にオプションを足していく

# その他の試験方法
prefix = 'input_mod_'
challenges = {
  :filter => [:rwls, :ftr, :wfr],
  :pos_sigma => [1e-4, 1e-2, 1e-0],
  :input_fetch => [:modal, :modal_with_noise, :rand],
}.to_a
try.call([]) # 再帰的にオプションを足していく