(in-package :cl-user)

(defpackage :vector3d
  (:use :common-lisp)
  (:export :ved3d
           :add-to
           :add
           :sub-to
           :sub
           :mul-by
           :mul
           :div-by
           :div
           :dot
           :cross
           :p-vec))

(in-package :vector3d)

(defclass vec3d ()
  ((x
    :type double-float
    :initarg :x
    :initform 0d0
    :reader g-x
    :writer s-x)
   (y
    :type double-float
    :initarg :y
    :initform 0d0
    :reader g-y
    :writer s-y)
   (z
    :type double-float
    :initarg :z
    :initform 0d0
    :reader g-z
    :writer s-z)))

(defmacro vec3d (x y z)
  `(make-instance 'vec3d :x ,x :y ,y :z ,z))


(declaim (inline add-dub))

(defun add-dub (x y)
  (declare (type double-float x y)
           (optimize (speed 3) (safety 0)))
  (+ x y))

(defun add-to (a b)
  (declare (type vec3d a b)
           (optimize (speed 1) (safety 1)))
  (s-x (add-dub (g-x a) (g-x b)) a)
  (s-y (add-dub (g-y a) (g-y b)) a)
  (s-z (add-dub (g-z a) (g-z b)) a)
  nil)

(defun add (a b)
  (declare (type vec3d a b)
           (optimize (speed 3) (safety 1)))
  (vec3d (+ (g-x a) (g-x b))
         (+ (g-y a) (g-y b))
         (+ (g-y a) (g-y b))))

(defun sub-to (a b)
  (declare (type vec3d a b)
           (optimize (speed 3) (safety 1)))
  (s-x (- (g-x a) (g-x b)) a)
  (s-y (- (g-y a) (g-y b)) a)
  (s-z (- (g-z a) (g-z b)) a)
  nil)

(defun sub (a b)
  (declare (type vec3d a b)
           (optimize (speed 3) (safety 1)))
  (vec3d (- (g-x a) (g-x b))
         (- (g-y a) (g-y b))
         (- (g-y a) (g-y b))))

(defun mul-by (a b)
  (declare (type vec3d a)
           (type double-float b)
           (optimize (speed 3) (safety 1)))
  (s-x (* (g-x a) b) a)
  (s-y (* (g-y a) b) a)
  (s-z (* (g-z a) b) a))

(defun mul (a b)
  (declare (type vec3d a)
           (type double-float b)
           (optimize (speed 3) (safety 1)))
  (vec3d (* (g-x a) b)
         (* (g-y a) b)
         (* (g-z a) b)))

(defun div-by (a b)
  (declare (type vec3d a)
          (type double-float b)
          (optimize (speed 3) (safety 1)))
  (mul-by a (/ 1 b)))

(defun div (a b)
  (declare (type vec3d a)
           (type double-float b)
           (optimize (speed 3) (safety 1)))
  (mul a (/ 1 b)))

(declaim (inline dot))
(defun dot (a b)
  (declare (type vec3d a b)
           (optimize (speed 3) (safety 1)))
  (+ (* (g-x a) (g-x b)) (* (g-y a) (g-y b)) (* (g-z a) (g-z b))))

(defun cross (a b)
  (declare (type vec3d a b)
           (optimize (speed 3) (safety 1)))
  (vec3d (- (* (g-y a) (g-z b)) (* (g-y b) (g-z a)))
         (- (* (g-z a) (g-x b)) (* (g-z b) (g-x a)))
         (- (* (g-x a) (g-y b)) (* (g-x b) (g-y a)))))

(defun p-vec (a)
  (declare (type vec3d a)
           (optimize (speed 3) (safety 1)))
  (format t "vec3d: x: ~a y: ~a z: ~a"
          (g-x a)
          (g-y a)
          (g-z a)))