;;;; fp-utils.scm  -*- Scheme -*-
;;;; Kon Lovett, Sep '18
;;;; Kon Lovett, May '17

;;;; Issues
;;
;; - all instances of (fl< -0.0 0.0) found ?

(module fp-utils

(;export
  ;
  (fpsum fpsum/list fpsum/vector fpsum/range)
  (fpprod fpprod/list fpprod/vector fpprod/range)
  fp! fp!+ fp!-
  fpharmonic
  ;
  fprandom
  ;
  fpmodulo fpquotient fpremainder
  ;
  fptruncate-with-precision fpround-with-precision
  fpceiling-with-precision fpfloor-with-precision
  fptruncate/to fpround/to fpceiling/to fpfloor/to
  ;
  fpdistance fpdistance*
  ;
  fpmax-and-min
  ;
  fp~= fp~<= fp~>=
  ;
  fpfib*
  fpcross-ratio
  fpleast-squares
  fptrapezoid
  ;DEPRECATED
  fpsummation)

(import scheme)
(import (chicken base))
(import (chicken foreign))
(import (chicken type))
(import (chicken flonum))
(import (chicken fixnum))
(import (srfi 4))
(import fx-inlines fx-utils)
(import fp-inlines)

;;;

(define-type float-vector (or (vector-of float) f32vector f64vector))
(define-type float-points (list-of (pair float float)))

(: C_fmod (float float -> float))
(: C_remainder (float float -> float))

(: fpsum/list ((list-of float) -> float))
(: fpsum/vector (float-vector -> float))
(: fpsum/range ((float -> float) float float -> float))
(: fpprod/list ((list-of float) -> float))
(: fpprod/vector (float-vector -> float))
(: fpprod/range ((float -> float) float float -> float))
(: fp! (float -> float))
(: fp!- (float float -> float))
(: fp!+ (float float -> float))
(: fpharmonic (float -> float))

(: fpmodulo (float float -> float))
(: fpquotient (float float -> float))
(: fpremainder (float float -> float))
(: fptruncate-with-precision (float #!optional (or float fixnum) -> float))
(: fpround-with-precision (float #!optional (or float fixnum) -> float))
(: fpceiling-with-precision (float #!optional (or float fixnum) -> float))
(: fpfloor-with-precision (float #!optional (or float fixnum) -> float))
(: fpdistance (float float float float -> float))
(: fpdistance* (float float float float -> float))
(: fpquo-and-rem (float float -> float float))
(: fpmax-and-min (float #!rest float -> float float))
(: fprandom (#!optional (or float fixnum) (or float fixnum) -> float))
(: fp~= (float float #!optional float -> boolean))
(: fp~<= (float float #!optional float -> boolean))
(: fp~>= (float float #!optional float -> boolean))
(: fpfib* (float -> float))
(: fpcross-ratio (float float float float -> float))
(: fpleast-squares (float-points -> float float))
(: fptrapezoid ((float -> float) float float -> (integer -> float)))

(: fpsummation (deprecated fpsum))

;;type-errors-basic

(declare (bound-to-procedure ##sys#signal-hook #;##sys#error-hook))

(define (signal-bounds-error loc . objs)
  (apply ##sys#signal-hook #:bounds-error loc objs) )

(define (signal-type-error loc . objs)
  (apply ##sys#signal-hook #:type-error loc objs) )

;;

;miscmacros
(define-syntax let/cc
  (syntax-rules ()
    ((let/cc k e0 e1 ...)
     (call-with-current-continuation
      (lambda (k) e0 e1 ...)))))

;https://oeis.org/A002163
(define-constant SQRT5 2.23606797749978969640917366873127623544061835961152572427089724541052092563780489941441440837878227)

#;
(define (vector-every? v p?)
  (let/cc return
    (let ((ed (vector-length o)))
      (do ((i 0 (fxadd1 i)))
         ((= i ed) #t)
        (unless (p? (vector-ref v i)) (return #f) ) ) ) ) )

(define ((vector-of? p?) o)
  (and (vector? o)
       (or (not (fxzero? (vector-length o))) (p? (vector-ref o 0)))
       #; ;kind much
       (vector-every? o p?)) )

;pairwise length cutof - >= 1
(define-constant PW-N 64)

(define flonum-vector? (vector-of? flonum?))

(define (vector-ops loc v)
  (cond ((flonum-vector? v) (values vector-length vector-ref))
        ((f32vector? v) (values f32vector-length f32vector-ref))
        ((f64vector? v) (values f64vector-length f64vector-ref))
        (else
          (signal-type-error loc
            "bad argument type - not a (vector-of float), f32vector, or f64vector"
            v))) )

;uses Neumaier summation formula or Pairwise

(define (*fpsum s c i recur cur nxt)
  (let* ((in (cur i))
         (t (fp+ s in)) )
    (if (fp>= (fpabs s) (fpabs in))
        ;then sum is bigger, low-order digits of input[i] are lost.
        (recur t (fp+ c (fp+ (fp- s t) in)) (nxt i))
        ;else low-order digits of sum are lost.
        (recur t (fp+ c (fp+ (fp- in t) s)) (nxt i))) ) )

(define (fpsum/list ls)
  (if (null? ls) 0.0
      (let sum ((s (car ls)) (c 0.0) (ls (cdr ls)))
        (if (null? ls) (fp+ s c)
            (*fpsum s c ls sum car cdr) ) ) ) )

(define-inline (*fpsum/vector vc lenof refof)
  (let ((n (lenof vc)))
    (if (fxzero? n) 0.0
        (let pw ((st 0) (ed n))
          (assert (fx< st ed))
          (let ((n (fx- ed st)))
            (if (fx<= n PW-N)
                ;then use error-correcting sum
                (let sum ((s (refof vc st)) (c 0.0) (i (fxadd1 st)))
                  (if (fx= i ed) (fp+ s c)
                      (*fpsum s c i sum (cut refof vc <>) fxadd1) ) )
                ;else pairwise
                (let ((m (fx/ n 2)))
                  (fp+ (pw st m) (pw m ed)) ) ) ) ) ) ) )

(define (fpsum/vector v)
  (let-values (((lenof refof) (vector-ops 'fpsum/vector v)))
    (*fpsum/vector v lenof refof)) )

(define fpsummation fpsum/list)

(define (fpsum/range f st ed)
  (unless (fp< st ed)
    (signal-bounds-error 'fpsum/ramge "bounds mismatch" st ed) )
  (let sum ((s (f st)) (c 0.0) (i (fpadd1 st)))
    (if (fp>= i ed) (fp+ s c)
        (*fpsum s c i sum f fpadd1) ) ) )

(define-syntax fpsum
  (syntax-rules ()
    ((fpsum ?x)
      (let ((x ?x))
        (if (list? x) (fpsum/list x) (fpsum/vector x))) )
    ((fpsum ?f ?st ?ed)
      (fpsum/range ?f ?st ?ed) ) ) )

;derived Neumaier product formula or Pairwise

(define (*fpprod p c i recur cur nxt)
  (let* ((in (cur i))
         (t (fp* p in)) )
    (if (fp>= (fpabs p) (fpabs in))
        ;then product is bigger, low-order digits of input[i] are lost.
        (recur t (fp* c (fp* (fp/ p t) in)) (nxt i))
        ;else low-order digits of product are lost.
        (recur t (fp* c (fp* (fp/ in t) p)) (nxt i))) ) )

(define (fpprod/list ls)
  (if (null? ls) 1.0
      (let prod ((p (car ls)) (c 1.0) (ls (cdr ls)))
        (if (null? ls)  (fp* p c)
            (*fpprod p c ls prod car cdr) ) ) ) )

(define-inline (*fpprod/vector vc lenof refof)
  (let ((n (lenof vc)))
    (if (fxzero? n) 1.0
        (let pw ((st 0) (ed n))
          (assert (fx< st ed))
          (let ((n (fx- ed st)))
            (if (fx<= n PW-N)
                ;then use error-correcting product
                (let prod ((p (refof vc st)) (c 1.0) (i (fxadd1 st)))
                  (if (fx= i ed)(fp* p c)
                      (*fpprod p c i prod (cut refof vc <>) fxadd1) ) )
                ;else pairwise
                (let ((m (fx/ n 2)))
                  (fp* (pw st m) (pw m ed)) ) ) ) ) ) ) )

(define (fpprod/vector v)
  (let-values (((lenof refof) (vector-ops 'fpprod/vector v)))
    (*fpprod/vector v lenof refof)) )

(define (fpprod/range f st ed)
  (unless (fp< st ed)
    (signal-bounds-error 'fpprod/ramge "bounds mismatch" st ed) )
  (let prod ((p (f st)) (c 1.0) (i (fpadd1 st)))
    (if (fp>= i ed) (fp* p c)
        (*fpprod p c i prod f fpadd1) ) ) )

(define-syntax fpprod
  (syntax-rules ()
    ((fpprod ?x)
      (let ((x ?x))
        (if (list? x) (fpprod/list x) (fpprod/vector x))) )
    ((fpprod ?f ?st ?ed)
      (fpprod/range ?f ?st ?ed) ) ) )

(define (fp! x) (fpprod/range identity 1.0 (fpadd1 x)))
(define (fp!- x fall) (fpprod/range identity (fp- x (fpsub1 fall)) (fpadd1 x)))
(define (fp!+ x rise) (fpprod/range identity x (fp+ x rise)))

(define (fpharmonic n) (fpsum/range / 1.0 (fp+ n 2.0)))

;;;

(define C_fmod (foreign-lambda double "fmod" double double))

(define C_remainder (foreign-lambda double "remainder" double double))

;;

(define (fpmodulo x y) (fptruncate (C_fmod x y)))

(define (fpquotient x y) (fptruncate (fp/ x y)))

(define (fpremainder x y) (fptruncate (C_remainder x y)))

;;;

;(so no conversion)
(define-constant PRECISION-DEFAULT 4.0)

;FIXME naive at best

(define-syntax make-unary-with-precision
  (syntax-rules ()
    ((make-unary-with-precision ?op)
      (lambda (n #!optional (p PRECISION-DEFAULT))
        (let ((_p (exact->inexact p)))
          (if (fpzero? _p)
              (?op n)
              (let ((pf (fpprecision-factor _p)))
                ;this division introduces unwanted precision bits
                (fp/ (?op (fp* n pf)) pf) ) ) ) ) ) ) )

;;

(define fptruncate-with-precision (make-unary-with-precision fptruncate))
(define fpround-with-precision (make-unary-with-precision fpround))
(define fpceiling-with-precision (make-unary-with-precision fpceiling))
(define fpfloor-with-precision (make-unary-with-precision fpfloor))

(define fptruncate/to fptruncate-with-precision)
(define fpround/to fpround-with-precision)
(define fpceiling/to fpceiling-with-precision)
(define fpfloor/to fpfloor-with-precision)

;;

(define (fpdistance x1 y1 x2 y2) (fpsqrt (fpdistance* x1 y1 x2 y2)))

(define (fpdistance* x1 y1 x2 y2) (fp+ (fpsqr (fp- x1 x2)) (fpsqr (fp- y1 y2))))

;;

(define (fpquo-and-rem fpn fpd) (values (fpquotient fpn fpd) (fpremainder fpn fpd)))

;;

(define (fpmax-and-min fp . fps)
  (let loop ((fps fps) (mx fp) (mn fp))
    (if (null? fps) (values mx mn)
        (let ((cur (car fps)))
          (loop (cdr fps) (fpmax mx cur) (fpmin mn cur)) ) ) ) )

;;

(define-inline (*fpinv x) (if (fpzero? x) 0.0 (fp/ 1.0 x)))
(define-inline (*fpinvfx x) (inexact->exact (*fpinv x)))
(define-inline (*fxinvfp x) (*fpinv (exact->inexact x)))

(define (fprandom #!optional (lim most-positive-fixnum) (low 0))
  (cond ((and (fixnum? lim) (fixnum? low))
          (if (fx>= low lim)
              (error 'fprandom "range swapped" lim low)
              ;invert maps fx to fp result
              (*fxinvfp (fxrandom lim low)) ) )
        ((and (flonum? lim) (flonum? low))
          (if (fp>= low lim)
              (error 'fprandom "range swapped" lim low)
              ;invert & swap maps fp to fx parameters
              ;result will be in [low .. lim]
              (fprandom (*fpinvfx low) (*fpinvfx lim)) ) )
        (else
          (error 'fprandom "no mixed-mode" lim low) ) ) )

;;

(define (fp~= x y #!optional (eps flonum-epsilon))
  ;NOTE minimum/maximum-flonum is smallest/largest positive normal flonum
  (or (fp= x y)
      (let* ((d (fp- x y))
             (abs-d (fpabs d)) )
              ;either 0 argument or << relative error?
        (cond ((or (fpzero? x) (fpzero? y) (fp< abs-d minimum-flonum))
                (fp< abs-d (fp* eps minimum-flonum)) )
              ;ensure |x| <= |y| so no /0
              ((fp> (fpabs x) (fpabs y))
                (fp~= y x eps) )
              (else
                (fp< (fpabs (fp/ d y)) eps) ) ) ) ) )

(define (fp~<= x y #!optional (eps flonum-epsilon)) (or (fp< x y) (fp~= x y eps)))

(define (fp~>= x y #!optional (eps flonum-epsilon)) (or (fp> x y) (fp~= x y eps)))

;;

(define-constant 1/SQRT5 (fp/ 1.0 SQRT5))
(define-constant MID1+SQRT5 (fp/ (fp+ 1.0 SQRT5) 2.0))
(define-constant MID1-SQRT5 (fp/ (fp- 1.0 SQRT5) 2.0))

(define (fpfib* n)
  (cond ((fpzero? n)
          0.0)
        ((fpnegative? n)
          (fpfib* (fpneg (fp/ 1.0 n))))
        (else
          (fpfloor (fp- (fp* 1/SQRT5 (expt MID1+SQRT5 n))
                        (fp* 1/SQRT5 (expt MID1-SQRT5 n))))) ) )

(define (fpcross-ratio a b c d)
  (fp/ (fp* (fp- a c) (fp- b d))
       (fp* (fp- a d) (fp- b c))) )

(define (fpleast-squares points)
  (do ((points points (cdr points))
       (sigma-x 0 (fp+ sigma-x (caar points)))
       (sigma-y 0 (fp+ sigma-y (cdar points)))
       (sigma-x-squared 0 (fp+ sigma-x-squared (fpsqr (caar points))))
       (sigma-xy 0 (fp+ sigma-xy (fp* (caar points) (cdar points))))
       (count 0 (fxadd1 count)) )
      ((null? points)
       (let* ((m (fp/ (fp- (fp* count sigma-xy) (fp* sigma-x sigma-y))
                      (fp- (fp* count sigma-x-squared) (fp* sigma-x sigma-x))))
              (b (fp/ (fp- sigma-y (fp* m sigma-x)) count)))
         (values m b))) ) )

(define (fptrapezoid f a b)
  (let ((b-a (fp- b a)))
    (lambda (n)
      (let* ((n (exact->inexact n))
             (dx (fp/ b-a n)) )
        (define (l i sum)
          (if (fp>= i n) (fp* sum dx)
              (let ((x (fp+ a (fp* i dx))))
                (l (fpadd1 i) (fp+ sum (f x))))))
          (l 1.0 (fp/ (fp+ (f a) (f b)) 2.0)) ) ) ) )

) ;fp-utils