#lang racket

(provide
 matrix?

 matrix-vals
 m-lines
 m-cols
 m-frame
 m-cs
 m-n
 
 m-rotation
 m-scaling
 m-translation
 
 m-line
 m-column
 
 m-translation-c
 
 m-neg
 +m
 -m
 *m
 m*p
 
 m-transform
 
 m-zero
 m-identity
 
 list<-matrix
 vector-lines<-matrix
 vector-cols<-matrix)


(define (v*v v1 v2)
  (foldl + 0 (vector->list (vector-map * v1 v2))))


(define (matrix-write m port mode)  
  (define (print-line v)
    (write-string
     (string-join
      (vector->list
       (vector-map (λ (e) (format "~a" e)) v))
      " ")
     port))
  
  (write-string "m(" port)
  (print-line (vector-copy (matrix-vals m) 0 4))
  (newline port)
  
  (write-string "  " port)
  (print-line (vector-copy (matrix-vals m) 4 8))
  (newline port)
  
  (write-string "  " port)
  (print-line (vector-copy (matrix-vals m) 8 12))
  (newline port)
  
  (write-string "  " port)
  (print-line (vector-copy (matrix-vals m) 12 16))
  (write-string ")" port))

(struct matrix (vals)
  #:property prop:custom-write matrix-write)


(define (m-lines v1 v2 v3)
  (let ((x1 (vector-ref v1 0))
        (y1 (vector-ref v1 1))
        (z1 (vector-ref v1 2))
        (w1 (vector-ref v1 3)))
    (let ((x2 (vector-ref v2 0))
          (y2 (vector-ref v2 1))
          (z2 (vector-ref v2 2))
          (w2 (vector-ref v2 3)))
      (let ((x3 (vector-ref v3 0))
            (y3 (vector-ref v3 1))
            (z3 (vector-ref v3 2))
            (w3 (vector-ref v3 3)))
        (matrix
         (vector x1 y1 z1 w1
                 x2 y2 z2 w2
                 x3 y3 z3 w3
                 0  0  0  1))))))

(define m-cols
  (case-lambda
    ((v1 v2 v3 v4)
     (let ((x1 (vector-ref v1 0))
           (y1 (vector-ref v1 1))
           (z1 (vector-ref v1 2)))
       (let ((x2 (vector-ref v2 0))
             (y2 (vector-ref v2 1))
             (z2 (vector-ref v2 2)))
         (let ((x3 (vector-ref v3 0))
               (y3 (vector-ref v3 1))
               (z3 (vector-ref v3 2)))
           (let ((x4 (vector-ref v4 0))
                 (y4 (vector-ref v4 1))
                 (z4 (vector-ref v4 2)))
             (matrix
              (vector x1 x2 x3 x4
                      y1 y2 y3 y4
                      z1 z2 z3 z4
                      0  0  0  1)))))))
    ((v)
     (let ((x1 (vector-ref v 0))
           (y1 (vector-ref v 1))
           (z1 (vector-ref v 2)))
       (let ((x2 (vector-ref v 4))
             (y2 (vector-ref v 5))
             (z2 (vector-ref v 6)))
         (let ((x3 (vector-ref v 8))
               (y3 (vector-ref v 9))
               (z3 (vector-ref v 10)))
           (let ((x4 (vector-ref v 12))
                 (y4 (vector-ref v 13))
                 (z4 (vector-ref v 14)))
             (matrix
              (vector x1 x2 x3 x4
                      y1 y2 y3 y4
                      z1 z2 z3 z4
                      0  0  0  1)))))))))

(define (m-frame c x y)
  (let ((xc (norm-c x))
        (zc (norm-c (cross-c x y))))
    (let ((yc (norm-c (cross-c zc xc))))
      (m-cols
       (vector<-xyz xc)
       (vector<-xyz yc)
       (vector<-xyz zc)
       (vector<-xyz c)))))

(define (m-cs c x y)
  (m-frame c (-c x c) (-c y c)))

(define (m-n c n)
  (let ((z (norm-c n)))
    (let ((x (norm-c (collinear-cross-c z))))
      (let ((y (norm-c (cross-c z x))))
        (m-cols
         (vector<-xyz x)
         (vector<-xyz y)
         (vector<-xyz z)
         (vector<-xyz c))))))

(define (m-rotation a n)
  (define (m-rotate-aux n1 n2 n3)
    (let ((c (cos a))
          (s (sin a))
          (t (- 1 (cos a)))
          (p (norm-c (xyz n1 n2 n3))))
      (let ((x (xyz-x p))
            (y (xyz-y p))
            (z (xyz-z p)))
        (let ((r00 (+ (* t x x) c))
              (r01 (- (* t x y) (* s z)))
              (r02 (+ (* t x z) (* s y)))
              
              (r10 (+ (* t x y) (* s z)))
              (r11 (+ (* t y y) c))
              (r12 (- (* t y z) (* s x)))
              
              (r20 (- (* t x z) (* s y))) ;;AML: was (- (* t x y) (* s y))
              (r21 (+ (* t y z) (* s x)))
              (r22 (+ (* t z z) c)))
          (matrix
           (vector r00 r01 r02 0
                   r10 r11 r12 0
                   r20 r21 r22 0
                   0   0   0   1))))))
  (if (or (= a 0) (=c? n u0))
      m-identity
      (apply m-rotate-aux (list-of-coord n))))

(define list-of-coord "BUM")

(define m-scaling
  (case-lambda
    ((x y z)
     (matrix
      (vector x 0 0 0
              0 y 0 0
              0 0 z 0
              0 0 0 1)))
    ((c)
     (m-scaling (xyz-x c) (xyz-y c) (xyz-z c)))))

(define m-translation
  (case-lambda
    ((x y z)
     (matrix
      (vector 1 0 0 x
              0 1 0 y
              0 0 1 z
              0 0 0 1)))
    ((c)
     (m-translation (xyz-x c) (xyz-y c) (xyz-z c)))))

(define (m-line m l)
  (vector-copy (matrix-vals m) (* l 4) (+ (* l 4) 4)))

(define (m-column m c)
  (let ((vals (matrix-vals m)))
    (vector
     (vector-ref vals c)
     (vector-ref vals (+ c 4))
     (vector-ref vals (+ c 8))
     (vector-ref vals (+ c 12)))))


(define (m-translation-c m)
  (let ((vals (matrix-vals m))
        (c 3))
    (xyz
     (vector-ref vals c)
     (vector-ref vals (+ c 4))
     (vector-ref vals (+ c 8)))))


(define (m-neg m)
  (matrix
   (vector-map - (matrix-vals m))))

(define (+m m1 m2)
  (matrix
   (vector-map + (matrix-vals m1) (matrix-vals m2))))

(define (-m m1 m2)
  (+m m1 (m-neg m2)))

(define (*m m1 m2)
  (matrix
   (list->vector
    (flatten
     (for/list ((i 4))
       (for/list ((j 4))
         (v*v (m-line m1 i) (m-column m2 j))))))))

; edit: should not give the user the option of distinguishing between coords and vectors
(define (m*p m p (vector? #f))
  (let* ((w (if vector? 0 1))
         (v (vector (xyz-x p) (xyz-y p) (xyz-z p) w)))
    (xyz (v*v (m-line m 0) v)
         (v*v (m-line m 1) v)
         (v*v (m-line m 2) v))))

(define (m-transform t a n s)
  (*m
   (*m
    (m-scaling (xyz-x s) (xyz-y s) (xyz-z s))
    (m-rotation a n))
   (m-translation (xyz-x t) (xyz-y t) (xyz-z t))))


(define m-zero
  (matrix
   (vector 0 0 0 0
           0 0 0 0
           0 0 0 0
           0 0 0 0)))

(define m-identity
  (matrix
   (vector 1 0 0 0
           0 1 0 0
           0 0 1 0
           0 0 0 1)))


(define (list<-matrix m)
  (vector->list (matrix-vals m)))

(define (vector-lines<-matrix m)
  (vector-copy (matrix-vals m)))

(define (vector-cols<-matrix m)
  (let ((v (matrix-vals m)))
    (let ((x1 (vector-ref v 0))
          (y1 (vector-ref v 1))
          (z1 (vector-ref v 2))
          (w1 (vector-ref v 3)))
      (let ((x2 (vector-ref v 4))
            (y2 (vector-ref v 5))
            (z2 (vector-ref v 6))
            (w2 (vector-ref v 7)))
        (let ((x3 (vector-ref v 8))
              (y3 (vector-ref v 9))
              (z3 (vector-ref v 10))
              (w3 (vector-ref v 11)))
          (vector x1 x2 x3 0
                  y1 y2 y3 0
                  z1 z2 z3 0
                  w1 w2 w3 1))))))

(provide
 norm-c ;;HACK solve this
 cross-c
 dot-c
 position?
 position-cs
 xyz xyz-on xyz-on-cs
 xyz-on-points
 xyz-on-z-rotation
 xyz-from-normal
 world-xy-cs
 world-yz-cs
 world-zx-cs
 world-yx-cs
 world-zy-cs
 world-xz-cs
 xy
 yz
 xz
 x y z
 xyz-x xyz-y xyz-z
 (rename-out [xyz-x cx] [xyz-y cy] [xyz-z cz])
 vector<-xyz
 roundc ;;HACK ugly name
 =c?  ;;HACK ugly name!
 +x +y +z +xy +xz +yz +xyz
 pol +pol pol-rho pol-phi
 cyl +cyl cyl-rho cyl-phi cyl-z
 sph +sph sph-rho sph-phi sph-psi
 +rho +phi +r +psi
 +c -c *c /c
 u0
 ux
 uy
 uz
 uxy
 uyz
 uxz
 uxyz
 -ux
 -uy
 -uz
 -uxy
 -uyz
 -uxz
 -uxyz
 pi/6
 pi/4
 pi/3
 pi/2
 3pi/2
 2pi
 3pi
 4pi
 -pi/6
 -pi/4
 -pi/3
 -pi/2
 -3pi/2
 -pi
 -2pi
 -3pi
 -4pi

 coterminal
 distance
 tr-matrix
 position-and-height
 inverted-position-and-height

 
 (rename-out (sxyz-cs-o cs-o)
             (sxyz-cs-x cs-x)
             (sxyz-cs-y cs-y)
             (sxyz-cs-z cs-z))

 as-origin
 as-world
 )

(struct position (cs))

;(struct vector (cs))

(struct 2d-position position ())

(struct 3d-position position ())

(define (fprint-3d-position sxyz port mode)
  (fprintf port "#<xyz:~A ~A ~A>"
           (sxyz-x sxyz)
           (sxyz-y sxyz)
           (sxyz-z sxyz))
  #;(let ((cs (position-cs sxyz)))
      (when (and cs (not (std-cs? cs)))
        (write-string " | " port)
        (write cs port)))
  #;(write-string ">" port))

(struct sxyz 3d-position (x y z)
  #:property
  prop:custom-write fprint-3d-position)

(struct srpz 3d-position (rho phi z))

(struct srpp 3d-position (rho phi psi))

(struct sxyz-cs (o x y z)
  #:property
  prop:custom-write (lambda (cs port mode)
                      (fprintf port "#cs[o:~A ux:~A uy:~A uz:~A]"
                               (sxyz-cs-o cs)
                               (sxyz-cs-x cs)
                               (sxyz-cs-y cs)
                               (sxyz-cs-z cs))))

(define (=c? c0 c1)
  (or (eq? c0 c1)
      (and (eq? (position-cs c0) (position-cs c1))
           (= (xyz-x c0) (xyz-x c1))
           (= (xyz-y c0) (xyz-y c1))
           (= (xyz-z c0) (xyz-z c1)))))

(define (roundc c)
  (sxyz (position-cs c)
        (round (sxyz-x c))
        (round (sxyz-y c))
        (round (sxyz-z c))))


(define world-xy-cs
  (sxyz-cs (sxyz #f 0 0 0) (sxyz #f 1 0 0) (sxyz #f 0 1 0) (sxyz #f 0 0 1)))

(define world-yx-cs
  (sxyz-cs (sxyz #f 0 0 0) (sxyz #f 0 1 0) (sxyz #f 1 0 0) (sxyz #f 0 0 -1)))

(define world-yz-cs
  (sxyz-cs (sxyz #f 0 0 0) (sxyz #f 0 1 0) (sxyz #f 0 0 1) (sxyz #f 1 0 0)))

(define world-zy-cs
  (sxyz-cs (sxyz #f 0 0 0) (sxyz #f 0 0 1) (sxyz #f 0 1 0) (sxyz #f -1 0 0)))

(define world-zx-cs
  (sxyz-cs (sxyz #f 0 0 0) (sxyz #f 0 0 1) (sxyz #f 1 0 0) (sxyz #f 0 1 0)))

(define world-xz-cs
  (sxyz-cs (sxyz #f 0 0 0) (sxyz #f 1 0 0) (sxyz #f 0 0 1) (sxyz #f 0 -1 0)))

(define std-cs world-xy-cs)

(define (std-cs? cs)
  (eq? cs std-cs))

;;Reposition the cs to put the position at the
;;origin
(define (as-origin p)
  (let ((cs (position-cs p)))
    (let ((o (sxyz-cs-o cs)))
      (sxyz (sxyz-cs (sxyz
                      #f ;;Is this OK?
                      (+ (sxyz-x p) (sxyz-x o))
                      (+ (sxyz-y p) (sxyz-y o))
                      (+ (sxyz-z p) (sxyz-z o)))
                     (sxyz-cs-x cs)
                     (sxyz-cs-y cs)
                     (sxyz-cs-z cs))
            0 0 0))))

(define (as-world p)
  (let ((cs (position-cs p))) 
    (if (or (not cs) (std-cs? cs))
        p
        (m*p (tr-matrix cs) p))))

(define (xyz x y z)
  (sxyz std-cs x y z))

(define (xy x y)
  (xyz x y 0))

(define (xz x z)
  (xyz x 0 z))

(define (yz y z)
  (xyz 0 y z))

(define (x x)
  (xyz x 0 0))

(define (y y)
  (xyz 0 y 0))

(define (z z)
  (xyz 0 0 z))

(define (xyz-on o x y)
  (sxyz (sxyz-cs o x y (norm-c (cross-c x y)))
        0 0 0))

(define (xyz-on-points p0 p1 p2 p3)
  (let ((n0 (-c p1 p0))
        (n1 (-c p2 p0)))
    (let ((n2 (cross-c n0 n1)))
      (if (< (dot-c n2 (-c p3 p0)) 0)
          (xyz-on p0 n0 n1)
          (xyz-on p0 n1 n0)))))

(define (xyz-on-cs x y z cs)
  (sxyz cs x y z))

(define (xyz-on-z-rotation x y z a)
  (sxyz (sxyz-cs (xyz x y z) (pol 1 a) (pol 1 (+ a pi/2)) uz)
        0 0 0))

(define (+xyz p dx dy dz)
  (cond [(sxyz? p)
         (sxyz (position-cs p)
               (+ (sxyz-x p) dx)
               (+ (sxyz-y p) dy)
               (+ (sxyz-z p) dz))]
        [else
         (error '+xyz "Implement conversions for ~A" p)]))

(define (+x p dx)
  (+xyz p dx 0 0))

(define (+y p dy)
  (+xyz p 0 dy 0))

(define (+z p dz)
  (+xyz p 0 0 dz))

(define (+xy p dx dy)
  (+xyz p dx dy 0))

(define (+xz p dx dz)
  (+xyz p dx 0 dz))

(define (+yz p dy dz)
  (+xyz p 0 dy dz))

(define (xyz-x p)
  (cond [(sxyz? p)
         (sxyz-x p)]
        [(srpz? p)
         (error "Implement conversions")]))

(define (xyz-y p)
  (cond [(sxyz? p)
         (sxyz-y p)]
        [(srpz? p)
         (error "Implement conversions")]))

(define (xyz-z p)
  (cond [(sxyz? p)
         (sxyz-z p)]
        [(srpz? p)
         (srpz-z p)]))

(define (cyl rho phi z)
  (xyz (* rho (cos phi))
       (* rho (sin phi))
       z))

(define (+cyl p rho phi z)
  (+c p (cyl rho phi z)))

(define (cyl-rho p)
  (let ((x (xyz-x p))
        (y (xyz-y p)))
    (sqrt (+ (* x x) (* y y)))))

(define (cyl-phi c)
  (sph-phi c))

(define (cyl-z c)
  (xyz-z c))

(define (pol rho phi)
  (cyl rho phi 0))

(define (+pol p rho phi)
  (+cyl p rho phi 0))

(define pol-rho cyl-rho)

(define pol-phi cyl-phi)

(define (sph rho phi psi)
  (let ((sin-psi (sin psi)))
    (xyz (* rho (cos phi) sin-psi)
         (* rho (sin phi) sin-psi)
         (* rho (cos psi)))))

(define (+sph p rho phi psi)
  (+c p (sph rho phi psi)))

(define (sph-rho p)
  (let ((x (xyz-x p))
        (y (xyz-y p))
        (z (xyz-z p)))
    (sqrt (+ (* x x) (* y y) (* z z)))))

(define (sph-phi p)
  (let ((x (xyz-x p))
        (y (xyz-y p)))
    (if (= 0 x y)
        0
        (atan (xyz-y p) (xyz-x p)))))

(define (sph-psi p)
  (let ((x (xyz-x p))
        (y (xyz-y p))
        (z (xyz-z p)))
    (if (= 0 x y z)
        0
        (atan (sqrt (+ (* x x) (* y y)))
              z))))

(define (+rho c rho)
  (cyl (+ (cyl-rho c) rho) (cyl-phi c) (cyl-z c)))

(define (+phi c phi)
  (cyl (cyl-rho c) (+ (cyl-phi c) phi) (cyl-z c)))

(define (+r c r)
  (sph (+ (sph-rho c) r) (sph-phi c) (sph-psi c)))

(define (+psi c psi)
  (sph (sph-rho c) (sph-phi c) (+ (sph-psi c) psi)))



;;HACK: these should check if the cs are
;;the same

(define (+c p0 p1)
  (sxyz (position-cs p0)
        (+ (sxyz-x p0) (sxyz-x p1))
        (+ (sxyz-y p0) (sxyz-y p1))
        (+ (sxyz-z p0) (sxyz-z p1))))

(define (-c p0 p1)
  (sxyz (position-cs p0)
        (- (sxyz-x p0) (sxyz-x p1))
        (- (sxyz-y p0) (sxyz-y p1))
        (- (sxyz-z p0) (sxyz-z p1))))

(define (*c p/r r/p)
  (let-values ([(p r)
                (if (number? p/r)
                    (values r/p p/r)
                    (values p/r r/p))])
    (sxyz (position-cs p)
          (* (sxyz-x p) r)
          (* (sxyz-y p) r)
          (* (sxyz-z p) r))))

(define (/c p r)
  (sxyz (position-cs p)
        (/ (sxyz-x p) r)
        (/ (sxyz-y p) r)
        (/ (sxyz-z p) r)))

(define u0 (xyz 0 0 0))

(define ux (xyz 1 0 0))
(define uy (xyz 0 1 0))
(define uz (xyz 0 0 1))
(define -ux (xyz -1 0 0))
(define -uy (xyz 0 -1 0))
(define -uz (xyz 0 0 -1))

(define uxy (xy 1 1))
(define uyz (yz 1 1))
(define uxz (xz 1 1))
(define -uxy (xy -1 -1))
(define -uyz (yz -1 -1))
(define -uxz (xz -1 -1))

(define uxyz (xyz 1 1 1))
(define -uxyz (xyz -1 -1 -1))

;;The circle system
(define pi/6 (/ pi 6))
(define pi/4 (/ pi 4))
(define pi/3 (/ pi 3))
(define pi/2 (/ pi 2))
(define 3pi/2 (* 3 pi/2))
(define 2pi (* 2 pi))
(define 3pi (* 3 pi))
(define 4pi (* 4 pi))

(define -pi/6 (/ pi -6))
(define -pi/4 (/ pi -4))
(define -pi/3 (/ pi -3))
(define -pi/2 (/ pi -2))
(define -3pi/2 (* -3 pi/2))
(define -pi (* -1 pi))
(define -2pi (* -2 pi))
(define -3pi (* -3 pi))
(define -4pi (* -4 pi))

(define (coterminal radians)
  (let ((k (truncate (/ radians 2pi))))
    (- radians (* k 2pi))))

(define (distance p0 p1)
  (let ((dx (- (xyz-x p1) (xyz-x p0)))
        (dy (- (xyz-y p1) (xyz-y p0)))
        (dz (- (xyz-z p1) (xyz-z p0))))
    (sqrt (+ (* dx dx) (* dy dy) (* dz dz)))))

(define-syntax (let-coords stx)
  (syntax-case stx ()
    ((_ (((x y z) p) ...) body ...)
     (syntax/loc stx
       (let ((x (xyz-x p)) ...
             (y (xyz-y p)) ...
             (z (xyz-z p)) ...)
         body ...)))))

(define (perpendicular-vector v)
  (let-coords (((x y z) v))
    (cond ((= z 0)
           (xyz 0 0 1))
          ((= y 0)
           (xyz 0 -1 0))
          ((= x 0)
           (xyz 1 0 0))
          (else
           (let ((x z)
                 (y z)
                 (z (- (+ x y)))) ;;inline normalization
             (let ((l (sqrt (+ (* x x) (* y y) (* z z)))))
               (xyz (/ x l) (/ y l) (/ z l))))))))

(define (xyz-from-normal p n)
  (let ((x (xyz-x n))
        (y (xyz-y n))
        (z (xyz-z n)))
    (let ((z-axis n))
      (let ((v0 (perpendicular-vector n)))
        (let ((x-axis (cross-c n v0)))
          (let ((y-axis (cross-c z-axis x-axis)))
            (sxyz (sxyz-cs p (norm-c x-axis) (norm-c y-axis) (norm-c z-axis))
                  0 0 0)))))))


(define (vector<-xyz p)
  (vector (sxyz-x p) (sxyz-y p) (sxyz-z p)))

(provide xyz<-vector)
(define (xyz<-vector v)
  (xyz (vector-ref v 0)
       (vector-ref v 1)
       (vector-ref v 2)))

(define (tr-matrix cs)
  (m-cols (vector<-xyz (sxyz-cs-x cs))
          (vector<-xyz (sxyz-cs-y cs))
          (vector<-xyz (sxyz-cs-z cs))
          (vector<-xyz (sxyz-cs-o cs))))

(provide xyz<-matrix)
(define (xyz<-matrix m)
  (let ((p (xyz<-vector (m-column m 3))))
    (sxyz (sxyz-cs (sxyz #f 0 0 0)
                   (xyz<-vector (m-column m 0))
                   (xyz<-vector (m-column m 1))
                   (xyz<-vector (m-column m 2)))
          (xyz-x p) (xyz-y p) (xyz-z p))))

(define (position-and-height p0 h/p1)
  (if (number? h/p1)
      (values p0 h/p1)
      (let ((p0w (as-world p0))
            (p1w (as-world h/p1)))
        (values (xyz-from-normal p0w (-c p1w p0w))
                (distance p0w p1w)))))

(define (inverted-position-and-height p0 h/p1)
  (if (number? h/p1)
      (values (xyz-from-normal (+z p0 h/p1) -uz) 
              h/p1)
      (values (xyz-from-normal h/p1 (-c p0 h/p1))
              (distance p0 h/p1))))


(define (norm-c v)
  (let-coords (((x y z) v))
    (let ((l (sqrt (+ (* x x) (* y y) (* z z)))))
      (xyz (/ x l) (/ y l) (/ z l)))))

(define (dot-c c1 c2)
  (+
   (* (xyz-x c1) (xyz-x c2))
   (* (xyz-y c1) (xyz-y c2))
   (* (xyz-z c1) (xyz-z c2))))

(define (cross-c v0 v1)
  (let-coords (((x0 y0 z0) v0)
               ((x1 y1 z1) v1))
    (xyz (- (* y0 z1) (* z0 y1))
         (- (* z0 x1) (* x0 z1))
         (- (* x0 y1) (* y0 x1)))))

(define (collinear-cross-c c)
  (define (collinear-pxp-1)
    (xyz 0 (xyz-z c) (- (xyz-y c))))
  
  (define (collinear-pxp-2)
    (xyz (xyz-z c) 0 (- (xyz-x c))))
  
  (define (collinear-pxp-3)
    (xyz (xyz-y c) (- (xyz-x c)) 0))
  
  (let ((n (collinear-pxp-1)))
    (if (=c? u0 n)
        (let ((n (collinear-pxp-2)))
          (if (=c? u0 n)
              (collinear-pxp-3)
              n))
        n)))

(define (angle-c c1 c2 (normal #f))
  (define (aux)
    (acos
     (dot-c
      (*c c1 (/ (sph-rho c1)))
      (*c c2 (/ (sph-rho c2))))))
  
  (define (continuous-aux)
    (let ((p (cross-c c1 c2)))
      (if (or
           (=c? u0 p)
           (> (dot-c normal p) 0))
          (aux)
          (- 2pi (aux)))))
  
  (if normal
      (continuous-aux)
      (aux)))

(define (collinearity-c c1 c2)
  (define (n//n n1 n2)
    (cond ((= n1 n2 0) #t)
          ((= n2 0) 0)
          (else (/ n1 n2))))
  
  (define (ns//ns)
    (filter
     (error "FINISH") ;;;(cλ (λ. not eqv?) true)
     (list
      (n//n (xyz-x c1) (xyz-x c2))
      (n//n (xyz-y c1) (xyz-y c2))
      (n//n (xyz-z c1) (xyz-z c2)))))
  
  (let ((ns (ns//ns)))
    (cond ((empty? ns) 0)
          ((apply = (cons (first ns) ns)) (first ns))
          (else 0))))

(provide
 (struct-out bbox)
 bbox-zero
 bbox-center
 bbox-length-x
 bbox-length-y
 bbox-length-z)


(define (bbox-print bb port mode)
  (fprintf port "<bbox: ~A ~A>" (bbox-min bb) (bbox-max bb)))

(struct bbox
  (min max)
  #:property prop:custom-write bbox-print)

(define bbox-zero
  (bbox u0 u0))

(define (bbox-center bb)
  (/c (+c (bbox-min bb) (bbox-max bb)) 2))

(define (bbox-length-x bb)
  (- (xyz-x (bbox-max bb)) (xyz-x (bbox-min bb))))

(define (bbox-length-y bb)
  (- (xyz-y (bbox-max bb)) (xyz-y (bbox-min bb))))

(define (bbox-length-z bb)
  (- (xyz-z (bbox-max bb)) (xyz-z (bbox-min bb))))