open StdLabels

(** Signature for points *)
module type P = sig
  type t

  val get_coord : t -> Gg.v2

  val id : t -> int

end

module Make(Point:P) = struct

  module type BUILDER = sig
    type t

    val repr
      : t -> (module Repr.M with type t = Point.t and type repr = 's) -> 's -> 's
  end

  type bezier =
    { p0:Point.t    (* The starting point *)
    ; p1:Point.t    (* The end point *)
    ; ctrl0:Gg.v2   (* The control point *)
    ; ctrl1:Gg.v2 } (* The control point *)

  type path =
    | Line of Point.t * Point.t
    | Curve of bezier

  type t =
    { id: int
    ; path : path array }

  let id
    : t -> int
    = fun {id; _} -> id

  let path
    : t -> path array
    = fun {path; _} -> path

  module ToFixed = struct
    type t = Point.t

    type repr = int * path list

    let create_path () = 0, []

    (* Start a new path. *)
    let start point t =
      let _ = point in
      t

    let line_to
      : t -> t -> repr -> repr
      = fun p1 p2 (i, t) ->
        ( i + 1
        , Line (p1, p2)::t)

    let quadratic_to
      : t -> Gg.v2 -> Gg.v2 -> t -> repr -> repr
      = fun p0 ctrl0 ctrl1 p1 (i, t) ->
        let curve = Curve
            { p0
            ; ctrl0
            ; ctrl1
            ; p1} in
        ( i + 1
        , curve::t)

    let stop t = t

    let get
      : int * path list -> path array
      = fun (n, t) ->

        (* The array is initialized with a magic number, and just after
           filled with the values from the list in reverse. All the elements are set.
        *)
        let res = Obj.magic (Array.make n 0) in
        List.iteri t
          ~f:(fun i elem -> Array.set res (n - i - 1) elem );
        res
  end

  let internal_id = ref 0

  let to_fixed
    : (module BUILDER with type t = 'a) -> 'a -> t
    = fun (type s) (module Builder: BUILDER with type t = s) t ->
      incr internal_id;
      { id = !internal_id
      ; path = Builder.repr t (module ToFixed) (ToFixed.create_path ())
               |> ToFixed.get
      }

  let repr
    : t -> (module Repr.M with type t = Point.t and type repr = 's) -> 's -> 's
    = fun (type s) {path; _} (module Repr : Repr.M with type t = Point.t and type repr = s) repr ->
      let repr_bezier p bezier =
        Repr.quadratic_to
          bezier.p0
          bezier.ctrl0
          bezier.ctrl1
          bezier.p1
          p in

      let _, repr = Array.fold_left path
          ~init:(true, repr)
          ~f:(fun (first, path) element ->
              match element with
              | Line (p0, p1) ->

                let path =  if first then
                    Repr.start p0 path
                  else path in

                ( false
                , Repr.line_to p0 p1 path )
              | Curve bezier ->
                let path =  if first then
                    Repr.start bezier.p0 path
                  else path in
                ( false
                , repr_bezier path bezier )
            ) in
      Repr.stop repr


  type approx =
    { distance : float
    ; closest_point : Gg.v2
    ; ratio : float
    ; p0 : Point.t
    ; p1 : Point.t }

  (** Return the distance between a given point and the curve. May return
      None if the point is out of the curve *)
  let distance
    : Gg.v2 -> t -> approx option
    = fun point beziers ->

      Array.fold_left beziers.path
        ~init:None
        ~f:(fun res -> function
            | Line (p0, p1) ->
              let box = Gg.Box2.of_pts (Point.get_coord p0) (Point.get_coord p1) in
              begin match Gg.Box2.mem point box with
                | false -> res
                | true ->
                  (* TODO Evaluate the normal *)
                  res
              end
            | Curve bezier ->

              let bezier' = Shapes.Bezier.(

                  { p0 = Point.get_coord bezier.p0
                  ; p1 = Point.get_coord bezier.p1
                  ; ctrl0 = bezier.ctrl0
                  ; ctrl1 = bezier.ctrl1 }
                ) in
              let ratio, point' = Shapes.Bezier.get_closest_point point bezier' in
              let distance' = Gg.V2.( norm (point - point') ) in
              match res with
              | Some {distance; _} when distance < distance' -> res
              | _ -> Some
                       { closest_point = point'
                       ; distance = distance'
                       ; p0 = bezier.p0
                       ; p1 = bezier.p1
                       ; ratio }
          )

  let map_point
    : t -> (Point.t -> Point.t) -> t
    = fun {id; path} f ->
      let path = Array.map path
          ~f:(function
              | Line (p1, p2) -> Line (f p1, f p2)
              | Curve bezier -> Curve {bezier with p0 = f bezier.p0 ; p1 = f bezier.p1}
            ) in
      {id; path}

  let iter
    : t -> f:(Point.t -> unit) -> unit
    = fun {path; _} ~f ->
      Array.iter path
        ~f:(function
            | Line (p1, p2) -> f p1; f p2
            | Curve bezier -> f bezier.p0 ; f bezier.p1
          )

  let remove_point
    : t -> Point.t -> t
    = fun {id; path} point ->

      (* First search the element to remove *)
      let idx = ref None
      and counter = ref 0  in

      let _ = Array.exists
          path
          ~f:(fun element ->

              let res = match element with
                | Line (p0, p1)
                | Curve {p0;p1;_} ->
                  if (Point.id p0) = (Point.id point) then (
                    idx := Some (!counter) ;
                    true
                  ) else if (Point.id p1) = (Point.id point) then (
                    idx := Some (!counter+1) ;
                    true
                  ) else
                    false
              in
              incr counter;
              res) in

      match !idx with
      | None -> {id; path}
      | Some 0 ->
        (* Remove the first point *)
        let path' = Array.init
            ((Array.length path)-1)
            ~f:( fun i -> Array.get path (i+1)) in
        { id
        ; path = path'
        }
      | Some n when n = (Array.length path) ->
        (* Remove the last point *)
        let path' = Array.init
            ((Array.length path)-1)
            ~f:( fun i -> Array.get path i) in
        { id
        ; path=path'
        }
      | Some n ->
        let path' = Array.init
            ((Array.length path)-1)
            ~f:(fun i ->
                if i < (n-1) then
                  Array.get path (i)
                else if i = (n-1) then
                  (* We know that the point is not the first nor the last one.
                     So it is safe to call n-1 or n + 1 point

                     We have to rebuild the point and set that
                     point_(-1).id = point_(+1).id
                  *)
                  let previous_p1 =
                    match Array.get path (i-1) with
                    | Line (_, p1) -> p1
                    | Curve c -> c.p1
                  in

                  match Array.get path (i+1) with
                  | Line (_, p1) -> Line (previous_p1, p1)
                  | Curve c -> Curve {c with p0 = previous_p1}

                else
                  Array.get path (i+1)
              ) in
        { id
        ; path=path'}

  let update
    : t -> path array -> t
    = fun {id; _} path -> {id; path}

end