open StdLabels

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

  val get_coord : t -> Gg.v2

  val id : t -> int

  val copy : t -> Gg.v2 -> t

end

module Make(Point:P) = struct

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

  module type BUILDER = sig
    type t

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

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


  type step =
    { point : Point.t
    ; move : path
    }

  type t = step array

  module ToFixed = struct
    type point = Point.t

    type t = int * step list

    let create_path () = 0, []

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

    let line_to
      : point -> point -> t -> t
      = fun p1 p2 (i, t) ->
        ( i + 1
        , { point = p1
          ; move = Line p2
          }:: t )

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

    let stop t = t

    let get
      : int * step list -> step 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 to_fixed
    : (module BUILDER with type t = 'a) -> 'a -> t
    = fun (type s) (module Builder: BUILDER with type t = s) t ->
      Builder.repr t (module ToFixed) (ToFixed.create_path ())
      |> ToFixed.get

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

      let _, repr = Array.fold_left t
          ~init:(true, repr)
          ~f:(fun (first, path) element ->
              let path =  if first then
                  Repr.start element.point path
                else path in
              match element.move with
              | Line p1 ->
                ( false
                , Repr.line_to element.point p1 path )
              | Curve bezier ->
                ( false
                , repr_bezier path element.point 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 t ->

      Array.fold_left t
        ~init:None
        ~f:(fun res step ->
            match step.move with
            | Line p1 ->
              let box = Gg.Box2.of_pts (Point.get_coord step.point) (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 step.point
                  ; 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 = step.point
                       ; p1 = bezier.p1
                       ; ratio }
          )

  let map
    : t -> (Point.t -> Point.t) -> t
    = fun t f ->
      Array.map t
        ~f:(fun step ->
            match step.move with
            | Line p2 ->
              { point = f step.point
              ; move = Line (f p2)
              }
            | Curve bezier ->
              let point = f step.point in
              { point
              ; move = Curve
                    { p1 = f bezier.p1
                    ; ctrl0 = Point.get_coord (f (Point.copy step.point bezier.ctrl0))
                    ; ctrl1 = Point.get_coord (f (Point.copy bezier.p1 bezier.ctrl1))
                    }
              }
          )

  let iter
    : t -> f:(Point.t -> unit) -> unit
    = fun t ~f ->
      Array.iter t
        ~f:(fun step ->
            match step.move with
            | Line p2 -> f step.point; f p2
            | Curve bezier -> f step.point ; f bezier.p1
          )

  let get_point'
    : step -> Point.t
    = fun { move ; _} ->
      match move with
      | Line p1 -> p1
      | Curve bezier -> bezier.p1

  (** Associate the return from the bezier point to an existing path *)
  let assoc_point
    : Shapes.Bezier.t -> step -> step
    = fun bezier step ->
      match step.move with
      | Line p1
      | Curve {p1; _} ->
        let p0' = Point.copy step.point bezier.Shapes.Bezier.p0
        and p1' = Point.copy p1 bezier.Shapes.Bezier.p1 in
        { point = p0'
        ; move = Curve
              { p1 = p1'
              ; ctrl0 = bezier.Shapes.Bezier.ctrl0
              ; ctrl1 = bezier.Shapes.Bezier.ctrl1
              }
        }


  let build_from_three_points p0 p1 p2 =
    let bezier =
      Shapes.Bezier.quadratic_to_cubic
      @@ Shapes.Bezier.three_points_quadratic
        (Point.get_coord p0)
        (Point.get_coord p1)
        (Point.get_coord p2) in

    (* The middle point is not exactly at the middle anymore (it can have been
       moved), we have the reevaluate it's position *)
    let ratio, _ = Shapes.Bezier.get_closest_point
        (Point.get_coord p1)
        bezier in

    let b0, b1 = Shapes.Bezier.slice ratio bezier in
    let p0' = Point.copy p0 b0.Shapes.Bezier.p0
    and p1' = Point.copy p1 b0.Shapes.Bezier.p1
    and p2' = Point.copy p2 b1.Shapes.Bezier.p1 in

    [| { point = p0'
       ; move =
           Curve { ctrl0 = b0.Shapes.Bezier.ctrl0
                 ; ctrl1 = b0.Shapes.Bezier.ctrl1
                 ; p1 = p1'
                 } }
     ; { point = p1'
       ; move = Curve { ctrl0 = b1.Shapes.Bezier.ctrl0
                      ; ctrl1 = b1.Shapes.Bezier.ctrl1
                      ; p1 = p2' }
       } |]

  (** Rebuild the whole curve by evaluating all the points *)
  let rebuild
    : t -> t option
    = fun t ->

      match Array.length t with
      | 0 -> None
      | 1 ->
        let step = Array.get t 0 in
        begin match step.move with
          | Curve {p1; _}
          | Line p1 ->
            Some
              [|
                { point = step.point
                ; move = Line p1 } |]
        end
      | 2 ->
        let p0 = (Array.get t 0).point
        and p1 = (Array.get t 1).point
        and p2 = get_point' @@ Array.get t 1 in
        Some (build_from_three_points p0 p1 p2)

      | _ ->

        (* Convert all the points in list *)
        let points = List.init
            ~len:((Array.length t) )
            ~f:(fun i -> Point.get_coord @@ get_point' (Array.get t i)) in
        let p0 = Point.get_coord @@ (Array.get t 0).point in

        let points = p0::points in

        (* We process the whole curve in a single block *)
        begin match Shapes.Bspline.to_bezier points with
          | Error `InvalidPath -> None
          | Ok beziers ->

            (* Now for each point, reassociate the same point information,
               We should have as many points as before *)
            let rebuilded = Array.map2 beziers t ~f:assoc_point in
            Some rebuilded
        end

  let find_pt_index
    : Point.t -> step array -> int option
    = fun point path ->
      (* First search the element to remove. The counter mark the position of
         the point to remove, not the segment itself. *)
      let idx = ref None
      and counter = ref 0  in

      let _ = Array.exists
          path
          ~f:(fun element ->
              let res =
                if (Point.id element.point) = (Point.id point) then (
                  idx := Some (!counter) ;
                  true
                ) else match element.move with
                  | Line p1
                  | Curve {p1;_} when (Point.id p1) = (Point.id point) ->
                    idx := Some (!counter+1) ;
                    true
                  | _ ->
                    false
              in
              incr counter;
              res) in
      !idx

  let remove_point
    : t -> Point.t -> t option
    = fun t point ->

      match Array.length t with
      | 0
      | 1 -> None
      | 2 ->
        (* Two segment, we get the points and transform this into a single line *)
        let p0 = (Array.get t 0).point
        and p1 = (Array.get t 1).point
        and p2 = get_point' @@ Array.get t 1 in
        let elms = List.filter [p0; p1; p2]
            ~f:(fun pt -> Point.id pt != Point.id point) in
        begin match elms with
          | p0::p1::[] ->
            Some
              [| { point = p0
                 ; move = Line p1 }|]
          | _ -> None
        end
      | l ->
        match find_pt_index point t with
        | None -> Some t
        | Some 0 ->
          (* Remove the first point *)
          let path = Array.init (l-1)
              ~f:( fun i -> Array.get t (i+1)) in
          Some path
        | Some n when n = (Array.length t) ->
          (* Remove the last point *)
          let path = Array.init (l-1)
              ~f:( fun i -> Array.get t i) in
          Some path
        | Some n ->
          let path' = Array.init (l-1)
              ~f:(fun i ->
                  if i < (n-1) then
                    Array.get t (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 p0 = (Array.get t i).point in

                    match (Array.get t (i+1)).move with
                    | Line p1 ->
                      { point = p0
                      ; move = Line p1 }
                    | Curve c ->
                      { point = p0
                      ; move = Curve c }

                  else
                    Array.get t (i+1)
                ) in
          rebuild path'

  let first_point
    : step -> Point.t
    = fun {point; _} -> point

  let replace_point
    : t -> Point.t -> t option
    = fun t p ->

      let add_path paths idx f points =
        if 0 <= idx && idx < Array.length paths then
          let path = Array.get t idx in
          Point.get_coord (f path)
          :: points
        else points in

      match Array.length t with
      | 0 -> None
      | 1 -> (* Only one point, easy ? *)
        let step = Array.get t 0 in
        begin match step.move with
          | Curve {p1; _}
          | Line p1 ->
            let p0 = if (Point.id step.point = Point.id p) then p else step.point
            and p1 = if (Point.id p1 = Point.id p) then p else p1 in
            Some [|
              { point = p0
              ; move = Line p1 }
            |]
        end

      | 2 ->
        let p0 = (Array.get t 0).point
        and p1 = (Array.get t 1).point
        and p2 = get_point' @@ Array.get t 1 in

        let p0 = if (Point.id p0 = Point.id p) then p else p0
        and p1 = if (Point.id p1 = Point.id p) then p else p1
        and p2 = if (Point.id p2 = Point.id p) then p else p2 in
        Some (build_from_three_points p0 p1 p2)

      (* More than two segmend, it is ok for a partial reevaluation *)
      | _ ->
        match find_pt_index p t with
        | None -> None
        | Some n ->
          let path = Array.copy t in

          let p0, p1 =

            if n < Array.length path then
              p, get_point' (Array.get path n)
            else
              (Array.get path (n -1)).point, p
          in

          let min_idx = max (n-3) 0 in

          let points =
            add_path path (n-3) first_point
            @@ add_path path (n-2) first_point
            @@ add_path path (n-1) first_point
            @@ (fun tl -> (Point.get_coord p)::tl)
            @@ add_path path n get_point'
            @@ add_path path (n+1) get_point'
            @@ add_path path (n+2) get_point'
            @@ [] in

          (* It is impressive how fast it is to evaluate the curve ! Maybe is the
             worker not required at all…
          *)
          let bezier_opt = Shapes.Bspline.to_bezier points in
          begin match bezier_opt with
            | Ok paths ->
              Array.iteri paths
                ~f:(fun i bezier ->
                    (* Only take two points before, and two after  *)
                    let idx = min_idx + i in
                    if (n-2 < idx) && (idx < n +2) && idx < Array.length path then
                      Array.set path idx (assoc_point bezier (Array.get path idx))
                  );
              Some path
            | Error _ ->
              let bezier', _ = Shapes.Bezier.three_points_quadratic
                  (Point.get_coord p)
                  (Point.get_coord @@ get_point' (Array.get path 0))
                  (Point.get_coord @@ get_point' (Array.get path 1))
                               |> Shapes.Bezier.quadratic_to_cubic
                               |> Shapes.Bezier.slice 0.5
              in
              Array.set path 0
                { point = p0
                ; move = (Curve
                            { ctrl0 = bezier'.Shapes.Bezier.ctrl0
                            ; ctrl1 = bezier'.Shapes.Bezier.ctrl1
                            ; p1
                            })
                };
              Some path
          end
end