(* 
val simplify: Object.t -> Object.t
*)

(* 
We have a tree of objects, and we have to compute the bounding spheres
around the objects. We can also reorganize the tree to get smaller bounding
spheres.
*)

open Config
open Misc
open Vector
open Object
open Point


let infinite_radius = pos_infty
  
    
let nl i =
  print_newline ();
  for i = 1 to i do
    Printf.printf "  ";
  done
  
let rec dump_obj i t =
  nl i;
  Printf.printf "Object:"; 
  nl i;
  Printf.printf "Center: %f %f %f" t.center.x t.center.y t.center.z;
  nl i;
  if t.radius >= infinite_radius then
    Printf.printf "Radius: infinite"
  else
    Printf.printf "Radius: %f" t.radius;
  nl i;
  match t.desc with
    Base base -> begin
        match base.kind with
          Cone -> Printf.printf "Cone"
        | Cube -> Printf.printf "Cube"
        | Cylinder -> Printf.printf "Cylinder"
        | Plane -> Printf.printf "Plane"
        | Sphere -> Printf.printf "Sphere"
      
      end
  | Union (t1,t2) -> 
      Printf.printf "Union:";
      dump_obj (i+1) t1;
      dump_obj (i+1) t2
  | Intersection (t1,t2) -> 
      Printf.printf "Intersection:";
      dump_obj (i+1) t1;
      dump_obj (i+1) t2
  | Difference (t1,t2) ->
      Printf.printf "Difference:";
      dump_obj (i+1) t1;
      dump_obj (i+1) t2
  | Null -> 
      Printf.printf "Null"
      
  
let origin = { x = 0.; y = 0.; z = 0. }
let null_object = { obj_template with center = origin }
  
let cone_radius = 1.
let cube_radius = (sqrt 3.)/. 2.
let cylinder_radius = (sqrt 5.) /. 2.
let plane_radius = pos_infty
let sphere_radius = 1.
  
let cone_center = { x = 0.0; y = 1.0; z = 0.0 }
let cube_center = { x = 0.5; y = 0.5; z = 0.5 }
let cylinder_center = { x = 0.; y = 0.5; z = 0. }
let plane_center = origin
let sphere_center = origin

let infinite t =
(*  print_char '+'; *)
  { desc = t.desc; center = origin; radius = infinite_radius }    
  
let union_bs t1 t2 =  
  let r1 = t1.radius in
  let r2 = t2.radius in
  if r1 >= infinite_radius || r2 >= infinite_radius then
    { desc = Union(t1,t2); center = origin; 
      radius = infinite_radius }    
  else
  let c1 = t1.center in
  let c2 = t2.center in
  let c1c2 = Vector.between c1 c2 in
  let dd2 = Vector.length2 c1c2 in
  let rr = r2 -. r1 in
  let rr2 = rr *. rr in
  if dd2 <= rr2 then (* take the biggest sphere *)
    if r1 <= r2 then 
      { desc = Union(t1,infinite t2); center = c2; radius = r2 }
    else
      { desc = Union(infinite t1,t2); center = c1; radius = r1 }
  else
  let d = sqrt dd2 in
  let alpha = rr /. (2. *. d) +. 0.5 in
  let center = point_along c1 c1c2 alpha in
  let radius = (d +. r1 +. r2) /. 2. in
  { desc = Union(t1,t2); center = center; radius = radius }

let intersection_bs t t1 t2 =         
  
  let r1 = t1.radius in
  let r2 = t2.radius in
  let c1 = t1.center in
  let c2 = t2.center in
  
  if r1 >= infinite_radius then
    { t with desc = Intersection(t1,t2); center = c2; radius = r2 }        
  else if r2 >= infinite_radius then
    { t with desc = Intersection(t1,t2); center = c1; radius = r1 }    
  else
  
  let c1c2 = Vector.between c1 c2 in
  let dd2 = Vector.length2 c1c2 in
  let rr = r2 -. r1 in
  let rr2 = rr *. rr in
  if dd2 <= rr2 then
    if r2 <= r1 then (* take the smallest one *)
      { t with desc = Intersection(t1,infinite t2); center = c2; radius = r2 }
    else
      { t with desc = Intersection(infinite t1,t2); center = c1; radius = r1 } 
  else
  let rpr = r1 +. r2 in
  let rpr2 = rpr *. rpr in
  if dd2 > rpr2 then null_object 
  else
  let diff = r1 *. r1 -. r2 *. r2 in
  if dd2 <= diff then
    { t with desc = Intersection(t1,infinite t2); center = c2; radius = r2 }
  else
  if dd2 <= -. diff then
    { t with desc = Intersection(infinite t1,t2); center = c1; radius = r1 } 
  else
  let d = sqrt dd2 in
  let te1 = r1 +. d +. r2 in
  let te2 = r1 +. d -. r2 in
  let te3 = r2 +. d -. r1 in
  let te4 = r1 +. r2 -. d in
  let te = (sqrt (te1 *. te2 *. te3 *. te4)) /. (2. *. d) in
  let alpha = (r1 *. r1 -. r2 *. r2) /. (2. *. dd2) +. 0.5 in
  let center = point_along c1 c1c2 alpha in
  let radius = te in
  { t with desc = Intersection(t1,t2); center = center; radius = radius }
  
let difference_bs t t1 t2 = 
  { t with desc = Difference(infinite t1,t2); 
    center = t1.center; radius = t1.radius }

let by_x p1 p2 = p1.center.x <= p2.center.x
let by_y p1 p2 = p1.center.y <= p2.center.y
let by_z p1 p2 = p1.center.z <= p2.center.z
  
let make_union list = 
  let tab = Array.of_list list in 
  let rec iter_x tab =
    let len = Array.length tab in
    if len = 0 then null_object else
    if len = 1 then tab.(0) else begin
        Sort.array by_x tab;
        let l1 = len / 2 in
        let t1 = iter_y (Array.sub tab 0 l1) in
        let t2 = iter_y (Array.sub tab l1 (len - l1)) in
        union_bs t1 t2
      end
  and  iter_y tab =
    let len = Array.length tab in
    if len = 0 then null_object else
    if len = 1 then tab.(0) else begin
        Sort.array by_y tab;
        let l1 = len / 2 in
        let t1 = iter_z (Array.sub tab 0 l1) in
        let t2 = iter_z (Array.sub tab l1 (len - l1)) in
        union_bs t1 t2
      end
  and  iter_z tab =
    let len = Array.length tab in
    if len = 0 then null_object else
    if len = 1 then tab.(0) else begin
        Sort.array by_z tab;
        let l1 = len / 2 in
        let t1 = iter_x (Array.sub tab 0 l1) in
        let t2 = iter_x (Array.sub tab l1 (len - l1)) in
        union_bs t1 t2 
      end
  in
  iter_x tab
  
let add_planes list pt =
  let rec iter t pt =
    match pt with
      [] -> t
    | p :: pt ->
        if t.desc == Null then iter p pt else
          iter { p with desc = Union(t,p) } pt
  in
  iter (make_union list) pt
  
(* This function returns two values: an object and a list of planes *)
let rec compute_bs t =
  if t.center == not_computed then
    match t.desc with
      Base base -> begin
          match base.kind with
            Cone -> [{ t with
                center = Matrix.apply_to_point base.obj2world cone_center;
                radius = base.max_scale_applied *. cone_radius }], []
          | Cube -> [{ t with
                center = Matrix.apply_to_point base.obj2world cube_center;
                radius = base.max_scale_applied *. cube_radius }], []
          | Cylinder -> [{ t with
                center = Matrix.apply_to_point base.obj2world cylinder_center;
                radius = base.max_scale_applied *. cylinder_radius }], []
          | Plane -> 
              [],
              [{ t with
                  center = origin;
                  radius = plane_radius; }]
          | Sphere -> [{ t with
                center = Matrix.apply_to_point base.obj2world sphere_center;
                radius = base.max_scale_applied *. sphere_radius}], []
        end
    
    | Union (t1,t2) -> 
        let (t1,p1) = compute_bs t1 in
        let (t2,p2) = compute_bs t2 in
        
        (if !no_sort then 
          match (t1,t2) with
              [t1],[t2] -> [union_bs t1 t2]
            | [], [t] -> [t]
            | [t], [] -> [t]
            | [], [] -> []
            | _ -> assert false
        else
          t1@t2), p1 @ p2
    
    | Intersection (t1,t2) ->
        let (t1,p1) = compute_bs t1 in
        let (t2,p2) = compute_bs t2 in
        
        let t1 = add_planes t1 p1 in
        let t2 = add_planes t2 p2 in
        
        if t1.desc == Null then [],[] else
        if t2.desc == Null then [],[] else 
            [intersection_bs t t1 t2], []
          
    | Difference (t1,t2) ->
        let (t1,p1) = compute_bs t1 in
        let (t2,p2) = compute_bs t2 in
        
        let t2 = add_planes t2 p2 in
        if t2.desc == Null then (t1,p1) else
          
        let t1 = add_planes t1 p1 in
        
        if t1.desc == Null then [],[] else
          
          [difference_bs t t1 t2], []
    
    | Null -> ([t],[])
  else ([t],[])  

let radius_coef r sup = 
  if r >= infinite_radius then sup else r *. r
    
let rec max_radius t sup =
  match t.desc with
  | Union (t1,t2) ->
      let sup = radius_coef t.radius sup in
      sup +. max_radius t1 sup +. max_radius t2 sup
  | Intersection (t1,t2) ->
      max_radius t1 0.0 +. max_radius t2 0.0
  | Difference (t1,t2) ->
      let sup = radius_coef t.radius sup in
      max_radius t1 sup +. max_radius t2 0.0
  | _ -> 0.0
      
      
let simplify t = 
  if !dump_tree then dump_obj 0 t;
  if not !no_bs then 
    if !no_sort then
      let (t,p) = compute_bs t in
      let t = add_planes t p in
      if !dump_tree then begin
          print_newline ();
          Printf.printf "AFTER SIMPLIFICATION:"; print_newline ();
          dump_obj 0 t;
        end;
      t
    else
    let (t1,p1) = no_sort := true; compute_bs t in
    let (t2,p2) = no_sort := false; compute_bs t in
    let t1 = add_planes t1 p1 in
    let t2 = add_planes t2 p2 in
    let r1 = max_radius t1 0.0 in
    let r2 = max_radius t2 0.0 in
    let t = if r1 > r2 then t2 else t1
    in
    
    if !dump_tree then begin
        print_newline ();
        Printf.printf "AFTER SIMPLIFICATION:"; print_newline ();
        dump_obj 0 t;
      end;
    t 
  else t
  
let inside_bs p v obj = 
  (obj.radius >= infinite_radius) ||
  ((obj.desc != Null) &&
(* Compute the intersection with the bounding box *)
    (
      (* Shift origin to obj.center *)
      let x = p.x -. obj.center.x
      and y = p.y -. obj.center.y
      and z = p.z -. obj.center.z
      and r = obj.radius
      in
        (* Check whether quadratic has positive discriminant *)
        let a = v.dx *. v.dx +. v.dy *. v.dy +. v.dz *. v.dz
        and b = x *. v.dx +. y *. v.dy +. z *. v.dz
        and c = x *. x +. y *. y +. z *. z -. r *. r
        in b *. b > a *. c
  ))

(* Previous version: *)
(**
      (* begin micro optimization of the code below *)
      let pox = obj.center.x -. p.x
      and poy = obj.center.y -. p.y
      and poz = obj.center.z -. p.z in
      let vpo = pox *. v.dx +. poy *. v.dy +. poz *. v.dz in
      let alpha = -. vpo /. (Vector.length2 v) in
      let hox = pox +. alpha *. v.dx
      and hoy = poy +. alpha *. v.dy
      and hoz = poz +. alpha *. v.dz in
      let r = obj.radius in
      hox *. hox +. hoy *. hoy +. hoz *. hoz <= r *. r
      (* end micro optimization *)
**)

(* Micro-optimized code was: *)
(**
      let po = Vector.between p obj.center in
      let vpo = Vector.dotproduct po v in
(*       if vpo < 0. then false else *)
      let v2 = Vector.length2 v in
      let alpha = -. vpo /. v2 in
      let ho = {
          dx = po.dx +. alpha *. v.dx;
          dy = po.dy +. alpha *. v.dy;
          dz = po.dz +. alpha *. v.dz;
        }
      in
      let ho2 = Vector.length2 ho in
      let r = obj.radius in
      ho2 <= r *. r
**)