#!/usr/bin/env ruby

=begin
共分散をプロットするツール
=end

$: << '~/src/eclipse/autopilot/common/param/swig/build_SWIG'
$: << '~/src/eclipse/autopilot/common/ruby'
$: << File::dirname(__FILE__)

require 'gnuplot_support'
require 'flight_log_reader.rb'
require 'stringio'

$stderr.puts "#{$0} target.est.csv [--mode=lng,lat]"

options = {
    :mode => :lng,
    :size => "1,0.6",
    :time_shift => 0,
    :skip => [],
    :sta => {:G0 => 9.81, :U0 => 20, :W0 => 0, :theta0 => 0},
    :ref => {},
    :route_xrange => nil,
    :route_yrange => nil,
    :onlyend => false,
    }
    
ARGV.reject!{|arg|
  hit = true
  case arg 
  when /^--mode=/ then
    options[:mode] = $'.to_sym
  when /^--(tshift)=/ then
    options[$1.to_sym] = $'.to_f
  when /^--skip=/ then
    options[:skip] << $'.to_sym
  when /^--trange=/ then
    obj = eval($')
    options[:time_range] = obj if obj.kind_of? Range
  when /^--prefix=/ then
    options[:fname_base] = $'
  when /^--sta=/ then
    k, v = $'.split(/,/)
    options[:sta][k.to_sym] = v.to_f
  when /^--ref=/ then
    k, v = $'.split(/,/)
    options[:ref][k.to_sym] = v.to_f
  when /^--(onlyend)=/ then
    options[$1.to_sym] = $'.to_f
  when /^--verbose=/ then
    Gnuplot.verbose if $' == 'on'
  when /^--([^=]+)=/ then
    options[$1.to_sym] = $'
  else
    hit = false
  end
  hit
}

$stderr.puts "opt(plot_est): " + options.inspect

if ARGV.empty? then
  raise "No file specified!!"
end

est_files = ARGV # 残りは全てファイルだと思って読み込む
est_file = est_files[0]
$stderr.puts "Target: #{est_file}"
options[:fname_base] ||= File::basename(est_file, ".*")

VALUES = ((options[:mode] == :lng) \
    ? [:u, :a, :theta, :q,
        [:Xu, "X_{u}"], [:Xa, "X_{/Symbol a}"], [:Xdt, "X_{{/Symbol d}_{t}}"], \
        [:Zu, "Z_{u}"], [:Za, "Z_{/Symbol a}"], [:Zq, "Z_{q}"], [:Zde, "Z_{{/Symbol d}_{e}}"], [:Zdt, "Z_{{/Symbol d}_{t}}"], 
        [:Mu, "M_{u}"], [:Maa, "M_{{/Symbol a} {/Symbol a}}"], [:Ma, "M_{/Symbol a}"], [:Mq, "M_{q}"], [:Mde, "M_{{/Symbol d}_{e}}"], [:Mdt, "M_{{/Symbol d}_{t}}"]] \
    : [:b, :p, :r, :phi, :psi, 
        [:Yb, "Y_{/Symbol b}"], [:Yp, "Y_{p}"], [:Yr, "Y_{r}"], [:Ydr, "Y_{{/Symbol d}_{r}}"], \
        [:Lbp, "L_{/Symbol b}"], [:Lpp, "L_{p}"], [:Lrp, "L_{r}"], [:Ldap, "L_{{/Symbol d}_{a}}"], [:Ldrp, "L_{{/Symbol d}_{r}}"], \
        [:Nbp, "N_{/Symbol b}"], [:Npp, "N_{p}"], [:Nrp, "N_{r}"], [:Ndap, "L_{{/Symbol d}_{a}}"], [:Ndrp, "L_{{/Symbol d}_{r}}"]])
VALUES_SYMBOL_LIST = VALUES.collect{|item| sym, label = item; sym}

options[:skip] = options[:skip] \
    + ((options[:mode] == :lng) \
        ? [:u, :a, :theta, :q] \
        : [:b, :p, :r, :phi, :psi])

est_datas = est_files.collect{|f|
  data = []
  # ラベル付ファイルが可能
  fname = (f =~ /,/ ? $` : f)
  if fname == '-' then
    $stdin.each{|line|
      break if line =~ /^e/ # gnuplotと同様の仕様、eで次のファイルとする
      data << line.chomp.split(/,/).collect{|item| item.to_f}
    }
  else
    open(fname){|io|
      io.each{|line|
        data << line.chomp.split(/,/).collect{|item| item.to_f}
      }
    }
  end
  data
}

if time_range = options[:time_range] then
  est_datas.each{|data|
    data.reject!{|item| !time_range.include?(item[0])}
  }
end

est_datas.collect!{|data| data.transpose}

# 最初のものについて、時系列グラフを書く
est_data = est_datas[0]
time_series = est_data[0]

if time_series and time_series.size > 1 and (time_series.last - time_series.first != 0) then
figures = {}
[
    [est_data[1..VALUES.size].collect{|item| item.collect{|v| v > 1E-3 ? v : 1E-3}}, 
        'raw', # 正の側
        proc{|plot| 
            plot.yrange '[0.005:1000]'
            wsize = 1.0
            hsize = 0.5
            if options[:size] =~ /,/ then
              hsize = $'.to_f / 2
              wsize = $`
            end
            plot.set('size', "#{wsize},#{hsize}")
            plot.set('tmargin', '1')
            plot.set('bmargin', '0')
            plot.set('origin', "0,#{hsize}")
            plot.set('format', 'x ""')
            plot.set('format y "%g"')
            plot.set('key top horizontal samplen 3')
            plot.label '"Stability Derivatives" at graph -0.12, graph 0 center rotate'
        }],
    [est_data[1..VALUES.size].collect{|item| item.collect{|v| v > 0 ? 1E-3 : (v * -1)}}, 
        'raw', # 負の側 
        proc{|plot| 
            plot.yrange '[1000:0.005]'
            wsize = 1.0
            hsize = 0.5
            if options[:size] =~ /,/ then
              hsize = $'.to_f / 2
              wsize = $`
            end
            plot.set('size', "#{wsize},#{hsize}")
            plot.set('tmargin', '0')
            plot.set('bmargin', '2')
            plot.set('origin', "0,0")
            plot.set('format y "-%g"')
            plot.set('nokey')
        }],
    [est_data[(1 + VALUES.size)..-1], 'cov', 
        proc{|plot|
            plot.ylabel 'Variance'
            plot.set('bmargin', '2')
            plot.set('tmargin', '1')
        }]
    ].each{|data, suffix, mod|
  cmd = ""
  Plotter::plot(cmd){|plot|
    plot.noxlabel
    plot.set('logscale', 'y')
    plot.set('lmargin', '10')
    plot.set('rmargin', '2')
    plot.set('key spacing 1.2')
    mod.call(plot)

    time_shift = options[:time_shift]
    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
    
    temp = []
    
    count = 0
    VALUES.each_with_index{|item, i|
      name, label = item
      next if options[:skip].include?(name)
      label = item.to_s unless label
      temp << Gnuplot::DataSet::new([
          time_series.collect{|v| sprintf("%3.2f", v  - time_shift)}, 
          data[i].collect{|v| sprintf("%3.3f", v)}]){|ds|
        with = "lines lw 2"
        with += " lc 9" if count == 5 # 黄色を回避
        if count > 8
          with += " lt #{(count + 1) % 8}"
          with += " lc #{count % 8}"
        end
        ds.with = with
        ds.title = label
      }
      count += 1
    }
    
    plot.data = temp
  }
  
  figures["#{options[:fname_base]}.#{suffix}.eps"] ||= []
  figures["#{options[:fname_base]}.#{suffix}.eps"] << cmd
}

figures.each{|eps, cmds|
  Gnuplot.open{|gp|
    #Plotter::plot(gp){|plot| # for X11
    Plotter::plot_eps(eps, gp){|plot| # for eps
      plot.set('size', options[:size])
      plot.set('multiplot')
    }
    
    cmds.each{|cmd|
      gp << cmd << "\n"
    }
    
    Plotter::plot(gp){|plot|
      plot.set('nomultiplot')
    }
  }
}

end

# ここから固有値解析
require 'gsl'

g = options[:sta][:G0]
u0 = options[:sta][:U0]
w0 = options[:sta][:W0]
theta0 = options[:sta][:theta0]

TARGET_VALUES_OP = \
    (options[:mode] == :lng) \
    ? [ \
        proc{|mat| 
            mat[0, 2] = -w0
            mat[0, 3] = -g * Math::cos(theta0)
            mat[1, 3] = -g / u0 * Math::sin(theta0)
            mat[3, 2] = 1},
        [:Xu, proc{|mat, v| mat[0, 0] = v}],
        [:Xa, proc{|mat, v| mat[0, 1] = v}],
        [:Zu, proc{|mat, v| mat[1, 0] = v / u0}],
        [:Za, proc{|mat, v| mat[1, 1] = v / u0}],
        [:Zq, proc{|mat, v| mat[1, 2] = 1.0 + (v / u0)}],
        [:Mu, proc{|mat, v| mat[2, 0] = v}],
        [:Ma, proc{|mat, v| mat[2, 1] = v}],
        [:Mq, proc{|mat, v| mat[2, 2] = v}]] \
    : [ \
        proc{|mat| 
            mat[0, 3] = -g / u0 * Math::cos(theta0)
            mat[3, 1] = 1
            mat[3, 2] = Math::tan(theta0)},
        [:Yb, proc{|mat, v| mat[0, 0] = v / u0}],
        [:Yp, proc{|mat, v| mat[0, 1] = (w0 + v) / u0}],
        [:Yr, proc{|mat, v| mat[0, 2] = -(u0 - v) / u0}],
        [:Lbp, proc{|mat, v| mat[1, 0] = v}],
        [:Lpp, proc{|mat, v| mat[1, 1] = v}],
        [:Lrp, proc{|mat, v| mat[1, 2] = v}],
        [:Nbp, proc{|mat, v| mat[2, 0] = v}],
        [:Npp, proc{|mat, v| mat[2, 1] = v}],
        [:Nrp, proc{|mat, v| mat[2, 2] = v}]]

ref_mat = nil
unless options[:ref].empty? then
  ref_mat = GSL::Matrix::zeros(4)
  TARGET_VALUES_OP[0].call(ref_mat)
  TARGET_VALUES_OP[1..-1].each{|item, op|
    op.call(ref_mat, options[:ref][item]) rescue nil
  }
end

analyzed_datas = est_datas.collect{|data|
  mats = []
  data[0].size.times{|i|
    mats << GSL::Matrix::zeros(4)
  } if data[0]
  
  mats.each{|mat|
    TARGET_VALUES_OP[0].call(mat)
  }
  TARGET_VALUES_OP[1..-1].each{|item, op|
    index = VALUES_SYMBOL_LIST.index(item)
    next unless index
    data[1 + index].zip(mats).each{|coef, mat|
      op.call(mat, coef)
    }
  } if data[0]
  mats_analyzed = []
  mats.each_with_index{|mat, i|
    a_eigen_vals, a_eigen_vecs = GSL::Eigen::nonsymmv(mat)
    mats_analyzed << [a_eigen_vals, a_eigen_vecs]
  }
  $stderr.puts "Matrices: #{mats_analyzed.size}"
  mats_analyzed
}

Plotter::plot_basic("#{options[:fname_base]}.route.eps"){|plot|
  plot.xlabel 'Real'
  plot.ylabel 'Imaginary'
  plot.set('size', options[:size])
  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', options[:route_xrange]) if options[:route_xrange]
  plot.set('yrange', options[:route_yrange]) if options[:route_yrange]
  
  plot_target = []
  
  analyzed_datas.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 = ""
    nobeginend = false
    if est_files[j] =~ /,/ then
      if $' == 'nobeginend' then
        nobeginend = true
      else
        title_prefix = $' + ' '
      end
    end
    
    if nobeginend then
      plot_target << Gnuplot::DataSet::new([ \
          x.flatten, y.flatten, color.flatten]){|ds|
        ds.with = "points ps 1 pt 7 lt rgbcolor variable"
        ds.notitle
      }
      next
    end
    
    if time_series and time_series.size > 1 and !options[:onlyend] 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}GPS Time = #{time_series[0]} [s]"
        } # 初期値は共通なので一つしか書かない
      end
      if time_series.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"
      if options[:onlyend] then
        ds.title = "#{title_prefix}"
      else
        ds.title = "#{title_prefix}GPS Time = #{time_series[-1]} [s]"  
      end
    }
  }
  
  if ref_mat then
    begin
      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
      }
      p ref_mat
      plot_target << Gnuplot::DataSet::new([ref_x, ref_y]){|ds|
            ds.with = "points ps 2 pt 3 lt 7"
            ds.title = "Reference"
          }
    rescue
      $stderr.puts $@
      $stderr.puts $!
    end
  end
      
  plot.data = plot_target 
}