#!/usr/bin/env ruby

HELP = '--help'

$stderr.puts "使い方: #{__FILE__} raw_data analyzed_data [--options=value]"
$stderr.puts "オプション一覧: #{__FILE__} #{HELP}"

wind_correction_mode = 'hino_0'
wind_parameter_file = nil
wind_t_avgstart = 276540
wind_t_avgfin = 276640
imu_type = 'IMU_E0'
imu_options = '--rotate=+y-x+z'
tool_dir = File::join([File::dirname(__FILE__), "..", "070423"])
correct_inputs_overflow = true

def set_option(option_str)
  if option_str =~ /^--([^=]+)=/ and (target = OPTIONS[$1]) then
    $stderr.puts "#{$1} => #{$'}"
    target[2].call($') if target[2]
  end
end

OPTIONS = {
  'config_file' =>
    ['設定ファイル', '(none)', Proc::new{|file| open(file).each{|line| set_option(line)}}],
  'wind_correction_mode' => 
    ['風データ校正モード', wind_correction_mode, Proc::new{|mode| wind_correction_mode = mode}],
  'wind_parameter_file' => 
    ['風データ校正ファイル', '(default)', Proc::new{|fname| wind_parameter_file = fname}],
  'wind_avgstart' => 
    ['圧力センサオフセット取得開始時刻', wind_t_avgstart, Proc::new{|t| wind_t_avgstart = t.to_f}],
  'wind_avgfin' => 
    ['圧力センサオフセット取得終了時刻', wind_t_avgfin, Proc::new{|t| wind_t_avgfin = t.to_f}],
  'IMU_type' => 
    ['IMUのタイプ', imu_type, Proc::new{|type| imu_type = type}],
  'IMU_options' => 
    ['IMUの軸設定など', "\"#{imu_options}\"", Proc::new{|options| imu_options = options}],
  'tool_dir' =>
    ['解析用ツールが格納されているディレクトリ', tool_dir, Proc::new{|dir| tool_dir = dir}],
  'correct_inputs_overflow' =>
    ['入力データの異常値をオーバーフローとみなし修正する', correct_inputs_overflow, Proc::new{|val| correct_inputs_overflow = eval(val)}],
}

if ARGV.include?(HELP) then
  OPTIONS.each{|name, arg|
    $stderr.print "--#{name}=#{arg[0]}"
    $stderr.print " 初期設定: #{arg[1]}" if arg[1]
    $stderr.puts
  }
  exit
end

if ARGV.size < 2 then exit end

ARGV[2..-1].each{|option| set_option(option)}

$stderr.puts "raw_data => #{ARGV[0]}"
$stderr.puts "analyzed_data => #{ARGV[1]}"

$: << tool_dir

require 'sylph_processed_reader'
require 'swig/build_SWIG/SylphideProcessor.so'

$: << File::join([File::dirname(__FILE__), "hino_nakagawa", "swig", "build_SWIG"])
require 'Pitot'

raw_reader = SylphideProcessor::SylphideLog::new(1024)
analyzed_reader = SylphReader

TEMP_DIR = "temp_#{File.basename(ARGV[0])}"
Dir::mkdir(TEMP_DIR) unless FileTest.directory?(TEMP_DIR)

class Array
  # 再帰的map
  def rmap(&block)
    return self.map{|x| x.kind_of?(Array) ? x.rmap(&block) : block.call(x)}
  end
end

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

=begin
1) 生データから抽出するもの
GPS時刻、風データ(H君のプログラムを参考に組み入れる)、制御データ
2) 処理済みデータから抽出するもの
GPS時刻、状態量(位置、速度、姿勢、センサバイアス)

いずれも時刻情報がおかしい場合は強制修正する
=end

control_data = []
sensor_data = []
wind_data = []

TEMP_RAW_DATA_FILE = File::join(TEMP_DIR, "temp_raw.dat")
begin
open(TEMP_RAW_DATA_FILE, 'r'){|io|
  control_data, sensor_data, wind_data = Marshal::load(io.read)
}
rescue
job = Proc::new{|io|
  gps_available = false
  while !io.eof?
    raw_reader.process(io.read(32)){|obs|
      t = obs.fetch_ITOW
      store_data = [t]
      case obs
      when SylphideProcessor::APacketObserver
        v = obs.values
        sensor_data << [t, v.values.to_a, v.temperature] if gps_available
      when SylphideProcessor::FPacketObserver
        v = obs.values
        inputs = v.servo_in.to_a
        outputs = v.servo_out.to_a
        if correct_inputs_overflow then
          inputs.collect!{|item| (item < 200) ? (item + 4096) : item}
        end
        control_data << [t, inputs, outputs] if gps_available
      when SylphideProcessor::PPacketObserver
        v = obs.values
        speed = v.air_speed.to_a
        alpha = v.air_alpha.to_a
        beta = v.air_beta.to_a
        case wind_correction_mode
        when 'hino_1', 'hino_2', 'hino_3'   # 8bytes/1packet
          if gps_available then
            # 符号付に変換
            speed = speed.pack("S*").unpack("s*")
            alpha = alpha.pack("S*").unpack("s*")
            beta = beta.pack("S*").unpack("s*")
            wind_data << [t - 0.04, speed[0], alpha[0],  beta[0], speed[1]]
            wind_data << [t - 0.02, alpha[1],  beta[1], speed[2], alpha[2]]
            wind_data << [t,         beta[2], speed[3], alpha[3],  beta[3]]
          end
        else            # 6bytes/1packet
          t -= 0.06
          4.times{|i|
            wind_data << [t, speed[i], alpha[i], beta[i]] if gps_available
            t += 0.02
          }
        end
      when SylphideProcessor::GPacketObserver
        if !gps_available and (obs.ubx_class == 0x02) and (obs.ubx_id == 0x10) then
          gps_available = (obs.num_of_sv >= 4)
        end
      end
    }
  end
}
if ARGV[0] == '-' then
  job.call($stdin)
else
  open(ARGV[0]){|io| job.call(io)}
end

# 時刻チェック、後ろからチェックすることにする
def mod_time(data, mod_interval)
  mod_data = []
  data.reverse.each_with_index{|item, i|
    if i > 0 then
      if (mod_data.first[0] <= item[0]) \
          or ((mod_data.first[0] - item[0]) > 10.0 \
          and item[0] < 500) then
        # 未来がでてきたり、電源ON時はシフトする
        item[0] = mod_data.first[0] - mod_interval
      end
    end
    mod_data.unshift(item)
  }
  return mod_data
end

sensor_data  = mod_time(sensor_data, 0.01)   # 100Hz
control_data = mod_time(control_data, 0.02)  # 50Hz
wind_data    = mod_time(wind_data, 0.02)     # 50Hz

open(TEMP_RAW_DATA_FILE, 'w'){|io|
  io << Marshal::dump([control_data, sensor_data, wind_data])
}
end

# 風データの処理(単純に移植)
case wind_correction_mode
when 'hino_0'
  parameter_file = wind_parameter_file \
      || File.join(File.dirname(File.expand_path(__FILE__)), 'hino_nakagawa', 'parameter.txt')
  $stderr.puts "Wind Parameter File => #{parameter_file}"
  
  # パラメータを読み取る
  parameter = (open(parameter_file).readlines)[0...9].collect!{|item| item =~ /[\d.]+/ ? $&.to_f : 0.0}
  $stderr.puts "Parameter #{parameter.inspect}"
  
  # オフセットの設定(時刻で指定)
  t_avg_range = (wind_t_avgstart..wind_t_avgfin)
  
  # LPFをかける関数
  def dlpf_9_1(offset, new_data)
    # 新オフセット = (旧オフセット * 9 + 新測定値 )/10
    data = new_data.clone
    offset.collect!{|item|
      item *= 9
      item += data.shift
      item /= 10
    }
  end
  
  # 生データからオフセット取得
  offset = [0.0, 0.0, 0.0]
  offset_setter = Proc::new{|_offset, _new_data| dlpf_9_1(_offset, _new_data)}
  wind_data.each{|t, *raw_data|
    offset_setter.call(offset, raw_data) if t_avg_range.include?(t)
  }
  
  # オフセット値を表示
  $stderr.puts "ADS Offset => #{offset.inspect}"
  
  # データを変換
  def conv_3raw(offset, raw_data, parameter)
    pressure = []
    raw_data.each_with_index{|item, i|
      #puts "#{item} #{offset[i]} #{parameter[7]} #{parameter[4+i]}"
      pressure << ((item - offset[i]) * parameter[7] / (parameter[4+i] * 16));
      #p pressure.last
    }
    converted = [0.0, 0.0, 0.0]
    unless pressure[1] < 0 then
      converted[0] = Math::sqrt(pressure[1] * 2 / (parameter[0] * parameter[1] + parameter[2] * 2))
    end
    
    unless converted[0] < 5.0 then # MIN_AIRSPEED = 5.0
      converted[1] = deg2rad((pressure[0] - pressure[2]) / pressure[1] / parameter[3])
    end
    return converted
  end
  
  convert_data = Proc::new{|_t, _raw_data, _offset, _parameter|
    conv_3raw(_offset, _raw_data, _parameter) 
  }
  wind_data.collect!{|t, *raw_data|
    [t - parameter[8], convert_data.call(t, raw_data, offset, parameter)]
  }
when 'hino_1'
  # 異常値の修正
  wind_data_mod = []
  wind_data.each{|t, *raw_data|
    i = 0
    raw_data = raw_data.collect{|item|
      i += 1
      j = 1
      if item.abs > 10000 then
        #p "#{t}, #{item}"
        left = wind_data_mod.last[i] rescue 0
        right = left
        j_limit = wind_data.size - wind_data_mod.size
        while (j < j_limit) \
            && ((right = wind_data[wind_data_mod.size + j][i]).abs > 10000)
          j += 1
        end
        item = (left * j + right) / (j + 1)
        #p "#{t}, #{item}, #{left}, #{right}"
      end
      item
    }
    wind_data_mod << [t, *raw_data]
  }
  wind_data = wind_data_mod
  
  parameter_file = wind_parameter_file \
      || File.join(File.dirname(File.expand_path(__FILE__)), 'hino_nakagawa', 'parameter2.txt')
  $stderr.puts "Wind Parameter File => #{parameter_file}"
  
  # パラメータを読み取る
  parameter = (open(parameter_file).readlines)[0...9].collect!{|item| item =~ /[\d.]+/ ? $&.to_f : 0.0}
  $stderr.puts "Parameter #{parameter.inspect}"

  # オフセットの設定(時刻で指定)
  t_avg_range = (wind_t_avgstart..wind_t_avgfin)
  
  # 生データからオフセット取得
  offset = [0.0, 0.0, 0.0, 0.0]
  samples = 0
  wind_data.each{|t, *raw_data|
    if t_avg_range.include?(t) then
      samples += 1
      offset.size.times{|i|
        offset[i] += raw_data[i]
      }
    end
  }
  offset.collect!{|item| item / samples}
  
  # オフセット値を表示
  $stderr.puts "ADS Offset => #{offset.inspect}, #{samples} samples."
  
  # データを変換
  def conv_3raw(offset, raw_data, parameter)
    pressure = []
    raw_data.each_with_index{|item, i|
      pressure << ((item - offset[i]) * parameter[7]);
    }
    converted = [0.0, 0.0, 0.0, 0.0]
    unless pressure[0] < 0 then
      converted[0] = Math::sqrt(pressure[0] * 2 / (parameter[0] * (parameter[1] + parameter[2])))
    end
    unless converted[0] < 5.0 then # MIN_AIRSPEED = 5.0
      converted[1] = deg2rad(pressure[1] / pressure[0] / parameter[3])
    end
    unless pressure[2] < 0 then
      converted[2] = Math::sqrt(pressure[2] * 2 / (parameter[0] * (parameter[4] + parameter[5])))
    end
    unless converted[2] < 5.0 then # MIN_AIRSPEED = 5.0
      converted[3] = deg2rad(pressure[3] / pressure[2] / parameter[6])
    end
    return [(converted[0] + converted[2]) / 2, -converted[3], converted[1]] # (aspd, alpha, beta)
  end
  
  convert_data = Proc::new{|_t, _raw_data, _offset, _parameter|
    conv_3raw(_offset, _raw_data, _parameter) 
  }
  wind_data.collect!{|t, *raw_data|
    [t - parameter[8], convert_data.call(t, raw_data, offset, parameter)]
  }
when 'hino_2'
  pitot3 = Pitot::Pitot3::new
  config_fname = wind_parameter_file \
      || File.join(File::dirname(__FILE__), "hino_nakagawa", "parameter3.txt")
  open(config_fname){|io| pitot3.read_config(io)}
  pitot3.offset_calc_start = wind_t_avgstart
  pitot3.t_start = wind_t_avgfin
  
  wind_data_res = []
  input = Pitot::Pitot3_input::new
  output = Pitot::Pitot3_output::new
  wind_data.each{|t, *v|
    input.time = t
    input.set_raw(*v)
    if pitot3.analyze_1step(input, output) then
      wind_data_res << \
        [output.time, output.aspd, \
            deg2rad(output.angle1), deg2rad(output.angle2)]  
    end 
  }
  
  wind_data = wind_data_res
when 'hino_3'
  wind_data_res = []
  wind_data.each{|t, *v|
    wind_data_res << \
        [t, v[0].to_f / 10, \
            deg2rad(v[1].to_f / 10), deg2rad(v[2].to_f / 10)]     
  }
  wind_data = wind_data_res
end
open(File::join(TEMP_DIR, 'wind_corrected.csv'), 'w'){|io|
  wind_data.each{|item| io.puts item.join(',')}
}

# 解析済みデータの読み込み
ANALYZED_LABELS = SylphReader::SYLPH_LABELS
TEMP_ANALYZED_DATA_FILE = File::join(TEMP_DIR, "temp_analyzed.dat")
analyzed_data = []
begin
open(TEMP_ANALYZED_DATA_FILE, 'r'){|io|
  analyzed_data = Marshal::load(io.read)
}
rescue
analyzed_data = analyzed_reader::read(\
    (ARGV[1] == '-' ? $stdin : ARGV[1]), {:denom => 1}) # -だったら標準入力から
analyzed_data2 = []

euler_indexes = ['Yaw', 'Pitch', 'Roll', 'Azimuth'].collect{|euler| ANALYZED_LABELS::index(euler) - 2}
while !analyzed_data.empty?
  mode, t, *data = analyzed_data.shift
  next_mode, next_t, *next_data = analyzed_data.first
  # modeでTUとMUの両方がある場合は後からの方(MU)を優先
  if t == next_t then next end
  # オイラー角をラジアンに
  euler_indexes.each{|i| data[i] = deg2rad(data[i])}
  analyzed_data2 << [mode, 
      (t.to_f / 1000), # msec => sec
      *data]
end
analyzed_data = analyzed_data2

open(TEMP_ANALYZED_DATA_FILE, 'w'){|io|
  io << Marshal::dump(analyzed_data)
}
end

# 計測用データの吐き出し
=begin
1) 時間範囲は解析済みデータの範囲にあわせる
2) 時刻の刻みについては周波数の低いcontrolあるいはwindにあわせる
=end

def interporate(top, bottom, t)
  top_t, *top_data = top
  bottom_t, *bottom_data = bottom
  coef_a = (bottom_t - t) / (bottom_t - top_t)
  coef_b = 1.0 - coef_a
  #p top_data, bottom_data
  top_data = top_data.rmap{|item| item * coef_a}.flatten
  i = 0
  new_data = bottom_data.rmap{|item| a = top_data[i]; i+= 1; item * coef_b + a}
  #p new_data
  return [t, *new_data]
end

valid_time_range = (analyzed_data.first[1]..analyzed_data.last[1])
# valid_time_rangeの1個前と後は使う
[control_data, sensor_data, wind_data].each{|item|
  previous_data1 = previous_data2 = nil
  next_data1 = next_data2 = nil
  on_range = false
  item.delete_if{|t, *data| 
    if valid_time_range.include?(t) then
      previous_data2 = previous_data1 if (!on_range && (valid_time_range.first != t))
      on_range = true
      next_data1 = (valid_time_range.last != t)
      false
    elsif on_range then
      #p valid_time_range.last, t, next_data1
      next_data2 = [t, *data] if next_data1
      on_range = false
      true
    else
      previous_data1 = [t, *data]
      true
    end
  }
  
  # 先頭と終端はピッタリあわせる
  
  # 先頭側の処理
  if previous_data2 then
    item.unshift(interporate(previous_data2, item.first, valid_time_range.first))
  end
  
  # 終端側の処理
  if next_data2 then
    item.push(interporate(item.last, next_data2, valid_time_range.last))
  end
}
$stderr.puts "Range: #{valid_time_range.inspect}"
$stderr.puts "C: #{control_data.size}, #{((control_data.first[0])..(control_data.last[0])).inspect unless control_data.empty?}"
$stderr.puts "S: #{sensor_data.size}, #{((sensor_data.first[0])..(sensor_data.last[0])).inspect unless sensor_data.empty?}"
$stderr.puts "W: #{wind_data.size}, #{((wind_data.first[0])..(wind_data.last[0])).inspect unless wind_data.empty?}"

# センサー生データを加速度、角速度(機体固定軸)に加工
log2inertial_force = "#{tool_dir}/build_VC9/log_AD_calib_CSV.exe"
IO::popen("#{log2inertial_force} #{imu_type} - #{imu_options}", 'r+'){|io|
  #[t, v.values.to_a, v.temperature]
  header = ["\0"] * 5
  header.unshift('A')
  io.print [header, \
        ["\0"] * (32 - header.size)].flatten.join
  sensor_data.map!{|t, ch_data, temperature|
    io.print [header, \
        ch_data.collect{|v| [v.to_i].pack('N')[1..3]}, \
        [temperature.to_i].pack('v')].flatten.join
    data = io.readline.split(/,/).collect!{|item| item.to_f}
    data.shift
    data[0] = t 
    data  # [t, accel(3), angular_v(3), temp]
  }
}

# モードの除去
ANALYZED_LABELS.shift
analyzed_data.collect!{|mode, t, *data| [t, *data]}

[analyzed_data, control_data, sensor_data, wind_data].each{|data|
  if data.empty? then next end
  $stderr.print "#{data.size}:"
  data[0..2].each{|item| $stderr.print " #{item[0]}"}
  $stderr.print ";"
  data[-3..-1].each{|item| $stderr.print " #{item[0]}"}
  $stderr.puts
}

# 最小のデータ数のものにデータをあわせる
min_size_data, *other_data = [analyzed_data, control_data, sensor_data, wind_data].sort{|a, b| a.size <=> b.size}

# データ数が0であった場合は無視
while min_size_data && min_size_data.empty?
  min_size_data = other_data.shift
end
if other_data.empty? then
  raise Exception::new("データ数が足りません orz")  
end

other_data.each{|item|
  t_s = min_size_data.collect{|t, *data| t}
  t_s.shift
  item2 = [item.shift]
  previous = item2.first
  while !t_s.empty?
    t_data = item.first
    t, *data = t_data
    if t < t_s.first
      previous = item.shift
      next
    end
    item2 << interporate(previous, t_data, t_s.shift)
  end
  item.replace(item2)
}
$stderr.puts "analyzed: #{analyzed_data.size}"
$stderr.puts "C: #{control_data.size}"
$stderr.puts "S: #{sensor_data.size}"
$stderr.puts "W: #{wind_data.size}"

[analyzed_data, control_data, sensor_data, wind_data].each{|data|
  if data.empty? then next end
  $stderr.print "#{data.size}:"
  data[0..2].each{|item| $stderr.print " #{item[0]}"}
  $stderr.print ";"
  data[-3..-1].each{|item| $stderr.print " #{item[0]}"}
  $stderr.puts
}

=begin
#時刻確認用出力
open('time.csv', 'w'){|io|
  [analyzed_data, control_data, sensor_data, wind_data].sort{|a, b| a.size <=> b.size}.first.size.times{|i|
    [analyzed_data, control_data, sensor_data, wind_data].each{|data|
      io.print "#{data[i][0]},"
    }
    io.puts
  }
}
=end

# センサから得られた加速度、角速度の加工
sensor_data.each_with_index{|item, i|
  # バイアスデータがある場合は修正する
  if analyzed_data[i].size > ANALYZED_LABELS.size then
    bias_data = [0, analyzed_data[i][-6..-1], 0].flatten
    # [t, accel(3), angular_v(3), temp]
    item.map!{|v| v += bias_data.shift}
  end
}
previous_t = sensor_data.first[0] - 1
previous = sensor_data.first[-4..-2]
sensor_data.map!{|item|
  # 温度の除去
  item.pop
  # 角加速度を求める
  delta_t = item[0] - previous_t
  old_previous = previous
  (previous = item[-3..-1]).each_with_index{|a_speed, i|
    item << (a_speed - old_previous[i]) / delta_t
  }
  previous_t = item[0]
  item
}

# 後は書き出しのみ
=begin
項目は
1) GPS時刻
2) 状態量(位置(long,lat,height)、速度(N,E,D)、姿勢(Y,P,R,Z))
3) 風データ(大気速度、迎角、横滑り角)
4) 制御データ(入力8、出力8)
5) 微分情報(加速度3軸(X,Y,Z)、角速度3軸(X,Y,Z)、角加速度3軸(X,Y,Z))
角度は位置以外すべてラジアン
=end

begin
  analyzed_data.size.times{|i|
    puts [analyzed_data[i][0],
        analyzed_data[i][1...ANALYZED_LABELS.size],
        (wind_data[i] || Array::new(4, 0))[1..-1],
        (control_data[i] || Array::new(17, 0))[1..-1],
        sensor_data[i][1..-1]].flatten.join(',')
    
  }
rescue Errno::EPIPE
  # パイプを使っていて、途中で切られた場合
end