within Requirements;

model StabilityRequirement "SR-WIND-02: The UAS shall maintain lateral and yaw stability under wind disturbance and recover within limits"
  // ---- System-level parameters (traceable to SR-WIND-02) ----
  parameter Real hoverTolerance = 1.5 "Maximum allowed lateral deviation during hover [m]";
  parameter Real yawTolerance = 5 "Maximum allowed yaw error [deg]";
  parameter Real recoveryBand = 0.5 "Position error band for recovery after disturbance [m]";
  parameter Real recoveryTime = 3 "Time to remain within recovery band [s]";
  parameter Real eps = 1e-6 "Numerical regularization term to avoid division by zero when normalizing the trajectory direction.";
  
  // ---- Inputs from the system ----
  Modelica.Blocks.Interfaces.RealInput posX "Measured X position [m]" annotation(
    Placement(transformation(origin = {22, 0}, extent = {{120, 40}, {80, 80}}), iconTransformation(origin = {20, 0}, extent = {{120, 40}, {80, 80}})));
  Modelica.Blocks.Interfaces.RealInput posY "Measured Y position [m]" annotation(
    Placement(transformation(origin = {20, 60}, extent = {{120, -80}, {80, -40}}), iconTransformation(origin = {20, 0}, extent = {{120, -80}, {80, -40}})));
  Modelica.Blocks.Interfaces.RealInput yaw "Measured yaw angle [deg]" annotation(
    Placement(transformation(origin = {20, -70}, extent = {{120, -10}, {80, 30}}), iconTransformation(origin = {20, -10}, extent = {{120, -10}, {80, 30}})));
  Modelica.Blocks.Interfaces.RealInput DronePositionConsign[3] "Commanded reference position [m]" annotation(
    Placement(transformation(origin = {-158, 120}, extent = {{-20, 120}, {20, 80}}, rotation = -90), iconTransformation(origin = {-40, 120},extent = {{-20, 120}, {20, 80}}, rotation = -90)));
  Modelica.Blocks.Interfaces.RealInput yawConsign "Commanded yaw angle [deg]" annotation(
    Placement(transformation(origin = {-40, 80}, extent = {{-60, 120}, {-20, 80}}, rotation = -90), iconTransformation(origin = {-158, 78},extent = {{-60, 120}, {-20, 80}}, rotation = -90)));
  // ---- Outputs for monitoring ----
  Modelica.Blocks.Interfaces.RealOutput lateralError "2D lateral position error magnitude [m]" annotation(
    Placement(transformation(origin = {-10, 20}, extent = {{-90, 50}, {-110, 70}}), iconTransformation(origin = {-10, 16}, extent = {{-90, 50}, {-110, 70}})));
  Modelica.Blocks.Interfaces.RealOutput yawError "Yaw error magnitude [deg]" annotation(
    Placement(transformation(origin = {-10, 20}, extent = {{-90, 20}, {-110, 40}}), iconTransformation(origin = {-10, 16}, extent = {{-90, 20}, {-110, 40}})));
  Modelica.Blocks.Interfaces.BooleanOutput withinHoverLimit "true if lateralError < hoverTolerance" annotation(
    Placement(transformation(origin = {-210, -34}, extent = {{-110, -10}, {-90, 10}}, rotation = 180), iconTransformation(origin = {-110, 16}, extent = {{10, -10}, {-10, 10}})));
  Modelica.Blocks.Interfaces.BooleanOutput withinYawLimit "true if yawError < yawTolerance" annotation(
    Placement(transformation(origin = {-210, -34}, extent = {{-110, -40}, {-90, -20}}, rotation = 180), iconTransformation(origin = {-110, -14}, extent = {{10, -10}, {-10, 10}})));
  Modelica.Blocks.Interfaces.BooleanOutput recovered "true if position remains within recoveryBand for recoveryTime" annotation(
    Placement(transformation(origin = {-210, -34}, extent = {{-110, -70}, {-90, -50}}, rotation = 180), iconTransformation(origin = {-110, -44}, extent = {{10, -10}, {-10, 10}})));
  Modelica.Blocks.Interfaces.BooleanOutput pass "true if all criteria are satisfied" annotation(
    Placement(transformation(origin = {-210, 22},extent = {{110, -100}, {90, -80}}, rotation = -0), iconTransformation(origin = {-110, -74}, extent = {{10, -10}, {-10, 10}})));

  Modelica.Blocks.Interfaces.RealOutput envUpperX;
  Modelica.Blocks.Interfaces.RealOutput envUpperY;
  
  Modelica.Blocks.Interfaces.RealOutput envLowerX;
  Modelica.Blocks.Interfaces.RealOutput envLowerY;
  Modelica.Blocks.Continuous.Derivative dRefX annotation(
    Placement(transformation(origin = {20, 30}, extent = {{-10, -10}, {10, 10}})));
  Modelica.Blocks.Continuous.Derivative dRefY annotation(
    Placement(transformation(origin = {18, -10}, extent = {{-10, -10}, {10, 10}})));
protected
  Real t_since_exit(start = 0) "Elapsed time since last exit from recovery band";
  Boolean insideBand(start = true);
  Real vx;
  Real vy;
  Real norm;
  Real nx;
  Real ny;
  Boolean outsideBand(start=false);
  Boolean timeout;
  
equation

  connect(DronePositionConsign[1], dRefX.u);
  connect(DronePositionConsign[2], dRefY.u);

  vx = dRefX.y;
  vy = dRefY.y;

  norm = sqrt(vx*vx + vy*vy + eps);

  nx = -vy / norm;
  ny =  vx / norm;

  lateralError =
    abs((posX - DronePositionConsign[1]) * nx +
        (posY - DronePositionConsign[2]) * ny);

  yawError = abs(Modelica.Math.wrapAngle(yaw - yawConsign));

  withinHoverLimit = noEvent(lateralError < hoverTolerance);
  withinYawLimit   = noEvent(yawError < yawTolerance);

  insideBand  = noEvent(lateralError <= recoveryBand);
  outsideBand = not insideBand;

  der(t_since_exit) =
    if noEvent(lateralError > recoveryBand)
    then 1
    else -t_since_exit;

  timeout   = noEvent(t_since_exit > recoveryTime);
  recovered = not timeout;

  pass = withinHoverLimit and withinYawLimit and recovered;

  envUpperX = DronePositionConsign[1] + recoveryBand * nx;
  envUpperY = DronePositionConsign[2] + recoveryBand * ny;

  envLowerX = DronePositionConsign[1] - recoveryBand * nx;
  envLowerY = DronePositionConsign[2] - recoveryBand * ny;

  annotation(
    Documentation(info = "
  <html>
  <h4>SR-WIND-02 – Performance in Wind</h4>
  <p>
  Source: EASA Means of Compliance Light-UAS.2512, 
  ISO 21384-3 (Clause 10 – Operational limitations).
  </p>
  <p>
  <b>Requirement:</b><br>
  Under a steady 8 m/s wind with 12 m/s gusts, the UAS shall:
  <ul>
    <li>Maintain lateral position error ≤ 1.5 m (95th percentile) during hover.</li>
    <li>Maintain yaw pointing error ≤ 5° (95th percentile).</li>
    <li>Recover to within 0.5 m acceptance enveloppe position error within 3 s after gust disturbance.</li>
  </ul>
  </p>
  <p>
  <b>Simulation Inputs:</b> Connect to AIDA_System position, yaw, and 
  reference signals.
  </p>
  </html>"),
    Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "SR-WIND-02"), Rectangle(extent = {{-104, -2}, {-104, -2}}), Rectangle(origin = {0, -1}, extent = {{-100, 101}, {100, -99}})}, coordinateSystem(initialScale = 0.1)));
end StabilityRequirement;