open Misc
open Config
open Point
open Object
open Vector  

type intervlist = (float * base_object * float * base_object) list
  
(* Union of two interval lists *)

let rec union_intervals (l1: intervlist) (l2: intervlist) =
  match (l1, l2) with
    ([], _) -> l2
  | (_, []) -> l1
  | ((beg1, objbeg1, end1, objend1 as int1) :: rem1,
     (beg2, objbeg2, end2, objend2 as int2) :: rem2) ->
      if end1 < beg2 then
        (* int1 strictly before int2 *)
        int1 :: union_intervals rem1 l2
      else
      if end2 < beg1 then
        (* int2 strictly before int1 *)
        int2 :: union_intervals l1 rem2
      else begin
        (* int1 and int2 overlap, merge *)
        let (beg, objbeg) =
          if beg1 < beg2 then (beg1, objbeg1) else (beg2, objbeg2) in
        if end1 < end2
        then union_intervals rem1 ((beg, objbeg, end2, objend2) :: rem2)
        else union_intervals ((beg, objbeg, end1, objend1) :: rem1) rem2
      end

(* Intersection of two interval lists *)

let rec intersect_intervals (l1: intervlist) (l2: intervlist) =
  match (l1, l2) with
    ([], _) -> []
  | (_, []) -> []
  | ((beg1, objbeg1, end1, objend1 as int1) :: rem1,
     (beg2, objbeg2, end2, objend2 as int2) :: rem2) ->
      if end1 < beg2 then
        (* int1 strictly before int2 *)
        intersect_intervals rem1 l2
      else
      if end2 < beg1 then
        (* int2 strictly before int1 *)
        intersect_intervals l1 rem2
      else begin
        (* int1 and int2 overlap, add intersection *)
        let (beg, objbeg) =
          if beg1 > beg2 then (beg1, objbeg1) else (beg2, objbeg2) in
        if end1 < end2
        then (beg, objbeg, end1, objend1) :: intersect_intervals rem1 l2
        else (beg, objbeg, end2, objend2) :: intersect_intervals l1 rem2
      end

(* Difference of two interval lists *)

let rec difference_intervals (l1: intervlist) (l2: intervlist) =
  match (l1, l2) with
    ([], _) -> []
  | (_, []) -> l1
  | ((beg1, objbeg1, end1, objend1 as int1) :: rem1,
     (beg2, objbeg2, end2, objend2 as int2) :: rem2) ->
      if end1 < beg2 then
        (* int1 strictly before int2, keep int1 *)
        int1 :: difference_intervals rem1 l2
      else
      if end2 < beg1 then
        (* int2 strictly before int1, throw int2 away *)
        difference_intervals l1 rem2
      else begin
        (* int1 and int2 overlap *)
        let pre =
          if beg1 < beg2 then [(beg1, objbeg1, beg2, objbeg2)] else [] in
        if end1 > end2
        then pre @ difference_intervals ((end2, objend2, end1, objend1) :: rem1) rem2
        else pre @ difference_intervals rem1 l2
      end

(* Intersection between the ray p + tv and the plane y = 0 *)

let intersect_ray_plane bobj p v =
  if p.y >= 0.0 then
    if v.dy >= 0.0 then [] else [(-. p.y /. v.dy, bobj, pos_infty, bobj)]
  else
    if v.dy >= 0.0 then [(0.0, bobj, -. p.y /. v.dy, bobj)]
                   else [(0.0, bobj, pos_infty, bobj)]

(* Intersection between the ray p + tv and the unit sphere
   x^2 + y^2 + z^2 = 1 *)
  
let intersect_ray_sphere bobj p v =
  let a = v.dx *. v.dx +. v.dy *. v.dy +. v.dz *. v.dz in
  let b = p.x *. v.dx +. p.y *. v.dy +. p.z *. v.dz in
  let c = p.x *. p.x +. p.y *. p.y +. p.z *. p.z -. 1.0 in
  let d = b *. b -. a *. c in
  if d <= 0.0 then [] else begin
    let sq = sqrt d in
    let (t0, t1) = (* Numerically stable solution of quadratic. *)
      if b >= 0.0 then c /. (-. b -. sq), (-. b -. sq) /. a
                  else c /. (-. b +. sq), (-. b +. sq) /. a
    in 
    let (t0, t1) = if t0 < t1 then (t0, t1) else (t1, t0) in
    if t1 <= 0.0 then begin
      assert (t0 <= 0.0);
      []
    end
    else if t0 >= 0.0 then begin
      assert (t1 >= 0.0);
      [(t0, bobj, t1, bobj)]
    end
    else
      [(0.0, bobj, t1, bobj)]
  end

(* Intersection between the ray p + tv and the unit cube
   x, y, z = 0 or 1 *)

let intersect_ray_slice bobj pc vc =
  if pc > 1.0 then
    if vc >= 0.0 then [] else [(1.0 -. pc) /. vc, bobj, -. pc /. vc, bobj]
  else if pc < 0.0 then
    if vc <= 0.0 then [] else [-. pc /. vc, bobj, (1.0 -. pc) /. vc, bobj]
  else
    if vc = 0.0 then [0.0, bobj, pos_infty, bobj]
    else if vc > 0.0 then [0.0, bobj, (1.0 -. pc) /. vc, bobj]
                     else [0.0, bobj, -. pc /. vc, bobj]

let intersect_ray_cube bobj p v =
  intersect_intervals (intersect_ray_slice bobj p.x v.dx)
    (intersect_intervals (intersect_ray_slice bobj p.y v.dy)
                         (intersect_ray_slice bobj p.z v.dz))

(* Intersection between the ray p + tv and the unit infinite cylinder
   x^2 + z^2 = 1 *)

let intersect_ray_infinite_cylinder bobj p v =
  let a = v.dx *. v.dx +. v.dz *. v.dz in
  let b = p.x *. v.dx +. p.z *. v.dz in
  let c = p.x *. p.x +. p.z *. p.z -. 1.0 in
  let d = b *. b -. a *. c in
  if d <= 0.0 then [] else begin
    let sq = sqrt d in
    let (t0, t1) = (* Numerically stable solution of quadratic. *)
      if b >= 0.0 then c /. (-. b -. sq), (-. b -. sq) /. a
                  else c /. (-. b +. sq), (-. b +. sq) /. a
    in 
    let (t0, t1) = if t0 < t1 then (t0, t1) else (t1, t0) in
    if t1 <= 0.0 then begin
      assert (t0 <= 0.0);
      []
    end
    else if t0 >= 0.0 then begin
      assert (t1 >= 0.0);
      [(t0, bobj, t1, bobj)]
    end
    else
      [(0.0, bobj, t1, bobj)]
  end

(* Intersection between the ray p + tv and the unit cylinder
   x^2 + z^2 <= 1  and 0 <= y <= 1 *)

let intersect_ray_cylinder bobj p v =
  intersect_intervals (intersect_ray_infinite_cylinder bobj p v)
                      (intersect_ray_slice bobj p.y v.dy)

(* Intersection between the ray p + tv and the unit infinite cone
   x^2 + z^2 = y^2 *)

let intersect_ray_infinite_cone bobj p v =
  let a = v.dx *. v.dx -. v.dy *. v.dy +. v.dz *. v.dz in
  let b = p.x *. v.dx -. p.y *. v.dy +. p.z *. v.dz in
  let c = p.x *. p.x -. p.y *. p.y +. p.z *. p.z in
  if a = 0.0 then begin
    if b = 0.0 then [] else
    let t = -. 0.5 *. c /. b in
    if c < 0.0 then
      if t <= 0.0 then [(0.0, bobj, pos_infty, bobj)]
                  else [(0.0, bobj, t, bobj)]
    else if t <= 0.0
      then []
      else [(t, bobj, pos_infty, bobj)]
  end else
  let d = b *. b -. a *. c in
  if d <= 0.0 then [] else begin
    let sq = sqrt d in
    let (t0, t1) = (* Numerically stable solution of quadratic. *)
      if b >= 0.0 then c /. (-. b -. sq), (-. b -. sq) /. a
                  else c /. (-. b +. sq), (-. b +. sq) /. a
    in 
    let (t0, t1) = if t0 < t1 then (t0, t1) else (t1, t0) in
    if t1 <= 0.0 then begin
      assert (t0 <= 0.0);
      if c < 0.0 then [(0.0, bobj, pos_infty, bobj)] else []
    end
    else if t0 >= 0.0 then begin
      assert (t1 >= 0.0);
      if c < 0.0 then [(0.0, bobj, t0, bobj); (t1, bobj, pos_infty, bobj)]
                 else [(t0, bobj, t1, bobj)]
    end
    else
      if c < 0.0 then [(0.0, bobj, t1, bobj)]
      else [(t1, bobj, pos_infty, bobj)]
  end

(* Intersection between the ray p + tv and the unit cone
   x^2 + z^2 <= y^2  and 0 <= y <= 1 *)

let intersect_ray_cone bobj p v =
  intersect_intervals (intersect_ray_infinite_cone bobj p v)
                      (intersect_ray_slice bobj p.y v.dy)

(* Build the interval list for the given ray and the given composite object *)

let rec intersect_ray_object p v obj =
  if !no_bs || Simplify.inside_bs p v obj then 
  match obj.desc with
    Base b ->
      let p' = Matrix.apply_to_point b.world2obj p
      and v' = Matrix.apply_to_vect b.world2obj v in
      begin match b.kind with
      | Cone -> intersect_ray_cone b p' v'
      | Cube -> intersect_ray_cube b p' v'
      | Cylinder -> intersect_ray_cylinder b p' v'
      | Plane -> intersect_ray_plane b p' v'
      | Sphere -> intersect_ray_sphere b p' v'
      end
  | Union(obj1, obj2) ->
      union_intervals (intersect_ray_object p v obj1)
                      (intersect_ray_object p v obj2)
  | Intersection(obj1, obj2) ->
      intersect_intervals (intersect_ray_object p v obj1)
                          (intersect_ray_object p v obj2)
  | Difference(obj1, obj2) ->
      difference_intervals (intersect_ray_object p v obj1)
        (intersect_ray_object p v obj2)
  | Null -> []
  else
    []
    
(* Return the closest base object intersecting the given ray, and
   the curvilinear abscissa t of the intersection point.
   Return None if no intersection.
   Return t = 0.0 if the viewpoint (origin of the ray) is inside an
   object. *)

let intersect_ray p v obj initial =
  match intersect_ray_object p v obj with
    [] -> None
  | (t, obj, _, _) :: rem ->
      assert (t >= 0.0);
      if t <= 0.0 && not initial then begin
        match rem with
          [] -> None
        | (t2, obj2, _, _) :: _ -> Some(t2, obj2)
      end else Some(t, obj)