#!/usr/bin/ruby

=begin
log_CSVで生成されたCSV形式をSylphideのlog形式に戻す
=end

$: << File::join(File::dirname(__FILE__), '..', '..', 'common', 'ruby')
#require 'util.rb'

$stderr.puts "Usage: #{__FILE__} [options] [page:]log.csv ..."

options = {
  :out => '-',
}

src = {}
  
ARGV.reject!{|arg|
  if arg =~ /^--([^=]+)=?/ then
    k, v = [$1.to_sym, $']
    $stderr.puts "option: #{k}; value: #{v}"
    options[k] = v
    true
  else
    false
  end
}



ARGV.each{|arg|
  if arg =~ /([A-Z]):|^(?!:)/ then
    src[($1 || :A).to_sym] = $'
  else
    $stderr.puts "Incorrect format => #{arg}."
    exit(-1)
  end
}

$stderr.puts (src.map{|k, v| "#{k}:#{v}"}.join(' & ') + " => #{options[:out]}")

exit(-1) if src.empty?

def open_src(fname)
  if fname == '-' then
    yield $stdin
  else
    open(fname, 'r'){|io|
      yield io
    }
  end
end

def add_check_sum(str)
  ch = [0, 0]
  str[2..-1].each_byte{|b|
    ch[0] += b
    ch[1] += ch[0]
  }
  str << ch.pack("C2")
end

def get_ascii(c)
  c.kind_of?(Fixnum) ? c : c.bytes.to_a[0]
end

itow_binary = {} # 時刻 => バイナリ

src.each{|page, file|
  # とりあえず現時点ではA, Gのみサポートすることにする
  case page
  when :A
    tickcount = 0
    # ファイルには1行あたり、行数, GPS時刻[s], A/D結果9ch
    open_src(file){|io|
      io.each{|line|
        data = line.chomp!.split(/,/)[1..-1]
        next if data.size < 10
        data[0] = (data[0].to_f * 1E3)
        data.collect!{|item| item.to_i}
        str = [get_ascii(?A), tickcount & 0xFF, data[0]].pack("CCV")
        data[1..-2].each{|i| str << [i].pack("N")[1..-1]}
        str << [data[-1]].pack("v")
        
        # 出力
        itow_ms = data[0]
        if itow_binary.include?(itow_ms) then
          itow_binary[itow_ms] << str
        else
          itow_binary[itow_ms] = str
        end
        
        tickcount += 1
      }
    }
  when :G
    # ファイルには1行あたり、GPS時刻[s], 緯度, 経度, 高度, 水平/垂直面上精度(2), NED速度(3), 速度精度
    open_src(file){|io|
      io.each{|line|
        data = line.chomp!.split(/,/)
        next if data.size < 10
        data.collect!{|item| item.to_f}
          
        itow_ms = (data[0] * 1E3).to_i
        
        # 0xB5, 0x62, 0x01, 0x02 # NAV-POSLLH # 位置
        str_pos = [0xB5, 0x62, 0x01, 0x02, 28, # ヘッダ, サイズ(28)
            itow_ms, (data[2] * 1E7).to_i, (data[1] * 1E7).to_i, # ITOW, 経度, 緯度
            (data[3] * 1E3).to_i, 0, (data[4] * 1E3).to_i, (data[5] * 1E3).to_i, # WGS高度, 海面高度(N/A), 水平/垂直面上精度
            ].pack("C4vV7")
        add_check_sum(str_pos) # str_pos.size = 36
        
        # 0xB5, 0x62, 0x01, 0x12 # NAV-VELNED # 速度
        str_vel = [0xB5, 0x62, 0x01, 0x12, 36, # ヘッダ, サイズ(36)
            itow_ms, data[6..8].collect{|v| (v * 1E2).to_i}, # ITOW, NED速度
            (Math::sqrt(data[6..8].inject(0){|res, item| res + (item ** 2)}) * 1E2).to_i, # 速度
            (Math::sqrt(data[6..7].inject(0){|res, item| res + (item ** 2)}) * 1E2).to_i, # 水平面上速度
            (Math::atan2(data[6], data[7]) / Math::PI * 180 * 1E5).to_i, (data[9] * 1E2).to_i, # ヘディング, 速度精度 
            0, # ヘディング精度(0)
            ].flatten.pack("C4vV9")
        add_check_sum(str_vel) # str_vel.size = 44
        
        # サイズ調整
        str = str_pos + str_vel
        if (last_size = str.size % 31) > 0 then
          str << ([0] * (31 - last_size)).pack('C*')
        end
        pages = ""
        (str.size / 31).times{|i|
          start_index = 31 * i
          pages << [get_ascii(?G)].pack("C")
          pages << str[(start_index)..(start_index + 30)]
        }
        
        # 出力
        if itow_binary.include?(itow_ms) then
          itow_binary[itow_ms] << pages
        else
          itow_binary[itow_ms] = pages
        end
      }
    }
  end
}

# 時刻順に並べ替え
write_data = proc{|io| 
  itow_binary.to_a.sort{|a, b| a[0] <=> b[0]}.each{|itow, binary|
    io.print binary
  }
}

# 出力
if options[:out] == '-' then
  write_data.call($stdout)
else
  open(options[:out], 'w'){|out|
    write_data.call(out)
  }
end