(define-module (aux v2d)
  #:export (v2d?
	    v2d+ v2d- v2d* v2d/
	    v2d-floor
	    v2d-length v2d-extend-to-length
	    v2d-midpoint v2d-slope v2d-intersection))

(define (v2d? obj)
  (and (pair? obj)
       (real? (car obj)) (finite? (car obj))
       (real? (cdr obj)) (finite? (cdr obj))))

(import (rnrs base (6)))

(define (ensure-v2d obj)
  (if (pair? obj) (begin (assert (v2d? obj)) obj)
      (ensure-v2d (cons obj obj))))

(define (v2d-map proc lst)
  (let ((v2ds (map-in-order ensure-v2d lst)))
    (ensure-v2d
     (cons (apply proc (map-in-order car v2ds))
	   (apply proc (map-in-order cdr v2ds))))))

(define (v2d-length obj)
  (let ((v2d (ensure-v2d obj)))
    (sqrt (+ (expt (car v2d) 2) (expt (cdr v2d) 2)))))

(define (v2d+ . lst) (v2d-map + lst))
(define (v2d- . lst) (v2d-map - lst))
(define (v2d* . lst) (v2d-map * lst))
(define (v2d/ . lst) (v2d-map / lst))

(define (v2d-floor obj)
  (let ((v2d (ensure-v2d obj)))
    (cons (floor (inexact->exact (car v2d)))
	  (floor (inexact->exact (cdr v2d))))))

(define (v2d-extend-to-length v2d length)
  (let* ((cur-length (v2d-length v2d))
	 (factor (/ length cur-length)))
    (v2d* v2d factor)))

(define (v2d-midpoint obj1 obj2)
  (v2d/ (v2d+ obj1 obj2) 2))

(define (v2d-slope obj)
  (let ((v2d (ensure-v2d obj)))
    (if (zero? (car v2d)) (inf) (/ (cdr v2d) (car v2d)))))

(define (v2d-intersection v2d1 v2d2 dir1 dir2)
  (let* ((p1 (ensure-v2d v2d1)) (p2 (ensure-v2d v2d2))
	 (d1 (ensure-v2d dir1)) (d2 (ensure-v2d dir2))
	 (u1? (zero? (car d1))) (u2? (zero? (car d2))))
    (cond
     (u1? (if u2?
	      (if (= (cdr p1) (cdr p2)) #f (v2d-midpoint p1 p2))
	      (let* ((a (v2d-slope d2))
		     (b (cdr (v2d- p2 (v2d* (cons 1 a) (car p2))))))
		(cons (car p1) (+ (* a (car p1)) b)))))
     (u2? (v2d-intersection p2 p1 d2 d1))
     (#t (let* ((a1 (v2d-slope d1)) (a2 (v2d-slope d2))
		(b1 (cdr (v2d- p1 (v2d* (cons 1 a1) (car p1)))))
		(b2 (cdr (v2d- p2 (v2d* (cons 1 a2) (car p2))))))
	   (if (= a1 a2)
	       #f
	       (let* ((x (/ (- b2 b1) (- a1 a2)))
		      (y (+ b1 (* a1 x))))
		 (cons x y))))))))