open StdLabels

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

  val empty : t

  val get_coord : t -> Gg.v2

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

end

module type REPR = sig
  type t

  type 'a repr

  (* Start a new path. *)
  val start
    : t -> 'a repr -> 'a repr

  val line_to
    : t -> t -> 'a repr -> 'a repr

  val quadratic_to
    : t -> Gg.v2 -> Gg.v2 -> t -> 'a repr -> 'a repr

  val stop
    : 'a repr -> 'a repr
end

module Make(Point:P) = struct

  (** Point creation  **)

  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 t = Point.t list * bezier list

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

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

  let get_new_segment connexion0 p5 p4 p3 p2 p1 =
    let p5' = Point.get_coord p5
    and p4' = Point.get_coord p4
    and p3' = Point.get_coord p3
    and p2' = Point.get_coord p2
    and p1' = Point.get_coord p1 in

    let points_to_link =
      [ p1'
      ; p2'
      ; p3'
      ; p4'
      ; p5' ] in
    Shapes.Bspline.to_bezier ?connexion0 points_to_link

  let empty = ([], [])

  let add_point
    : Point.t -> t -> t * fixedPath option
    = fun lastPoint (path, beziers) ->
      let (let*) v f =
        match v with
        | Ok bezier ->
          if Array.length bezier > 0 then
            f (Array.get bezier 0)
          else
            ( (lastPoint::path, beziers)
            , None )
        | _ ->
          ( (lastPoint::path, beziers)
          , None )
      in

      let connexion0 = match beziers with
        | hd::_ -> Some (Point.get_coord hd.p1)
        | _ -> None in

      match path with
      | p4::p3::p2::p1::_ ->
        let* bezier = get_new_segment connexion0
            lastPoint p4 p3 p2 p1 in

        let bezier_point =
          { p0 = p2
          ; p1 = p1
          ; ctrl0 = bezier.Shapes.Bezier.ctrl1
          ; ctrl1 = bezier.Shapes.Bezier.ctrl0
          } in

        (* We remove the last point and add the bezier curve in the list*)
        let firsts = lastPoint::p4::p3::p2::[] in
        ( (firsts, bezier_point::beziers)
        , None )
      | _ ->
        ( ( lastPoint::path
          , beziers)
        , None )

  let replace_last
    : Point.t -> t -> t * fixedPath option
    = fun lastPoint ((path, beziers) as t) ->
      match path, beziers with
      | _::(tl), beziers ->

        ( ( lastPoint::tl
          , beziers )
        , None )
      | _ ->
        add_point lastPoint t

  let peek2
    : t -> (Point.t * Point.t) option
    = fun (path, _) ->
      match path with
      | h1::h2::_ -> Some (h1, h2)
      | _ -> None

  let peek
    : t -> Point.t option
    = fun (path, _) ->
      match path with
      | [] -> None
      | hd::_ -> Some hd

  (**  Complete path **)

  module Draw(Repr:REPR with type t = Point.t) = struct

    (** Drawing path **)

    let draw
      : t -> 'a Repr.repr -> 'a Repr.repr
      = fun (points, beziers) path ->

        (* Represent the last points *)
        let path = match points with
          | [] ->
            ( path )
          | p1::[] ->
            ( Repr.start p1 path )
          | p1::p2::[] ->
            let path =
              Repr.start p1 path
              |> Repr.line_to p1 p2 in
            ( path )
          | p0::p1::p2::[] ->
            let path = Repr.start p0 path in

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

            let p0' = Point.copy p0 b.Shapes.Bezier.p0
            and p2' = Point.copy p1 b.Shapes.Bezier.p1 in

            ( Repr.quadratic_to
                p0'
                b.Shapes.Bezier.ctrl0
                b.Shapes.Bezier.ctrl1
                p2'
                path )
          | (p0::_ as points) ->

            let (let*) v f =
              match v with
              | Ok beziers -> f beziers
              | _ -> path in

            let points' = List.map ~f:Point.get_coord points in
            let connexion = match beziers with
              | [] -> None
              | hd ::_ -> Some (Point.get_coord hd.p1) in

            let* beziers = Shapes.Bspline.to_bezier ?connexion1:connexion points' in

            (* Stdlib does not provide fold_left_i function and we need to map
               each bezier point with the associated point in the curve.

               So I use references here for keeping each result element

            *)
            let path = ref path in
            let point = ref p0 in

            List.iteri
              points
              ~f:(fun i pt ->

                  (* The first iteration is ignored, as we need both previous and
                     current point for the two point in the curve.

                     Do not forget that there is always n-1 bezier curve for n
                     points *)
                  if i > 0 then (

                    let bezier = Array.get beziers (i - 1) in

                    path := Repr.quadratic_to
                        !point
                        bezier.Shapes.Bezier.ctrl0
                        bezier.Shapes.Bezier.ctrl1
                        pt
                        (!path);
                    point := pt;
                  )
                );
            ( !path )
        in

        (* Now represent the already evaluated points. Much easer to do, just
           iterate on them *)
        Repr.stop @@ List.fold_left beziers
          ~init:path
          ~f:(fun path bezier ->
              let p0' = bezier.p0
              and ctrl0 = bezier.ctrl0
              and ctrl1 = bezier.ctrl1
              and p1' = bezier.p1 in
              Repr.quadratic_to p0' ctrl0 ctrl1 p1' path
            )
  end

  module ToFixed = struct
    type t = Point.t

    type 'a 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 -> 'a repr -> 'a repr
      = fun p1 p2 (i, t) ->
        ( i + 1
        , Line (p1, p2)::t)


    let quadratic_to
      : t -> Gg.v2 -> Gg.v2 -> t -> 'a repr -> 'a 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) ->
        let res = Array.make n Empty in
        List.iteri t
          ~f:(fun i elem -> Array.set res (n - i - 1) elem );
        res
  end

  let id = ref 0
  module FixedBuilder = Draw(ToFixed)
  let to_fixed
    : t -> fixedPath
    = fun t ->
      incr id;
      { id = !id
      ; path = FixedBuilder.draw t (ToFixed.create_path ())
               |> ToFixed.get
      }

  module DrawFixed(Repr:REPR with type t = Point.t) = struct


    let repr_bezier p bezier =
      Repr.quadratic_to
        bezier.p0
        bezier.ctrl0
        bezier.ctrl1
        bezier.p1
        p

    let draw
      : fixedPath -> 'a Repr.repr -> 'a Repr.repr
      = fun {path; _} repr ->

        let _, repr = Array.fold_left path
            ~init:(true, repr)
            ~f:(fun (first, path) element ->
                match element with
                | Empty -> (true, path)
                | 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
  end

  let box
    : bezier -> Gg.box2
    = fun bezier ->
      Gg.Box2.of_pts
        (Point.get_coord bezier.p0)
        (Point.get_coord bezier.p1)
      |> (fun b -> Gg.Box2.add_pt b bezier.ctrl0)
      |> (fun b -> Gg.Box2.add_pt b bezier.ctrl1)

  let distance
    : Gg.v2 -> fixedPath -> float option =
    fun point beziers ->

    Array.fold_left beziers.path
      ~init:None
      ~f:(fun res path ->
          match path with
          | Empty -> None
          | 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 ->
            begin match Gg.Box2.mem point (box bezier) with
              | false -> res
              | true ->

                let bezier' = Shapes.Bezier.(

                    { p0 = Point.get_coord bezier.p0
                    ; p1 = Point.get_coord bezier.p1
                    ; ctrl0 = bezier.ctrl0
                    ; ctrl1 = bezier.ctrl1 }
                  ) in
                let _, point' = Shapes.Bezier.get_closest_point point bezier' in
                let distance = Gg.V2.( norm (point - point') ) in
                match res with
                | None -> Some distance
                | Some d -> if d < distance then res else (Some distance)
            end
        )

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