open Position
open Global

let routing_success p = 
  if with_graphics && (p.src_id = !main_node) then Draw.draw_route p
 

let routing_fail p = 
  if with_graphics && (p.src_id = !main_node) then Draw.draw_route p


(* ---------------------------------------------------------------------- *)
(* Forwarding function                                                    *)
(* ---------------------------------------------------------------------- *)

let forward self p = 
  let d = distance2 self.pos p.dest_pos in
  let d_min, best_node =
    List.fold_left 
      (fun (d_min, current_best) node ->
        if node.id = p.dest_id then
          (* The destination is in the neighborhood *)
          (-1, node)
        else
          let d = distance2 node.pos p.dest_pos in
          if d < d_min then
            (d, node)
          else 
            (d_min, current_best))
      (d, self)
      self.neighbors
  in  
  if self.id = best_node.id then 
    None
  else 
    Some best_node


(* ---------------------------------------------------------------------- *)
(* Search best position information function                              *)
(* ---------------------------------------------------------------------- *)

let search_best_age = Search_best_age.search_best_age


(* ---------------------------------------------------------------------- *)
(* Anchor function                                                        *)
(* ---------------------------------------------------------------------- *)

let anchor kind self p =
  let pos_tbl = 
    match kind with
    | LE -> self.pos_tbl_le 
    | ELIP -> self.pos_tbl_elip
  in
  let pos_dest, date = Pos_tbl.get pos_tbl p.dest_id in
  (* local search *)
  if abs (self.date - date) < (p.dest_pos_age / 2) then
    (p.anchors <- (self, Some self, 0, 0) :: p.anchors;
     Some (pos_dest, abs (self.date - date)))
  else
    (* search in the neighborhood *)
    match 
      search_best_age kind self p.dest_id (p.dest_pos_age/2) self.neighbors
    with
    | Some (node, (pos, age)), level, nb_nodes, overhead ->
        p.anchors <- (self, Some node, level, nb_nodes) :: p.anchors;
        Pos_tbl.set pos_tbl p.dest_id pos (self.date - age);
        Some (pos, age)
    | None, level, nb_nodes, overhead -> 
        (* routing fail *)
        p.anchors <- (self, None, level, nb_nodes) :: p.anchors;
        None

(* ---------------------------------------------------------------------- *)
(* processing functions                                                   *)
(* ---------------------------------------------------------------------- *)
let pos_tbl_update kind self p =
  let pos_tbl = 
    match kind with
    | LE -> self.pos_tbl_le 
    | ELIP -> self.pos_tbl_elip
  in
  begin match p.header with
  | H_ELIP (Some src_pos) -> Pos_tbl.update pos_tbl p.src_id src_pos self.date
  | _ -> ()
  end;
  Pos_tbl.update pos_tbl p.dest_id p.dest_pos (self.date - p.dest_pos_age)


(* ---------------------------------------------------------------------- *)
(* Routage function                                                       *)
(* ---------------------------------------------------------------------- *)
let rec routing kind self p =
  p.route <- self :: p.route;
  pos_tbl_update kind self p;
  if self.id = p.dest_id then
    (* routing success *)
    routing_success p
  else 
    (* recherche du node voisin le plus proche de la dest *)
    match forward self p with
    | Some next_node ->
        routing kind next_node p
    | None ->
        (* anchor node *)
        if p.dest_pos_age = 0 then
          routing_fail p
        else
          match anchor kind self p with
          | None -> 
              routing_fail p
          | Some (pos, age) ->
              p.dest_pos <- pos;
              p.dest_pos_age <- age;
              routing kind self p


(* ---------------------------------------------------------------------- *)
(* Route function                                                         *)
(* ---------------------------------------------------------------------- *)

let route kind src_node dest_id =
  let pos_tbl = 
    match kind with
    | LE -> src_node.pos_tbl_le
    | ELIP -> src_node.pos_tbl_elip
  in
  let p = 
    match kind with
    | LE -> make_le_packet src_node dest_id 
    | ELIP -> make_elip_packet src_node dest_id 
  in
  let _, pos_dest_date = Pos_tbl.get pos_tbl p.dest_id in
  if pos_dest_date <> Pos_tbl.no_info then
    routing kind src_node p
  else
    (* anchor node *)
    if p.dest_pos_age = 0 then
      routing_fail p
    else
      match anchor kind src_node p with
      | None -> 
          routing_fail p
      | Some (pos, age) ->
          p.dest_pos <- pos;
          p.dest_pos_age <- age;
          routing kind src_node p