(module (engine math) ()
(import scheme
	(chicken base)
	(chicken module)
	(srfi 1)
	(srfi 99))

(export PI PI/2)
(define PI
  3.141592653589793238462643383279502884197169399375105820974944592307816406286)

(define PI/2
  (/ PI 2))

(export rad-to-deg deg-to-rad)
;; Radians and degrees conversion
(define (rad-to-deg rad)
  (* rad
     (/ 180 PI)))
(define (deg-to-rad deg)
  (* deg
     (/ PI 180)))

(export *float-precision* approx-=)
(define *float-precision* (make-parameter 0.001))
;; Approximately equal - for real number comparison
(define (approx-= x y)
  (< (abs (- x y)) (*float-precision*)))

;; Somewhat reliable fixnum conversion
(export number->integer)
(define (number->integer number)
  (assert (number? number))
  (inexact->exact (round number)))

;; Vector exports
(export vec vec? vec2? v-x
	set-v-x! v-y set-v-y!)

;; 2D Vector type
;; TODO: this could be done with a macro to save some definitions
(define-record-type <vector2>
  (int:make-vector2 x y)
  vec2?
  (x vector2-x int:set-vector2-x!)
  (y vector2-y int:set-vector2-y!))

;; Type safe 2D vector constructor
(define (vec . args)
  (assert (every number? args))
  (apply (case (length args)
	   ((2) int:make-vector2))
	 args))

(define vec?
  (disjoin vec2?))

;; Vector utility functions
(define (v-x component)
  (assert (record? component))
  ((rtd-accessor (record-rtd component) 'x) component))

(define (set-v-x! component x)
  (assert (record? component))
  (assert (number? x))
  ((rtd-mutator (record-rtd component) 'x) component x))

(define (v-y component)
  (assert (record? component))
  ((rtd-accessor (record-rtd component) 'y) component))

(define (set-v-y! component y)
  (assert (record? component))
  (assert (number? y))
  ((rtd-mutator (record-rtd component) 'y) component y))

;; Vector operations
(export v= v+ v- v* v/)

;; Vector equality
(define (v= . vecs)
  (assert (every record? vecs))
  (assert (every vec? vecs))
  (assert (every (lambda (x) (eqv? x (rtd-name (record-rtd (car vecs)))))
		 (map (compose rtd-name record-rtd) vecs)))
  (and (apply = (map v-x vecs))
       (apply = (map v-y vecs))))

;; Vector addition
;; Note that each operand can be either a vector OR a number
;; If a number, that number is added to EVERY member of the vector
(define (v+ . operands)
  (assert (every (disjoin number? (conjoin record? vec?)) operands))
  (let ((vecs (filter vec? operands)))
    (assert (every (lambda (x) (eqv? x (rtd-name (record-rtd (car vecs)))))
		   (map (compose rtd-name record-rtd) vecs))))
  (let ((x-parts (map (lambda (v) (if (vec? v) (v-x v) v)) operands))
	(y-parts (map (lambda (v) (if (vec? v) (v-y v) v)) operands)))
    (vec (apply + x-parts)
	 (apply + y-parts))))

;; Vector subtractions
;; Note that each operand can be either a vector OR a number
;; If a number, that number is subtracted from EVERY member of the vector
(define (v- . operands)
  (assert (every (disjoin number? (conjoin record? vec?)) operands))
  (let ((vecs (filter vec? operands)))
    (assert (every (lambda (x) (eqv? x (rtd-name (record-rtd (car vecs)))))
		   (map (compose rtd-name record-rtd) vecs))))
  (let ((x-parts (map (lambda (v) (if (vec? v) (v-x v) v)) operands))
	(y-parts (map (lambda (v) (if (vec? v) (v-y v) v)) operands)))
    (vec (apply - x-parts)
	 (apply - y-parts))))

;; Vector multiplication
;; Note that each operand can be either a vector OR a number
;; If a number, that number is multiplied to EVERY member of the vector
(define (v* . operands)
  (assert (every (disjoin number? (conjoin record? vec?)) operands))
  (let ((vecs (filter vec? operands)))
    (assert (every (lambda (x) (eqv? x (rtd-name (record-rtd (car vecs)))))
		   (map (compose rtd-name record-rtd) vecs))))
  (let ((x-parts (map (lambda (v) (if (vec? v) (v-x v) v)) operands))
	(y-parts (map (lambda (v) (if (vec? v) (v-y v) v)) operands)))
    (vec (apply * x-parts)
	 (apply * y-parts))))

;; Vector division
;; Note that each operand can be either a vector OR a number
;; If a number, EVERY member of the vector is divided by that number
(define (v/ . operands)
  (assert (every (disjoin number? (conjoin record? vec?)) operands))
  (let ((vecs (filter vec? operands)))
    (assert (every (lambda (x) (eqv? x (rtd-name (record-rtd (car vecs)))))
		   (map (compose rtd-name record-rtd) vecs))))
  (let ((x-parts (map (lambda (v) (if (vec? v) (v-x v) v)) operands))
	(y-parts (map (lambda (v) (if (vec? v) (v-y v) v)) operands)))
    (vec (apply / x-parts)
	 (apply / y-parts))))

;; More complex vector functions
(export vector-magnitude vector-normalize vector-dot
	vector-angle-between)

;; Magnitude
(define (vector-magnitude vec)
  (assert ((disjoin vec2?) vec))
  (cond
   ((vec2? vec)
    (sqrt (+ (expt (v-x vec) 2)
	     (expt (v-y vec) 2))))))

;; Dot product of vectors
(define (vector-dot vec1 vec2)
  (assert (and (record? vec1)
	       (record? vec2)))
  (assert (eq? (rtd-name (record-rtd vec1))
	       (rtd-name (record-rtd vec2))))
  (assert ((disjoin vec2?) vec1))
  (cond
   ((vec2? vec1)
    (+ (* (v-x vec1) (v-x vec2))
       (* (v-y vec1) (v-y vec2))))))

;; Angle between vectors
(define (vector-angle-between vec1 vec2)
  (assert (and (record? vec1)
	       (record? vec2)))
  (assert (eq? (rtd-name (record-rtd vec1))
	       (rtd-name (record-rtd vec2))))
  (assert ((disjoin vec2?) vec1))
  (cond
   ((vec2? vec1)
    (acos (/ (vector-dot vec1 vec2)
	     (* (vector-magnitude vec1)
		(vector-magnitude vec2)))))))

;; Normalization
(define (vector-normalize v)
  (assert ((disjoin vec2?) v)) ;; TODO: This assertion should be moved out of here
  (let ((magnitude (vector-magnitude v)))
    (cond
     ((vec2? v)
      (vec (/ (v-x v)
	      magnitude)
	   (/ (v-y v)
	      magnitude))))))
)