#!ruby -Ks
# -*- mode: ruby; encoding: sjis -*-
# Last updated: <2014/06/11 03:02:43 +0900>
#
# NMatrix (NArray) のテスト

require 'benchmark'

require 'matrix' # 標準添付のMatrixライブラリ

require 'narray' # NArray

# require 'narray/narray' # SciRubyのNArray
# require 'nmatrix' # SciRubyのNMatrix

# 8つの係数を求める(Matrix、8x8行列使用版)
#
def get_proj_param(s, d)
  # x0, y0, x1, y1, x2, y2, x3, y3 = d.flatten.map {|item| item.to_f }
  # sx0, sy0, sx1, sy1, sx2, sy2, sx3, sy3 = s.flatten.map {|item| item.to_f }
  x0 = d[0][0].to_f
  x1 = d[1][0].to_f
  x2 = d[2][0].to_f
  x3 = d[3][0].to_f
  y0 = d[0][1].to_f
  y1 = d[1][1].to_f
  y2 = d[2][1].to_f
  y3 = d[3][1].to_f
  sx0 = s[0][0].to_f
  sx1 = s[1][0].to_f
  sx2 = s[2][0].to_f
  sx3 = s[3][0].to_f
  sy0 = s[0][1].to_f
  sy1 = s[1][1].to_f
  sy2 = s[2][1].to_f
  sy3 = s[3][1].to_f

  m = Matrix[
             [ sx0, sy0, 1.0, 0.0, 0.0, 0.0, -x0 * sx0, -x0 * sy0 ],
             [ sx1, sy1, 1.0, 0.0, 0.0, 0.0, -x1 * sx1, -x1 * sy1 ],
             [ sx2, sy2, 1.0, 0.0, 0.0, 0.0, -x2 * sx2, -x2 * sy2 ],
             [ sx3, sy3, 1.0, 0.0, 0.0, 0.0, -x3 * sx3, -x3 * sy3 ],
             [ 0.0, 0.0, 0.0, sx0, sy0, 1.0, -y0 * sx0, -y0 * sy0 ],
             [ 0.0, 0.0, 0.0, sx1, sy1, 1.0, -y1 * sx1, -y1 * sy1 ],
             [ 0.0, 0.0, 0.0, sx2, sy2, 1.0, -y2 * sx2, -y2 * sy2 ],
             [ 0.0, 0.0, 0.0, sx3, sy3, 1.0, -y3 * sx3, -y3 * sy3 ]
            ]

  n = m.inv # 逆行列を取得

  g_a = n[0,0] * x0 + n[0,1] * x1 + n[0,2] * x2 + n[0,3] * x3 + n[0,4] * y0 + n[0,5] * y1 + n[0,6] * y2 + n[0,7] * y3
  g_b = n[1,0] * x0 + n[1,1] * x1 + n[1,2] * x2 + n[1,3] * x3 + n[1,4] * y0 + n[1,5] * y1 + n[1,6] * y2 + n[1,7] * y3
  g_c = n[2,0] * x0 + n[2,1] * x1 + n[2,2] * x2 + n[2,3] * x3 + n[2,4] * y0 + n[2,5] * y1 + n[2,6] * y2 + n[2,7] * y3
  g_d = n[3,0] * x0 + n[3,1] * x1 + n[3,2] * x2 + n[3,3] * x3 + n[3,4] * y0 + n[3,5] * y1 + n[3,6] * y2 + n[3,7] * y3
  g_e = n[4,0] * x0 + n[4,1] * x1 + n[4,2] * x2 + n[4,3] * x3 + n[4,4] * y0 + n[4,5] * y1 + n[4,6] * y2 + n[4,7] * y3
  g_f = n[5,0] * x0 + n[5,1] * x1 + n[5,2] * x2 + n[5,3] * x3 + n[5,4] * y0 + n[5,5] * y1 + n[5,6] * y2 + n[5,7] * y3
  g_g = n[6,0] * x0 + n[6,1] * x1 + n[6,2] * x2 + n[6,3] * x3 + n[6,4] * y0 + n[6,5] * y1 + n[6,6] * y2 + n[6,7] * y3
  g_h = n[7,0] * x0 + n[7,1] * x1 + n[7,2] * x2 + n[7,3] * x3 + n[7,4] * y0 + n[7,5] * y1 + n[7,6] * y2 + n[7,7] * y3

  return [g_a, g_b, g_c, g_d, g_e, g_f, g_g, g_h]
end

# 8つの係数を求める(NMatrix、8x8行列使用版)
#
def get_proj_param_nmat(s, d)
  x0 = d[0][0].to_f
  x1 = d[1][0].to_f
  x2 = d[2][0].to_f
  x3 = d[3][0].to_f
  y0 = d[0][1].to_f
  y1 = d[1][1].to_f
  y2 = d[2][1].to_f
  y3 = d[3][1].to_f
  sx0 = s[0][0].to_f
  sx1 = s[1][0].to_f
  sx2 = s[2][0].to_f
  sx3 = s[3][0].to_f
  sy0 = s[0][1].to_f
  sy1 = s[1][1].to_f
  sy2 = s[2][1].to_f
  sy3 = s[3][1].to_f

  # m = NArray::NMatrix[
  m = NMatrix[
              [ sx0, sy0, 1.0, 0.0, 0.0, 0.0, -x0 * sx0, -x0 * sy0 ],
              [ sx1, sy1, 1.0, 0.0, 0.0, 0.0, -x1 * sx1, -x1 * sy1 ],
              [ sx2, sy2, 1.0, 0.0, 0.0, 0.0, -x2 * sx2, -x2 * sy2 ],
              [ sx3, sy3, 1.0, 0.0, 0.0, 0.0, -x3 * sx3, -x3 * sy3 ],
              [ 0.0, 0.0, 0.0, sx0, sy0, 1.0, -y0 * sx0, -y0 * sy0 ],
              [ 0.0, 0.0, 0.0, sx1, sy1, 1.0, -y1 * sx1, -y1 * sy1 ],
              [ 0.0, 0.0, 0.0, sx2, sy2, 1.0, -y2 * sx2, -y2 * sy2 ],
              [ 0.0, 0.0, 0.0, sx3, sy3, 1.0, -y3 * sx3, -y3 * sy3 ]
                       ]


  n = m.inverse # 逆行列を取得

  g_a = n[0,0] * x0 + n[1,0] * x1 + n[2,0] * x2 + n[3,0] * x3 + n[4,0] * y0 + n[5,0] * y1 + n[6,0] * y2 + n[7,0] * y3
  g_b = n[0,1] * x0 + n[1,1] * x1 + n[2,1] * x2 + n[3,1] * x3 + n[4,1] * y0 + n[5,1] * y1 + n[6,1] * y2 + n[7,1] * y3
  g_c = n[0,2] * x0 + n[1,2] * x1 + n[2,2] * x2 + n[3,2] * x3 + n[4,2] * y0 + n[5,2] * y1 + n[6,2] * y2 + n[7,2] * y3
  g_d = n[0,3] * x0 + n[1,3] * x1 + n[2,3] * x2 + n[3,3] * x3 + n[4,3] * y0 + n[5,3] * y1 + n[6,3] * y2 + n[7,3] * y3
  g_e = n[0,4] * x0 + n[1,4] * x1 + n[2,4] * x2 + n[3,4] * x3 + n[4,4] * y0 + n[5,4] * y1 + n[6,4] * y2 + n[7,4] * y3
  g_f = n[0,5] * x0 + n[1,5] * x1 + n[2,5] * x2 + n[3,5] * x3 + n[4,5] * y0 + n[5,5] * y1 + n[6,5] * y2 + n[7,5] * y3
  g_g = n[0,6] * x0 + n[1,6] * x1 + n[2,6] * x2 + n[3,6] * x3 + n[4,6] * y0 + n[5,6] * y1 + n[6,6] * y2 + n[7,6] * y3
  g_h = n[0,7] * x0 + n[1,7] * x1 + n[2,7] * x2 + n[3,7] * x3 + n[4,7] * y0 + n[5,7] * y1 + n[6,7] * y2 + n[7,7] * y3

  return [g_a, g_b, g_c, g_d, g_e, g_f, g_g, g_h]
end

# 8つの係数を求める(Matrix、4x4 2x2行列使用版)
#
# @note 参考ページ
#       射影変換(ホモグラフィ)について理解してみる　その５ - デジタル・デザイン・ラボラトリーな日々
#       http://yaju3d.hatenablog.jp/entry/2013/09/02/013609
#
def get_param_4x4_2x2(s, d)
  def zz(v); return ((v == 0)? 0.0000000001 : v.to_f); end

  x1 = zz(d[0][0])
  x2 = zz(d[1][0])
  x3 = zz(d[2][0])
  x4 = zz(d[3][0])
  y1 = zz(d[0][1])
  y2 = zz(d[1][1])
  y3 = zz(d[2][1])
  y4 = zz(d[3][1])

  sx1 = zz(s[0][0])
  sx2 = zz(s[1][0])
  sx3 = zz(s[2][0])
  sx4 = zz(s[3][0])
  sy1 = zz(s[0][1])
  sy2 = zz(s[1][1])
  sy3 = zz(s[2][1])
  sy4 = zz(s[3][1])

  txs = Matrix[
               [sx1, sy1, -sx1 * x1, -sy1 * x1],
               [sx2, sy2, -sx2 * x2, -sy2 * x2],
               [sx3, sy3, -sx3 * x3, -sy3 * x3],
               [sx4, sy4, -sx4 * x4, -sy4 * x4]]
  tys = Matrix[
               [sx1, sy1, -sx1 * y1, -sy1 * y1],
               [sx2, sy2, -sx2 * y2, -sy2 * y2],
               [sx3, sy3, -sx3 * y3, -sy3 * y3],
               [sx4, sy4, -sx4 * y4, -sy4 * y4]]

  tx = txs.inverse
  ty = tys.inverse

  kx1 = tx[0,0] * x1 + tx[0,1] * x2 + tx[0,2] * x3 + tx[0,3] * x4
  kx2 = tx[1,0] * x1 + tx[1,1] * x2 + tx[1,2] * x3 + tx[1,3] * x4
  kx3 = tx[2,0] * x1 + tx[2,1] * x2 + tx[2,2] * x3 + tx[2,3] * x4
  kx4 = tx[3,0] * x1 + tx[3,1] * x2 + tx[3,2] * x3 + tx[3,3] * x4
  ky1 = ty[0,0] * y1 + ty[0,1] * y2 + ty[0,2] * y3 + ty[0,3] * y4
  ky2 = ty[1,0] * y1 + ty[1,1] * y2 + ty[1,2] * y3 + ty[1,3] * y4
  ky3 = ty[2,0] * y1 + ty[2,1] * y2 + ty[2,2] * y3 + ty[2,3] * y4
  ky4 = ty[3,0] * y1 + ty[3,1] * y2 + ty[3,2] * y3 + ty[3,3] * y4

  kc1 = tx[0,0] + tx[0,1] + tx[0,2] + tx[0,3]
  kc2 = tx[1,0] + tx[1,1] + tx[1,2] + tx[1,3]
  kc3 = tx[2,0] + tx[2,1] + tx[2,2] + tx[2,3]
  kc4 = tx[3,0] + tx[3,1] + tx[3,2] + tx[3,3]
  kf1 = ty[0,0] + ty[0,1] + ty[0,2] + ty[0,3]
  kf2 = ty[1,0] + ty[1,1] + ty[1,2] + ty[1,3]
  kf3 = ty[2,0] + ty[2,1] + ty[2,2] + ty[2,3]
  kf4 = ty[3,0] + ty[3,1] + ty[3,2] + ty[3,3]

  det_1 = kc3 * (-kf4) - (-kf3) * kc4
  det_1 = 0.0000000001 if det_1 == 0
  det_1 = 1.0 / det_1

  g_c = (-kf4 * det_1) * (kx3 - ky3) + (kf3 * det_1) * (kx4 - ky4)
  g_f = (-kc4 * det_1) * (kx3 - ky3) + (kc3 * det_1) * (kx4 - ky4)

  g_g = kx3 - g_c * kc3
  g_h = kx4 - g_c * kc4
  g_a = kx1 - g_c * kc1
  g_b = kx2 - g_c * kc2
  g_d = ky1 - g_f * kf1
  g_e = ky2 - g_f * kf2

  return [g_a, g_b, g_c, g_d,
          g_e, g_f, g_g, g_h]
end

# 8つの係数を求める(NMatrix、4x4 2x2行列使用版)
#
# @note 参考ページ
#       射影変換(ホモグラフィ)について理解してみる　その５ - デジタル・デザイン・ラボラトリーな日々
#       http://yaju3d.hatenablog.jp/entry/2013/09/02/013609
#
def get_param_4x4_2x2_nmat(s, d)
  def zz(v); return ((v == 0)? 0.0000000001 : v.to_f); end

  x1 = zz(d[0][0])
  x2 = zz(d[1][0])
  x3 = zz(d[2][0])
  x4 = zz(d[3][0])
  y1 = zz(d[0][1])
  y2 = zz(d[1][1])
  y3 = zz(d[2][1])
  y4 = zz(d[3][1])

  sx1 = zz(s[0][0])
  sx2 = zz(s[1][0])
  sx3 = zz(s[2][0])
  sx4 = zz(s[3][0])
  sy1 = zz(s[0][1])
  sy2 = zz(s[1][1])
  sy3 = zz(s[2][1])
  sy4 = zz(s[3][1])

  txs = NMatrix[
                [sx1, sy1, -sx1 * x1, -sy1 * x1],
                [sx2, sy2, -sx2 * x2, -sy2 * x2],
                [sx3, sy3, -sx3 * x3, -sy3 * x3],
                [sx4, sy4, -sx4 * x4, -sy4 * x4]]
  tys = NMatrix[
                [sx1, sy1, -sx1 * y1, -sy1 * y1],
                [sx2, sy2, -sx2 * y2, -sy2 * y2],
                [sx3, sy3, -sx3 * y3, -sy3 * y3],
                [sx4, sy4, -sx4 * y4, -sy4 * y4]]

  tx = txs.inverse
  ty = tys.inverse

  kx1 = tx[0,0] * x1 + tx[1,0] * x2 + tx[2,0] * x3 + tx[3,0] * x4
  kx2 = tx[0,1] * x1 + tx[1,1] * x2 + tx[2,1] * x3 + tx[3,1] * x4
  kx3 = tx[0,2] * x1 + tx[1,2] * x2 + tx[2,2] * x3 + tx[3,2] * x4
  kx4 = tx[0,3] * x1 + tx[1,3] * x2 + tx[2,3] * x3 + tx[3,3] * x4
  ky1 = ty[0,0] * y1 + ty[1,0] * y2 + ty[2,0] * y3 + ty[3,0] * y4
  ky2 = ty[0,1] * y1 + ty[1,1] * y2 + ty[2,1] * y3 + ty[3,1] * y4
  ky3 = ty[0,2] * y1 + ty[1,2] * y2 + ty[2,2] * y3 + ty[3,2] * y4
  ky4 = ty[0,3] * y1 + ty[1,3] * y2 + ty[2,3] * y3 + ty[3,3] * y4

  kc1 = tx[0,0] + tx[1,0] + tx[2,0] + tx[3,0]
  kc2 = tx[0,1] + tx[1,1] + tx[2,1] + tx[3,1]
  kc3 = tx[0,2] + tx[1,2] + tx[2,2] + tx[3,2]
  kc4 = tx[0,3] + tx[1,3] + tx[2,3] + tx[3,3]
  kf1 = ty[0,0] + ty[1,0] + ty[2,0] + ty[3,0]
  kf2 = ty[0,1] + ty[1,1] + ty[2,1] + ty[3,1]
  kf3 = ty[0,2] + ty[1,2] + ty[2,2] + ty[3,2]
  kf4 = ty[0,3] + ty[1,3] + ty[2,3] + ty[3,3]

  det_1 = kc3 * (-kf4) - (-kf3) * kc4
  det_1 = 0.0000000001 if det_1 == 0
  det_1 = 1.0 / det_1

  g_c = (-kf4 * det_1) * (kx3 - ky3) + (kf3 * det_1) * (kx4 - ky4)
  g_f = (-kc4 * det_1) * (kx3 - ky3) + (kc3 * det_1) * (kx4 - ky4)

  g_g = kx3 - g_c * kc3
  g_h = kx4 - g_c * kc4
  g_a = kx1 - g_c * kc1
  g_b = kx2 - g_c * kc2
  g_d = ky1 - g_f * kf1
  g_e = ky2 - g_f * kf2

  return [g_a, g_b, g_c, g_d,
          g_e, g_f, g_g, g_h]
end



def dump_cof_diff(cof1, cof2)
  puts
  s = "abcdefgh"
  8.times do |i|
    d = cof1[i] - cof2[i]
    puts "#{s[i, 1]} = diff: #{d} , #{cof1[i]}, #{cof2[i]}"
  end
end


src = [[0, 0],[512, 0], [512, 512], [0, 512]]
dst = [ [160, 100], [320, 20], [600, 460], [100, 300]]

cof1 = []
cof2 = []
cof3 = []
cof4 = []
n = 10000
Benchmark.bm(20) do |x|
  x.report("Matrix 8x8:      ") { n.times {|i| cof1 = get_proj_param(src, dst)} }
  x.report("Matrix 4x4 2x2:  ") { n.times {|i| cof2 = get_param_4x4_2x2(src, dst)} }

  x.report("NMatrix 8x8:     ") { n.times {|i| cof3 = get_proj_param_nmat(src, dst) } }
  x.report("NMatrix 4x4 2x2: ") { n.times {|i| cof4 = get_param_4x4_2x2_nmat(src, dst) } }

  dump_cof_diff(cof1, cof2)
  dump_cof_diff(cof1, cof3)
  dump_cof_diff(cof1, cof4)
end