(define (get_initial_octree_bounding_box scene_objects)
  (let
    ((scene_object '())
     (octree_bounding_box '()))
    (let loop ()
      (if (not (null? scene_objects))
        (begin
          (set! scene_object (car scene_objects))
          (get_bounding_box scene_object bounding_box_lower bounding_box_upper)
          (if (null? octree_bounding_box) ; initially null
            (begin
              (set! octree_bounding_box
                (new_AABB (new_point 0.0 0.0 0.0) (new_point 0.0 0.0 0.0)))
              (copy_point (vector-ref octree_bounding_box _AABB_LOWER)
                          bounding_box_lower)
              (copy_point (vector-ref octree_bounding_box _AABB_UPPER)
                          bounding_box_upper))
            (begin
              (if (< (vector-ref bounding_box_lower _X)
                     (vector-ref
                       (vector-ref octree_bounding_box _AABB_LOWER) _X))
                (vector-set! (vector-ref octree_bounding_box _AABB_LOWER) _X
                  (vector-ref bounding_box_lower _X)))
              (if (< (vector-ref bounding_box_lower _Y)
                     (vector-ref
                       (vector-ref octree_bounding_box _AABB_LOWER) _Y))
                (vector-set! (vector-ref octree_bounding_box _AABB_LOWER) _Y
                  (vector-ref bounding_box_lower _Y)))
              (if (< (vector-ref bounding_box_lower _Z)
                     (vector-ref
                       (vector-ref octree_bounding_box _AABB_LOWER) _Z))
                (vector-set! (vector-ref octree_bounding_box _AABB_LOWER) _Z
                  (vector-ref bounding_box_lower _Z)))

              (if (> (vector-ref bounding_box_upper _X)
                     (vector-ref
                       (vector-ref octree_bounding_box _AABB_UPPER) _X))
                (vector-set! (vector-ref octree_bounding_box _AABB_UPPER) _X
                  (vector-ref bounding_box_upper _X)))
              (if (> (vector-ref bounding_box_upper _Y)
                     (vector-ref
                       (vector-ref octree_bounding_box _AABB_UPPER) _Y))
                (vector-set! (vector-ref octree_bounding_box _AABB_UPPER) _Y
                  (vector-ref bounding_box_upper _Y)))
              (if (> (vector-ref bounding_box_upper _Z)
                     (vector-ref
                       (vector-ref octree_bounding_box _AABB_UPPER) _Z))
                (vector-set! (vector-ref octree_bounding_box _AABB_UPPER) _Z
                  (vector-ref bounding_box_upper _Z)))))
          (set! scene_objects (cdr scene_objects))
          (loop))))

    (if (null? octree_bounding_box) ; there were no objects so default     
      (set! octree_bounding_box
        (new_AABB (new_point 0.0 0.0 0.0) (new_point 0.0 0.0 0.0))))

    (expand_AABB_bounds octree_bounding_box)

    octree_bounding_box))

(define (select_objects_in_octree objects octree_AABB)
  (let
    ((new_octree_objects '())
     (scene_object '()))
    (let loop ()
      (if (not (null? objects))
        (begin
          (set! scene_object (car objects))
          (get_bounding_box scene_object bounding_box_lower bounding_box_upper)
          (if (not (or
            (< (vector-ref bounding_box_upper _X)
               (vector-ref (vector-ref octree_AABB _AABB_LOWER) _X))
            (> (vector-ref bounding_box_lower _X)
               (vector-ref (vector-ref octree_AABB _AABB_UPPER) _X))
            (< (vector-ref bounding_box_upper _Y)
               (vector-ref (vector-ref octree_AABB _AABB_LOWER) _Y))
            (> (vector-ref bounding_box_lower _Y)
               (vector-ref (vector-ref octree_AABB _AABB_UPPER) _Y))
            (< (vector-ref bounding_box_upper _Z)
               (vector-ref (vector-ref octree_AABB _AABB_LOWER) _Z))
            (> (vector-ref bounding_box_lower _Z)
               (vector-ref (vector-ref octree_AABB _AABB_UPPER) _Z))))
            (set!
              new_octree_objects
              (cons scene_object new_octree_objects)))
          (set! objects (cdr objects))
          (loop))))
    new_octree_objects))

(define (new_octree level octree_objects bounding_box)
  (let*
    ((octree (make-vector 4)))
    (vector-set! octree _OCTREE_SCENE_OBJECT_COUNT 0) ; object count initially 0
    (vector-set! octree _OCTREE_BOUNDING_BOX '()) ; bounding box null by default
    (vector-set! octree _OCTREE '()) ; subtrees null by default
    (vector-set! octree _OCTREE_OBJECTS
      (select_objects_in_octree octree_objects bounding_box))
    (vector-set!
      octree _OCTREE_SCENE_OBJECT_COUNT
      (length (vector-ref octree _OCTREE_OBJECTS))) ; scene object count

; (display level) (display " ") (display (length (vector-ref octree 0))) (newline)
    (if (not (= (vector-ref octree _OCTREE_SCENE_OBJECT_COUNT) 0))
      (begin
        (vector-set! octree 2 bounding_box)
        (if (and (> (vector-ref octree _OCTREE_SCENE_OBJECT_COUNT)
                    max_leaf_objects)
                 (< level max_levels))
          ; subdivide
          (let
            ((delta_x
              (/ (- (vector-ref (vector-ref bounding_box _AABB_UPPER) _X)
                    (vector-ref (vector-ref bounding_box _AABB_LOWER) _X))
                 2.0))
             (delta_y
               (/ (- (vector-ref (vector-ref bounding_box _AABB_UPPER) _Y)
                     (vector-ref (vector-ref bounding_box _AABB_LOWER) _Y))
                  2.0))
             (delta_z
               (/ (- (vector-ref (vector-ref bounding_box _AABB_UPPER) _Z)
                     (vector-ref (vector-ref bounding_box _AABB_LOWER) _Z))
                  2.0)))
            (vector-set! octree _OCTREE (make-vector octree_nodes)) ; subtrees
            (do ((z 0 (+ z 1))) ((> z 1))
              (do ((y 0 (+ y 1))) ((> y 1))
                (do ((x 0 (+ x 1))) ((> x 1))
                  (vector-set! (vector-ref octree _OCTREE) (+ (* z 4) (* y 2) x)
                    (new_octree
                      (+ level 1)
                      (vector-ref octree _OCTREE_OBJECTS) ; octree objects
                      (new_AABB
                        (new_point
                          (+ (vector-ref
                               (vector-ref bounding_box _AABB_LOWER) _X)
                             (* delta_x x))
                          (+ (vector-ref
                               (vector-ref bounding_box _AABB_LOWER) _Y)
                             (* delta_y y))
                          (+ (vector-ref
                               (vector-ref bounding_box _AABB_LOWER) _Z)
                             (* delta_z z)))
                        (new_point
                          (+ (vector-ref
                               (vector-ref bounding_box _AABB_LOWER) _X)
                             (* delta_x x) delta_x)
                          (+ (vector-ref
                               (vector-ref bounding_box _AABB_LOWER) _Y)
                             (* delta_y y) delta_y)
                          (+ (vector-ref
                               (vector-ref bounding_box _AABB_LOWER) _Z)
                             (* delta_z z) delta_z))))))))))))
    octree))

(define (octree_get_min_distance_to_all_objects_in_leaf
  ray_source camera_direction
  dir_x dir_y dir_z
  objects_in_leaf intersection_object)
  (let loop ()
    (if (not (null? objects_in_leaf))
      (let
        ((octree_object (car objects_in_leaf)))
        (set! return_intersection_point
          (intersect_ray
            octree_object ray_source
            dir_x dir_y dir_z intersection_point
            0)) ; 0 is no shadow test
       ;(set! intersection_tests (+ intersection_tests 1))
        (if (not (null? return_intersection_point))
          (begin
            (vec_sub intersection_point ray_source temp_normalized)
            (normalize temp_normalized temp_normalized)
            (if (> (dot camera_direction temp_normalized) 0.0)
              ; point not behind ray source
              (let
                ((d (distance ray_source intersection_point)))
                (if (< d (vector-ref
                           intersection_object _INTERSECTION_OBJECT_DISTANCE))
                  (begin
                    (vector-set!
                      intersection_object _INTERSECTION_OBJECT_DISTANCE d)
                    (vector-set!
                      intersection_object _INTERSECTION_OBJECT_SCENE_OBJECT
                      octree_object)
                    (copy_point
                      (vector-ref
                        intersection_object
                        _INTERSECTION_OBJECT_MIN_POINT) ; min intersection pt
                      intersection_point)))))))
          (set! objects_in_leaf (cdr objects_in_leaf))
          (loop)))))

(define (ray_traverse_octree
  level octree ray camera_direction intersection_object)
  (if (not (null? (vector-ref octree _OCTREE_BOUNDING_BOX)))
    (if (=
      (ray_intersects_AABB
        (vector-ref octree _OCTREE_BOUNDING_BOX)
        ray
        (vector-ref ray _RAY_CLASSIFICATION))
      1)
      (if (or (<= (vector-ref
                    octree
                    _OCTREE_SCENE_OBJECT_COUNT) max_leaf_objects) ; leaf node
              (= level max_levels))
        (octree_get_min_distance_to_all_objects_in_leaf
          (vector-ref ray _RAY_ORIGIN) ; origin
          camera_direction
          (vector-ref (vector-ref ray _RAY_DIRECTION) _X) ; dir x
          (vector-ref (vector-ref ray _RAY_DIRECTION) _Y) ; dir y
          (vector-ref (vector-ref ray _RAY_DIRECTION) _Z) ; dir z
          (vector-ref octree _OCTREE_OBJECTS)
          intersection_object)
        (do ((i 0 (+ i 1))) ((= i octree_nodes))
          (ray_traverse_octree
            (+ level 1)
            (vector-ref (vector-ref octree _OCTREE) i) ; subtree
            ray camera_direction intersection_object))))))

(define (octree_shadow_test_all_objects_in_leaf
  octree
  origin ray_direction
  dir_x dir_y dir_z
  objects_in_leaf intersection_object
  light_distance)
  (let
    ((ret 0))
    (let loop ()
      (if (and (not (null? objects_in_leaf)) (= ret 0))
        (let
          ((octree_object (car objects_in_leaf)))
          (set! return_intersection_point
            (intersect_ray
              octree_object origin
              dir_x dir_y dir_z
              intersection_point
              1)) ; 1 is a shadow test
         ;(set! intersection_tests (+ intersection_tests 1))
          (if (not (null? return_intersection_point))
            (begin
              (vec_sub intersection_point origin temp_normalized)
              (normalize temp_normalized temp_normalized)
              (if (> (dot ray_direction temp_normalized) 0.0)
                ; point not behind ray source
                (let
                  ((d (distance origin intersection_point)))
                  (if (and (>= d epsilon) (< d light_distance))
                    (set! ret 1))))))
          (set! objects_in_leaf (cdr objects_in_leaf))
          (loop))))
    ret))

(define (shadow_ray_traverse_octree
  level octree ray intersection_object light_distance)
  (if (not (null? (vector-ref octree _OCTREE_BOUNDING_BOX)))
    (if (= (ray_intersects_AABB
             (vector-ref octree _OCTREE_BOUNDING_BOX)
             ray
             (vector-ref ray _RAY_CLASSIFICATION))
           1)
      (if (or (<= (vector-ref octree _OCTREE_SCENE_OBJECT_COUNT)
                  max_leaf_objects) ; leaf node
              (= level max_levels))
        (octree_shadow_test_all_objects_in_leaf
          octree
          (vector-ref ray _RAY_ORIGIN)
          (vector-ref ray _RAY_DIRECTION)
          (vector-ref (vector-ref ray _RAY_DIRECTION) _X) ; dir x
          (vector-ref (vector-ref ray _RAY_DIRECTION) _Y) ; dir y
          (vector-ref (vector-ref ray _RAY_DIRECTION) _Z) ; dir z
          (vector-ref octree _OCTREE_OBJECTS)
          intersection_object
          light_distance)
        ; else subtree
        (let
          ((i 0)
           (processing 1)
           (ret 0))
          (let loop ()
            (if (= processing 1)
              (begin
                (set! ret
                  (shadow_ray_traverse_octree
                    (+ level 1)
                    (vector-ref (vector-ref octree _OCTREE) i) ; subtree
                    ray intersection_object light_distance))
                (if (= ret 1)
                  (set! processing 0)
                  (begin
                    (set! i (+ i 1))
                    (if (= i octree_nodes)
                      (set! processing 0))))
                (loop))))
          ret))
        0)
   0))