class Array
  def strictly_mon_inc?
    self.each_cons(2).all? { |lo, hi| lo < hi }
  end
end

class Ecu
  class Interpolator
    def self.interp1(x_ref, y_ref, x)
      raise ArgumentError unless x_ref.size == y_ref.size
      raise ArgumentError unless x_ref.strictly_mon_inc?

      return y_ref.first if x < x_ref.first
      return y_ref.last  if x > x_ref.last

      x1, x2, fx = find_position(x_ref, x)

      interp(y_ref[x1], y_ref[x2], fx)
    end

    def self.interp2(x_ref, y_ref, z_ref, x, y)
      raise ArgumentError unless z_ref.size == y_ref.size
      raise ArgumentError unless z_ref.all? { |row| row.size == x_ref.size }
      raise ArgumentError unless x_ref.strictly_mon_inc?
      raise ArgumentError unless y_ref.strictly_mon_inc?

      x1, x2, fx = find_position(x_ref, x)
      y1, y2, fy = find_position(y_ref, y)

      interp(interp(z_ref[y1][x1], z_ref[y2][x1], fy),
             interp(z_ref[y1][x2], z_ref[y2][x2], fy),
             fx)
    end

    private

    def self.find_position(ref, x)
      x = x.to_f
      n = ref.size - 1
      return [0, 0, 0] if x < ref.first
      return [n, n, 0] if x > ref.last

      ref.each_cons(2).each do |lo, hi|
        if (lo..hi).cover?(x)
          f = (x - lo)/(hi - lo)
          return [ref.index(lo), ref.index(hi), f]
        end
      end
    end

    def self.interp(x, y, f)
      x*(1-f) + y*f
    end
  end
end