(use srfi-4 parametric-curve)

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

(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-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))

(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)))