#!/usr/bin/ruby
=begin
make_flight_log.rbで生成したフライトログと
対応するGAIAログから、
プロトタイプのエラーモデルを作成する
=end

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

require 'allan'

if ARGV.size < 2 then
  $stderr.puts "Usage: #{__FILE__} input.log gaia"
  exit
end

$: << '~/src/eclipse/autopilot/common/ruby/'
$: << '~/src/eclipse/autopilot/note/060609/analysis/'
$: << '~/src/eclipse/autopilot/note/080821_si/'

require 'flight_log_reader'
require 'gaia_reader'
require 'WGS84'

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

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

FLIGHT_LOG_LABELS = FlightLogReader::FLIGHT_LOG_LABELS
GAIA_LABELS = [
  GaiaReader::GAIA_LABELS,
  'Roll rateA',
  'Pitch rateA',
  'Yaw rateA'
].flatten!

flight_data = FlightLogReader::read(ARGV.shift, 1){|moment|
  (FLIGHT_LOG_LABELS.index(:Yaw)..FLIGHT_LOG_LABELS.index(:Azimuth)).each{|i|
    moment[i] = rad2deg(moment[i])
  }
  (FLIGHT_LOG_LABELS.index(:RollRate)..FLIGHT_LOG_LABELS.index(:YawAccel)).each{|i|
    moment[i] = rad2deg(moment[i])
  }
  moment
}

$stderr.puts flight_data.size

gaia_data = GaiaReader::read(ARGV.shift, {:denom => 1}){|moment|
  moment[GAIA_LABELS.index('ITOW')] /= 1000.0 # ms => s
  moment
}

ANGLE_ACCEL_CONVERT_LIST = [
  ['Roll rateA',   'Roll rate'],
  ['Pitch rateA',  'Pitch rate'],
  ['Yaw rateA',    'Yaw rate']
].collect!{|flight, gaia|
  [GAIA_LABELS.index(flight), GAIA_LABELS.index(gaia)]
}
previous_moment = gaia_data[0]
gaia_data[1..-1].each{|item|
  # 角加速度の生成
  ANGLE_ACCEL_CONVERT_LIST.each{|to_index, from_index|
    item[to_index] = \
        (item[from_index] - previous_moment[from_index]) \
        / (item[GAIA_LABELS.index('ITOW')] - previous_moment[GAIA_LABELS.index('ITOW')])
  }
  previous_moment = item
}

$stderr.puts gaia_data.size

# 時刻あわせを行う
flight_data2 = []
gaia_data2 = []

flight = nil
gaia = nil

while !flight_data.empty?
  flight ||= flight_data.shift
  gaia ||= gaia_data.shift
  break unless gaia 
  flight_t = flight[FLIGHT_LOG_LABELS.index(:Time)]
  gaia_t = gaia[GAIA_LABELS.index('ITOW')]
  diff_t = flight_t - gaia_t
  if (-0.005..0.005).include?(diff_t) then
    flight_data2 << flight
    gaia_data2 << gaia
    flight = gaia = nil
  elsif diff_t > 0
    gaia = nil
  else
    flight = nil
  end
end

#p flight_data2[0..2].zip(gaia_data2[0..2])

if flight_data2.size != gaia_data2.size then raise end

FLIGHT_LOG_TARGET = [
    :Yaw,
    :Pitch,
    :Roll,
    :AccX,
    :AccY,
    :AccZ,
    :RollRate,
    :PitchRate,
    :YawRate,
    :RollAccel,
    :PitchAccel,
    :YawAccel
  ]
GAIA_TARGET = [
    'Yaw',
    'Pitch',
    'Roll',
    'Acc X',
    'Acc Y',
    'Acc Z',
    'Roll rate',
    'Pitch rate', 
    'Yaw rate',
    'Roll rateA',
    'Pitch rateA', 
    'Yaw rateA'
  ]
  
DIFF_FB = 12.02 - 7.2 # GAIAからSYLPHまでの機軸距離(前が正)
DIFF_RL = 0.55-(-0.36) # GAIAからSYLPHまでのスパン方向距離(右が正)

COMPARE_TAGET = FLIGHT_LOG_TARGET.zip(GAIA_TARGET).collect{|flight, gaia|
  [FLIGHT_LOG_LABELS.index(flight), GAIA_LABELS.index(gaia)]
}
compare_data = []
flight_data2.size.times{|i|
  temp = []
  flight = flight_data2[i]
  gaia = gaia_data2[i]
  temp << flight[FLIGHT_LOG_LABELS.index(:Time)]
  #puts flight.join(',')
  #puts gaia.join(',')
  COMPARE_TAGET.each{|v1, v2|
    #puts "#{flight[v1]}, #{gaia[v2]}"
    case v1
    when :Yaw
      roll_diff = flight[v1] - gaia[v2]
      if roll_diff >= 180 then
        roll_diff -= 360 
      elsif roll_diff <= -180
        roll_diff += 360 
      end
      temp << roll_diff
    else
      temp << flight[v1] - gaia[v2]
    end
  }
  compare_data << temp
}

=begin
compare_data.each{|item|
  puts item.join(' ') # Alavar用
}
exit
=end

$stderr.puts compare_data.size

tau = compare_data[1][0] - compare_data[0][0]
(1...(compare_data[0].size)).each{|i|
  data = compare_data[0...(2**13)].collect{|item| item[i]}
  data_mean = data.mean
  data.collect!{|item| item - data_mean}
  puts data_mean
  avar = Allan.avar(data, tau)
  puts avar[-1].join(',')
  avar[0..-2].each{|item|
    puts item.join(',')
  }
  puts
}