#lang scheme
;; plane.ss
;; (Geometric) plane-related operations

(require "../utils.ss")
(require "point.ss")
(require "vector.ss")
(require "scheme-functions.ss")

(provide (struct-out plane))
(define-struct plane
  (p n)
  #:transparent)

(provide make-plane*)
(define make-plane*
  (case-lambda
    [(p1 p2 p3)
     (make-plane p1 (cross-product (p->q p1 p2)
                                   (p->q p2 p3)))]
    [(p n) (make-plane p n)]))

(provide plane->4pts)
(define (plane->4pts plane)
  (define n (plane-n plane))
  (define p (plane-p plane))
  (define d (- (+ (* (vx n) (cx p))
                  (* (vy n) (cy p))
                  (* (vz n) (cz p)))))
  (define (plane-point-z x y)
    (xyz x y
         (/ (- 0 (* (vx n) (cx p)) (* (vy n) (cy p))) (vz n))))
  (define (plane-point-y x z)
    (xyz x
         (/ (- 0 (* (vx n) (cx p)) (* (vz n) (cz p))) (vy n))
         z))
  (define (plane-point-x y z)
    (xyz (/ (- 0 (* (vz n) (cz p)) (* (vy n) (cy p))) (vx n))
         y z))
  (define (three-non-colinear a b c d)
    (if (v-colinear (p->q a b) (p->q a c))
        (list a b d)
        (list a b c)))
  (let ((l (append
            (three-non-colinear p
                                (plane-point-x (add1 (cy p)) (cz p))
                                (plane-point-y (add1 (cx p)) (cz p))
                                (plane-point-z (add1 (cx p)) (cy p)))
            ;; This last point helps us define the slicing stuff...
            (list (p+v p n)))))
    (display* "4pts: " l)l))



(provide plane-xy+ plane-xy-
         plane-xz+ plane-xz-
         plane-yz+ plane-yz-)
(define plane-xy+
  (make-plane origin (vxyz 0 0  1)))
(define plane-xy-
  (make-plane origin (vxyz 0 0 -1)))

(define plane-xz+
  (make-plane origin (vxyz 0  1 0)))
(define plane-xz-
  (make-plane origin (vxyz 0 -1 0)))

(define plane-yz+
  (make-plane origin (vxyz  1 0 0)))
(define plane-yz-
  (make-plane origin (vxyz -1 0 0)))


;;;; AXIS
(provide (struct-out axis)
         axis-pp axis-pv
         axis-plane)

(define-struct axis
  (p v))

(define (axis-pp p1 p2)
  (axis-pv p1 (p->q p1 p2)))
(define axis-pv make-axis)
(define axis-plane
  (case-lambda
    [(p)
     (axis-pv (plane-p p) (plane-n p))]
    [(p pt)
     (axis-pv pt (plane-n p))]))

(provide x-axis y-axis z-axis)
(define x-axis (axis-pp origin (xyz 1 0 0)))
(define y-axis (axis-pp origin (xyz 0 1 0)))
(define z-axis (axis-pp origin (xyz 0 0 1)))


(provide rotate-by)
(define rotate-by
  (case-lambda
    [(axis angle)
     (lambda (p) (rotate-by p axis angle))]
    [(p axis angle)
;;TODOOOOOOOOOOO     ;; Taken from apache commons math.geometry.Rotation
     ;; http://commons.apache.org/math/apidocs/org/apache/commons/math/geometry/Rotation.html#Rotation(org.apache.commons.math.geometry.Vector3D,%20double)
     (let* ([point  (if (axis? axis) (axis-p axis) origin)]
            [vector (if (axis? axis) (axis-v axis) axis)]
            [norm (vlength vector)]
            [half-angle (* 0.5 angle)]
            [coeff (/ (sin half-angle) norm)]
            [q0 (cos half-angle)]
            [q1 (* coeff (vx vector))]
            [q2 (* coeff (vy vector))]
            [q3 (* coeff (vz vector))]

            ;; Translate 'point to the origin
            [p* (+ p (p->q point origin))]
            ;; Rotate it around the origin
            [x (cx p*)]
            [y (cy p*)]
            [z (cz p*)]
            [s (+ (* x q1) (* y q2) (* z q3))]
            [p**
             (xyz
              (- (* 2 (+ (* q0 (- (* x q0) (- (* z q2) (* y q3)))) (* s q1))) x)
              (- (* 2 (+ (* q0 (- (* y q0) (- (* x q3) (* z q1)))) (* s q2))) y)
              (- (* 2 (+ (* q0 (- (* z q0) (- (* y q1) (* x q2)))) (* s q3))) z))]
            ;; Translate to 'point again
            [p*** (+ p** (p->q origin point))])
       p***)]))