#!/usr/bin/ruby

require "hpdf"

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

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

if ARGV.size < 1 then
  exit(-1)
end

options = {}
if ARGV.size > 1 then
  ARGV[1..-1].each{|item|
    if item =~ /--([^=]+)=/ then
      options[$1.intern] = $'
    end
  }
end
options[:scaling] = options[:scaling] ? options[:scaling].to_f : 5
options[:data_interval] = options[:data_interval] ? options[:data_interval].to_f : 0.4
p options

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

def rad2deg(rad)
  return rad / Math::PI * 180
end

=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
DATA_LIST = [
    [:itow , ?A - ?A],
    [:long , ?B - ?A],
    [:lat  , ?C - ?A],
    [:alt  , ?D - ?A],
    [:pitch, ?I - ?A],
    [:q    , ?I - ?A + 26],
    [:u    , ?L - ?A],
    [:alpha, ?M - ?A],]
def DATA_LIST.index(str)
  return DATA_LIST.collect{|label, index| label}.index(str)
end

input_file = ARGV.shift
data = []
open(input_file).each{|line|
  line_data = line.chop!.split(/,/)
  data << DATA_LIST.collect{|label, index|
    line_data[index].to_f
  }
}

pdf = HPDFDoc.new
pdf.set_compression_mode(HPDFDoc::HPDF_COMP_ALL)
page = pdf.add_page
page.set_height(500)
image = pdf.load_png_image_from_file("#{File::dirname(__FILE__)}/MitsubishiUAV_side.png")
#p image.get_color_space

def page.assign_image(image, x, y, options = {})
  rad = options[:rad] || deg2rad(options[:deg] || 0.0)
  c_rad, s_rad = [Math::cos(rad), Math::sin(rad)]
  scale = options[:scale] || 1.0
  iw, ih = image.get_size
  iw *= scale
  ih *= scale
  if !options[:origin] && options[:centering] then
    iw_half = iw / 2
    ih_half = ih / 2
    options[:origin] = [ \
        (c_rad * iw_half - s_rad * ih_half), 
        (s_rad * iw_half + c_rad * ih_half)]
  end
  origin_x, origin_y = options[:origin] || [0, 0]
  
  # 本体
  self.gsave
  self.concat(iw * c_rad, iw * s_rad, ih * -s_rad, ih * c_rad, \
      x - origin_x, y - origin_y)
  self.execute_xobject(image)
  self.grestore
  
  # 機体軸
=begin
  self.gsave
  self.set_line_width(0.02)
  self.concat(c_rad, s_rad, -s_rad, c_rad, x, y)
  self.move_to(0, 0)
  self.line_to(20, 0)
  self.stroke
  self.grestore
=end
end

ARC_RADIUS = 40
def page.assign_wind(x, y, alpha, pitch, options = {})
  alpha_deg = rad2deg(alpha)
  c_pitch, s_pitch = [Math::cos(pitch), Math::sin(pitch)]
  
  # 迎角を表現
  self.gsave
  self.concat(c_pitch, s_pitch, -s_pitch, c_pitch, x, y)
  
  if alpha >= 0 then
    self.set_rgb_fill(1.0, 0, 0)
    self.move_to(0, 0)
    #self.line_to(ARC_RADIUS * Math::cos(alpha_rad), ARC_RADIUS * Math::sin(alpha_rad))
    self.arc(0, 0, ARC_RADIUS, 90 - alpha_deg, 90)
  else
    self.set_rgb_fill(0.0, 0, 1.0)
    self.move_to(0, 0)
    #self.line_to(0, ARC_RADIUS)
    self.arc(0, 0, ARC_RADIUS, 90, 90 - alpha_deg)
  end
  
  self.line_to(0, 0)
  self.fill
  self.grestore
end

#page.draw_image(image, 0, 0, image.get_width, image.get_height)
#page.assign_image(image, 0, 0, :deg=>30, :scale => 0.5, :centering => true)

init_x = 100
init_y = 250
itow_index = DATA_LIST::index(:itow)
lat_index = DATA_LIST::index(:lat)
long_index = DATA_LIST::index(:long)
h_index = DATA_LIST::index(:alt)
pitch_index = DATA_LIST::index(:pitch)
alpha_index = DATA_LIST::index(:alpha)

base_llh = System_LLH::new( \
    [deg2rad(data[0][lat_index]), deg2rad(data[0][long_index]), data[0][h_index]])

traj_x = []
traj_y = []

# まずはじめに飛行履歴を算出
data.each{|item|
  rel_llh = System_LLH::new( \
      [deg2rad(item[lat_index]), deg2rad(item[long_index]), item[h_index]])
  enu = System_ENU::relative(rel_llh, base_llh)
  #p [itow, enu]
  traj_x << init_x + Math::sqrt((enu.e ** 2) + (enu.n ** 2)) * options[:scaling]
  traj_y << init_y + enu.u * options[:scaling]
}
page.set_width(traj_x[-1] + 100)

# 使うデータのインデックスを求める
use_data = {:indexes => [0], :itow => [data[0][itow_index]]}
data[1..-1].each_with_index{|item, i|
  itow = item[itow_index]
  if itow >= use_data[:itow].last + options[:data_interval] then
    use_data[:indexes] << i + 1
    use_data[:itow] << itow
  end
}

# 飛行機、風を描く
use_data[:indexes].each{|i|
  pitch = data[i][pitch_index]
  alpha = data[i][alpha_index]
  center_x = traj_x[i]
  center_y = traj_y[i]
  page.assign_image(image, center_x, center_y, \
      :rad => pitch, :scale => 0.03, :centering => true)
  page.assign_wind(center_x, center_y, alpha, pitch)
}

# 飛行履歴を描く
page.gsave
page.set_line_width(0.5)
page.set_rgb_stroke(0.0, 0.6, 0.2)
page.set_line_cap(HPDFDoc::HPDF_ROUND_END)
page.move_to(traj_x[0], traj_y[0])
[use_data[:indexes][0..-2], use_data[:indexes][1..-1]].transpose.each{|i, j|
  page.curve_to3( \
      traj_x[(i + j) / 2], traj_y[(i + j) / 2], \
      traj_x[j], traj_y[j])
}
page.stroke
page.grestore


# ステップ数の注釈
page.gsave
page.set_font_and_size(pdf.get_font("Helvetica", nil), 18)
page.begin_text
txt = "Time step: #{options[:data_interval]} sec"
tw = page.text_width(txt)
x = (page.get_width - tw) / 2
y = page.get_height - 50
page.move_text_pos(x, y)
page.show_text(txt)
page.end_text
page.grestore

pdf.save_to_file("#{File.basename(input_file, '.*')}_lon.pdf")