(* =====================================================================
   30_prog_control.st - PROGRAM CONTROL, section 4.5

   Contains no located variable reference of any kind.  Every input
   arrives through the process image globals written by IO_MUX, and
   every output leaves the same way.  This is what makes the pass 2
   simulation a mux change rather than a control change.

   Execution order below follows section 4.5 step for step.
   ===================================================================== *)

PROGRAM CONTROL
VAR_EXTERNAL
    (* process image in *)
    g_LevelRaw_mm   : INT;
    g_Level_mm      : INT;
    g_Level_m       : REAL;
    g_Inflow_Lps    : REAL;
    g_Disch_Lps     : REAL;
    g_PumpP_kPa     : ARRAY[1..3] OF REAL;
    g_Vib_mms       : ARRAY[1..3] OF REAL;
    g_LSHH          : BOOL;
    g_LSLL_Wet      : BOOL;
    g_SpillDetected : BOOL;
    g_ThermalOK     : ARRAY[1..3] OF BOOL;
    g_SealLeak      : ARRAY[1..3] OF BOOL;
    g_MainsOK       : BOOL;

    (* commands and setpoints in, unvalidated *)
    g_cmd_Mode      : INT;
    g_cmd_Word      : INT;
    g_cmd_Param     : INT;
    g_sp_Level      : INT;
    g_sp_StartDuty  : INT;
    g_sp_StartP2    : INT;
    g_sp_StartP3    : INT;
    g_sp_StopAll    : INT;
    g_sp_HighAlarm  : INT;
    g_sp_MinSpeed   : INT;
    g_sp_ServiceHrs : INT;

    (* published out *)
    g_o_RunCmd      : ARRAY[1..3] OF BOOL;
    g_o_Running     : ARRAY[1..3] OF BOOL;
    g_o_Available   : ARRAY[1..3] OF BOOL;
    g_o_Tripped     : ARRAY[1..3] OF BOOL;
    g_o_InAuto      : BOOL;
    g_o_HighLevel   : BOOL;
    g_o_SpillActive : BOOL;
    g_o_Level_mm    : INT;
    g_o_Inflow_x10  : INT;
    g_o_Disch_x10   : INT;
    g_o_PumpsRun    : INT;
    g_o_Speed_x10   : INT;
    g_o_TimeToSpill : INT;
    g_o_TimeToLSHH  : INT;
    g_o_NetAccum    : INT;
    g_o_RunHours    : ARRAY[1..3] OF INT;
    g_o_VolToSpill  : INT;
    g_o_StationState: INT;
    g_o_PumpState   : ARRAY[1..3] OF INT;
    g_o_DutyPump    : INT;
    g_o_AlarmWord   : INT;
    g_o_CmdAck      : INT;
END_VAR

VAR CONSTANT
    SPILL_MM        : INT := 6000;      (* spill weir, mm               *)
    LEVEL_MAX_MM    : INT := 7000;      (* LIT-101 range top            *)
    HARD_MIN_HZ     : REAL := 38.0;
    HARD_MAX_HZ     : REAL := 50.0;
END_VAR

VAR
    (* --- function block instances.  Three explicit pump instances
           rather than an ARRAY OF FB_PUMP: arrays of function blocks
           are the kind of construct section 9 warns about. --------- *)
    Pump1       : FB_PUMP;
    Pump2       : FB_PUMP;
    Pump3       : FB_PUMP;
    Duty        : FB_DUTY_SELECT;
    LvlCtl      : FB_LEVEL_CTRL;
    Head        : FB_HEADROOM;

    (* --- validated setpoints, seeded with the section 2.3 defaults
           and held at the last good value on a bad write ---------- *)
    v_Mode          : INT := 1;
    v_SpLevel       : INT := 4200;
    v_StartDuty     : INT := 4000;
    v_StartP2       : INT := 4500;
    v_StartP3       : INT := 5000;
    v_StopAll       : INT := 1000;
    v_HighAlarm     : INT := 5200;
    v_MinSpeed      : INT := 380;
    v_ServiceHrs    : INT := 4000;
    SpRejected      : BOOL;     (* latched, bit 15, cleared by cmd 6   *)
    SpOK            : BOOL;

    (* --- command handshake ------------------------------------------ *)
    CmdBusy         : BOOL;
    ResetTrip       : ARRAY[1..3] OF BOOL;
    ResetHours      : ARRAY[1..3] OF BOOL;
    Lockout         : ARRAY[1..3] OF BOOL;
    AckAlarms       : BOOL;
    p               : INT;

    (* --- staging ---------------------------------------------------- *)
    PumpsRequired   : INT;      (* held across scans - the hysteresis  *)
    PumpsAllowed    : INT;      (* after start stagger                 *)
    StaggerTmr      : TON;
    StaggerArm      : BOOL;

    (* --- interlocks -------------------------------------------------- *)
    DryRun          : BOOL;
    DryLockout      : BOOL;
    LevelRangeFault : BOOL;
    LevelFrozen     : BOOL;
    LevelFault      : BOOL;
    LevelRef        : INT;
    LevelMoved      : BOOL;
    FrozenTmr       : TON;
    AnyRunning      : BOOL;

    (* --- duty selector interface ------------------------------------ *)
    Avail           : ARRAY[1..3] OF BOOL;
    Hours           : ARRAY[1..3] OF REAL;
    SvcDue          : ARRAY[1..3] OF BOOL;
    RunNow          : ARRAY[1..3] OF BOOL;
    Req             : ARRAY[1..3] OF BOOL;

    (* --- misc -------------------------------------------------------- *)
    Speed           : REAL;
    MinSpeedHz      : REAL;
    SpLevel_m       : REAL;
    HighLevel       : BOOL;
    PumpsRun        : INT;
    Alarm           : DINT;
    i               : INT;
    r               : REAL;
    Primed          : BOOL := FALSE;
END_VAR

(* =====================================================================
   Step 1 - read and clamp setpoints, section 2.3

   Every setpoint is validated as a set, not individually: the start
   levels only make sense in order.  A rejected write holds the last
   good value and raises bit 15 rather than acting on it.
   ===================================================================== *)

(* Mode *)
IF (g_cmd_Mode = 1) OR (g_cmd_Mode = 2) THEN
    v_Mode := g_cmd_Mode;
ELSE
    SpRejected := TRUE;
END_IF;

(* Level setpoints.  A start level at or above the spill weir must
   never be accepted, section 2.3. *)
SpOK := TRUE;
IF (g_sp_StopAll < 0) OR (g_sp_StopAll >= g_sp_StartDuty) THEN
    SpOK := FALSE;
END_IF;
IF (g_sp_StartDuty >= g_sp_StartP2) OR (g_sp_StartDuty >= SPILL_MM) THEN
    SpOK := FALSE;
END_IF;
IF (g_sp_StartP2 >= g_sp_StartP3) OR (g_sp_StartP2 >= SPILL_MM) THEN
    SpOK := FALSE;
END_IF;
IF (g_sp_StartP3 >= SPILL_MM) THEN
    SpOK := FALSE;
END_IF;
IF (g_sp_Level <= g_sp_StopAll) OR (g_sp_Level >= SPILL_MM) THEN
    SpOK := FALSE;
END_IF;
IF (g_sp_HighAlarm <= 0) OR (g_sp_HighAlarm > SPILL_MM) THEN
    SpOK := FALSE;
END_IF;

IF SpOK THEN
    v_SpLevel    := g_sp_Level;
    v_StartDuty  := g_sp_StartDuty;
    v_StartP2    := g_sp_StartP2;
    v_StartP3    := g_sp_StartP3;
    v_StopAll    := g_sp_StopAll;
    v_HighAlarm  := g_sp_HighAlarm;
ELSE
    SpRejected := TRUE;
END_IF;

(* Minimum drive speed, Hz x 10, bounded by the hard physical limits *)
IF (g_sp_MinSpeed >= 380) AND (g_sp_MinSpeed <= 500) THEN
    v_MinSpeed := g_sp_MinSpeed;
ELSE
    SpRejected := TRUE;
END_IF;

(* Service interval *)
IF g_sp_ServiceHrs > 0 THEN
    v_ServiceHrs := g_sp_ServiceHrs;
ELSE
    SpRejected := TRUE;
END_IF;

MinSpeedHz := INT_TO_REAL(v_MinSpeed) / 10.0;
IF MinSpeedHz < HARD_MIN_HZ THEN
    MinSpeedHz := HARD_MIN_HZ;
END_IF;
SpLevel_m := INT_TO_REAL(v_SpLevel) / 1000.0;


(* =====================================================================
   Step 2 - command word and acknowledge, section 3.3

   Executes on the rising edge of a non-zero %MW1, echoes the value to
   %QW20, then takes no further action until %MW1 returns to 0.
   ===================================================================== *)

(* one-shot pulses, consumed by the FB_PUMP calls later this scan *)
FOR i := 1 TO 3 DO
    ResetTrip[i]  := FALSE;
    ResetHours[i] := FALSE;
END_FOR;
AckAlarms := FALSE;

IF (g_cmd_Word <> 0) AND NOT CmdBusy THEN
    CmdBusy := TRUE;
    p       := g_cmd_Param;

    CASE g_cmd_Word OF
        1:  (* reset all trips *)
            FOR i := 1 TO 3 DO
                ResetTrip[i] := TRUE;
            END_FOR;
            (* the dry run lockout is manual-reset and only clears once
               the level has actually recovered, section 5 *)
            IF g_Level_mm > v_StopAll THEN
                DryLockout := FALSE;
            END_IF;

        2:  (* reset trip on pump in %MW2 *)
            IF (p >= 1) AND (p <= 3) THEN
                ResetTrip[p] := TRUE;
            END_IF;

        3:  (* lock out pump in %MW2 *)
            IF (p >= 1) AND (p <= 3) THEN
                Lockout[p] := TRUE;
            END_IF;

        4:  (* release lockout on pump in %MW2 *)
            IF (p >= 1) AND (p <= 3) THEN
                Lockout[p] := FALSE;
            END_IF;

        5:  (* reset run hours on pump in %MW2 - service done *)
            IF (p >= 1) AND (p <= 3) THEN
                ResetHours[p] := TRUE;
            END_IF;

        6:  (* acknowledge alarms *)
            AckAlarms  := TRUE;
            SpRejected := FALSE;
    END_CASE;

    g_o_CmdAck := g_cmd_Word;

ELSIF g_cmd_Word = 0 THEN
    CmdBusy    := FALSE;
    g_o_CmdAck := 0;
END_IF;


(* =====================================================================
   Level signal integrity, section 5

   A frozen transmitter reading a plausible value is the failure that
   actually causes spills, and a range check alone cannot see it.
   ===================================================================== *)

LevelRangeFault := (g_LevelRaw_mm < 0) OR (g_LevelRaw_mm > LEVEL_MAX_MM);

IF NOT Primed THEN
    LevelRef := g_LevelRaw_mm;
    Primed   := TRUE;
END_IF;

IF ABS(g_LevelRaw_mm - LevelRef) > 1 THEN
    LevelRef   := g_LevelRaw_mm;
    LevelMoved := TRUE;
ELSE
    LevelMoved := FALSE;
END_IF;

AnyRunning := Pump1.Running OR Pump2.Running OR Pump3.Running;
FrozenTmr(IN := AnyRunning AND NOT LevelMoved, PT := T#10m);
LevelFrozen := FrozenTmr.Q;

LevelFault := LevelRangeFault OR LevelFrozen;


(* =====================================================================
   Step 3 - determine PumpsRequired from level

   The band between StopAll and StartDuty holds the previous value.
   That hysteresis is the whole point; it is never recomputed from
   scratch.
   ===================================================================== *)

IF NOT LevelFault THEN
    IF g_Level_mm >= v_StartP3 THEN
        PumpsRequired := 3;
    ELSIF g_Level_mm >= v_StartP2 THEN
        PumpsRequired := 2;
    ELSIF g_Level_mm >= v_StartDuty THEN
        PumpsRequired := 1;
    ELSIF g_Level_mm <= v_StopAll THEN
        PumpsRequired := 0;
    END_IF;
    (* otherwise: hold *)
ELSE
    (* Fall back to discrete level control, section 5.  LSHH and LSLL
       are independent instruments and remain trustworthy. *)
    IF g_LSHH THEN
        PumpsRequired := 3;
    END_IF;
    (* otherwise: hold, and let the LSLL override below stop the
       station if the well is actually dry *)
END_IF;


(* =====================================================================
   Step 4 - LSHH override.  Start all available, bypass min-off.
   ===================================================================== *)
IF g_LSHH THEN
    PumpsRequired := 3;
END_IF;


(* =====================================================================
   Step 5 - LSLL override.  Fail-safe: the instrument reads TRUE when
   wet, so a broken wire reads dry and stops the station.
   ===================================================================== *)
DryRun := NOT g_LSLL_Wet;
IF DryRun THEN
    PumpsRequired := 0;
    DryLockout    := TRUE;      (* latched, manual reset via command 1 *)
END_IF;
IF DryLockout THEN
    PumpsRequired := 0;
END_IF;


(* =====================================================================
   Step 6 - station mode off
   ===================================================================== *)
IF v_Mode = 2 THEN
    PumpsRequired := 0;
END_IF;


(* =====================================================================
   Step 11 (applied here, before selection) - stagger starts

   Held second and third starts by 30 s each, to limit inrush and the
   hydraulic transient.  Applied before FB_DUTY_SELECT because it
   limits how many units may start, which is an input to selection,
   not a correction applied afterwards.  Stops are never staggered.

   LSHH bypasses the stagger as well as the min-off timers, so that
   the emergency response is immediate.
   ===================================================================== *)
IF g_LSHH THEN
    PumpsAllowed := PumpsRequired;
    StaggerArm   := FALSE;
ELSE
    StaggerTmr(IN := StaggerArm, PT := T#30s);
    IF PumpsAllowed < PumpsRequired THEN
        IF PumpsAllowed = 0 THEN
            PumpsAllowed := 1;              (* first unit starts at once *)
            StaggerArm   := FALSE;
        ELSIF StaggerTmr.Q THEN
            PumpsAllowed := PumpsAllowed + 1;
            StaggerArm   := FALSE;
        ELSE
            StaggerArm := TRUE;
        END_IF;
    ELSE
        IF PumpsAllowed > PumpsRequired THEN
            PumpsAllowed := PumpsRequired;
        END_IF;
        StaggerArm := FALSE;
    END_IF;
END_IF;


(* =====================================================================
   Step 7 - duty selection
   ===================================================================== *)
Avail[1]  := Pump1.Available;   Avail[2]  := Pump2.Available;   Avail[3]  := Pump3.Available;
Hours[1]  := Pump1.RunHours;    Hours[2]  := Pump2.RunHours;    Hours[3]  := Pump3.RunHours;
SvcDue[1] := Pump1.ServiceDue;  SvcDue[2] := Pump2.ServiceDue;  SvcDue[3] := Pump3.ServiceDue;
RunNow[1] := Pump1.Running;     RunNow[2] := Pump2.Running;     RunNow[3] := Pump3.Running;

Duty(Available     := Avail,
     RunHours      := Hours,
     ServiceDue    := SvcDue,
     RunningNow    := RunNow,
     PumpsRequired := PumpsAllowed);

Req[1] := Duty.RunRequest[1];
Req[2] := Duty.RunRequest[2];
Req[3] := Duty.RunRequest[3];


(* =====================================================================
   Step 8 - level control.  On LSHH force 50.0 Hz.
   ===================================================================== *)
LvlCtl(Level    := g_Level_m,
       Setpoint := SpLevel_m,
       Enable   := (PumpsAllowed > 0),
       MinSpeed := MinSpeedHz,
       MaxSpeed := HARD_MAX_HZ);

Speed := LvlCtl.Speed;
IF g_LSHH THEN
    Speed := HARD_MAX_HZ;
END_IF;


(* =====================================================================
   Step 9 - the pumps
   ===================================================================== *)
Pump1(RunRequest      := Req[1],
      SpeedRef        := Speed,
      ThermalOK       := g_ThermalOK[1],
      SealLeak        := g_SealLeak[1],
      Vibration       := g_Vib_mms[1],
      DischPressure   := g_PumpP_kPa[1],
      ResetTrip       := ResetTrip[1],
      Lockout         := Lockout[1],
      MinOffBypass    := g_LSHH,
      ServiceInterval := INT_TO_REAL(v_ServiceHrs),
      ResetHours      := ResetHours[1]);

Pump2(RunRequest      := Req[2],
      SpeedRef        := Speed,
      ThermalOK       := g_ThermalOK[2],
      SealLeak        := g_SealLeak[2],
      Vibration       := g_Vib_mms[2],
      DischPressure   := g_PumpP_kPa[2],
      ResetTrip       := ResetTrip[2],
      Lockout         := Lockout[2],
      MinOffBypass    := g_LSHH,
      ServiceInterval := INT_TO_REAL(v_ServiceHrs),
      ResetHours      := ResetHours[2]);

Pump3(RunRequest      := Req[3],
      SpeedRef        := Speed,
      ThermalOK       := g_ThermalOK[3],
      SealLeak        := g_SealLeak[3],
      Vibration       := g_Vib_mms[3],
      DischPressure   := g_PumpP_kPa[3],
      ResetTrip       := ResetTrip[3],
      Lockout         := Lockout[3],
      MinOffBypass    := g_LSHH,
      ServiceInterval := INT_TO_REAL(v_ServiceHrs),
      ResetHours      := ResetHours[3]);


(* =====================================================================
   Step 10 - headroom
   ===================================================================== *)
Head(Level          := g_Level_m,
     Inflow         := g_Inflow_Lps,
     TotalDischarge := g_Disch_Lps);


(* =====================================================================
   Step 12 - publish
   ===================================================================== *)

PumpsRun := 0;
IF Pump1.Running THEN PumpsRun := PumpsRun + 1; END_IF;
IF Pump2.Running THEN PumpsRun := PumpsRun + 1; END_IF;
IF Pump3.Running THEN PumpsRun := PumpsRun + 1; END_IF;

HighLevel := g_Level_mm >= v_HighAlarm;

g_o_RunCmd[1]    := Pump1.RunCmd;
g_o_RunCmd[2]    := Pump2.RunCmd;
g_o_RunCmd[3]    := Pump3.RunCmd;
g_o_Running[1]   := Pump1.Running;
g_o_Running[2]   := Pump2.Running;
g_o_Running[3]   := Pump3.Running;
g_o_Available[1] := Pump1.Available;
g_o_Available[2] := Pump2.Available;
g_o_Available[3] := Pump3.Available;
g_o_Tripped[1]   := Pump1.Tripped;
g_o_Tripped[2]   := Pump2.Tripped;
g_o_Tripped[3]   := Pump3.Tripped;

g_o_PumpState[1] := Pump1.State;
g_o_PumpState[2] := Pump2.State;
g_o_PumpState[3] := Pump3.State;

g_o_InAuto      := (v_Mode = 1);
g_o_HighLevel   := HighLevel;
g_o_SpillActive := g_SpillDetected;

g_o_Level_mm    := g_Level_mm;
g_o_PumpsRun    := PumpsRun;
g_o_DutyPump    := Duty.DutyPump;

(* scaled analogues, clamped into 16-bit signed range *)
r := g_Inflow_Lps * 10.0;
IF r > 32767.0 THEN r := 32767.0; ELSIF r < -32768.0 THEN r := -32768.0; END_IF;
g_o_Inflow_x10 := REAL_TO_INT(r);

r := g_Disch_Lps * 10.0;
IF r > 32767.0 THEN r := 32767.0; ELSIF r < -32768.0 THEN r := -32768.0; END_IF;
g_o_Disch_x10 := REAL_TO_INT(r);

r := Speed * 10.0;
IF r > 32767.0 THEN r := 32767.0; ELSIF r < 0.0 THEN r := 0.0; END_IF;
g_o_Speed_x10 := REAL_TO_INT(r);

r := Head.NetInflow * 10.0;
IF r > 32767.0 THEN r := 32767.0; ELSIF r < -32768.0 THEN r := -32768.0; END_IF;
g_o_NetAccum := REAL_TO_INT(r);

r := Head.VolToSpill;
IF r > 32767.0 THEN r := 32767.0; ELSIF r < 0.0 THEN r := 0.0; END_IF;
g_o_VolToSpill := REAL_TO_INT(r);

g_o_TimeToSpill := Head.TimeToSpill;
g_o_TimeToLSHH  := Head.TimeToLSHH;

r := Pump1.RunHours;
IF r > 32767.0 THEN r := 32767.0; END_IF;
g_o_RunHours[1] := REAL_TO_INT(r);
r := Pump2.RunHours;
IF r > 32767.0 THEN r := 32767.0; END_IF;
g_o_RunHours[2] := REAL_TO_INT(r);
r := Pump3.RunHours;
IF r > 32767.0 THEN r := 32767.0; END_IF;
g_o_RunHours[3] := REAL_TO_INT(r);

(* --- station state, section 3.1 ---------------------------------- *)
IF v_Mode = 2 THEN
    g_o_StationState := 0;                      (* Off                 *)
ELSIF g_LSHH THEN
    g_o_StationState := 4;                      (* Emergency           *)
ELSIF DryLockout THEN
    g_o_StationState := 5;                      (* Dry run lockout     *)
ELSIF LevelFault THEN
    g_o_StationState := 6;                      (* Fault               *)
ELSIF HighLevel THEN
    g_o_StationState := 3;                      (* High level          *)
ELSIF PumpsRun > 0 THEN
    g_o_StationState := 2;                      (* Pumping             *)
ELSE
    g_o_StationState := 1;                      (* Idle                *)
END_IF;

(* --- alarm bitmask, section 6.

   Accumulated in a DINT because bit 15 does not fit a signed INT.
   Values at or above 32768 are folded into the negative half of the
   16-bit word; CI Server must read %QW17 as UNSIGNED. ------------- *)
Alarm := 0;
IF HighLevel                            THEN Alarm := Alarm + 1;     END_IF;  (* bit0  *)
IF g_LSHH                               THEN Alarm := Alarm + 2;     END_IF;  (* bit1  *)
IF DryRun OR DryLockout                 THEN Alarm := Alarm + 4;     END_IF;  (* bit2  *)
IF g_SpillDetected                      THEN Alarm := Alarm + 8;     END_IF;  (* bit3  *)
IF Pump1.Tripped                        THEN Alarm := Alarm + 16;    END_IF;  (* bit4  *)
IF Pump2.Tripped                        THEN Alarm := Alarm + 32;    END_IF;  (* bit5  *)
IF Pump3.Tripped                        THEN Alarm := Alarm + 64;    END_IF;  (* bit6  *)
IF Pump1.SealAlarm                      THEN Alarm := Alarm + 128;   END_IF;  (* bit7  *)
IF Pump2.SealAlarm                      THEN Alarm := Alarm + 256;   END_IF;  (* bit8  *)
IF Pump3.SealAlarm                      THEN Alarm := Alarm + 512;   END_IF;  (* bit9  *)
IF Pump1.VibAlarm                       THEN Alarm := Alarm + 1024;  END_IF;  (* bit10 *)
IF Pump2.VibAlarm                       THEN Alarm := Alarm + 2048;  END_IF;  (* bit11 *)
IF Pump3.VibAlarm                       THEN Alarm := Alarm + 4096;  END_IF;  (* bit12 *)
IF LevelFault                           THEN Alarm := Alarm + 8192;  END_IF;  (* bit13 *)
IF NOT g_MainsOK                        THEN Alarm := Alarm + 16384; END_IF;  (* bit14 *)
IF SpRejected                           THEN Alarm := Alarm + 32768; END_IF;  (* bit15 *)

IF Alarm >= 32768 THEN
    g_o_AlarmWord := DINT_TO_INT(Alarm - 65536);
ELSE
    g_o_AlarmWord := DINT_TO_INT(Alarm);
END_IF;

END_PROGRAM