diffusion.ml

open Printf

module Ground_Pprz = Pprz.Messages(struct let name = "ground" end)
module LL = Latlong
open LL

type plume = { mutable utm_x : float; mutable utm_y : float; mutable value : int; utm_zone : int }

(* NW of Muret ref *)
let muret = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)43.4624; posn_long=(Deg>>Rad)1.2727}
let royan = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)45.7122; posn_long=(Deg>>Rad)(-1.2037)}
let source = fun () -> { utm_x = muret.LL.utm_x -. 300.; utm_y = muret.LL.utm_y -. 300.; value = 255; utm_zone = muret.LL.utm_zone}

let available_ids = ref []
let gen_id =
  let x = ref 0 in
  fun () ->
    match !available_ids with
      [] ->
	incr x; !x
    | x::xs ->
	available_ids := xs;
	x

let dt = 1. (* s *)
let mixing_length = 5. (* m/s *)
let wind_x = ref 0.
let wind_y = ref 0.


let ivy_bus = Defivybus.default_ivy_bus

let plumes = Hashtbl.create 97
let t = ref 0

let one_step = fun () ->
  incr t;

  (* New plume *)
  if !t mod 5 = 0 then
    Hashtbl.add plumes (gen_id ()) (source ());

  (* Eddy + wind *)
  Hashtbl.iter (fun id plume ->
    let a = Random.float (2.*.LL.pi) in
    plume.utm_x <- plume.utm_x +. (mixing_length*.cos a +. !wind_x)*. dt;
    plume.utm_y <- plume.utm_y +. (mixing_length*.sin a +. !wind_y)*. dt;
    plume.value <- plume.value - 1;
    if plume.value <= 0 then begin
      Hashtbl.remove plumes id;
      available_ids := id :: !available_ids;
    end)
    plumes


let my_id = "diffusion"
let send_on_ivy = fun () ->
  if Hashtbl.length plumes > 0 then
  let ids = ref []
  and xs= ref []
  and ys = ref []
  and vs = ref [] in
  Hashtbl.iter (fun id plume ->
    ids := string_of_int id :: !ids;
    let wgs84 = LL.of_utm WGS84 {LL.utm_zone = plume.utm_zone ; utm_x = plume.utm_x; utm_y = plume.utm_y } in
    xs := sprintf "%.6f" ((Rad>>Deg)wgs84.posn_lat) :: !xs;
    ys := sprintf "%.6f" ((Rad>>Deg)wgs84.posn_long) :: !ys;
    vs := sprintf "%d" plume.value :: !vs)
    plumes ;
  let ids = String.concat "," !ids