#!/usr/local/bin/ruby -w
#
# vim:sw=2 ts=8:et sta:fdm=marker
#
#
# Copyright (c) 2007 Ariff Abdullah <ariff@FreeBSD.org>
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
# 1. Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in the
#    documentation and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
# OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
# OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
# SUCH DAMAGE.
#
#        $MyBSD$
#
# Date: Fri Jul 13 15:48:57 MYT 2007
#   OS: FreeBSD yuki.MyBSD.org.my 6.2-STABLE i386
#

require 'mkfilter'

nmult   = Z_MULT[Z_QUALITY_5]
rolloff = 0.80
beta    = 10.06
nq      = Z_DRIFT_ONE

Z_DCOEFF = []
prev = nil
Z_COEFF = makeFilter(nmult, rolloff, beta, nq)
Z_COEFF.collect! do |v|
  vv = (v * (1 << Z_COEFF_SHIFT)).floor()
  if prev != nil
    Z_DCOEFF << vv - prev
  end
  prev = vv
  vv
end
Z_DCOEFF << 0
#Z_COEFF = []
#COEFF_FLOAT.each do |v| Z_COEFF << (v * (1 << Z_SHIFT)).floor end

Z_COEFF_SIZE = Z_COEFF.size()

class ZResampler
  RESERVOIR     = 8192
  RESERVOIR_MAX = 131072
  ZOH       = 0
  LINEAR    = 1
  SINC      = 2
  GAIN = (Z_GAIN_SCALE * (1 << Z_GAIN_SHIFT)).floor()
  def initialize(rtype, inrate, outrate, channels, bps)
    @inrate = inrate
    @outrate = outrate
    div = gcd(@inrate, @outrate)
    @gx = @inrate / div
    @gy = @outrate / div
    @channels = channels
    @bps = bps
    @alpha = 0
    @gx_total = 0
    @gy_total = 0
    @z_dx = -1
    @z_dy = Z_FULL_ONE
    @z_scale = Z_ONE

    case rtype
      when ZOH
        @z_size = 1
        @z_func = method(:z_feed_zoh)
      when LINEAR
        @z_size = 1
        @z_func = method(:z_feed_linear)
      when SINC
        fact = (@gy << Z_SHIFT) / @gx
        if fact < Z_ONE
          @z_dy = ((fact << Z_FULL_SHIFT) + (Z_ONE >> 1)) >> Z_SHIFT
          @z_scale = fact
        else
          @z_dy = Z_FULL_ONE
          @z_scale = Z_ONE
        end
        @z_dx = @z_dy / @gy
        #@z_size = (Z_COEFF_SIZE - 4) / (@z_dy >> Z_SHIFT)
        @z_size = ((Z_COEFF_SIZE << (Z_SHIFT << 1)) / @z_dy) >> Z_SHIFT;
        #@z_size = (Z_COEFF_SIZE - 1) / (@z_dy >> Z_SHIFT)
        #if @z_size != ((Z_COEFF_SIZE - 2) / (@z_dy >> Z_SHIFT))
        #       raise
        #end
        @z_func = method(:z_feed_sinc)
    else
      raise ArgumentError, "invalid resample method '#{rtype}'"
    end

    @z_type = rtype

    p [:ZResampler, "z_size=#{@z_size}"]
    minreq = gy2gx(1)
    @z_full = roundpow2((@z_size * 2) + minreq)
    while @z_full < RESERVOIR_MAX && (@z_full - (@z_size * 2)) < RESERVOIR
      ozf = @z_full
      @z_full <<= 1
      p [:Circular_Buffer_Resize, ozf, @z_full]
    end
    p [:Circular_Buffer_Final_Size, @z_full, :Min_Request, minreq]

    @z_mask = @z_full - 1
    @z_delay = Array.new(@z_full * @channels, 0)

    @z_start = z_prev(@z_size * 2, 1)
    @z_pos = z_next(@z_start, 1)

    @z_min = -(1 << ((@bps * 8) - 1))
    @z_max = -(@z_min + 1)

    # stats
    @b_alpha = 0
  end
  attr_reader :gx, :gy, :alpha, :gx_total, :gy_total, :b_alpha
  attr_reader :z_size, :z_pos, :z_last, :z_full, :z_mask, :z_delay
  def gy2gx(v)
    #((v * @dt) + Z_MASK - @z_time) >> Z_SHIFT
    ((@gx * v) + @gy - @alpha - 1) / @gy
  end
  def gx2gy(v)
    (@alpha + (v * @gy)) / @gx
  end
  def xydrift(xv, yv)
    (xv * @gy) - (yv * @gx)
  end
  def feed(fd, obuf, v)
    v /= @channels
    return 0 if v == 0
    ocount = 0
    reqin = gy2gx(v) - z_fetched()
    sdrift = (@gx + @gy - 1) / @gy
    adrift = xydrift(sdrift, 1)
    raise if reqin < 0
    @buf ||= []
    while true
      #p [:DATA, @gx, @gy, @alpha, :DATA]
      if reqin > 0
        zf = z_free()
        fetch = min(zf, reqin)
        if fetch == 0
          fetched = z_fetched()
          start = @z_start - (@z_size * 2) + 1
          cp = ((@z_size * 2) + fetched) - 1
          0.upto(cp) do |i|
            0.upto(@channels - 1) do |ch|
              @z_delay[(i * @channels) + ch] =
                @z_delay.fetch(((i + start) * @channels) + ch)
            end
          end
          #p [:Circular_Buffer_Move, @z_start, @z_pos, cp, reqin, zf, fetched]
          #@z_start = (@z_size * 2) - 1
          #@z_pos = @z_start + fetched + 1
          @z_start = z_prev(@z_size * 2, 1)
          @z_pos = z_next(@z_start, fetched + 1)
          #@z_pos = z_next(@z_size * 2, fetched)
          zf = z_free()
          fetch = min(zf, reqin)
        end
        if fetch != 0
          @buf.replace(fd.read(fetch * @channels * @bps).unpack('s*'))
          fsz = @buf.size() / @channels
          raise if fsz > fetch
          return ocount * @channels if fsz == 0
          reqin -= fsz
          0.upto(fsz - 1) do |i|
            0.upto(@channels - 1) do |ch|
              @z_delay[(@z_pos * @channels) + ch] = @buf[(i * @channels) + ch]
            end
            @z_pos += 1
            #@z_pos &= @z_mask
            raise if @z_pos > @z_full
          end
          if fsz != fetch
            reqin = 0
          end
        end
      end
      reqout = min(gx2gy(z_fetched()), v - ocount)
      raise if reqout < 0
      while reqout > 0
        @z_func.call(@z_start, ocount, obuf)
        @alpha += adrift
        if @alpha < @gy
          drift = sdrift
        else
          drift = sdrift - 1
          @alpha -= @gy
        end
        @z_start += drift
        #drift = gy2gx(1)
        #p [:DATA, @gx, @gy, :DATA]
        #@alpha += xydrift(drift, 1)
        @b_alpha = @alpha if @alpha > @b_alpha
        raise "alpha=#{alpha} < 0!" if @alpha < 0
        #@z_start += drift
        #@z_start &= @z_mask
        raise if @z_start > @z_full
        @gx_total += drift
        ocount += 1
        reqout -= 1
      end
      #p [:DATA, @z_start, @z_pos, @z_full, :DATA] if z_fetched() == 0
      break if reqin == 0 || ocount == v
    end

    raise "reqin=#{reqin} != 0" if reqin != 0
    raise "z_delay corrupted" if @z_delay.size != @z_full * @channels
    @gy_total += ocount
    raise "ocount=#{ocount} > #{v}" if ocount > v
    ocount * @channels
  end
  def z_feed_zoh(pos, opos, obuf)
    0.upto(@channels - 1) do |ch|
      obuf[(opos * @channels) + ch] = @z_delay.fetch((pos * @channels) + ch)
    end
  end
  def z_feed_linear(pos, opos, obuf)
    d1 = (@alpha << Z_LINEAR_SHIFT) / @gy
    d2 = Z_LINEAR_ONE - d1
    prev = z_prev(pos, 1)
    0.upto(@channels - 1) do |ch|
      v = (@z_delay.fetch((pos * @channels) + ch) << 8) * d2
      v += (@z_delay.fetch((prev * @channels) + ch) << 8) * d1
      v >>= Z_LINEAR_SHIFT
      obuf[(opos * @channels) + ch] = v >> 8
    end
  end
  def z_feed_sinc(pos, opos, obuf)
    center = z_prev(pos, @z_size)
    #d1 = (@alpha << Z_SHIFT) / @gy
    #d2 = Z_ONE - d1
    #d1 = (d1 * @z_dy) >> Z_SHIFT
    #d2 = (d2 * @z_dy) >> Z_SHIFT
    d1 = @alpha * @z_dx
    d2 = @z_dy - d1
    #d1 <<= Z_DRIFT_SHIFT
    #d2 <<= Z_DRIFT_SHIFT
    0.upto(@channels - 1) do |ch|
      z1 = d1
      z2 = d2
      c1 = 0
      c2 = 0
      v = 0
      #j = [:center, @z_pos, :center]
      1.upto(@z_size) do |i|
        c1 += z1 >> Z_SHIFT
        z1 &= Z_MASK
        #coef = Z_COEFF[c1] +
        #    ((z1 * (Z_COEFF[c1 + 1] - Z_COEFF[c1])) >> Z_COEFF_SHIFT)
        coef = Z_COEFF[c1] + ((Z_DCOEFF[c1] * (z1 >> Z_COEFF_UNSHIFT)) >> Z_COEFF_SHIFT)
        z = z_next(center, i)
        vv = (coef * (@z_delay.fetch((z * @channels) + ch) << 8)) >> Z_COEFF_SHIFT
        #intcheck(vv, __LINE__)
        #v += (vv * @z_scale) >> Z_SHIFT
        v += vv
        #intcheck(v, __LINE__)
        #raise "1: #{z} #{@z_start}" if @z_start < z
        #j << [z1, coef, z]

        #coef = Z_COEFF[c2] +
        #    ((z2 * (Z_COEFF[c2 + 1] - Z_COEFF[c2])) >> Z_COEFF_SHIFT)
        c2 += z2 >> Z_SHIFT
        z2 &= Z_MASK
        coef = Z_COEFF[c2] + ((Z_DCOEFF[c2] * (z2 >> Z_COEFF_UNSHIFT)) >> Z_COEFF_SHIFT)
        z = z_prev(center, i - 1)
        vv = (coef * (@z_delay.fetch((z * @channels) + ch) << 8)) >> Z_COEFF_SHIFT
        #intcheck(vv, __LINE__)
        #v += (vv * @z_scale) >> Z_SHIFT
        v += vv
        #intcheck(v, __LINE__)
        #raise "2: #{z} #{@z_start}" if @z_start < z
        #j.unshift([z2, coef, z])
        #intcheck(v, __LINE__)
        #intcheck(z1, __LINE__)
        #intcheck(z2, __LINE__)
        z1 += @z_dy
        z2 += @z_dy
      end
      if @z_scale != Z_ONE
        v = (v * @z_scale) >> Z_SHIFT
      end
      #p j
      #exit
      #if @fact < Z_ONE
      #  v *= @fact
        #intcheck(v, __LINE__)
      #  v >>= Z_SHIFT
      #end
      obuf[(opos * @channels) + ch] = z_clamp(v >> 8)
    end
  end
  def z_next(off, v)
    (off + v) & @z_mask
    #(off + v) % @z_full
  end
  def z_prev(off, v)
    (off - v) & @z_mask
    #(off - v) % @z_full
  end
  def z_fetched
    ((@z_pos - @z_start) & @z_mask) - 1
    #((@z_pos - @z_start) % @z_full) - 1
  end
  def z_free
    @z_full - @z_pos
  end
  def Z_SCALE(v)
    (GAIN * v) >> Z_GAIN_SHIFT
  end
  private
  def z_clamp(v)
    (v > @z_max) ? @z_max : ((v < @z_min) ? @z_min : v)
  end
  def intcheck(v, line)
    if v > 0x7fffffff
      printf("line: %d - 32bit overflow [%d]\n", line, v)
    elsif v < -0x80000000
      printf("line: %d - 32bit underflow [%d]\n", line, v)
    elsif v > 0x7fffffffffffffff
      printf("line: %d - 64bit overflow [%d]\n", line, v)
    elsif v < -0x8000000000000000
      printf("line: %d - 64bit underflow [%d]\n", line, v)
    end
  end
  def min(x, y)
    (x < y) ? x : y
  end
  def max(x, y)
    (x > y) ? x : y
  end
  def roundpow2(v)
    i = 0
    while (1 << i) < v
      i += 1
    end
    1 << i
  end
  def gcd(tx, ty)
    x = tx
    y = ty
    while y != 0
      w = x % y
      x = y;
      y = w;
    end
    x
  end
end

#
# simulate IO irregularity
#
class FakeIO
  def initialize(f)
    @fd = File.open(f, 'r')
    @idx = 0
    @buf = ''
  end
  def read(sz)
    rsz = srand(128)
    rsz &= ~3
    rsz = 4 if rsz < 4
    rsz = sz if sz < rsz
    @idx += 1
    if @idx == 9
      @idx = 0
      rsz = 0
    end
    begin
      @buf.replace(@fd.sysread(rsz))
    rescue TypeError
      @buf.replace('')
    end
    @buf
  end
  def stat
    @fd.stat()
  end
  def seek(pos, set)
    @fd.sysseek(pos, set)
  end
  def close
    @fd.close()
  end
end

from = 44100
to = 48000
bps = 2
channels = 2
file = 'udial.wav'
ofile = 'x.raw'
header = 44
osize = 128

r = ZResampler.new(ZResampler::SINC, from, to, channels, bps)
obuf = Array.new(osize, 0)
fd = FakeIO.new(file)
fsz = fd.stat.size() - header
fd.seek(header, IO::SEEK_SET)
fdo = File.open(ofile, 'w')

#=begin
begin
  STDOUT.sync = true
  total = 0

  while true
    cnt = r.feed(fd, obuf, osize)
    #break if cnt == 0
    total += cnt
    fdo.syswrite(obuf.slice(0, cnt).pack('s*'))
    STDOUT.printf("Bytes in: [%d / %d] [%d/%d:%d]\r",
      fsz, header + (r.gx_total * channels * bps), r.gx, r.gy, r.b_alpha)
    STDOUT.flush()
    #obuf.collect! do |v| 0 end
  end
  printf("\nFINISH\n")
rescue EOFError
  printf("\nEOF\n")
rescue
  printf("\nERROR\n")
  p [$!.message, $!.backtrace]
end
fd.close()
fdo.close()
#1.upto(159) do |v|
#  r.feed()
#  printf("%10d: [%10d in:out %-10d] [z_pos: %10d]\n",
#    v, r.gx_total, r.gy_total, r.z_pos)
#end

p [:TOTAL, (r.gy_total.to_f * from.to_f / to.to_f).ceil, [r.gx_total, r.gy_total], r.b_alpha]
#=end