(import scheme chicken)
(require-library srfi-1 extras)
(import (only srfi-1 fold every)
        (only extras printf))
(require-extension srfi-4)

; Blit FROM[I,END) to TO[J,?].

(define (srfi-4-vector-blit! vector-ref vector-set! )
  (lambda (from i end to j)      
    (assert (< i end))
    (let recur ((i i) (j j))
      (if (< i end)
          (let ((vi (vector-ref from i)))
            (vector-set! to j vi)
            (recur (+ i 1) (+ j 1)))
          ))
      ))


(define f64vector-blit!
  (srfi-4-vector-blit! f64vector-ref f64vector-set!))


;; Given array A and indices p, q, r such that p < q < r, 
;; merge subarray A[p..q) and subarray A[q..r) into array B[n..]

(define (srfi-4-vector-merge! vector-ref vector-set! vblit! vector-length)
  (lambda (elt< a p q r b n)
    (assert (and (< p q) (< q r) (<= (vector-length a) r)))
    (let recur ((i p) (j q) (k n))
      (if (and (< i q) (< j r))
          (let ((ai (vector-ref a i))
                (bj (vector-ref a j)))
            (if (elt< i ai j bj)
                (begin
                  (vector-set! b k ai)
                  (recur (+ 1 i) j (+ 1 k)))
                (begin
                  (vector-set! b k bj)
                  (recur i (+ 1 j) (+ 1 k)))
                ))
          (if (< i q)
              (vblit! a i q b k)
              (if (< j r)
                  (vblit! a j r b k))))
      b
      ))
  )



(define f64vector-merge!
  (srfi-4-vector-merge! f64vector-ref f64vector-set! f64vector-blit! f64vector-length ))

(define (elt< i vi j vj) (< vi vj))

(define a (make-f64vector (inexact->exact 1e6) 1.))
(define b (make-f64vector (inexact->exact 1e6) .))

(time (f64vector-merge! elt< a 0 (inexact->exact 5e5) (inexact->exact 1e6) b 0))