Science/trackPatch_yoyo.tl

# -   Copyright (c) 2024 MBARI
# -   MBARI Proprietary Information. Confidential. All Rights Reserved
# -   Unauthorized copying or distribution of this file via any medium is strictly
# -   prohibited.
# -
# -   WARNING - This file contains information whose export is restricted by the
# -   Export Administration Act of 1979 (Title 50, U.S.C., App. 2401 et seq.), as
# -   amended. Violations of these export laws are subject to severe civil and/or
# -   criminal penalties.
mission trackPatch_yoyo {
  """
  Vehicle yo-yos with patch detect enabled for oil concentration.

  NOTE:
  oil concentrations may vary strongly with depth!

  Suggested settings for simulation:

  ```
  Config/sim/[username]/Simulator.cfg:

  oceanModelData = "Resources/BurgerOilfieldSpill.nc:mass_concentration_of_petroleum_hydrocarbons_in_sea_water:kg/m3";
  eastCurrent = 0.05 m/s;   // vel y wrto LV
  northCurrent = 0.005 m/s; // vel x wrto LV

  Config/sim/[username]/workSite.cfg:

  initLat = 71.2225805280196 arcdeg;   // Initial latitude
  initLon = -163.5093252100575 arcdeg; // Initial longitude

  Config/sim/[username]/sim.cfg:

  WetLabsBB2FL.loadAtStartup = 0 bool;
  WetLabsSeaOWL_UV_A.loadAtStartup = 1 bool;
  WetLabsSeaOWL_UV_A.simulateSensors = 1 bool;
  WetLabsSeaOWL_UV_A.serial = "SEAOWLA2K-019";
  WetLabsSeaOWL_UV_A.scaleFactor700 = 3.204e-7 1/m/sr/count;
  WetLabsSeaOWL_UV_A.darkCounts700 = 48 count;
  WetLabsSeaOWL_UV_A.scaleFactorFDOM = 8.096e-3 ppb/count;
  WetLabsSeaOWL_UV_A.darkCountsFDOM = 50 count;
  WetLabsSeaOWL_UV_A.fdomAccuracy = 8.19 ppb;
  WetLabsSeaOWL_UV_A.scaleFactorChl = 2.170e-3 ug/l/count;
  WetLabsSeaOWL_UV_A.darkCountsChl = 50 count;
  WetLabsSeaOWL_UV_A.chlAccuracy = 1.475 ug/l;
  WetLabsSeaOWL_UV_A.scaleFactorOil = 2.8 ppb/count;
  WetLabsSeaOWL_UV_A.darkCountsOil = 50 count;
  WetLabsSeaOWL_UV_A.oilAccuracy = 3.6 ppb;
  NavChartDb.charts = "US1AK90M,US2AK92M,US5AK9NM,US5AK9OM,US5AK9PM,US5AK9QM,US5AK9RM,US5AK9SM,US5AK9TM,US5AK9UM";
  ```
  """

  arguments {
    # Vehicle safety parameters

    MissionTimeout = 12 hour
      """
      Timeout for the entire mission
      """

    AcousticTrackingTimeout = 240 hour
      """
      If the vehicle does not receive an acoustic signal for more than this
      length of time, it will surface for communications. Set longer than
      MissionTimeout to effectively disable.
      """

    Speed = 1 meter_per_second
      """
      Vehicle speed.
      """

    PeakDetectChl = true
      """
      Chlorophyll patch tracking.
      """

    # debug
    #     </Description><False/></DefineArg>

    PeakDetectSalinity = false
      """
      Salinity patch tracking.
      """

    # debug
    #     </Description><True/></DefineArg>

    PeakDetectOil = false
      """
      Oil patch tracking.
      """

    # debug
    #     </Description><True/></DefineArg>

    DetectTheTrough = false
      """
      Whether to track a trough. If false, track peak; if true, track trough.
      Default is false.
      """

    # debug
    #     </Description><True/></DefineArg>

    MinAltitude = 5 meter
      """
      Minimum altitude for the entire mission.
      """

    MaxDepth = 50 meter
      """
      Maximum depth for the entire mission.
      """

    MinWaterDepth = 10 meter
      """
      Minimum water depth for the entire mission.
      """

    MinOffshore = 2000 meter
      """
      Minimum offshore distance for the entire mission.
      """

    MarginMinOffshore = 100 meter
      """
      If closer than MinOffshore+MarginMinOffshore, turn back from shore.
      """

    MinDistFromPolygonSides = 200 meter
      """
      If a operational safety polygon is specified and the vehicle is closer
      than this minimum distance to the polygon sides, turn back.
      """

    NeedCommsTimeInTransit = 60 minute
      """
      Maximum time between surfacings for communications during transit
      """

    NeedCommsTimePatchTracking = 300 minute
      """
      Maximum time between surfacings for communications during patch
      tracking. This is checked only on every NumRoundsBtnCrossLegSurfacings
      rounds of CrossLeg.
      """

    NeedCommsTimeMarginPatchTracking = 60 minute
      """
      In case vehicle does not surface on periodic rounds of InLeg or
      CrossLeg, vehicle will be forced to surface at
      NeedCommsTimePatchTracking+NeedCommsTimeMarginPatchTracking.
      """

    WaitAfterBouncing = 5 minute
      """
      After the vehicle bounces off the boundary (defined by minimum distance
      to shore or polygon sides), wait this long before re-checking the
      distance.
      """

    LatPatchTrackingStart = NaN degree
      """
      Latitude of patch tracking start location. If nan, transit is skipped.
      """

    # debug
    #     </Description><Units:degree/><Value>36.810</Value></DefineArg>

    LonPatchTrackingStart = NaN degree
      """
      Longitude of patch tracking start location. If nan, transit is skipped.
      """

    # debug
    #     </Description><Units:degree/><Value>-121.855</Value></DefineArg>

    Repeat = 5 count
      """
      Maximum number of rounds (one round = one full set of sweeps in four
      directions)
      """

    # debug
    #     </Description><Units:count/><Value>200</Value></DefineArg>

    NumRoundsBtnCrossLegSurfacings = 5 count
      """
      Every so many rounds, vehicle surfaces on CrossLeg. Set to a big number
      to practially disable surfacing on CrossLeg.
      """

    WpMaxDistance = 1000 meter
      """
      Maximum length of each tracking leg
      """

    WpTimeout = 4000 second
      """
      Maximum duration of each tracking leg
      """

    TransitYoYoMinDepth = 2 meter
      """
      Minimum depth while performing the YoYo behavior during transit.
      """

    TransitYoYoMaxDepth = 30 meter
      """
      Maximum depth while performing the YoYo behavior during transit.
      """

    PatchTrackingYoYoMinDepth = 2 meter
      """
      Minimum depth while performing the YoYo behavior during patch tracking.
      """

    PatchTrackingYoYoMaxDepth = 25 meter
      """
      Maximum depth while performing the YoYo behavior during patch tracking.
      """

    YoYoMinAltitude = 9 meter
      """
      Minimum altitude while performing the YoYo behavior (for
      bottom-terminated YoYos).
      """

    YoYoUpPitch = 20 degree
      """
      Vehicle up pitch while performing the YoYo behavior.
      """

    YoYoDownPitch = -20 degree
      """
      Vehicle down pitch while performing the YoYo behavior.
      """

    PeakShallowBnd = 5 meter
      """
      Shallow depth bound for detecting peak on each descent or ascent
      profile.
      """

    PeakDeepBnd = 20 meter
      """
      Deep depth bound for detecting peak on each descent or ascent profile.
      """

    BeginThresholdChl = 1.0 microgram_per_liter
      """
      If non-NaN, sets chlorophyll threshold for starting horizontal patch
      detection
      """

    BeginThresholdSalt = 33.0 psu
      """
      If non-NaN, sets salinity threshold for starting horizontal patch
      detection
      """

    BeginThresholdOil = 0.2 kilogram_per_cubic_meter
      """
      If non-NaN, sets threshold for starting horizontal patch detection
      """

    WpBearing = 90 degree
      """
      Bearing being followed in the current leg. Set this to initialize start
      direction of patch tracking.
      """

    # debug
    #     </Description><Units:degree/><Value>270</Value></DefineArg>

    OffPeakFraction = 90 percent
      """
      When filtered horizontal value is this fraction of the peak, consider it
      outside the patch.
      """

    # debug
    #     </Description><Units:percent/><Value>75</Value></DefineArg>

    DetectionTimeout = 30 minute
      """
      Look for signal for this long. If none found within this timeout, switch
      to the next leg.
      """

    TimeWindowPeakRpt = NaN minute
      """
      If greater than zero, report a peak every such time window has elapsed.
      If NaN or zero, this variable is ignored.
      """

    LowPassWindowLen = 20 count
      """
      Low-pass window length (based on depth sensor sampling interval 0.4
      second) for low-pass filtering.
      """

    MedianFilterLength = 5 count
      """
      Median filter length (only for chlorophyll fluorescence which tends to
      have spikes). The median-filtered signal enters the succeeding low-pass
      filter. If set to 1, then no median filtering, i.e., the raw
      fluorescence signal directly enters the low-pass filter.
      """

    NumOfProfilesSlidingwindow = 50 count
      """
      Length of horizontal sliding window. The highest per-yoyo peak (after
      low-pass filtering by a filter of length FilterWidthHorizontal) is
      picked out within this window.
      """

    FilterWidthHorizontal = 4 count
      """
      Width of boxcar filter used in horizontal patch detection
      """

    DepChangeThreshAttitudeFlip = 2.0 meter
      """
      Depth change threshold for determining vehicle attitude flip.
      """

    WaterFrame = true
      """
      Set WaterFrame true to drive towards waypoints in the water frame of
      reference
      """

    PatchMode = false
      """
      If true, use center of "in peak" range as the turn-around point. If
      false use maximum horizontal peak location as the turn-around point.
      """

    Turn = 90 degree
      """
      Amount to turn at each leg
      """

    MassDefault = Control:VerticalControl.massDefault
      """
      Static setting for mass during the mission. Defaults to massDefault
      setting in the Config/Control.cfg file.
      """

    BuoyancyNeutral = Control:VerticalControl.buoyancyNeutral
      """
      Buoyancy bladder position while performing the YoYo behavior. Defaults
      to buoyancyNeutral setting in the Config/Control.cfg file.
      """
  }

  output {
    # Internal variable settings

    Reverse = 180 degree
      """
      Change in bearing which reverses vehicle direction
      """

    TwoPi = 360 degree
      """
      Applied to stop run-up of angles
      """

    CntRounds = 1 count
      """
      Count of sweep rounds (initilized to 1).
      """

    PatchDetectOn = false
      """
      Mission sets this to true when the patch has been crossed. Initialized
      to False.
      """

    WpDone = true
      """
      Mission sets this to true when the cross-patch waypoint has been
      reached. Initialized to True.
      """

    CrossLegToPatchCenterCompleted = false
      """
      Initialized to False.
      """

    CrossLegSearchForPeakStarted = false
      """
      Initialized to False.
      """

    AllDone = false
      """
      Initialized to False.
      """

    FlagTurnBack = false
      """
      Flag of turning back from shore. Initialized to False.
      """

    PatchPeakChl = NaN microgram_per_liter

    PatchPeakSalinity = NaN psu

    PatchPeakOil = NaN kilogram_per_cubic_meter

    PatchPeakDepth = NaN meter

    PatchPeakLatitude = NaN degree

    PatchPeakLongitude = NaN degree

    PatchPeakDistance = 0 meter

    StoppedForNaNPatchPeak = false
      """
      Mission stopped because patch peak value is NaN. Initialized to False.
      """

    StoppedForBeingOutsideOfPolygon = false
      """
      Mission stopped because the vehicle is outside of the polygon.
      Initialized to False.
      """

    ElapsedSinceStartOrLastDistCheck = 0 hour
      """
      The mission sets this variable to reset the timer for checking distance
      to shore and polygon sides.
      """
  }

  # The timeout

  timeout duration=MissionTimeout

  # And now the mission

  # Uncomment to speed up simulation
  #    <Guidance:SetSpeed>
  #        <Parallel/>
  #        <Setting><Guidance:SetSpeed.period/><Units:millisecond/><Value>800</Value></Setting>
  #    </Guidance:SetSpeed>

  behavior Guidance:AltitudeEnvelope {
    run in parallel

    set minAltitude = MinAltitude
  }

  behavior Guidance:DepthEnvelope {
    run in parallel

    set maxDepth = MaxDepth
  }

  # GPS Update.  Don't go too long w/o a GPS fix.

  insert id="NeedComms" Insert/NeedComms.tl

  behavior Guidance:OffshoreEnvelope {
    run in parallel

    set minOffshore = MinOffshore
  }

  behavior Guidance:WaterDepthEnvelope {
    run in parallel

    set minWaterDepth = MinWaterDepth
  }

  behavior Guidance:Pitch {
    run in parallel

    set massPosition = MassDefault
  }

  behavior Guidance:Buoyancy {
    run in parallel

    set position = BuoyancyNeutral
  }

  insert Insert/AbortDrift.tl {
    redefineArg AcousticTimeout = AcousticTrackingTimeout
  }

  # If we're executing here or below, lets' get science data!

  insert Insert/Science.tl {
    redefineArg PeakDetectChlActive = PeakDetectChl
    redefineArg PeakDetectSalinityActive = PeakDetectSalinity
    redefineArg PeakDetectOilActive = PeakDetectOil
    redefineArg TimeWindowPeakReport = TimeWindowPeakRpt
    redefineArg LowPassWindowLength = LowPassWindowLen
    redefineArg MedianFilterLen = MedianFilterLength
    redefineArg PeakShallowBound = PeakShallowBnd
    redefineArg PeakDeepBound = PeakDeepBnd
    redefineArg DepChangeThreshForAttitudeFlip = DepChangeThreshAttitudeFlip
  }

  behavior Guidance:SetSpeed {
    run in parallel

    set speed = Speed
  }

  aggregate SyslogWpMaxDistanceReached {
    run when ( called )

    aggregate SyslogWpMaxDistanceReachedPatchPeakIsNaN {
      run in sequence

      break if (
        PeakDetectChl
        and ( PatchPeakChl >= 0.0 microgram_per_liter )
        or (
          PeakDetectSalinity
          and ( PatchPeakSalinity >= 0.0 psu )
        )
        or (
          PeakDetectOil
          and ( PatchPeakOil >= 0.0 kilogram_per_cubic_meter )
        )
        or StoppedForNaNPatchPeak
      )

      syslog important "WpMaxDistance reached. Patch peak value is NaN. Stopping mission."

      behavior Guidance:Execute {
        run in sequence

        set command = "stop"
      }

      assign in sequence StoppedForNaNPatchPeak = true
    }

    aggregate SyslogWpMaxDistanceReachedChl {
      run in sequence

      break if (
        not (
          PeakDetectChl
          and ( PatchPeakChl >= 0.0 microgram_per_liter )
        )
      )

      syslog important "WpMaxDistance reached. Patch value is " + PatchPeakChl~microgram_per_liter
           + "at " + PatchPeakLatitude~degree + "," + PatchPeakLongitude~degree + ", distance = "
           + PatchPeakDistance~meter
    }

    aggregate SyslogWpMaxDistanceReachedSalinity {
      run in sequence

      break if (
        not (
          PeakDetectSalinity
          and ( PatchPeakSalinity >= 0.0 psu )
        )
      )

      syslog important "WpMaxDistance reached. Patch value is " + PatchPeakSalinity~psu + "at "
           + PatchPeakLatitude~degree + "," + PatchPeakLongitude~degree + ", distance = "
           + PatchPeakDistance~meter
    }

    aggregate SyslogWpMaxDistanceReachedOil {
      run in sequence

      break if (
        not (
          PeakDetectOil
          and ( PatchPeakOil >= 0.0 kilogram_per_cubic_meter )
        )
      )

      syslog important "WpMaxDistance reached. Patch value is "
           + PatchPeakOil~kilogram_per_cubic_meter + "at " + PatchPeakLatitude~degree + ","
           + PatchPeakLongitude~degree + ", distance = " + PatchPeakDistance~meter
    }
  }

  aggregate SyslogTurnBack {
    run when ( called )

    aggregate SyslogTurnBackFromShoreChl {
      run in sequence

      break if (
        not (
          PeakDetectChl
          and ( Universal:distance_from_shore <= ( MinOffshore + MarginMinOffshore ) )
        )
      )

      syslog important "Latitude = " + Universal:latitude~degree + ", longitude = "
           + Universal:longitude~degree + ". Turn back from shore because distance_from_shore ("
           + Universal:distance_from_shore~meter + ") is less than OffshoreDistanceTurnBack ("
           + MinOffshore~meter + "+" + MarginMinOffshore~meter + "). Patch value is "
           + PatchPeakChl~microgram_per_liter + "at " + PatchPeakLatitude~degree + ","
           + PatchPeakLongitude~degree + ", distance = " + PatchPeakDistance~meter
    }

    aggregate SyslogTurnBackFromWallChl {
      run in sequence

      break if (
        not (
          PeakDetectChl
          and Derivation:ShortestDistanceToPolygonSidesCalculator.loadAtStartup
          and ( Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides <= MinDistFromPolygonSides )
        )
      )

      syslog important "Latitude = " + Universal:latitude~degree + ", longitude = "
           + Universal:longitude~degree
           + ". Turn back from wall because shortest_distance_from_polygon_sides ("
           + Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides~meter
           + ") to side "
           + Derivation:ShortestDistanceToPolygonSidesCalculator.side_shortest_distance~count
           + " is less than MinDistFromPolygonSides (" + MinDistFromPolygonSides~meter
           + "). Patch value is " + PatchPeakChl~microgram_per_liter + "at " + PatchPeakLatitude~degree
           + "," + PatchPeakLongitude~degree + ", distance = " + PatchPeakDistance~meter
    }

    aggregate SyslogTurnBackFromShoreSalinity {
      run in sequence

      break if (
        not (
          PeakDetectSalinity
          and ( Universal:distance_from_shore <= ( MinOffshore + MarginMinOffshore ) )
        )
      )

      syslog important "Latitude = " + Universal:latitude~degree + ", longitude = "
           + Universal:longitude~degree + ". Turn back from shore because distance_from_shore ("
           + Universal:distance_from_shore~meter + ") is less than OffshoreDistanceTurnBack ("
           + MinOffshore~meter + "+" + MarginMinOffshore~meter + "). Patch value is "
           + PatchPeakSalinity~psu + "at " + PatchPeakLatitude~degree + "," + PatchPeakLongitude~degree
           + ", distance = " + PatchPeakDistance~meter
    }

    aggregate SyslogTurnBackFromWallSalinity {
      run in sequence

      break if (
        not (
          PeakDetectSalinity
          and Derivation:ShortestDistanceToPolygonSidesCalculator.loadAtStartup
          and ( Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides <= MinDistFromPolygonSides )
        )
      )

      syslog important "Latitude = " + Universal:latitude~degree + ", longitude = "
           + Universal:longitude~degree
           + ". Turn back from wall because shortest_distance_from_polygon_sides ("
           + Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides~meter
           + ") to side "
           + Derivation:ShortestDistanceToPolygonSidesCalculator.side_shortest_distance~count
           + " is less than MinDistFromPolygonSides (" + MinDistFromPolygonSides~meter
           + "). Patch value is " + PatchPeakSalinity~psu + "at " + PatchPeakLatitude~degree + ","
           + PatchPeakLongitude~degree + ", distance = " + PatchPeakDistance~meter
    }

    aggregate SyslogTurnBackFromShoreOil {
      run in sequence

      break if (
        not (
          PeakDetectOil
          and ( Universal:distance_from_shore <= ( MinOffshore + MarginMinOffshore ) )
        )
      )

      syslog important "Latitude = " + Universal:latitude~degree + ", longitude = "
           + Universal:longitude~degree + ". Turn back from shore because distance_from_shore ("
           + Universal:distance_from_shore~meter + ") is less than OffshoreDistanceTurnBack ("
           + MinOffshore~meter + "+" + MarginMinOffshore~meter + "). Patch value is "
           + PatchPeakOil~kilogram_per_cubic_meter + "at " + PatchPeakLatitude~degree + ","
           + PatchPeakLongitude~degree + ", distance = " + PatchPeakDistance~meter
    }

    aggregate SyslogTurnBackFromWallOil {
      run in sequence

      break if (
        not (
          PeakDetectOil
          and Derivation:ShortestDistanceToPolygonSidesCalculator.loadAtStartup
          and ( Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides <= MinDistFromPolygonSides )
        )
      )

      syslog important "Latitude = " + Universal:latitude~degree + ", longitude = "
           + Universal:longitude~degree
           + ". Turn back from wall because shortest_distance_from_polygon_sides ("
           + Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides~meter
           + ") to side "
           + Derivation:ShortestDistanceToPolygonSidesCalculator.side_shortest_distance~count
           + " is less than MinDistFromPolygonSides (" + MinDistFromPolygonSides~meter
           + "). Patch value is " + PatchPeakOil~kilogram_per_cubic_meter + "at "
           + PatchPeakLatitude~degree + "," + PatchPeakLongitude~degree + ", distance = "
           + PatchPeakDistance~meter
    }
  }

  aggregate TransitToPatchTrackingStartLocation {
    run in sequence

    break if (
      isNaN ( LatPatchTrackingStart )
      or ( isNaN ( LonPatchTrackingStart ) )
    )

    assign in sequence NeedComms:DiveInterval = NeedCommsTimeInTransit

    behavior Guidance:DepthEnvelope {
      run in parallel

      set minDepth = TransitYoYoMinDepth
      set maxDepth = TransitYoYoMaxDepth
      set downPitch = YoYoDownPitch
      set upPitch = YoYoUpPitch
    }

    behavior Guidance:AltitudeEnvelope {
      """
      Another altitude envelope for the YoYo behavior. This envelope
      should fall within the limits of the standard safety envelopes
      in Insert/StandardEnvelopes.tl in order to avoid commanding high
      pitch angles for bottom-terminated YoYos.
      """

      run in parallel

      set minAltitude = YoYoMinAltitude
    }

    behavior Guidance:YoYo {
      run in parallel

      set downPitch = YoYoDownPitch
      set upPitch = YoYoUpPitch
    }

    behavior Guidance:Waypoint {
      run in sequence

      set latitude = LatPatchTrackingStart
      set longitude = LonPatchTrackingStart
    }
  }

  aggregate MissionStart {
    run in sequence

    call id="MissionStartComms" refId="NeedComms"

    assign in sequence ElapsedSinceStartOrLastDistCheck = 0 hour
  }

  aggregate PatchTracking {
    run in sequence repeat=Repeat

    break if ( AllDone )

    assign in sequence NeedComms:DiveInterval = NeedCommsTimePatchTracking + NeedCommsTimeMarginPatchTracking

    assign in sequence CrossLegToPatchCenterCompleted = false

    assign in sequence CrossLegSearchForPeakStarted = false

    behavior Guidance:DepthEnvelope {
      run in parallel

      set minDepth = PatchTrackingYoYoMinDepth
      set maxDepth = PatchTrackingYoYoMaxDepth
      set downPitch = YoYoDownPitch
      set upPitch = YoYoUpPitch
    }

    behavior Guidance:AltitudeEnvelope {
      """
      Another altitude envelope for the YoYo behavior. This envelope
      should fall within the limits of the standard safety envelopes
      in Insert/StandardEnvelopes.tl in order to avoid commanding high
      pitch angles for bottom-terminated YoYos.
      """

      run in parallel

      set minAltitude = YoYoMinAltitude
    }

    behavior Guidance:YoYo {
      run in parallel

      set downPitch = YoYoDownPitch
      set upPitch = YoYoUpPitch
    }

    aggregate OutsideOfPolygon {
      run in parallel

      break if (
        not ( Derivation:ShortestDistanceToPolygonSidesCalculator.loadAtStartup )
        or (
          Derivation:ShortestDistanceToPolygonSidesCalculator.loadAtStartup
          and Derivation:ShortestDistanceToPolygonSidesCalculator.inside_polygon
        )
        or StoppedForBeingOutsideOfPolygon
      )

      syslog important "Latitude = " + Universal:latitude~degree + ", longitude = "
           + Universal:longitude~degree + ". The vehicle is outside of the polygon. Stopping mission."

      behavior Guidance:Execute {
        run in sequence

        set command = "stop"
      }

      assign in sequence StoppedForBeingOutsideOfPolygon = true
    }

    aggregate ChlPatchDetection {
      run when (
        PatchDetectOn
        and PeakDetectChl
      )

      behavior Trigger:PeakDetectHorizontal id="PatchTrackTrigger" {
        run in sequence

        break if ( WpDone )

        set patchTracking = true
        set detectFrom = Science:PeakChl
        set detectTrough = DetectTheTrough
        set depthFrom = Science:PeakChlDepth
        set latitudeFrom = Science:PeakChlLatitude
        set longitudeFrom = Science:PeakChlLongitude
        set beginThreshold = BeginThresholdChl
        set offPeakFraction = OffPeakFraction
        set filterWidth = FilterWidthHorizontal
        set numProfilesSlidingwindow = NumOfProfilesSlidingwindow
        set detectTimeout = DetectionTimeout
        set centerPeak = PatchMode
        outputArg PatchPeakChl = peakDetect
        outputArg PatchPeakDepth = peakDepth
        outputArg PatchPeakLatitude = peakLatitude
        outputArg PatchPeakLongitude = peakLongitude
        outputArg PatchPeakDistance = peakDistance
      }

      syslog important "Peak found before WpMaxDistance is reached. Patch value is "
           + PatchPeakChl~microgram_per_liter + "at " + PatchPeakLatitude~degree + ","
           + PatchPeakLongitude~degree + ", distance = " + PatchPeakDistance~meter

      sendData service=express (
        PatchPeakChl,
        PatchPeakDepth,
        PatchPeakLatitude,
        PatchPeakLongitude
      )

      assign in sequence PatchDetectOn = false

      assign in sequence WpDone = true
    }

    aggregate SalinityPatchDetection {
      run when (
        PatchDetectOn
        and PeakDetectSalinity
      )

      behavior Trigger:PeakDetectHorizontal id="PatchTrackTrigger" {
        run in sequence

        break if ( WpDone )

        set patchTracking = true
        set detectFrom = Science:PeakSalt
        set depthFrom = Science:PeakSaltDepth
        set latitudeFrom = Science:PeakSaltLatitude
        set longitudeFrom = Science:PeakSaltLongitude
        set beginThreshold = BeginThresholdSalt
        set offPeakFraction = OffPeakFraction
        set filterWidth = FilterWidthHorizontal
        set numProfilesSlidingwindow = NumOfProfilesSlidingwindow
        set detectTimeout = DetectionTimeout
        set centerPeak = PatchMode
        outputArg PatchPeakSalinity = peakDetect
        outputArg PatchPeakDepth = peakDepth
        outputArg PatchPeakLatitude = peakLatitude
        outputArg PatchPeakLongitude = peakLongitude
        outputArg PatchPeakDistance = peakDistance
      }

      syslog important "Peak found before WpMaxDistance is reached. Patch value is "
           + PatchPeakSalinity~psu + "at " + PatchPeakLatitude~degree + "," + PatchPeakLongitude~degree
           + ", distance = " + PatchPeakDistance~meter

      sendData service=express (
        PatchPeakSalinity,
        PatchPeakDepth,
        PatchPeakLatitude,
        PatchPeakLongitude
      )

      assign in sequence PatchDetectOn = false

      assign in sequence WpDone = true
    }

    aggregate OilPatchDetection {
      run when (
        PatchDetectOn
        and PeakDetectOil
      )

      behavior Trigger:PeakDetectHorizontal id="PatchTrackTrigger" {
        run in sequence

        break if ( WpDone )

        set patchTracking = true
        set detectFrom = Science:PeakOil
        set depthFrom = Science:PeakOilDepth
        set latitudeFrom = Science:PeakOilLatitude
        set longitudeFrom = Science:PeakOilLongitude
        set beginThreshold = BeginThresholdOil
        set offPeakFraction = OffPeakFraction
        set filterWidth = FilterWidthHorizontal
        set numProfilesSlidingwindow = NumOfProfilesSlidingwindow
        set detectTimeout = DetectionTimeout
        set centerPeak = PatchMode
        outputArg PatchPeakOil = peakDetect
        outputArg PatchPeakDepth = peakDepth
        outputArg PatchPeakLatitude = peakLatitude
        outputArg PatchPeakLongitude = peakLongitude
        outputArg PatchPeakDistance = peakDistance
      }

      syslog important "Peak found before WpMaxDistance is reached. Patch value is "
           + PatchPeakOil~kilogram_per_cubic_meter + "at " + PatchPeakLatitude~degree + ","
           + PatchPeakLongitude~degree + ", distance = " + PatchPeakDistance~meter

      sendData service=express (
        PatchPeakOil,
        PatchPeakDepth,
        PatchPeakLatitude,
        PatchPeakLongitude
      )

      assign in sequence PatchDetectOn = false

      assign in sequence WpDone = true
    }

    aggregate CrossLegToPatchCenter {
      """
      This is a short run back to patch center.
      """

      run in sequence

      assign in sequence WpBearing = ( WpBearing + Reverse ) % TwoPi

      # debug
      #
      #             <Syslog Severity="Important">CntRounds= <Arg Name="CntRounds"/><Units:count/>, Repeat= <Arg Name="Repeat"/><Units:count/></Syslog>

      syslog important "Bearing for CrossLeg: "
           + WpBearing~degree

      behavior Guidance:Waypoint id="CrossWp" {
        run in sequence

        timeout duration=WpTimeout

        set distanceDelta = PatchPeakDistance
        set distanceDeltaBearing = WpBearing
        set waterFrame = WaterFrame
      }

      assign in sequence PatchDetectOn = true

      assign in sequence WpDone = false

      assign id="PresetValueBeforeCrossLegSearchForPeak" in sequence PatchPeakDistance = 0 meter

      assign in sequence CrossLegToPatchCenterCompleted = true
    }

    aggregate SurfaceAfterCrossLegToPatchCenter {
      run when (
        CntRounds % NumRoundsBtnCrossLegSurfacings == 0 count
        and CrossLegToPatchCenterCompleted
        and ( not ( CrossLegSearchForPeakStarted ) )
        or (
          elapsed ( Universal:time_fix ) > NeedCommsTimePatchTracking
          and CrossLegToPatchCenterCompleted
          and ( not ( CrossLegSearchForPeakStarted ) )
        )
      )

      break if ( CntRounds % Repeat == 0 count )

      call id="CrossLegComms" refId="NeedComms"
    }

    aggregate TerminateMissionAfterCrossLegToPatchCenter {
      run in sequence

      break if (
        not (
          CntRounds % Repeat == 0 count
          and CrossLegToPatchCenterCompleted
        )
      )

      syslog important "Desired rounds of crosslegs completed and back to patch center. Terminate mission."

      assign in sequence AllDone = true
    }

    aggregate CrossLegSearchForPeak {
      """
      This is a long run to the edge of the patch to search for the
      peak.
      """

      run in sequence

      break if (
        WpDone
        or ( PatchDetectOn == false )
      )

      assign in sequence CrossLegSearchForPeakStarted = true

      assign in sequence FlagTurnBack = false

      syslog important "Bearing for continued CrossLeg: " + WpBearing~degree

      aggregate CrossLegSearchForPeakNormal {
        run in sequence

        break if (
          Universal:distance_from_shore <= ( MinOffshore + MarginMinOffshore )
          and ( elapsed ( ElapsedSinceStartOrLastDistCheck ) > WaitAfterBouncing )
          or (
            Derivation:ShortestDistanceToPolygonSidesCalculator.loadAtStartup
            and ( Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides <= MinDistFromPolygonSides )
            and ( elapsed ( ElapsedSinceStartOrLastDistCheck ) > WaitAfterBouncing )
          )
        )

        # debug
        #                 <Syslog Severity="Important">Continued CrossLeg: distance_from_shore = <Universal:distance_from_shore/><Units:meter/>. <Arg Name="MinOffshore"/><Units:meter/>+<Arg Name="MarginMinOffshore"/><Units:meter/>. </Syslog>
        #
        #                 <Syslog>CrossLeg. latitude = <Universal:latitude/><Units:degree/>, longitude = <Universal:longitude/><Units:degree/>, shortest_distance_to_polygon_sides = <Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides/><Units:meter/>.</Syslog>

        behavior Guidance:Waypoint id="CrossPeakWp" {
          run in sequence

          timeout duration=WpTimeout

          set distanceDelta = WpMaxDistance
          set distanceDeltaBearing = WpBearing
          set waterFrame = WaterFrame
        }

        call id="SyslogWpMaxDistanceReachedOnCrossLeg" refId="SyslogWpMaxDistanceReached"

        assign in sequence PatchDetectOn = false

        assign in sequence WpDone = true
      }

      aggregate CrossLegSearchForPeakTurnBack {
        run in sequence

        break if (
          FlagTurnBack
          or (
            not (
              Universal:distance_from_shore <= ( MinOffshore + MarginMinOffshore )
              and ( elapsed ( ElapsedSinceStartOrLastDistCheck ) > WaitAfterBouncing )
              or (
                Derivation:ShortestDistanceToPolygonSidesCalculator.loadAtStartup
                and ( Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides <= MinDistFromPolygonSides )
                and ( elapsed ( ElapsedSinceStartOrLastDistCheck ) > WaitAfterBouncing )
              )
            )
          )
        )

        call id="SyslogTurnBackOnCrossLeg" refId="SyslogTurnBack"

        assign in sequence ElapsedSinceStartOrLastDistCheck = 0 hour

        assign in sequence PatchDetectOn = false

        assign in sequence WpDone = true

        assign in sequence FlagTurnBack = true
      }
    }

    aggregate InLeg {
      run in sequence

      assign in sequence WpBearing = ( WpBearing + Reverse ) % TwoPi

      syslog important "Bearing for InLeg: " + WpBearing~degree

      behavior Guidance:Waypoint id="InLegWp" {
        """
        Simply drive back towards the patch center
        """

        run in sequence

        timeout duration=WpTimeout

        set distanceDelta = PatchPeakDistance
        set distanceDeltaBearing = WpBearing
        set waterFrame = WaterFrame
      }

      assign in sequence WpBearing = ( WpBearing - Turn ) % TwoPi

      assign id="FlipSignOfTurnAngle" in sequence Turn = ( Turn + Reverse ) % TwoPi

      syslog important "Bearing for OutLeg: " + WpBearing~degree

      assign in sequence PatchDetectOn = true

      assign in sequence WpDone = false

      assign id="PresetValueBeforeOutLeg" in sequence PatchPeakDistance = 0 meter
    }

    aggregate OutLeg {
      """
      Drive outward until edge of patch is detected.
      """

      run in sequence

      break if (
        WpDone
        or ( PatchDetectOn == false )
      )

      assign in sequence FlagTurnBack = false

      assign in sequence CntRounds = CntRounds + 1 count

      aggregate OutLegNormal {
        run in sequence

        break if (
          Universal:distance_from_shore <= ( MinOffshore + MarginMinOffshore )
          and ( elapsed ( ElapsedSinceStartOrLastDistCheck ) > WaitAfterBouncing )
          or (
            Derivation:ShortestDistanceToPolygonSidesCalculator.loadAtStartup
            and ( Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides <= MinDistFromPolygonSides )
            and ( elapsed ( ElapsedSinceStartOrLastDistCheck ) > WaitAfterBouncing )
          )
        )

        # debug
        #                 <Syslog Severity="Important">OutLeg: distance_from_shore = <Universal:distance_from_shore/><Units:meter/>. <Arg Name="MinOffshore"/><Units:meter/>+<Arg Name="MarginMinOffshore"/><Units:meter/>. </Syslog>

        behavior Guidance:Waypoint id="OutLegWp" {
          run in sequence

          timeout duration=WpTimeout

          set distanceDelta = WpMaxDistance
          set distanceDeltaBearing = WpBearing
          set waterFrame = WaterFrame
        }

        call id="SyslogWpMaxDistanceReachedOnOutLeg" refId="SyslogWpMaxDistanceReached"

        assign id="AssignPatchDetectOffInOutLeg" in sequence PatchDetectOn = false

        assign id="AssignWpDoneInOutLeg" in sequence WpDone = true
      }

      aggregate OutLegTurnBack {
        run in sequence

        break if (
          FlagTurnBack
          or (
            not (
              Universal:distance_from_shore <= ( MinOffshore + MarginMinOffshore )
              and ( elapsed ( ElapsedSinceStartOrLastDistCheck ) > WaitAfterBouncing )
              or (
                Derivation:ShortestDistanceToPolygonSidesCalculator.loadAtStartup
                and ( Derivation:ShortestDistanceToPolygonSidesCalculator.shortest_distance_to_polygon_sides <= MinDistFromPolygonSides )
                and ( elapsed ( ElapsedSinceStartOrLastDistCheck ) > WaitAfterBouncing )
              )
            )
          )
        )

        call id="SyslogTurnBackOnOutLeg" refId="SyslogTurnBack"

        assign in sequence ElapsedSinceStartOrLastDistCheck = 0 hour

        assign in sequence PatchDetectOn = false

        assign in sequence WpDone = true

        assign in sequence FlagTurnBack = true
      }
    }
  }
}