(use numbers srfi-4 parametric-curve matchable)

(define machine-epsilon
  (let loop ((e 1.0))
     (if (= 1.0 (+ e 1.0))
         (* 2 e)
         (loop (/ e 2)))))

(printf "machine-epsilon = ~A~%" machine-epsilon)

(define my-c (linear-curve 3 '((0 3) (0 4) (1 0)) 0 20))
(define my-scaled-c (scale-curve '(1. 1. 5.) my-c))

(print "machine-epsilon = " machine-epsilon)

(define my-c (linear-curve 4 '((1 3) (1 4) (1 0)) 0 20))

(printf "my-c (5) = ~A~%" ((sample-curve my-c) 5))
(printf "my-c (0,5,10,15,20) = ~A~%" ((sample-curve* my-c) (list 0 5 10 15 20)))
(printf "my-c bbox = ~A~%" (bbox-curve my-c))

(assert (every (lambda (x y) 
                 (printf "x = ~A y = ~A abs (x-y) = ~A~%" x y (abs (- x y) ))
                 (<= (abs (- x y)) machine-epsilon))
               ((sample-curve my-c) 1.) (list 4. 5. 1.)))

(assert (every
	 (lambda (xv yv) 
           (every (lambda (x y) 
                    (printf "x = ~A y = ~A abs (x-y) = ~A~%" x y (abs (- x y) ))
                    (<= (abs (- x y)) machine-epsilon))
                (f64vector->list xv) (f64vector->list yv)))

	 ((sample-curve* my-c) (list 0. 5. 10. 15. 20.)) 
	 (list (f64vector 3.0 8.0 13.0 18.0 23.0)
	       (f64vector 4.0 9.0 14.0 19.0 24.0)
	       (f64vector 0.0 5.0 10.0 15.0 20.0)
	       )))

(define my-scaled-c (scale-curve '(1. 1. 5.) my-c))

(printf "my-scaled-c (5) = ~A~%" ((sample-curve my-scaled-c) 5))
(printf "my-scaled-c (0,5,10,15,20) = ~A~%" ((sample-curve* my-scaled-c) (list 0 5 10 15 20)))
(printf "my-scaled-c bbox = ~A~%" (bbox-curve my-scaled-c))

(assert (every (lambda (x y)
                 (printf "x = ~A y = ~A abs (x-y) = ~A~%" x y (abs (- x y) ))
                 (<= (abs (- x y)) machine-epsilon))
               ((sample-curve my-scaled-c) 1.) 
               (list 4. 5. 5.)))
(assert (every
	 (lambda (xv yv) (map (lambda (x y) (< (abs (- x y)) machine-epsilon)) 
                              (f64vector->list xv) (f64vector->list yv)))
	 ((sample-curve* my-c) (list 0 5 10 15 20)) 
	 (list (f64vector 3.0 3.0 3.0 3.0 3.0)
	       (f64vector 4.0 4.0 4.0 4.0 4.0)
	       (f64vector 0.0 5.0 10.0 15.0 20.0)
	       )))

(assert (every (lambda (x y) 
                 (printf "x = ~A y = ~A abs (x-y) = ~A~%" x y (abs (- x y) ))
                 (< (abs (- x y)) machine-epsilon))
               (list 8. 9. 25.)
               ((sample-curve my-scaled-c) 5) 
               ))

(assert (every
	 (lambda (xv yv) (map (lambda (x y) (< (abs (- x y)) machine-epsilon)) 
                              (f64vector->list xv) (f64vector->list yv)))
	 ((sample-curve* my-scaled-c) (list 0 5 10 15 20)) 
	 (list (f64vector 3.0 3.0 3.0 3.0 3.0)
	       (f64vector 4.0 4.0 4.0 4.0 4.0)
	       (f64vector 0.0 25.0 50.0 75.0 100.0)
	       )))

(printf "my-scaled-c = ~A~%" my-scaled-c)

(printf "iterate my-scaled-c = ~A~%" (iterate-curve my-scaled-c 5))


(printf "fold my-scaled-c = ~A~%" 
        (fold-curve my-scaled-c 5 
           (match-lambda* (((x y z) init) (cons (list (* x 10) (* y 20) (* z 30)) init)))
           '()
           ))


(define s (line-segment 3 (list 1 2 3)))

(printf "line segment = ~A~%" s)

(printf "line segment arc length (analytical) = ~A~%" (sqrt (+ 9 4 1)))
(printf "line segment arc length (step = 0.1) = ~A~%" (arc-length s 0.1))

(printf "iterate line segment = ~A~%" (iterate-curve s 5))

(define ts (translate-curve (list 4 5 6) s))

(printf "translated line segment = ~A~%" ts)

(printf "iterate translated line segment = ~A~%" (iterate-curve ts 5))

(printf "iterate translated line segment = ~A~%" (iterate-curve ts 2))


(define (Rx theta)
  (list (f64vector 1 0 0)
        (f64vector 0 (cos theta) (sin theta))
        (f64vector 0 (- (sin theta)) (cos theta))
        ))

(define (Ry theta)
  (list (f64vector (cos theta) 0 (- (sin theta)))
        (f64vector 0 1 0)
        (f64vector (sin theta) 0 (cos theta))
        ))

(define (Rz theta)
  (list (f64vector (cos theta) (sin theta) 0)
        (f64vector (- (sin theta)) (cos theta) 0)
        (f64vector 0 0 1)
        ))

(define (transform a v)
  (let ((r1 (map (lambda (u) (f64vector-ref u 0)) a))
        (r2 (map (lambda (u) (f64vector-ref u 1)) a))
        (r3 (map (lambda (u) (f64vector-ref u 2)) a)))
    (list (fold + 0.0 (map * r1 v))
          (fold + 0.0 (map * r2 v))
          (fold + 0.0 (map * r3 v)))
    ))

(define segrx (line-segment 3 (transform (Rx 60) (list 1 1 1))))
(printf "x rotated segment (0,1) = ~A~%" ((sample-curve* segrx) (list 0 0.5 1)))
(define segry (line-segment 3 (transform (Ry 60) (list 1 1 1))))
(printf "y rotated segment (0,1) = ~A~%" ((sample-curve* segry) (list 0 0.5 1)))
(define segrz (line-segment 3 (transform (Rz 60) (list 1 1 1))))
(printf "z rotated segment (0,1) = ~A~%" ((sample-curve* segrz) (list 0 0.5 1)))

(define PI 3.14159265358979323846)
(define (rad t) (/ (* t PI) 180))

(define ellipse-c 
  (let ((a 0.2) (b 0.5))
    (simple-curve 
     10 1
     (list 
      (lambda (t) (* a (cos (rad t))))
      (lambda (t) (* b (sin (rad t))))
      (lambda (t) (cos (rad t)))
      )
     0.0 360.0)))

(printf "ellipse (0,360) = ~A~%" 
        (iterate-curve ellipse-c 10))

(define (sqr x) (* x x))

(define (vel x y z ax) 
  (print "ax = " ax)
  (match-let
   (((lst (x0 y0 z0)) ax))
   (print "lst = " lst)
   (print "x0 = " x0)
   (print "y0 = " y0)
   (print "z0 = " z0)
   (print "x = " x)
   (print "y = " y)
   (print "z = " z)
   (let* ((v (sqrt (+ (sqr (- x x0))
                      (sqr (- y y0))
                      (sqr (- z z0)))))
          (ax1 (list (cons v lst) (list x y z))))
     (print "ax1 = " ax1)
     ax1)
   ))

(printf "ellipse velocities = ~A~%" 
        (car
         (fold-curve ellipse-c 10 
                     (match-lambda*
                      (((x y z) init) (begin (print "init = " init) (vel x y z init))))
                     '(() (0.0 0.0 0.0))
                    )))

(define sc (line-segment 3 (list 10 10 10)))
(define pc1
    (simple-curve 
     10 1
     (list 
      (lambda (t) 0.0)
      (lambda (t) (sin (* 5 t)))
      (lambda (t) 0.0)
      )
     0.0 1.0))



(define (make-harmonic amp period phase n)
  (let* ((freq (/ (* 2 PI) (/ 1.0 period)))
         (c (simple-curve 
             (inexact->exact n) 1
             (list (lambda (t) (* amp (cos (+ (* freq t) phase))))
                   (lambda (t) t)
                   (lambda (t) 0.0))
             0.0 1.0)))
    c
    ))
        
(define pc2 (make-harmonic 5.0 28.0 0.0 50))


(print "line segment = " )
(pp (iterate-curve sc 10))
(print "perturbation curve 1 = " )
(pp (iterate-curve pc1 10))
(print "perturbed line segment 1 = " )
(pp (iterate-curve (compose-curve (list + + +) pc1 sc) 10))
(print "perturbation curve 2 = " )
(pp (iterate-curve pc2 50))
(print "perturbed line segment 2 = " )
(pp (iterate-curve (compose-curve (list + + +) pc2 sc) 10))