exception Yikes of string

(* not realy vector related, but we'll put it here for lack of a better place *)
let sane_mod (a:int) (b:int) =
  let modres = a mod b in
  if modres < 0
  then
    modres + b
  else
    modres

(* helper function for subtracting occlusion angles *)
(* moves a closer to 0.0 by amount b *)
let abs_sub (a:float) (b:float) =
  assert (b >= 0.0);
  if a > 0.0
  then
    if b < a
    then
      (a -. b)
    else 
      0.0
  else
    if b < (-.a)
    then
      (a +. b)
    else
      0.0

let clamp (min:float) (x:float) (max:float) =
  if x < min then min
  else
    if x > max then max
    else
      x

type vec = {x:float; y:float; z:float}
(* type vec = float array *)

type ray = {
    origin : vec;
    dir    : vec;
}

type plane = vec * float

(* a few constant definitions borrowed from http://www.ffconsultancy.com/free/ray_tracer/ *)
let delta = sqrt epsilon_float 
let pi = 4. *. (atan 1.)
let dcos d = cos((pi *. d) /. 180.0)
let dsin d = sin((pi *. d) /. 180.0)
let dtan d = tan((pi *. d) /. 180.0)
let dinvtan t = (atan t) *. (180.0 /. pi)
let dinvcos c = (acos c) *. (180.0 /. pi)
let sqrt2 = sqrt 2.0


let x vctr = vctr.x
let y vctr = vctr.y
let z vctr = vctr.z

(* vec array access *)
(*
let va (v:vec) i = match i with 
  0 -> v.x
| 1 -> v.y
| _ -> v.z
*)

(* surprisingly, this works, but it seems a bit slower than array access *)
let va (v:vec) i =
  (Obj.magic v).(i)

(* vec array assignment
let vaa (v:vec) i f = match i with
  0 -> v.x <- f
| 1 -> v.y <- f
| 2 -> v.z <- f
| _ -> raise (Failure "bad vector assignment index")
*)

let vec (x1:float) (y1:float) (z1:float) = {x=x1; y=y1; z=z1}

let vdot v1 v2 = 
  let a = v1.x *. v2.x
  and b = v1.y *. v2.y
  and c = v1.z *. v2.z
  in a+.b+.c

let vcross v1 v2 = 
  vec
    ((v1.y*.v2.z) -. (v1.z*.v2.y))
    ((v1.z*.v2.x) -. (v1.x*.v2.z))
    ((v1.x*.v2.y) -. (v1.y*.v2.x))

let vmap  f (v1:vec) = 
  vec (f v1.x) (f v1.y) (f v1.z)

let vmap2 f (v1:vec) (v2:vec) = 
  vec (f v1.x v2.x) (f v1.y v2.y) (f v1.z v2.z)

let vinvert (v1:vec)  = vec (-.v1.x) (-.v1.y) (-.v1.z)
let vlensqr (v1:vec)  = (vdot v1 v1)
let vlen (v1:vec)     = sqrt (vlensqr v1)

let vadd v1 v2 =
  vec (v1.x +. v2.x) (v1.y +. v2.y) (v1.z +. v2.z)

let vsub (v1:vec) (v2:vec) =
  vec (v1.x -. v2.x) (v1.y -. v2.y) (v1.z -. v2.z)

let vinc v1 n =
  vec (v1.x +. n) (v1.y +. n) (v1.z +. n)

let vdec v1 n =
  vec (v1.x -. n) (v1.y -. n) (v1.z -. n)

let vdiv v1 v2 =
  vec (v1.x /. v2.x) (v1.y /. v2.y) (v1.z /. v2.z)

let vcopy (v1:vec) =
  vec v1.x v1.y v1.z

let vmax (v1:vec) (v2:vec) =
  vec (max v1.x v2.x) (max v1.y v2.y) (max v1.z v2.z)

let vmin (v1:vec) (v2:vec) =
  vec (min v1.x v2.x) (min v1.y v2.y) (min v1.z v2.z)

let vscale v1 fac =
  let x1 = v1.x *. fac
  and y1 = v1.y *. fac
  and z1 = v1.z *. fac
  in
  vec x1 y1 z1

let vsign v =
  vec 
    (if v.x >= 0.0 then 1.0 else (-.1.0))
    (if v.y >= 0.0 then 1.0 else (-.1.0))
    (if v.z >= 0.0 then 1.0 else (-.1.0))

(* add v2 to v1 after scaling by fac *)
let vscaleadd v1 v2 fac =
  vec
    (v1.x +. (v2.x *. fac))
    (v1.y +. (v2.y *. fac))
    (v1.z +. (v2.z *. fac))

let vnorm (a:vec) =
  let x = a.x
  and y = a.y
  and z = a.z
  in
  let len = 1.0 /. (sqrt ((x *. x) +. (y *. y) +. (z *. z)))
  in vec (x*.len) (y*.len) (z*.len)

(* normalize, but return the length as well *)
let vnormlen (a:vec) = 
  let x = a.x
  and y = a.y
  and z = a.z
  in
  let len = (sqrt ((x *. x) +. (y *. y) +. (z *. z))) in
  let invlen = 1.0 /. len in
  ((vec (x*.invlen) (y*.invlen) (z*.invlen)),len)

let normalized (v:vec) =
  let delta2 = 0.00002 in (* delta is too sensitive, it seems *)
  let x=(vlen v) -. 1.0 in
  if x > delta2 || x < (-.delta2)
  then
    (print_endline ("not normalized, len is " ^ (string_of_float (x +. 1.0) )); false)
  else
    true

let vrcp (a:vec) =
  vec (1.0 /. a.x) (1.0 /. a.y) (1.0 /. a.z)

(* bisect two vectors *)
let bisect v1 v2 =
  vnorm (vadd v1 v2)

let vdistsqr v1 v2 =
  let d = vsub v2 v1
  in vlensqr d

(* euclidean distance *)
let vdist v1 v2 = 
  let d = vsub v2 v1 in vlen d

(* manhattan length *)
let vmanlen v1 = (abs_float v1.x) +. (abs_float v1.y) +. (abs_float v1.z)

(* manhattan distance *)
let vmandist v1 v2 = 
  let d = vsub v2 v1 
  in vmanlen d

let vzero = vec 0.0 0.0 0.0
    
let vtostring v = "<" ^ (string_of_float (v.x)) ^ "," ^ (string_of_float (v.y)) ^ "," ^ (string_of_float (v.z)) ^ ">" 


(* angle between two normalized vectors *)
let vang v1 v2 =
  dinvcos (vdot v1 v2)

(* angle between two normalized rays *)
let angular_dist ray1 ray2 =
  vang ray1.dir ray2.dir

(* volume of bounding box, assumes v1 is min values, v2 is max *)
let vol v1 v2 =
  (max (v2.x -. v1.x) 0.0) *.
    (max (v2.y -. v1.y) 0.0) *.
    (max (v2.z -. v1.z) 0.0)
    
(* surface area of a bounding box *)
let sa v1 v2 =
  let dx = v2.x -. v1.x
  and dy = v2.y -. v1.y
  and dz = v2.z -. v1.z in
  assert (dx >= 0.0 && dy >= 0.0 && dz >= 0.0);
  ((dx *. dy) +. (dy *. dz) +. (dx *. dz)) *. 2.0

(* ray origin is point on plane, dir is normal *)
let plane ray =
  (ray.dir, -.(vdot ray.origin ray.dir ))

(* vector going through both points, with origin at first point *)
let ray p1 p2 =
  {origin=p1; dir=vnorm (vsub p2 p1)}

let ray_unnormalized p1 p2 =
  {origin=p1; dir=vsub p2 p1}

let ray_2pt from target =
  {origin=from; dir=(vnorm (vsub target from))}

(* find intersection with plane *)
(* from graphics gems -- an efficient ray-polygon intersection *)
(* it seems that the ray need not be normalized *)
let plane_intersect ray (n,d) =
  let t = -.((d +. (vdot n ray.origin)) /. (vdot n ray.dir))
  in vadd ray.origin (vscale ray.dir t)

(* return vector v after reflecting off a surface with normal norm *)
let reflect v norm =
  assert (normalized v);
  assert (normalized norm);
  (vadd v (vscale norm (2.0 *. (-.(vdot v norm)))))

let max3 a b c =
  let tmp = if a > b then a else b
  in
  if tmp > c then tmp else c

(* given triangle with sides with lengths a b and c, find angle opposite side c *)
let sss2a a b c =
  dinvcos ( -.((((c*.c) -. ((a*.a) +. b*.b))) /. (2. *. a *. b)) )

(* given a side, angle, and side of a triangle, produce the length of the opposite side *)
let sas2s s1 a s2 =
  sqrt (((s1 *. s1) +. (s2 *.s2)) -. ((2. *. s1 *. s2 *. (dcos a))))

(* given hypotenuse and one side of a right triangle, find the other side *)
let ssra h s1 =
  if s1 > h 
  then raise (Failure "side longer than hypotenuse")
  else
    sqrt ((h *. h) -. (s1 *. s1))

(* this should work if we don't have any sides for which r1 + r2 < side length *)
let tri_covered_allsides_ok ab bc ac ar br cr =
  let apb = sss2a ar br ab
  and bpc = sss2a br cr bc
  and apc = sss2a ar cr ac
  in
  if apb +. bpc +. apc < 360.0 
  then true 
  else false

let tri_covered_ab_bc_ok ab bc ac ar br cr =
  if ar +. cr > ac 
  then
    tri_covered_allsides_ok ab bc ac ar br cr
  else
    let balt = ab *. dsin (sss2a ab ac bc) in 
    if br < balt 
    then false (* br doesn't even reach far side *)
    else
      let dp = ssra br balt in
      let ap = ssra ab balt 
      and cp = ssra bc balt in
      if dp +. ar > ap && dp +. cr > cp
      then
	true
      else
	false

(* precondition: ar +. br > ab *)
let tri_covered_ab_ok ab bc ac ar br cr =
  if ar +. br < ab then
    raise (Failure "tri_covered_ab_ok precondition failed")
  else
    if br +. cr > bc 
    then
      tri_covered_ab_bc_ok ab bc ac ar br cr
    else
      if ar +. cr > ac 
      then
	tri_covered_ab_bc_ok ab ac bc br ar cr
      else
	false

let tri_covered ab bc ac ar br cr =
  assert (ar >= 0.0);
  assert (br >= 0.0);
  assert (cr >= 0.0);
  if ar > ab && ar > ac
  then true
  else 
    if br > ab && br > bc
    then true
    else
      if cr > ac && cr > bc
      then true
      else
	let ar_ = ref ar
	and br_ = ref br
	and cr_ = ref cr in
	let () = begin
	  if !ar_ > ab +. !br_ then br_ := (!ar_ -. ab) +. delta else ();
	  if !ar_ > ac +. !cr_ then cr_ := (!ar_ -. ac) +. delta else ();
	  if !br_ > ab +. !ar_ then ar_ := (!br_ -. ab) +. delta else ();
	  if !br_ > bc +. !cr_ then cr_ := (!br_ -. bc) +. delta else ();
	  if !cr_ > ac +. !ar_ then ar_ := (!cr_ -. ac) +. delta else ();
	  if !cr_ > bc +. !br_ then br_ := (!cr_ -. bc) +. delta else ();
	end in

	if ar +. br > ab then
	  tri_covered_ab_ok ab bc ac !ar_ !br_ !cr_
	else
	  if br +. cr > bc then
	    tri_covered_ab_ok bc ac ab !br_ !cr_ !ar_
	  else
	    false