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

(provide xyz-on-z-rot)
(define (xyz-on-z-rot p a)
  (xyz-on p (pol 1 a) (pol 1 (+ a pi/2))))

(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 base-u0 (xyz 0 0 0))
(define (u0 [p base-u0]) (sxyz (position-cs p) 0 0 0))
(define (ux [p base-u0]) (sxyz (position-cs p) 1 0 0))
(define (uy [p base-u0]) (sxyz (position-cs p) 0 1 0))
(define (uz [p base-u0]) (sxyz (position-cs p) 0 0 1))
(define (-ux [p base-u0]) (sxyz (position-cs p) -1 0 0))
(define (-uy [p base-u0]) (sxyz (position-cs p) 0 -1 0))
(define (-uz [p base-u0]) (sxyz (position-cs p) 0 0 -1))
(define (uxy [p base-u0]) (sxyz (position-cs p) 1 1 0))
(define (uyz [p base-u0]) (sxyz (position-cs p) 0 1 1))
(define (uxz [p base-u0]) (sxyz (position-cs p) 1 0 1))
(define (-uxy [p base-u0]) (sxyz (position-cs p) -1 -1 0))
(define (-uyz [p base-u0]) (sxyz (position-cs p) 0 -1 -1))
(define (-uxz [p base-u0]) (sxyz (position-cs p) -1 0 -1))
(define (uxyz [p base-u0]) (sxyz (position-cs p) 1 1 1))
(define (-uxyz [p base-u0]) (sxyz (position-cs p) -1 -1 -1))

(define (rotc p a axis)
  (error "Must be implemented"))

(define (rotcx p a)
  (rotc p a (ux p)))

(define (rotcy p a)
  (rotc p a (uy p)))

(define (rotcz p a)
  (rotc p a (uz p)))

;;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 p0)) 
              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
 bbox-corners)


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

(define (bbox-corners bb)
  (let ((x0 (xyz-x (bbox-min bb)))
        (y0 (xyz-y (bbox-min bb)))
        (z0 (xyz-z (bbox-min bb)))
        (x1 (xyz-x (bbox-max bb)))
        (y1 (xyz-y (bbox-max bb)))
        (z1 (xyz-z (bbox-max bb))))
    (list (xyz x0 y0 z0)
          (xyz x1 y0 z0)
          (xyz x1 y1 z0)
          (xyz x0 y1 z0)
          (xyz x0 y0 z1)
          (xyz x1 y0 z1)
          (xyz x1 y1 z1)
          (xyz x0 y1 z1))))