#!/usr/bin/env ruby

$: << '~/src/eclipse/autopilot/common/ruby/'

require 'gnuplot_support'

TARGETS = [
    ['Carrier phase [L1 cycles]', 'CP', 'to_f'],
    ['Pseudorange [m]', 'PR', 'to_f'],
    ['Doppler [Hz]', 'DP', 'to_f'],
    ['Quality indicator', 'Q', 'to_i'],
    ['Signal strength C/N0 [dbHz]', 'SS', 'to_i'],
    ['Loss of lock indicator (RINEX)', 'LI', 'to_i']
  ]

def plot(fname, t, y, y_label)
  Plotter::plot_basic(fname){|plot|
		plot.xlabel "'GPS Time [sec]'"
			
    yield plot if block_given?
		
		items = [
				Gnuplot::DataSet::new([t, y]){|ds|
					ds.with = "lines lw 3"
					ds.title = y_label
				}
			]
		
		plot.data = items
	}
end

data = {}

if ARGV.empty? then
# カレントディレクトリからファイルを拾ってくる標準的な動作
Dir.open('.').each{|file|
  if file !~ /ublox_sv_(\d+)\.csv/ then next end
  sv_number = $1
  data[sv_number] = []
  open(file).each{|line|
    line.chop!
    values = line.split(/,\s*/)
    #p values
    #if values[5].to_i != 0 then next end
    values[0] = values[0].to_f
    TARGETS.each_with_index{|item, i|
      values[i + 1] = eval("values[i + 1].#{item[2]}")
    }
    data[sv_number] << values
  }
}
else
# log.datから直接
$: << File::join([File::dirname(__FILE__), "swig"])
require 'build_SWIG/SylphideProcessor.so'

status_flags = 0
grabber = proc{|observer|
  case observer
  when SylphideProcessor::GPacketObserver
    next unless observer.valid?
    case [observer.ubx_class, observer.ubx_id]
    when [0x01, 0x06] # Solution
      status_flags = observer.solution.status_flags
    when [0x02, 0x10] # Raw
      next if ((status_flags & SylphideProcessor::GSolution::TOW_VALID) == 0)
      observer.num_of_sv.times{|i|
        raw = observer.raw(i)
        #CP, Pseudo, Doppler, PRN, Q, SS, LLI
        data[raw.sv_number] ||= []
        data[raw.sv_number] << [
            observer.fetch_ITOW,
            raw.carrier_phase, raw.pseudo_range, raw.doppler,
            raw.quarity, raw.signal_strength, raw.lock_indicator]
      }
    end
  end
}

input = ARGV.shift
input = $stdin if input == '-'
processor = SylphideProcessor::SylphideLog::new(1024)
pages = 0
while !input.eof?
  pages += 1
  processor.process(input.read(32)){|observer|
    grabber.call(observer)
  }
  $stderr.puts "#{pages} pages processed" if (pages % 10000) == 0
end
end

data.reject!{|k, prop|
  next true unless ((1..32).include?(k) || [129, 137].include?(k)) # MSAS以外のPRN33以上の衛星は除外
  next true unless prop.size > 100 # 少なくとも100ショットは必要
  false
}

# 時間スケールの導出
t_mins = []
t_maxs = []
data.each{|key, v|
  t_mins << v[0][0]
  t_maxs << v[-1][0]
}
t_mins.sort!{|a, b| a <=> b}
t_maxs.sort!{|a, b| a <=> b}
#p t_mins
#p t_maxs
t_min = (t_mins[0] / 100).to_i * 100
t_max = ((t_maxs[-1] / 100).to_i + 1) * 100
#p t_min
#p t_max

# 有効時間のグラフ
AVAILABLE_THRESHOLD = 5;
sv_available = {}
data.each{|key, v|
  sv_available[key] = []
  start_t = previous_t = v[0][0]
  v[1..-1].each{|raw_data|
    if (raw_data[0] - previous_t) > AVAILABLE_THRESHOLD then
      sv_available[key] << [start_t, previous_t]
      previous_t = start_t = raw_data[0]
    else
      previous_t = raw_data[0]
    end
  }
  sv_available[key] << [start_t, v[-1][0]]
}
#p sv_available

AVAILABLE_FIG_FNAME = 'ublox_sv_available.eps'
Plotter::plot_basic(AVAILABLE_FIG_FNAME){|plot|
	plot.xlabel "'GPS Time [sec]'"
		
  plot.xrange <<-__STRING__
[#{t_min}:#{t_max}]
  __STRING__
  plot.set 'format y ""'
  plot.yrange <<-__STRING__
[-1:#{sv_available.size}]
  __STRING__
  plot.set 'ytics 1'
  plot.set 'title "Available satellites" font "Helvetica,20"'
  
  object_index = 1
	sv_available.to_a.sort{|a, b| b[0] <=> a[0]}.each_with_index{|key_v, i|
    key = key_v[0]
    v = key_v[1]
    plot.set "label \"PRN#{key}\" at #{t_min}, #{i} right offset -1, 0"
    v.each{|start_end|
      plot.set <<-__STRING__
object #{object_index} rect \
from #{start_end[0]}, #{i-0.3} to #{start_end[1]}, #{i+0.3} \
fc rgbcolor "red" fs solid 0.7 bo -1
      __STRING__
      object_index += 1
      
      #"lines lt 1 lc #{i} lw 20"
    }
	}
	plot.data = [
      Gnuplot::DataSet::new([[0], [0]]){|ds|
				ds.with = "lines lw 0"
				ds.notitle
			}
    ]
}

#exit

L1_WAVE_LENGTH = 2.99792458E8 / 1.57542E9

#epsファイルの生成
eps_files = {}
data.each{|key, v|

  t = v.collect{|item| item[0]}
  eps_files[key] = []
  TARGETS.each_with_index{|target, i|
    y = v.collect{|item| item[i+1]}
    fname = "ublox_sv_#{key}_#{target[1]}.eps"
    eps_files[key] << fname
    plot(fname, t, y, target[0]){|plot|
      plot.xrange <<-__STRING__
[#{t_min}:#{t_max}]
			__STRING__
    }
  }
  
  # 擬似距離とキャリアの比較
  t_delta_pseudo = [[], []]
  t_delta_carrier = [[], []]
  
  previous_data = v[0]
  v[1..-1].each{|item|
    t_center = (item[0] + previous_data[0]) / 2
    delta_carrier = (item[1] - previous_data[1]) * L1_WAVE_LENGTH
    if delta_carrier.abs < 1000 then # スリップしたものは除去
      t_delta_carrier[0] << t_center
      t_delta_carrier[1] << delta_carrier
    end
    delta_pseudo = item[2] - previous_data[2]
    if delta_pseudo.abs < 1000 then  # スリップしたものは除去
      t_delta_pseudo[0] << t_center
      t_delta_pseudo[1] << delta_pseudo
    end
    previous_data = item
  }
  if t_delta_pseudo[0].empty? and t_delta_carrier[0].empty? then 
    next 
  end
    
  eps_files[key] << "ublox_sv_#{key}_delta_cp_pr.eps"
  Plotter::plot_basic(eps_files[key][-1]){|plot|
		plot.xlabel "'GPS Time [sec]'"
			
    plot.xrange <<-__STRING__
[#{t_min}:#{t_max}]
    __STRING__
    
    items = [
		    Gnuplot::DataSet::new(t_delta_pseudo){|ds|
					ds.with = "lines lw 2"
					ds.title = "{/Symbol D} Pseudo Range [m]"
				},
				Gnuplot::DataSet::new(t_delta_carrier){|ds|
					ds.with = "lines lw 2"
					ds.title = "{/Symbol D} Carrier Phase [m]"
				}
			]
		
		plot.data = items
	}
}

#exit

#TeXファイルの生成
tex_doc =<<-'__TEX_STRING__'
\documentclass[disablejfam]{jsarticle}

% pakages
\usepackage{times, mathptmx}
\usepackage{graphicx}
\usepackage{makeidx}
\usepackage{bm}
\usepackage{amsmath, amssymb}
\usepackage{tabularx}
\usepackage{slashbox}
\usepackage{enumerate}
% \usepackage{hyperref}
\usepackage{ascmac}
\usepackage[dvipdfm,bookmarks=true,bookmarksnumbered=true,bookmarkstype=toc,colorlinks=true,linkcolor=blue]{hyperref}
\usepackage[left=20mm,right=20mm,top=20mm,bottom=20mm]{geometry}

\begin{document}

\title{u-blox Raw Data}
\maketitle

\begin{center}
\begin{screen}
\begin{itemize}
\item
u-blox Raw Datas

\end{itemize}
\end{screen}
\end{center}
__TEX_STRING__

tex_doc +=<<-__TEX_STRING__
\\begin{center}
\\includegraphics[width=150mm]{#{AVAILABLE_FIG_FNAME}}
\\end{center}
\\parindent 0truemm
__TEX_STRING__

eps_files.to_a.sort{|a, b| a[0] <=> b[0]}.each{|item|
  sv_number = item[0]
  fnames = item[1]
  
  tex_doc +=<<-__TEX_STRING__
\\clearpage
\\section*{PRN#{sv_number}}
  __TEX_STRING__
  
  fnames.each{|fname|
    tex_doc +=<<-__TEX_STRING__
\\includegraphics[width=80mm]{#{fname}}
    __TEX_STRING__
  }
}

tex_doc +=<<-'__TEX_STRING__'
\end{document}
__TEX_STRING__

ftex_name = 'ublox_raw_datas.tex'
ftex_p = open(ftex_name, 'w')
ftex_p.print tex_doc
ftex_p.close

print `platex #{ftex_name}`
print `platex #{ftex_name}`
print `dvipdfmx #{ftex_name.gsub(/\.tex/, '.dvi')}`
#print `rm *.eps`