open Gg.V2

let norm_angle vect =
  mod_float
    ((angle vect) +. Gg.Float.two_pi)
    Gg.Float.two_pi


let intersection
  : (Gg.v2 * Gg.v2) -> (Gg.v2 * Gg.v2) -> Gg.v2 option
  = fun (p0, p1) (p2, p3) ->
    let i = p1 - p0
    and j = p3 - p2 in

    let d = (x i *. y j) -. (y i *. x j) in
    if Float.( (abs d) <= 0.01 ) then
      None
    else
      let m = ((x i) *. (y p0)
               -. (x i) *. (y p2)
               -. (y i) *. (x p0)
               +. (y i) *. (x p2)) /. d in
      Some (p2 + m * j)
      (*
      let k = ((x j) *. (y p0)
               -. (x j) *. (y p2)
               -. (y j) *. (x p0)
               +. (y j) *. (x p2)) /. d in
      Some (p0 + k * i)
      *)


let center
  : Gg.v2 -> Gg.v2 -> Gg.v2 -> Gg.v2 option
  = fun p0 p1 p2 ->
    (* deltas *)
    let d1 = p1 - p0
    and d2 = p2 - p1 in

    (* perpendiculars *)
    let d1p = ortho d1
    and d2p = ortho d2 in

    (* Chord midpoints *)
    let m1 = half (p0 + p1)
    and m2 = half (p1 + p2) in

    (* midpoint offsets *)
    let m1n = m1 + d1p
    and m2n = m2 + d2p in

    intersection (m1, m1n) (m2, m2n)

let rotate
  : Gg.v2 -> float -> Gg.v2
  = fun p0 theta ->
    let r = x (to_polar p0) in
    of_polar (v r theta)

let equal_point
  : float -> Gg.v2 -> Gg.v2 -> bool
  = fun eps p0 p1 ->
    Gg.V2.equal_f (fun v0 v1 ->  (Float.abs (v1 -. v0)) <= eps ) p0 p1