#!/usr/bin/ruby

=begin
磁気モデル用のファイル
=end

class MagneticFieldModel
  attr_reader :name, :year, :coefs
  def initialize(opt)
    @name = opt[:name]
    @year = opt[:year]
    @coefs = opt[:coefs]
  end
  def lerp(another_model, year) # 線形補完
    self_weight = (another_model.year - year) / (another_model.year - @year)
    another_weight = 1.0 - self_weight
    coefs = [
        @coefs.collect{|same_dof| same_dof.collect{|v| v * self_weight}},
        another_model.coefs.collect{|same_dof| same_dof.collect{|v| v * another_weight}}] \
          .sort{|a, b| b.size <=> a.size}
    coefs[1].each_with_index{|same_dof, i|
      same_dof.each_with_index{|v, j|
        coefs[0][i][j] += v
      }
    }
    MagneticFieldModel::new(:name => "#{@name}_#{another_model.name}", :year => year, :coefs => coefs[0])
  end
  
  A2 = 40680631.59 # WGS84
  B2 = 40408299.98 # WGS84
  EARTHS_RADIUS = 6371.2
  
  def calc_field(latitude_rad, longitude_rad, height_meter_wgs84)    
    slat = Math::sin(latitude_rad)
    clat = Math::cos(latitude_rad)
    
    # 緯度の補正
    begin
      latitude_deg = latitude_rad / Math::PI * 180
      if (90.0 - latitude_deg) < 1E-3 then
        clat = Math::cos((90.0 - 1E-3) / 180 * Math::PI) # 300 ft. from North pole
      elsif (90.0 + latitude_deg) < 1E-3 then
        clat = Math::cos((-90.0 + 1E-3) / 180 * Math::PI) # 300 ft. from South pole
      end
    end
    
    sl = [Math::sin(longitude_rad)]
    cl = [Math::cos(longitude_rad)]
    
    res_NED = [0.0] * 3
    
    sd, cd = [0.0, 1.0]
    aa, bb, cc, dd, r, rr = []
    
    # TODO: WGS84高度から地球中心距離への変換(だと思う)
    begin
      aa = A2 * clat * clat
      bb = B2 * slat * slat
      cc = aa + bb
      dd = Math::sqrt(cc)
      height_km_wgs84 = 1E-3 * height_meter_wgs84
      r = Math::sqrt(height_km_wgs84 * (height_km_wgs84 + 2.0 * dd) + (A2 * aa + B2 * bb) / cc)
      cd = (height_km_wgs84 + dd) / r
      sd = (A2 - B2) / dd * slat * clat / r
      slat = slat * cd - clat * sd
      clat = clat * cd + slat * sd
    end
    
    aa = Math::sqrt(3.0)
    p = [
        2.0 * slat,
        2.0 * clat,
        4.5 * slat * slat - 1.5,
        3.0 * aa * clat * slat]
    q = [
        -clat,
        slat,
        -3.0 * clat * slat,
        aa * (slat * slat - clat * clat)]
    
    k, l, m, n = [0, 0, 0, -1]
    k_max = ((@coefs.size * (@coefs.size + 3)) / 2)
    while k < k_max
      if m > n then
        m = -1
        n += 1
        
        rr = (EARTHS_RADIUS / r) ** (n + 3)
      end
      fm, fn = [m + 1, n + 1]
      if k > 3 then
        if m == n then
          aa = Math::sqrt(1.0 - 0.5 / fm)
          j = k - n - 2
          p[k] = (1.0 + 1.0 / fm) * aa * clat * p[j]
          q[k] = aa * (clat * q[j] + slat/fm * p[j])
          sl[m] = sl[m-1] * cl[0] + cl[m-1] * sl[0]
          cl[m] = cl[m-1] * cl[0] - sl[m-1] * sl[0]
        else
          aa = Math::sqrt(fn * fn - fm * fm)
          bb = Math::sqrt(((fn - 1.0) * (fn - 1.0)) - (fm * fm)) / aa
          cc = (2.0 * fn - 1.0) / aa
          i, j = [k - n - 1, k - 2 * n - 1]
          p[k] = (fn + 1.0) * (cc * slat / fn * p[i] - bb / (fn - 1.0) * p[j])
          q[k] = cc * (slat * q[i] - clat / fn * p[i]) - bb * q[j]
        end
      end
      aa = rr * @coefs.flatten[l]
      
      if m < 0 then
        res_NED[0] += aa * q[k]
        res_NED[2] -= aa * p[k]
        l += 1
      else
        bb = rr * @coefs.flatten[l + 1]
        cc = aa * cl[m] + bb * sl[m]
        res_NED[0] += cc * q[k]
        res_NED[2] -= cc * p[k]
        if clat > 0 then
          res_NED[1] += (aa * sl[m] - bb * cl[m]) * fm * p[k]/((fn + 1.0) * clat)
        else
          res_NED[1] += (aa * sl[m] - bb * cl[m]) * q[k] * slat
        end
        l += 2
      end
      k += 1
      m += 1
    end

    aa = res_NED[0]
    res_NED[0] = aa * cd + res_NED[2] * sd
    res_NED[2] = res_NED[2] * cd - aa * sd
    
    return res_NED
  end
end

class DGRF2005 < MagneticFieldModel
  YEAR = 2005.0
  COEFS = [
      [-29554.63, -1669.05, 5077.99],
      [-2337.24, 3047.69, -2594.5, 1657.76, -515.43],
      [1336.3, -2305.83, -198.86, 1246.39, 269.72, 672.51, -524.72],
      [920.55, 797.96, 282.07, 210.65, -225.23, -379.86, 145.15, 100.0, -305.36],
      [-227.0, 354.41, 42.72, 208.95, 180.25, -136.54, -123.45, -168.05, -19.57, -13.55, 103.85],
      [73.6, 69.56, -20.33, 76.74, 54.75, -151.34, 63.63, -14.58, -63.53, 14.58, 0.24, -86.36, 50.94],
      [79.88, -74.46, -61.14, -1.65, -22.57, 38.73, 6.82, 12.3, 25.35, 9.37, 10.93, 5.42, -26.32, 1.94, -4.64],
      [24.8, 7.62, 11.2, -11.73, -20.88, -6.88, 9.83, -18.11, -19.71, 10.17, 16.22, 9.36, 7.61, -11.25, -12.76, -4.87, -0.06],
      [5.58, 9.76, -20.11, 3.58, 12.69, -6.94, 12.67, 5.01, -6.72, -10.76, -8.16, -1.25, 8.1, 8.76, 2.92, -6.66, -7.73, -9.22, 6.01],
      [-2.17, -6.12, 2.19, 1.42, 0.1, -2.35, 4.46, -0.15, 4.76, 3.06, -6.58, 0.29, -1.01, 2.06, -3.47, 3.77, -0.86, -0.21, -2.31, -2.09, -7.93],
      [2.95, -1.6, 0.26, -1.88, 1.44, 1.44, -0.77, -0.31, -2.27, 0.29, 0.9, -0.79, -0.58, 0.53, -2.69, 1.8, -1.08, 0.16, -1.58, 0.96, -1.9, 3.99, -1.39],
      [-2.15, -0.29, -0.55, 0.21, 0.23, 0.89, 2.38, -0.38, -2.63, 0.96, 0.61, -0.3, 0.4, 0.46, 0.01, -0.35, 0.02, -0.36, 0.28, 0.08, -0.87, -0.49, -0.34, -0.08, 0.88],
      [-0.16, -0.88, -0.76, 0.3, 0.33, 0.28, 1.72, -0.43, -0.54, 1.18, -1.07, -0.37, -0.04, 0.75, 0.63, -0.26, 0.21, 0.35, 0.53, -0.05, 0.38, 0.41, -0.22, -0.1, -0.57, -0.18, -0.82],]
  def initialize; super(:name => self.class.to_s, :year => YEAR, :coefs => COEFS); end
end

class DGRF2010 < MagneticFieldModel
  YEAR = 2010.0
  COEFS = [
      [-29496.57, -1586.42, 4944.26],
      [-2396.06, 3026.34, -2708.54, 1668.17, -575.73],
      [1339.85, -2326.54, -160.4, 1232.1, 251.75, 633.73, -537.03],
      [912.66, 808.97, 286.48, 166.58, -211.03, -356.83, 164.46, 89.4, -309.72],
      [-230.87, 357.29, 44.58, 200.26, 189.01, -141.05, -118.06, -163.17, -0.01, -8.03, 101.04],
      [72.78, 68.69, -20.9, 75.92, 44.18, -141.4, 61.54, -22.83, -66.26, 13.1, 3.02, -78.09, 55.4],
      [80.44, -75.0, -57.8, -4.55, -21.2, 45.24, 6.54, 14.0, 24.96, 10.46, 7.03, 1.64, -27.61, 4.92, -3.28],
      [24.41, 8.21, 10.84, -14.5, -20.03, -5.59, 11.83, -19.34, -17.41, 11.61, 16.71, 10.85, 6.96, -14.05, -10.74, -3.54, 1.64],
      [5.5, 9.45, -20.54, 3.45, 11.51, -5.27, 12.75, 3.13, -7.14, -12.38, -7.42, -0.76, 7.97, 8.43, 2.14, -8.42, -6.08, -10.08, 7.01],
      [-1.94, -6.24, 2.73, 0.89, -0.1, -1.07, 4.71, -0.16, 4.44, 2.45, -7.22, -0.33, -0.96, 2.13, -3.95, 3.09, -1.99, -1.03, -1.97, -2.8, -8.31],
      [3.05, -1.48, 0.13, -2.03, 1.67, 1.65, -0.66, -0.51, -1.76, 0.54, 0.85, -0.79, -0.39, 0.37, -2.51, 1.79, -1.27, 0.12, -2.11, 0.75, -1.94, 3.75, -1.86],
      [-2.12, -0.21, -0.87, 0.3, 0.27, 1.04, 2.13, -0.63, -2.49, 0.95, 0.49, -0.11, 0.59, 0.52, 0.0, -0.39, 0.13, -0.37, 0.27, 0.21, -0.86, -0.77, -0.23, 0.04, 0.87],
      [-0.09, -0.89, -0.87, 0.31, 0.3, 0.42, 1.66, -0.45, -0.59, 1.08, -1.14, -0.31, -0.07, 0.78, 0.54, -0.18, 0.1, 0.38, 0.49, 0.02, 0.44, 0.42, -0.25, -0.26, -0.53, -0.26, -0.79],]
  def initialize; super(:name => self.class.to_s, :year => YEAR, :coefs => COEFS); end
end

class IGRF2015 < MagneticFieldModel
  YEAR = 2015.0
  COEFS = [
      [-29442.0, -1501.0, 4797.1],
      [-2445.1, 3012.9, -2845.6, 1676.7, -641.9],
      [1350.7, -2352.3, -115.3, 1225.6, 244.9, 582.0, -538.4],
      [907.6, 813.7, 283.3, 120.4, -188.7, -334.9, 180.9, 70.4, -329.5],
      [-232.6, 360.1, 47.3, 192.4, 197.0, -140.9, -119.3, -157.5, 16.0, 4.1, 100.2],
      [70.0, 67.7, -20.8, 72.7, 33.2, -129.9, 58.9, -28.9, -66.7, 13.2, 7.3, -70.9, 62.6],
      [81.6, -76.1, -54.1, -6.8, -19.5, 51.8, 5.7, 15.0, 24.4, 9.4, 3.4, -2.8, -27.4, 6.8, -2.2],
      [24.2, 8.8, 10.1, -16.9, -18.3, -3.2, 13.3, -20.6, -14.6, 13.4, 16.2, 11.7, 5.7, -15.9, -9.1, -2.0, 2.1],
      [5.4, 8.8, -21.6, 3.1, 10.8, -3.3, 11.8, 0.7, -6.8, -13.3, -6.9, -0.1, 7.8, 8.7, 1.0, -9.1, -4.0, -10.5, 8.4],
      [-1.9, -6.3, 3.2, 0.1, -0.4, 0.5, 4.6, -0.5, 4.4, 1.8, -7.9, -0.7, -0.6, 2.1, -4.2, 2.4, -2.8, -1.8, -1.2, -3.6, -8.7],
      [3.1, -1.5, -0.1, -2.3, 2.0, 2.0, -0.7, -0.8, -1.1, 0.6, 0.8, -0.7, -0.2, 0.2, -2.2, 1.7, -1.4, -0.2, -2.5, 0.4, -2.0, 3.5, -2.4],
      [-1.9, -0.2, -1.1, 0.4, 0.4, 1.2, 1.9, -0.8, -2.2, 0.9, 0.3, 0.1, 0.7, 0.5, -0.1, -0.3, 0.3, -0.4, 0.2, 0.2, -0.9, -0.9, -0.1, 0.0, 0.7],
      [0.0, -0.9, -0.9, 0.4, 0.4, 0.5, 1.6, -0.5, -0.5, 1.0, -1.2, -0.2, -0.1, 0.8, 0.4, -0.1, -0.1, 0.3, 0.4, 0.1, 0.5, 0.5, -0.3, -0.4, -0.4, -0.3, -0.8],]
  def initialize; super(:name => self.class.to_s, :year => YEAR, :coefs => COEFS); end
end

if $0 == __FILE__
  require 'stringio'
  
  options = {:mode => :cpp}
    
  ARGV.reject!{|arg|
    if arg =~ /--([^=]+)=?/ then
      options[$1.to_sym] = $'
      true
    else
      false
    end
  }
  
  $stderr.puts options.inspect
  
  class Model
    def initialize(name, year, dof)
      @name = name
      @year = year
      @dof = dof
      @coefs = []
    end
    attr_reader :name, :year
    attr_accessor :dof, :coefs
  end
  
  target_file = ARGV.shift || 'http://www.ngdc.noaa.gov/IAGA/vmod/igrf12coeffs.txt'
  if target_file =~ /^(?:http|ftp):\/\// then
    require 'open-uri'
  end
  
  prefix = File::basename(target_file, '.*')
  data = {}
  
  if target_file =~ /\.COF$/ then
    # cofファイルコンバータとして働くようにする
    current = nil
    open(target_file).each{|line|
      if line =~ /^ *([A-Z]+\d+) *([\.\d]+) *(\d+)/ then
        current = data[$1] = Model::new($1, $2.to_f, $3.to_i)
      elsif line =~ /^ *(\d+) *(\d+) *([-\.\d]+) *([-\.\d]+)/ then
        current.coefs << $3.to_f
        current.coefs << $4.to_f if $2.to_i != 0 
      end
    }
  elsif target_file =~ /\.txt$/ then
    # ex) http://www.ngdc.noaa.gov/IAGA/vmod/igrf12coeffs.txt
    values = []
    open(target_file).each{|line|
      next if line =~ /^ *#/
      values << line.chomp!.split(/ +/)
    }
    values = values.transpose
    dof_check_indexes = []
    values[2][2..-1].each_with_index{|v, i|
      dof_check_indexes << [i, values[1][i + 2].to_i] if v.to_i == 0 # m=0となる行を調べて、そのときのnも記録
    }
    dof_check_indexes.reverse!
    values[3..-1].each{|items|
      next if items[0] == "SV" 
      year = items[1].to_f
      model = Model::new("#{items[0]}#{year < 2000 ? sprintf('%02d', year.to_i % 100) : year.to_i}", year, 0)
      next if options[:filter] && !eval("model.name #{options[:filter]}")
      model.coefs = items[2..-1].collect{|v| v.to_f}
      dof_check_indexes.each{|i, dof|
        if model.coefs[i..-1].find{|v| v != 0} then # 値が一つでもはいっていれば有効
          model.dof = dof
          break
        end
        model.coefs.slice!(i..-1) # 削除
      }
      data[model.name] = model
    }
  else
    exit(-1)  
  end

  models = data.to_a.sort{|a, b| a[1].year <=> b[1].year}.collect{|name, model| model}
  
  if options[:mode].to_sym == :ruby then
    models.each{|model|
      last_index = 0
      coefs_str = StringIO::new
      model.dof.times{|i|
        next_index = ((i ** 2) + (i * 4) + 3) # \Sigma_{k=0}^{i} {(i * 2) + 3}
        coefs_str.puts
        coefs_str.print "#{' ' * 6}#{model.coefs[last_index...next_index].inspect},"
        last_index = next_index
      }
      puts <<-__RUBY_SOURCE__
class #{model.name} < MagneticFieldModel
  YEAR = #{model.year}
  COEFS = [#{coefs_str.string}]
  def initialize; super(:name => self.class.to_s, :year => YEAR, :coefs => COEFS); end
end
      __RUBY_SOURCE__
    }
    exit(0)
  end
  
  instances_str = StringIO::new
  models.each{|model|
    instances_str.puts "  static const typename MagneticFieldGeneric<FloatT>::model_t #{model.name};"
  }
  puts <<-__CPP_SOURCE__
template <class FloatT>
class #{prefix}Generic : public MagneticFieldGeneric<FloatT> {
#{instances_str.string}};

typedef #{prefix}Generic<double> #{prefix};
  __CPP_SOURCE__
  
  puts
  
  models.each{|model|
    coefs_str = StringIO::new
    j = 0
    k = 1
    model.coefs.each_with_index{|v, i|
      if i >= j then
        if i > 0 then
          coefs_str.print "// #{k / 2}"
          coefs_str.puts
          coefs_str.print " " * 8
        end
        k += 2
        j += k
      end
      coefs_str.print "#{v}, "
    }
    puts <<-__CPP_SOURCE__
template <class FloatT>
const typename MagneticFieldGeneric<FloatT>::model_t #{prefix}Generic<FloatT>::#{model.name} = {
    "#{model.name}",
    #{model.year},
    #{model.dof},
    {#{coefs_str.string}} // #{model.dof}
    };
    __CPP_SOURCE__
  }
end