#!/usr/bin/ruby

$: << File::join([File::dirname(__FILE__), "..", "..", "common", "ruby"])
require "ublox.rb"

class UBXExtractor
  
  def initialize(options = {})
    @buf = ""
    @ubx_packets = []
  end
  
  attr_reader :ubx_packets
  
  def process(page)
    if page[0] == ?G then
      @buf += page[1..-1]
      while @buf[/\xb5\x62/]
        #$stderr.p @buf.unpack("C*").collect{|v| sprintf('%02x', v)}.join(' ')
        @buf = @buf[($~.begin(0))..-1]
        #$stderr.p @buf.unpack("C*").collect{|v| sprintf('%02x', v)}.join(' ')
        if @buf.size <= 8 then break end
        packet_size = @buf[4] + (@buf[5] << 8) + 8
        if @buf.size <= packet_size then break end
        packet = @buf[0, packet_size].unpack("C*")
        if U_blox::valid_parity?(packet) then
          @ubx_packets << packet
          @buf = @buf[packet_size..-1]
        else
          @buf = @buf[2..-1]
        end
      end
      return true
    else
      return false
    end
  end
  
  def fetch_llh
    return @ubx_packets.collect{|packet|
      llh = U_blox::fetch_longlatheight(packet)
      llh && [U_blox::fetch_ITOW(packet), llh]
    }.delete_if{|item| item == nil}
  end
  
  def fetch_velned
    return @ubx_packets.collect{|packet|
      velned = U_blox::fetch_velned(packet)
      velned && [U_blox::fetch_ITOW(packet), velned]
    }.delete_if{|item| item == nil}
  end
end

if __FILE__ == $0 then

$stderr.puts "Usage #{__FILE__} log.dat"

if ARGV.size < 1 then exit end

  file_name = ARGV.shift
  extractor = UBXExtractor::new
  
  open(file_name){|io|
    io.binmode
    page_count = -1
    while !io.eof?
      page_count += 1
      extractor.process(io.read(32))
    end
  }
  
  #p extractor.ubx_packets
  statistics = {}
  extractor.ubx_packets.each{|packet|
    unless statistics.include?(packet[2..3]) then
      statistics[packet[2..3]] = []
    end
    statistics[packet[2..3]] << packet
  }
  statistics.each{|key, packets|
    puts "#{key.collect{|item| "%#04x" % item}.join(' ')} => #{packets.size}"
  }
  if target_packets = statistics[[0x01, 0x06]] then
    target_packets.each{|packet|
      puts "#{U_blox::fetch_ITOW(packet)} => #{packet[6 + 47]} SVs, flag = #{packet[6 + 10]}"
    }
  end
  
  base_name = File.basename(file_name, ".*")
  
  # 位置の出力
  open("#{base_name}_pos.csv", "w"){|io|
    extractor.fetch_llh.each{|itow, llh|
      lat, long, height = llh
      io.puts "#{itow}, #{lat}, #{long}, #{height}"
    }  
  }
  # 速度の出力
  open("#{base_name}_vel.csv", "w"){|io|
    extractor.fetch_velned.each{|itow, velned|
      v_north, v_east, v_down = velned
      io.puts "#{itow}, #{v_north}, #{v_east}, #{v_down}"
    }
  }
end