<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
       xmlns:Control="Tethys/Control"
       xmlns:Guidance="Tethys/Guidance" 
       xmlns:Units="Tethys/Units"
       xmlns:Universal="Tethys/Universal"
       xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
       xsi:schemaLocation="Tethys http://okeanids.mbari.org/tethys/Xml/Tethys.xsd
                           Tethys/Control http://okeanids.mbari.org/tethys/Xml/Control.xsd
                           Tethys/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd
                           Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
                           Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
       Id="TestMission">

<!--
TEST_CODE_START
r2d = 180 / 3.14159
upAngle = 25
downAngle = -25
delta = 4.5
initDelay = 150 * 2.5
upDownDelayUntil= 150
upDownStopAt= 225
upDelayCount = 0
downDelayCount = 0
upAngleTotal = 0
upAngleCount = 0
downAngleTotal = 0
downAngleCount = 0
data = slate.read("platform_pitch_angle")[0]
for i in range(len(data)):
  angle = float(data[i].data)
  if( i < initDelay ):
    continue
  if angle >= 0:
    if downDelayCount > 0:
      downDelayCount = 0
      upDelayCount = 1
    else:
      upDelayCount = upDelayCount + 1
    if( upDelayCount >= upDownDelayUntil and upDelayCount < upDownStopAt ):
      upAngleTotal = upAngleTotal + angle
      upAngleCount = upAngleCount + 1
  else:
    if upDelayCount > 0:
      upDelayCount = 0
      downDelayCount = 1
    else:
      downDelayCount = downDelayCount + 1
    if( downDelayCount >= upDownDelayUntil and downDelayCount < upDownStopAt ):
      downAngleTotal = downAngleTotal + angle
      downAngleCount = downAngleCount + 1
      
if upAngleCount > 0 and downAngleCount > 0:
  upAngleAvg = upAngleTotal / upAngleCount * r2d
  downAngleAvg = downAngleTotal / downAngleCount * r2d
  if utils.withinDelta(upAngleAvg, upAngle, delta) and \
     utils.withinDelta(downAngleAvg, downAngle, delta):
    passed = True
  else:
    if not utils.withinDelta(upAngleAvg, upAngle, delta):
      print("|upAngleAvg(%f) - upAngle(%f)| > delta(%f)" % (upAngleAvg, upAngle, delta) )
    if not utils.withinDelta(downAngleAvg, downAngle, delta):
      print("|downAngleAvg(%f) - downAngle(%f)| > delta(%f)" % (downAngleAvg, downAngle, delta) )
else:
  print("upAngleCount = %d, downAngleCount = %d" % (upAngleCount, downAngleCount) )
TEST_CODE_END
-->

    <DefineArg Name="UpAngle"><Units:degree/><Value>25</Value></DefineArg>

    <DefineArg Name="DownAngle"><Units:degree/><Value>-25</Value></DefineArg>

    <Timeout Duration="P2H"/>

    <Insert Filename="RegressionTests/InsertHighPriority.xml"/>

    <Insert Filename="RegressionTests/InsertSurfaceOps.xml"/>

    <Aggregate Id="TestAggregate">

        <Sequence/>

        <Guidance:AltitudeEnvelope>
            <Parallel/>
            <Setting><Guidance:AltitudeEnvelope.minAltitude/><Units:meter/><Value>6</Value></Setting>
            <Setting><Guidance:AltitudeEnvelope.upPitch/><Arg Name="UpAngle"/></Setting>
            <Setting><Guidance:AltitudeEnvelope.downPitch/><Arg Name="DownAngle"/></Setting>
        </Guidance:AltitudeEnvelope>

        <Guidance:DepthEnvelope>
            <Parallel/>
            <Setting><Guidance:DepthEnvelope.minDepth/><Units:meter/><Value>5</Value></Setting>
            <Setting><Guidance:DepthEnvelope.maxDepth/><Units:meter/><Value>270</Value></Setting>
            <Setting><Guidance:DepthEnvelope.upPitch/><Arg Name="UpAngle"/></Setting>
            <Setting><Guidance:DepthEnvelope.downPitch/><Arg Name="DownAngle"/></Setting>
        </Guidance:DepthEnvelope>

        <Guidance:SetSpeed>
            <Parallel/>
            <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>1</Value></Setting>
        </Guidance:SetSpeed>

        <Guidance:YoYo>
            <Parallel/>
            <Setting><Guidance:YoYo.upPitch/><Arg Name="UpAngle"/></Setting>
            <Setting><Guidance:YoYo.downPitch/><Arg Name="DownAngle"/></Setting>
        </Guidance:YoYo>

        <Guidance:Waypoint>
            <Sequence/>
            <Setting><Guidance:Waypoint.latitudeDelta/><Units:degree/><Value>-0.015</Value></Setting>
            <Setting><Guidance:Waypoint.longitudeDelta/><Units:degree/><Value>-0.015</Value></Setting>
        </Guidance:Waypoint>

    </Aggregate>

</Mission>
