#!/usr/bin/env ruby

class SylphNReader

  PAGE_SIZE = 32
  SYLPH_N_PAGE_LABELS = [
  		'ITOW',
  		'longitude',
  		'latitude',
  		'height',
  		'v_north',
  		'v_east',
  		'v_down',
  		'Heading',
  		'Pitch',
  		'Roll'
  	]

=begin
=== N0ページ仕様(32bytes) ===
 Header (4byte)
   'N' (1byte)
   シーケンスNo (1byte)
   Nページの種類 = 0 (1byte)
   拡張 (1byte)
 ITOW (4bytes) [msec]
 位置 (12bytes)
   緯度 (4bytes -2147483648 〜 2147483647)
     -90.0000000 〜 90.0000000 [deg] * 10000000
   経度 (4bytes -2147483648 〜 2147483647)
     -180.0000000 〜 180.0000000 [deg] * 10000000
   高度 (4bytes -2147483648 〜 2147483647)
     -214748.3648 〜 214748.3647 [m] * 10000
 速度 (6bytes)
   NED (2bytes -32768 〜 32767)
     -327.68 〜 327.68 [m/s] * 100
 姿勢 (6ytes)
   ヘディング (2bytes -32768 〜 32767)
     -180.00 〜 180.00 [deg] * 100
   ロール、ピッチ (2bytes -32768 〜 32767)
     -90.00 〜 90.00 [deg] * 100
=end
  def SylphNReader.io_read(io, options = {})
    denom = (options[:denom].to_i || 10)
    target = (options[:target][0] || 0)
    data = []
    itow_ms = 0
    
    while !(io.eof?)
      page = io.read(PAGE_SIZE)
      next unless page[0] == ?N
      itow_ms = page[4..7].unpack("L")[0] if page[2] == 0
      next unless page[2] == target
     
      case target
      when 0
        decoded = []
        decoded << itow_ms.to_f / 1000
        decoded << page[8..15].unpack("ll").collect{|v| v.to_f / 10000000}
        decoded << page[16..19].unpack("l").collect{|v| v.to_f / 10000}
        decoded << page[20..25].unpack("sss").collect{|v| v.to_f / 100}
        decoded << page[26..31].unpack("sss").collect{|v| v.to_f / 100}
        data << decoded.flatten!
      when ?1
=begin
FHI記録吸出し用
['N', seq_num, '1', [0..4], float * 7]
=end
        if page[3] == 0 then
          if (data.size > 1) && (data[-1].size != data[0].size) then
            data.pop
          end
          data << [itow_ms.to_f * 1E-3, page[1]]
        elsif data.empty?
          next
        elsif (data[-1][1] != page[1])
          next
        end
        data[-1] << page[4..31].unpack("f7")
      end
    end
    
    res = []
    data.each_with_index{|v, i|
      res << v if i % denom == 0
    }
    
    return res
  end
  
  def SylphNReader.read(file, options = {})
    open(file){|io|
      return SylphNReader.io_read(io, options)
    }
  end
end

if $0 == __FILE__ then
  $stderr.puts "N page reader"
  $stderr.puts "Usage1 : #{__FILE__} (log.dat)"
  $stderr.puts "Usage2 : cat (log.dat) | #{__FILE__}"
  data = nil
  if ARGV.size > 0 then
    options = {}
    fname = ARGV.shift
    ARGV.each{|item|
      next unless item =~ /--([^=]+)=/
      options[$1.to_sym] = $'
    }
    data = SylphNReader::read(fname, options)
  else
    data = SylphNReader::io_read($stdin)
  end
  if data then
    data.each{|item|
      puts item.join(',')
    }
  end
end


__END__
値の説明
A列) 時刻[s](ロック前は非常に小さな値が入る)
B列) シーケンス番号(0〜255で繰り返す、必ずしも0からはじまるとは限らない)
C列以降) 以下、FHImain.cppより順に
FHILog[0] = FHI_nav.Vn;
FHILog[1] = FHI_nav.Ve;
FHILog[2] = FHI_nav.Vd;
FHILog[3] = FHI_nav.phi;   
FHILog[4] = FHI_nav.theta; 
FHILog[5] = FHI_nav.psi;   
FHILog[6] = FHI_nav.p; 
FHILog[7] = FHI_nav.q; 
FHILog[8] = FHI_nav.r; 
FHILog[9] = FHI_nav.Xs;
FHILog[10] = FHI_nav.Ys;
FHILog[11] = FHI_nav.ALTGEO;
FHILog[12] = FHI_nav.Ax;
FHILog[13] = FHI_nav.Ay;
FHILog[14] = FHI_nav.Az;
FHILog[15] = FHI_nav.alpha; 
FHILog[16] = FHI_nav.beta;  
FHILog[17] = FHI_nav.Eas;   
FHILog[18] = FHI_nav.dyna_P;
FHILog[19] = FHI_nav.phiC;  
FHILog[20] = FHI_nav.thetaC;
FHILog[21] = FHI_nav.gammaC;
FHILog[22] = FHI_nav.psiC;  
FHILog[23] = FHI_nav.AltC;  
AA) FHILog[24] = FHI_servo.AilC;   
FHILog[25] = FHI_servo.ElvC;  
FHILog[26] = FHI_servo.RudC;  
FHILog[27] = FHI_servo.FlapC; 
FHILog[28] = FHI_servo.PLAC;
FHILog[29] = FHI_nav.init_flag;
FHILog[30] = FHI_nav.FHItime;
FHILog[31] = m_dEasC;
FHILog[32] = 0.0;
FHILog[33] = 0.0;
FHILog[34] = 0.0;