open Mctypes
open Mc_ext
open Mathext

fun abs(a : float) returns (o : float)
let
  o = if 0.0 <=. a then a else -. a;
tel

(*  -- Returns 1.0 if input is greater than 0.0,
  -- -1.0 if input is less than 0.0
  -- and 0.0 if input is equal to 0.0 *)
fun sign(a : float) returns (o : float)
let
  o = if a >. 0.0 then 1.0 else if 0.0 = a then 0.0 else -. 1.0;
tel (* calculate arctan(y/x)  *)


  node myarctan(y, x : float) returns (atan : float)
  var l6 : float; l4 : bool; l1 : float;
  let
    atan =
      if l4
      then if x <. 0.0 then pi +. l1 else l1
      else pi /. 2.0 *. sign(y);
  (*  l6 = (activate div every l4 initial default 0.0)(y, x);  *)
    l6 = if l4 then y /. x else 0.0 -> pre l6;
    l4 = abs(x) >. 0.1;
    l1 = Mathext.atanr(l6);
  tel

(* compute if a given track is equal to one of the mission tracks
   belonging to the mission track array at the previous tick *)
fun missiontrackequalsprevious(previousone, actualone : tmissiontrack)
    returns (equal : bool)
let
    equal =
      0 <> previousone.m_id & previousone.m_id = actualone.m_id or
      abs(previousone.m_pos.x -. actualone.m_pos.x) <. 100.0 &
      abs(previousone.m_pos.y -. actualone.m_pos.y) <. 100.0 &
      not (abs(previousone.m_pos.x) <. 0.1 &
       abs(previousone.m_pos.y) <. 0.1 &
       abs(actualone.m_pos.x) <. 0.1 &
       abs(actualone.m_pos.y) <. 0.1 )
tel

  (* compute track visibility (appearance on radar screen)
     according to track position and speed  *)
fun calctrackvisible1(position : tposition;
                             speed : tspeed)
    returns (trackvisible : bool)
let
  trackvisible =
      not (abs(position.x) <. 0.001 & abs(position.y) <. 0.001 &
       abs(speed.sx) <. 0.001 &
       abs(speed.sy) <. 0.001);
tel

fun missiontrackexist1(acc_tracknumber : int;
                              missiontrack,
                              previousmissiontrack : tmissiontrack)
  returns (tracknumbertoset : int)
let
    tracknumbertoset =
      if missiontrackequalsprevious(missiontrack, previousmissiontrack) &
      0 <> previousmissiontrack.m_tracknumber
      then previousmissiontrack.m_tracknumber
      else acc_tracknumber;
tel

(* compute if a given track is equal to one of the mission tracks
   belonging to the mission track array at the previous tick *)
fun missiontrackequalsprevious_orig(previousone, actualone : tmissiontrack)
    returns (equal : bool)
var l43 : bool;
let
    l43 = previousone.m_tracknumber <> 0;
    equal =
      l43 &
      (l43 & 0 <> previousone.m_id & previousone.m_id = actualone.m_id or
       abs(previousone.m_pos.x -. actualone.m_pos.x) <. 100.0 &
       abs(previousone.m_pos.y -. actualone.m_pos.y) <. 100.0 &
       not (abs(previousone.m_pos.x) <. 0.1 &
        abs(previousone.m_pos.y) <. 0.1 &
        abs(actualone.m_pos.x) <. 0.1 &
        abs(actualone.m_pos.y) <. 0.1));
tel

fun util_radtodeg(input1 : float) returns (output1 : float)
let
  output1 = input1 /. (2.0 *. pi) *. 360.0;
tel

fun util_degtorad(input1 : float) returns (output1 : float)
let
  output1 = 2.0 *. pi *. input1 /. 360.0;
tel

(* if speedabs is small (speed.x and speed.y are also small), trackangle is set to 0.
   otherwise, trackangle is computed to be in the range [-180, 180]
    degrees thanks to the acosr; sign is given, from the asinr. *)
fun calctrackangle(speed : tspeed; speedabs : tmetresseconde)
    returns (trackangle : float)
var l51 : bool; l48, l47 : float;
let
    trackangle =
      util_radtodeg(if l51 then 0.0 else Mathext.acosr(l47) *. l48) *.
      (l48 *.
       sign(Mathext.asinr(speed.sy /. (if l51 then 1.0 else speedabs))));
    l51 = speedabs <. 0.01;
    l48 = sign(l47);
    l47 = speed.sx /. (if l51 then 1.0 else speedabs);
tel

(* compute track visibility (appearance on radar screen)
   according to track position *)
fun calctrackvisible(position : tposition)
    returns (trackvisible : bool)
let
 trackvisible =
      not (abs(position.x) <. 0.001 & abs(position.y) <. 0.001);
tel

fun missiontrackexist( missiontrack,
                       previousmissiontrack : tmissiontrack;
                       acc_tracknumber : int)
    returns (tracknumbertoset : int)
let
    tracknumbertoset =
      if missiontrackequalsprevious(missiontrack, previousmissiontrack)
      then previousmissiontrack.m_tracknumber
      else acc_tracknumber;
tel

(* calculate: result = rate of closing / distance *)
node calculatevrdivd(vr, d : float) returns (result : float)
var l13 : float; l11 : bool;
let
  result = if l11 then l13 else 0.0;
(*    l13 = (activate div every l11 initial default 0.0)(vr, d);  *)
  l13 = if l11 then vr /. d else 0.0 -> pre l13;
  l11 = d >. 0.1;
tel

(* sort two mission tracks according to:
  1) their (rate of closing / distance) ratio
  2) target type
  3) detection or not by the radar *)
node trackalowerprioritythanb(a, b : tmissiontrack)
  returns (prioritary : bool)
let
  prioritary =
      a.m_targettype = Ttargettype_friend or not a.m_detectedbyradar or
      a.m_detectedbyradar &
      calculatevrdivd(a.m_sr, a.m_d) <. calculatevrdivd(b.m_sr, b.m_d) &
      b.m_detectedbyradar;
tel

(* compute if two tracks speeds are equal *)
fun comparespeeds(speed1, speed2 : tspeed)
    returns (equal : bool)
let
  equal =
      abs(speed1.sx -. speed2.sx) <. 1.0 &
      abs(speed1.sy -. speed2.sy) <. 1.0;
tel

(* compute a "prioritized" track number according to its
   priority and target type *)
fun calculateprioritizedtracknb(missiontrack : tmissiontrack)
    returns (prioritizedtracknb : int)
let
  prioritizedtracknb =
      if missiontrack.m_targettype <> Ttargettype_friend &
      missiontrack.m_priority <> 0
      then missiontrack.m_tracknumber
      else 0;
tel

(* sort two real inputs *)
fun sortreals(a, b : float) returns (newa, newb : float)
var l2 : bool;
let
  l2 = a <. b;
  newb = if l2 then a else b;
  newa = if l2 then b else a;
tel

(* compute if two tracks positions are equal *)
fun comparepositions(pos1, pos2 : tposition)
    returns (equal : bool)
let
    equal =
      abs(pos1.x -. pos2.x) <. 0.1 & abs(pos1.y -. pos2.y) <. 0.1;
tel

(* compute if two tracks are equal (according to their position
   and speed)  *)
fun comparetracks(pos1, pos2 : tposition;
                         v1, v2 : tspeed)
  returns (equal : bool)
let
    equal = comparepositions(pos1, pos2) & comparespeeds(v1, v2);
tel


(* set the track number of a mission track *)
fun setmissiontracknumber(missiontrack : tmissiontrack; number : int)
    returns (newmissiontrack : tmissiontrack)
let
    newmissiontrack = { missiontrack with .m_tracknumber = number };
tel

(* compute if a mission track is null (or empty) according to
   its position and speed *)
fun missiontrackisnull(missiontrack : tmissiontrack)
    returns (isnull : bool)
let
    isnull =
      comparetracks(missiontrack.m_pos, kInitPosition,
        missiontrack.m_speed, kInitSpeed);
tel

(* calculate the new track number for a mission track, according to:
  1) the mission track data
  2) the previous mission tracks array
  3) the current (highest) track number *)
fun calculatemissiontracknumber(
               previousmissiontracks : tmissiontracksarray;
               missiontrack : tmissiontrack;
               currenttracknumber : int)
    returns (newmissiontrack : tmissiontrack;
             newtracknumber : int)
var setnewtracknumber : bool; previoustracknumber : int;
let
    setnewtracknumber =
      previoustracknumber = 0 & not missiontrackisnull(missiontrack);
    newtracknumber =
      if setnewtracknumber then currenttracknumber + 1 else currenttracknumber;
    previoustracknumber =
      fold <<ksizemissiontracksarray>> missiontrackexist
      <(missiontrack)>
      (previousmissiontracks, 0);
    newmissiontrack =
      setmissiontracknumber(missiontrack, if setnewtracknumber
        then newtracknumber
        else previoustracknumber);
tel

(* compute a mission track target type according to its identifier *)
fun calculatetracktargettypefromid(id : int)
    returns (targettype : ttargettype)
let
  targettype =
      if 0 = id
      then Ttargettype_unknown
      else if id <= 500
      then Ttargettype_friend
      else Ttargettype_foe;
tel

(* calculate the derivative of a value x(n) according to its
   ante-previous value x(n-2) *)
node myderivative(inv, period : float) returns (o : float)
var l2 : float;
let
 (* l2 = fby (inv; 2; 0.0);  *)
  l2 = 0.0 fby (0.0 fby inv);
  o =
      if abs(l2) <. 0.1 or abs(inv) <. 0.1
      then 0.0
      else 0.0 -> (inv -. l2) /. (2.0 *. period);
  tel

(* calculate a track speed vector according to the position vector *)
node calculatetrackspeedfrompos(position : tposition)
    returns (speed : tspeed)
let
  speed =
      { sx = myderivative(position.x, t);
        sy = myderivative(position.y, t) };
tel

(* generate the (up to 2) tracks detected by a sensor (radar
   or iff) from the environment (made of 4 tracks)  *)
fun selectdetectedtrack(index : int;
                        tracks : ttracksarray;
                        defaulttrack : ttrack)
    returns (trackselected : ttrack)
let
  trackselected = tracks.[index] default defaulttrack;
tel

(* set the priority of a mission track *)
fun setmissiontrackpriority(missiontrack : tmissiontrack;
                                   priority : int)
    returns (newmissiontrack : tmissiontrack)
let
  newmissiontrack =
   { missiontrack with .m_priority =
               if missiontrack.m_detectedbyradar then priority else 0 }
tel

(* invert two mission tracks if the first one is null (or empty)  *)
fun sortblockmissiontrack(a, b : tmissiontrack)
    returns (newa, newb : tmissiontrack)
var l7 : bool;
let
  l7 = missiontrackisnull(a);
  newb = if l7 then a else b;
  newa = if l7 then b else a;
tel

(* sort two mission tracks according to:
   1) their (rate of closing / distance) ratio
   2) target type
   3) detection or not by the radar *)
node sortblockpriorities(a, b : tmissiontrack)
  returns (newa, newb : tmissiontrack)
var l25 : bool;
let
    l25 = trackalowerprioritythanb(a, b);
    newb = if l25 then a else b;
    newa = if l25 then b else a;
tel

fun position_equal(p1, p2 : tposition) returns (res:bool)
let
  res = (p1.x = p2.x) & (p1.y = p2.y)
tel

fun speed_equal(s1, s2 : tspeed) returns (res:bool)
let
  res = (s1.sx = s2.sx) & (s1.sy = s2.sy)
tel

(* convert an iff track (position + identifier) into a mission
   track (position + speed + distance + rate of closing +
   detected by radar/iff + tracknumber + target type) *)
node convertifftracktomissiontrack(ifftrack : tifftrack)
  returns (missiontrack : tmissiontrack)
let
  missiontrack =
      { m_pos             = ifftrack.i_pos;
        m_speed           = if position_equal(kInitPosition, ifftrack.i_pos)
        then kInitSpeed
        else calculatetrackspeedfrompos(ifftrack.i_pos);
        m_id              = ifftrack.i_id;
        m_priority        = 0;
        m_d               = 0.0;
        m_sabs            = 0.0;
        m_sr              = 0.0;
        m_detectedbyradar = false;
        m_detectedbyiff   = not (position_equal(ifftrack.i_pos, kInitPosition) &
         ifftrack.i_id = 0);
        m_tracknumber     = 0;
        m_targettype      = calculatetracktargettypefromid(ifftrack.i_id);
        m_isvisible       = calctrackvisible(ifftrack.i_pos);
        m_angle           = 0.0 };
tel

(* convert an radar track (position + speed + distance +
   rate of closing) into a mission track (position + speed +
   distance + rate of closing + detected by radar/iff +
   tracknumber + target type)  *)
fun convertrdrtracktomissiontrack(rdrtrack : trdrtrack)
    returns (missiontrack : tmissiontrack)
let
    missiontrack =
      { m_pos             = rdrtrack.r_pos;
        m_speed           = rdrtrack.r_s;
        m_id              = 0;
        m_priority        = 0;
        m_d               = rdrtrack.r_d;
        m_sabs            = rdrtrack.r_sabs;
        m_sr              = rdrtrack.r_sr;
        m_detectedbyradar = not (position_equal(rdrtrack.r_pos, kInitPosition) &
         speed_equal(rdrtrack.r_s, kInitSpeed) &
         rdrtrack.r_d = 0.0 &
         rdrtrack.r_sabs = 0.0 &
         rdrtrack.r_sr = 0.0);
        m_detectedbyiff   = false;
        m_tracknumber     = 0;
        m_targettype      = Ttargettype_unknown;
        m_isvisible       = calctrackvisible(rdrtrack.r_pos);
        m_angle           = calctrackangle(rdrtrack.r_s, rdrtrack.r_sabs) };
tel

(* calculate the magnitude of a vector (2d)  *)
fun vectnorme(a, b : float) returns (c : float)
let
  c = Mathext.sqrtr(a *. a +. b *. b);
tel

(* extract the x and y (position) values from a track (ttrack type) *)
fun extracttrackposxy(track : ttrack)
    returns (x, y : tmetres)
let
  y = track.t_pos.y;
  x = track.t_pos.x;
tel

(* elaborate radar track data (position, speed, distance, rate of closing)
  according to an environment track (position only)  *)
node elaboraterdrtrack(track : ttrack)
    returns (rdrtrack : trdrtrack)
var d, v, vr, vx, vy, x, y : float; l142 : tspeed;
let
    (*activate ifblock1 if d = 0.0
      then vr = 0.0;
      else var xnorm, ynorm : real;
      let
        ynorm = y / d;
        xnorm = x / d;
        vr = - (vx * xnorm + vy * ynorm);
      tel
    returns vr;*)
    switch d = 0.0
      | true do  vr = 0.0
      | false
        var xnorm, ynorm : float;
        do
          ynorm = y /. d;
          xnorm = x /. d;
          vr = -. (vx *. xnorm +. vy *. ynorm);
    end;

    (x, y) = extracttrackposxy(track);
    rdrtrack =
      { r_pos  = { x = x;
                 y = y };
        r_s    = { sx = vx;
                 sy = vy };
        r_d    = d;
        r_sabs = v;
        r_sr   = vr };
    v = vectnorme(vx, vy);
    d = vectnorme(x, y);
    vy = l142.sy;
    vx = l142.sx;
    l142 = calculatetrackspeedfrompos({ x = x; y = y });
tel

(* Detects a rising edge (false to true transition ).
   The output is true during the transition clock cycle.
   The output is initialized to false. *)
node risingEdge(re_Input : bool) returns (re_Output : bool)
let
  re_Output = not (re_Input -> pre re_Input) & re_Input;
tel

(* safe state machine for the computing of radar or iff state
   state ident: state.0  *)
node statecmd(onoffbuttonpressed : bool (*last = false*);
                currentstate : tsensorstate)
    returns (onoffcmd : bool)
let
    automaton
      state Off
        do  onoffcmd = false;
        unless onoffbuttonpressed & currentstate = TState_OFF then On

      state On
        do  onoffcmd = true;
        unless onoffbuttonpressed & currentstate = TState_ON then Off
    end
tel

(* compute the new radar state each time on/off button
   is pressed *)
node mc_rdrstatecmd(rdronoffbutton : bool; currentrdrstate : tsensorstate)
    returns (rdronoffcmd : bool)
let
  rdronoffcmd =
      statecmd(risingEdge(rdronoffbutton), currentrdrstate);
tel

(* compute the new iff state each time on/off button
   is pressed *)
node mc_iffstatecmd(iffonoffbutton : bool; currentiffstate : tsensorstate)
    returns (iffonoffcmd : bool)
let
  iffonoffcmd =
      statecmd(risingEdge(iffonoffbutton), currentiffstate);
tel

(* safe state machine for the computing of radar mode
   state ident: state.6  *)
node rdrmodecmd(currentstate : tsensorstate;
                  modebuttonpressed : bool(* last = false*);
                  currentmode : trdrmode)
    returns (modecmd : bool)
let
    automaton
      state Wide
        do  modecmd = false;
        unless (modebuttonpressed &
          (currentstate = TState_ON &
           currentmode = TRdrMode_WIDE)) then Narrow

      state Narrow
        do  modecmd = true;
        unless (modebuttonpressed &
          (currentstate = TState_ON &
           currentmode = TRdrMode_NARROW)) then Wide
     end
tel

(* compute the new radar mode each time on/off button
   is pressed *)
node mc_rdrmodecmd(currentrdrstate : tsensorstate;
                     rdrmodebutton : bool;
                     currentrdrmode : trdrmode)
    returns (rdrmodecmd : bool)
let
    rdrmodecmd =
      rdrmodecmd(currentrdrstate, risingEdge(rdrmodebutton),
        currentrdrmode);
tel

(* compute the radar mode, according to the corresponding
   input command from the mission computer *)
fun radar_mode(modecmd : bool) returns (mode : trdrmode)
let
  mode = if modecmd then TRdrMode_NARROW else TRdrMode_WIDE;
tel

(* compute the radar state, according to:
   - the corresponding input command from the mission computer
   - the failure state of the radar  *)
node radar_state(onoffcmd, failure : bool)
    returns (initializing : bool; st : tsensorstate)
var x : bool;
let
   initializing = st = TState_OFF & onoffcmd;
(* x = fby (onoffcmd; 5; false) *)
   x = false fby false fby false fby false fby false fby onoffcmd;
   st =
      if failure
      then TState_FAIL
      else if (if onoffcmd then x else false)
      then TState_ON
      else TState_OFF;
tel

(* elaborate and generate the (up to 2) tracks detected
   by the radar (position + speed + distance + rate of
   closing) *)
node radar_tracks(st : tsensorstate;
                    tracks : ttracksarray;
                    rdrdetectedtracks : tdetectedrdrtracksarray)
    returns (rdrtracks : trdrtracksarray)
var
    l22 : ttrack^ksizerdrtracksarray;
    l30 : trdrtrack^ksizerdrtracksarray;
let
    rdrtracks = if st = TState_ON then l30 else kinitrdrtrackarray;
    l30 = map<<ksizerdrtracksarray>> elaboraterdrtrack(l22);
    l22 =
      map<<ksizerdrtracksarray>> selectdetectedtrack(
        rdrdetectedtracks, tracks^ksizerdrtracksarray,
        kinittrack^ksizerdrtracksarray);
tel

(* scade representation for the radar, generating:
   1) the radar state
   2) the radar mode
   3) the (up to 2) tracks detected by the radar  *)
node radar(onoffcmd, modecmd, failure : bool;
             rdrdetectedtracks : tdetectedrdrtracksarray;
             tracks : ttracksarray)
  returns (initializing : bool;
             st : tsensorstate;
             mode : trdrmode;
             rdrtracks : trdrtracksarray)
let
    rdrtracks = radar_tracks(st, tracks, rdrdetectedtracks);
    mode = radar_mode(modecmd);
    (initializing, st) = radar_state(onoffcmd, failure);
tel

(* compute the iff state, according to:
   - the corresponding input command from the mission computer
   - the failure state of the iff *)
node iff_state(onoffcmd, failure : bool)
    returns (initializing : bool; st : tsensorstate)
var x : bool;
let
    initializing = st = TState_OFF & onoffcmd;
(*  x = fby (onoffcmd; 5; false) *)
    x = false fby false fby false fby false fby false fby onoffcmd;
    st =
      if failure
      then TState_FAIL
      else if (if onoffcmd then x else false)
      then TState_ON
      else TState_OFF;
tel

fun ifftrack_of_track(track : ttrack)
    returns (ifftrack : tifftrack)
let
  ifftrack = { i_pos = track.t_pos; i_id = track.t_id };
tel

(* elaborate and generate the (up to 2) tracks detected
   by the iff (position + identifier) *)
fun iff_tracks(st : tsensorstate;
                      tracks : ttracksarray;
                      iffdetectedtracks : tdetectedifftracksarray)
    returns (ifftracks : tifftracksarray)
var l34 : ttrack^ksizeifftracksarray;
    l40 : tifftracksarray;
let
    l34 =
      map<<ksizeifftracksarray>> selectdetectedtrack(
        iffdetectedtracks, tracks^ksizeifftracksarray,
        kinittrack^ksizeifftracksarray);
    l40 = map<<ksizeifftracksarray>> ifftrack_of_track(l34);
    ifftracks = if st = TState_ON then l40 else kinitifftrackarray;
  tel


(* scade representation for the iff, generating:
   1) the iff state
   2) the (up to 2) tracks detected by the iff  *)
node iff(tracks : ttracksarray;
           failure : bool;
           iffdetectedtracks : tdetectedifftracksarray;
           onoffcmd : bool)
    returns (st : tsensorstate;
             ifftracks : tifftracksarray;
             initializing : bool)
let
  ifftracks = iff_tracks(st, tracks, iffdetectedtracks);
  (initializing, st) = iff_state(onoffcmd, failure);
tel

node advrandr(min, max : float) returns (output1 : float)
let
  output1 = (max -. min) *. rand() +. min;
tel

node advrandi(min, max, step : int) returns (output1 : int)
var l8 : int;
let
  l8 = if 0 <> step then step else 1;
  output1 = (int_of_float (float_of_int (max - min) *. rand())
   + min) / (l8 * l8);
tel

(* for one given track, generate:
   1) its new position according to:
   - its previous position, the input speed and slope
   if set/reset button not pressed
   - the input initial position if set/reset button pressed
   2) its identifier according to the input identifier  *)
node createtracks_createonetrack_init_rand()
    returns (sloperadinit, speedinit, xmeterinit, ymeterinit : float;
             idinit : int)
let
    speedinit = advrandr(250.0, 1000.0) *. t;
    ymeterinit = nm *. advrandr(-. 10.0, 10.0);
    xmeterinit = advrandr(-. 10.0, 10.0) *. nm;
    sloperadinit = 2.0 *. pi *. advrandr(0.0, 360.0) /. 360.0;
    idinit = advrandi(0, 1000, 10);
tel

(* for one given track, generate:
   1) its new position according to:
   - its previous position, the input speed and slope
   if set/reset button not pressed
   - the input initial position if set/reset button pressed
   2) its identifier according to the input identifier  *)
node createtracks_createonetrack_rand(res : bool)
    returns (track : ttrack)
var id : int; sloperad, speedt, x0, y0, l9, l18 : float;
let
 (* (sloperad, speedt, x0, y0, id) =
      (activate createtracks_createonetrack_init_rand every reset initial default (
        0., 0., 0., 0., 0))(); *)
  (sloperad, speedt, x0, y0, id) =
    if res then createtracks_createonetrack_init_rand()
             else (0.0, 0.0, 0.0, 0.0, 0) fby (sloperad, speedt, x0, y0, id);
  l18 = y0 -> Mathext.sinr(sloperad) *. speedt +. (y0 -> pre l18);
  l9 = x0 -> (x0 -> pre l9) +. speedt *. Mathext.cosr(sloperad);
  track = { t_pos = { x = l9; y = l18 }; t_id  = id };
tel

(* generate up to 4 tracks (position + identifier) according
   to the graphical track inputs panel. *)
node createtracks_rand(res : bool)
    returns (tracks : ttracksarray)
let
    tracks = map<<ksizetracksarray>> createtracks_createonetrack_rand(res^ksizetracksarray);
tel

node createalltracks(res : bool)
  returns (tracks : ttracksarray)
let
(*  tracks = (restart createtracks_rand every res)(res); *)
    reset
      tracks = createtracks_rand(res);
    every res
tel


(* merge a mission track detected by the radar with a
   mission track detected by the iff if they have the same
   position and speed.
   in that case, newrdrmissiontrack is the merged track, and newiffmissiontrack is reset to "empty".
   otherwise, outputs = inputs  *)
fun fusionrdrifftracks(iffmissiontrack, rdrmissiontrack
                               : tmissiontrack)
    returns (newiffmissiontrack, newrdrmissiontrack
              : tmissiontrack)
var l90 : bool;
let
    newrdrmissiontrack =
      if l90
      then { m_pos             = rdrmissiontrack.m_pos;
             m_speed           = rdrmissiontrack.m_speed;
             m_id              = iffmissiontrack.m_id;
             m_priority        = rdrmissiontrack.m_priority;
             m_d               = rdrmissiontrack.m_d;
             m_sabs            = rdrmissiontrack.m_sabs;
             m_sr              = rdrmissiontrack.m_sr;
             m_detectedbyradar = rdrmissiontrack.m_detectedbyradar;
             m_detectedbyiff   = iffmissiontrack.m_detectedbyiff;
             m_tracknumber     = 0;
             m_targettype      = iffmissiontrack.m_targettype;
             m_isvisible       = rdrmissiontrack.m_isvisible;
             m_angle           = rdrmissiontrack.m_angle }
      else rdrmissiontrack;
    l90 =
      comparetracks(rdrmissiontrack.m_pos, iffmissiontrack.m_pos,
        rdrmissiontrack.m_speed, iffmissiontrack.m_speed);
    newiffmissiontrack =
      if l90
      then kinitmissiontrack
      else iffmissiontrack;
tel

(* merge tracks data received from both radar and iff sensors *)
fun mc_tracks_fusion_onerdrwithifftracks(rdrtrack : tmissiontrack;
       ifftracks : tmissiontrack^ksizeifftracksarray)
    returns (fusionnedrdrtrack : tmissiontrack;
            fusionnedifftracks : tmissiontrack^ksizeifftracksarray)
let
    (fusionnedifftracks, fusionnedrdrtrack) =
      mapfold<<ksizeifftracksarray>> fusionrdrifftracks(ifftracks, rdrtrack);
tel

(* merge tracks data received from both radar and iff sensors *)
node mc_tracks_fusion(rdrtracks : trdrtracksarray;
                        ifftracks : tifftracksarray)
    returns (missiontracks : tmissiontracksarray)
var
    mergedrdrtracks : tmissiontrack^ksizerdrtracksarray;
    mergedifftracks : tmissiontrack^ksizeifftracksarray;
    l140 : tmissiontrack^ksizerdrtracksarray;
    l139 : tmissiontrack^ksizeifftracksarray;
let
    missiontracks = mergedrdrtracks @ mergedifftracks;
    (mergedrdrtracks, mergedifftracks) =
      mapfold<<ksizerdrtracksarray>> mc_tracks_fusion_onerdrwithifftracks(l140, l139);
    l140 = map<<ksizerdrtracksarray>> convertrdrtracktomissiontrack(rdrtracks);
    l139 = map<<ksizeifftracksarray>> convertifftracktomissiontrack(ifftracks);
tel


fun prio_tracknumbernotinarray(missiontracktracknumber,
                               prioritytrack : int; acc : bool)
    returns (notinarray : bool)
let
    notinarray = acc & missiontracktracknumber <> prioritytrack;
tel

(* replace the lowest priority track in priorityarray by missiontrack *)
node prio_selecthighestprioritynotinpriorityarray(
       missiontrack : tmissiontrack;
       prioritiesarray : tpriorityList;
       accprioritymissiontrack : tmissiontrack)
    returns (prioritymissiontrack : tmissiontrack)
var
    missiontracknotinpriorittiesarray,
    missiontrackhashigherprioritythanacc : bool;
let
    missiontrackhashigherprioritythanacc =
      not trackalowerprioritythanb(missiontrack,
        accprioritymissiontrack);
    missiontracknotinpriorittiesarray =
      fold<<4>> prio_tracknumbernotinarray(missiontrack.m_tracknumber^4,
        prioritiesarray, true);
    prioritymissiontrack =
      if missiontracknotinpriorittiesarray & missiontrackhashigherprioritythanacc
      then missiontrack
      else accprioritymissiontrack;
tel

(* for each missiontrack
   if priority higher than all in priorityarray and not in priorityarray
   then, copy in priorityarray at index *)
node prio_selectprioritarymissiontracks(missiontracks : tmissiontracksarray;
                                          prioritiesarray : tpriorityList;
                                          indexpriority : int)
    returns (newprioritiesarray : tpriorityList)
var missiontrackwithhighestpriority : tmissiontrack;
let
    newprioritiesarray =
     [ prioritiesarray with [indexpriority] =
          missiontrackwithhighestpriority.m_tracknumber ];
    missiontrackwithhighestpriority =
      fold<<ksizemissiontracksarray>> prio_selecthighestprioritynotinpriorityarray(
        missiontracks,
        prioritiesarray^ksizemissiontracksarray, kinitmissiontrack);
tel

fun prio_setpriorityinmissiontrack(prioritytracknumber : int;
                                   priorityindex : int;
                                   missiontrack : tmissiontrack)
    returns (missiontrackwithprio : tmissiontrack)
  let
    missiontrackwithprio =
      if prioritytracknumber = missiontrack.m_tracknumber
      then setmissiontrackpriority(missiontrack, priorityindex + 1)
      else missiontrack;
  tel

fun prio_setpriorityinmissiontrackarray(priorityarray : tpriorityList;
                                        missiontrack : tmissiontrack)
    returns (missiontrackwithprio : tmissiontrack)
let
    missiontrackwithprio =
      foldi<<4>> prio_setpriorityinmissiontrack(priorityarray, missiontrack);
tel



(* set the priority in missiontracks:
   1) set the highest prority
   2) set the second priority=highest different from the previous
   3) set the 3rd priority=highest different from the previous
   3) set the 4th priority=highest different from the previous
   => the 4 priority track should be in an array (initialized to "empty")
   operator selectprioritymissiontracks inputs
   - missiontracks
   - prioritytrack set (to perform the "different from the previous")
   *test for each missiontrack: the higest, and not already in prioritytracks.
   *then, set the ith element of prioritytracks with the one found
   for each missiontrack, if prioritary higher than the lowest 4 prioritary
   old: compute each detected track priority, and sort tracks
   according to their priority *)
node mc_tracks_prio(missiontracks : tmissiontracksarray)
    returns (missiontrackswithprio : tmissiontracksarray)
var prioritytracknumbers : tpriorityList;
let
    missiontrackswithprio =
      map<<ksizemissiontracksarray>> prio_setpriorityinmissiontrackarray
        <(prioritytracknumbers)> (missiontracks);
    prioritytracknumbers =
      prio_selectprioritarymissiontracks(missiontracks,
        prio_selectprioritarymissiontracks(missiontracks,
          prio_selectprioritarymissiontracks(missiontracks,
            prio_selectprioritarymissiontracks(missiontracks, 0^4, 0), 1), 2),
        3);
tel

(* associate a track number to each detected track *)
node mc_tracks_tracknumber(withouttracknb : tmissiontracksarray)
    returns (withtracknumber : tmissiontracksarray)
var l81 : int;
let
    (withtracknumber, l81) =
      mapfold<<ksizemissiontracksarray>> calculatemissiontracknumber
          <(kinitmissiontrackarray -> pre withtracknumber)>
           (withouttracknb, 0 -> pre l81);
tel

(* 1) merge tracks data received from both radar and iff sensors
   2) associate a track number to each detected track
  3) compute each detected track priority, and sort tracks
   according to their priority  *)
node mc_tracks(rdrtracks : trdrtracksarray;
                 ifftracks : tifftracksarray)
    returns (missiontracks : tmissiontracksarray)
let
    missiontracks =
      mc_tracks_prio(mc_tracks_tracknumber(mc_tracks_fusion(rdrtracks,
          ifftracks)));
tel

(* scade representation for the mission computer, computing:
   - the new radar state
   - the new radar mode
   - the new iff state
   - the (up to 4) tracks detected by the fighter *)
node mc(currentrdrstate : tsensorstate;
          currentrdrmode : trdrmode;
          rdrtracks : trdrtracksarray;
          rdronoffbutton, rdrmodebutton, iffonoffbutton : bool;
          currentiffstate : tsensorstate;
          ifftracks : tifftracksarray)
    returns (rdronoffcmd, rdrmodecmd : bool;
             missiontracks : tmissiontracksarray;
             iffonoffcmd : bool)
let
    missiontracks = mc_tracks(rdrtracks, ifftracks);
    iffonoffcmd = mc_iffstatecmd(iffonoffbutton, currentiffstate);
    rdrmodecmd = mc_rdrmodecmd(currentrdrstate, rdrmodebutton, currentrdrmode);
    rdronoffcmd = mc_rdrstatecmd(rdronoffbutton, currentrdrstate);
tel

(* This node implements the "implies" logical operator (not(A) or B). *)
fun implies(a, b : bool) returns (c : bool)
let
  c = (not a) or b;
tel

fun dv_detectedbyiff(missiontrack : tmissiontrack; accin : bool)
    returns (accout : bool)
let
   accout = accin & not (missiontrack.m_tracknumber <> 0);
tel

fun dv_sametracknumber(missiontrack1,
                       missiontrack2 : tmissiontrack;
                       accin : bool)
    returns (accout : bool)
let
  accout =
      accin or
      missiontrack1.m_tracknumber = missiontrack2.m_tracknumber &
      missiontrack2.m_tracknumber <> 0;
tel

fun dv_tracknumberexist(missiontrack : tmissiontrack;
                        missiontracks : tmissiontracksarray;
                        accin : bool)
    returns (accout : bool)
var l36 : bool;
let
    l36 =
      fold<<ksizemissiontracksarray>> dv_sametracknumber(
        missiontrack^ksizemissiontracksarray, missiontracks, false);
    accout = accin or l36;
tel

node dv_proof1(currentrdrstate : tsensorstate;
                 rdronoffbutton, rdronoffcmd : bool)
    returns (proof1 : bool)
let
  proof1 =
      implies(risingEdge(rdronoffbutton) &
        currentrdrstate = TState_FAIL, rdronoffcmd =
        (false -> pre rdronoffcmd));
tel

fun dv_proof2(ifftracks : tifftracksarray;
                     missiontracks : tmissiontracksarray)
    returns (proof2 : bool)
var l33 : bool;
let
    l33 =
      fold<<ksizemissiontracksarray>> dv_detectedbyiff(missiontracks, true);
    proof2 = implies(ifftracks = kinitifftrackarray, l33);
tel

(* verifiy that all non null tracknumbers are different *)
fun dv_proof3(missiontracks : tmissiontracksarray)
    returns (proof3 : bool)
var l33 : bool;
let
    l33 =
      fold<<ksizemissiontracksarray>> dv_tracknumberexist(
        missiontracks, missiontracks^ksizemissiontracksarray, false);
    proof3 = not l33;
tel

node dv_observer(currentrdrstate : tsensorstate;
                   currentrdrmode : trdrmode;
                   rdrtracks : trdrtracksarray;
                   rdronoffbutton, rdrmodebutton, iffonoffbutton : bool;
                   currentiffstate : tsensorstate;
                   ifftracks : tifftracksarray)
    returns (proof1, proof2, proof3 : bool)
var l3 : tmissiontracksarray; l1,l4,l5 : bool;
let
    proof3 = dv_proof3(l3);
    proof2 = dv_proof2(ifftracks, l3);
    proof1 = dv_proof1(currentrdrstate, rdronoffbutton, l1);
    (l1, l4, l3, l5) =
      mc(currentrdrstate, currentrdrmode, rdrtracks, rdronoffbutton,
        rdrmodebutton, iffonoffbutton, currentiffstate, ifftracks);
tel

(* Top node of the Mission Computer SCADE model.
   The Fighter (MC + Radar + Iff), its environment
   (CreateTracks) and links to the graphical interface (GUI)
   are constituting this model. *)
node fighterdebug(res, rdronoffclicked, iffonoffclicked : bool)
    returns (missiontracks : tmissiontracksarray)
var
    l4 : trdrtracksarray;
    l3 : trdrmode;
    l6 : tifftracksarray;
    l5 : tsensorstate;
    l12, l11, l10 : bool;
    l172 : tsensorstate;
    l179 : ttracksarray;
    l200, l201:bool; (*TODO*)
let
    l179 = createalltracks(res);
    (l10, l11, missiontracks, l12) =
      mc(l172, l3, l4, rdronoffclicked, false, iffonoffclicked, l5, l6);
    (l5, l6, l200) = iff(l179, false, [1, 2, 3], false -> pre l12);
    (l201, l172, l3, l4) =
      radar(false -> pre l10, false -> pre l11, false, [0, 1, 2, 3, 4],
        l179);
tel

(* top node of the mission computer scade model.
   the fighter (mc + radar + iff), its environment
   (createtracks) and links to the graphical interface (gui)
   are constituting this model. *)
node dv_fighterdebug(res, rdronoffclicked, iffonoffclicked : bool)
    returns (proof3 : bool)
let
  proof3 =
      dv_proof3(fighterdebug(res, rdronoffclicked, iffonoffclicked));
tel

fun dv_debug(missiontracks : tmissiontracksarray)
    returns (proof3 : bool)
let
  proof3 = dv_proof3(missiontracks);
tel