<?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

# NOTE: This mission tests to see if the vehicle
#       moves as fast as it thinks it is.
delta       = 0.1                 # what delta is considered ok
targetDepth = 20                  # what is our target depth
targetDelta = 0.010               # how much to move from first waypoint
targetDistance = 12               # how far we can be from the waypoint
(lats, lons, depths, speeds) = slate.read("latitude", "longitude", "depth", "platform_speed_wrt_sea_water")

# find out our target lat, lon
for i in range(len(lats)):
  if lats[i].data != "nan" and lons[i].data != "nan" and math.fabs(float(lats[i].data)) > 1e-4 and math.fabs(float(lons[i].data)) > 1e-4:
    target = Location(float(lats[i].data) + targetDelta, float(lons[i].data), "degree")
    break

tspeed = 0
tspeedCount = 0
reachedTargetDepth = False
lastloc = None
for i in range(len(lats)):
  loc = Location(float(lats[i].data), float(lons[i].data), "degree")
  depth = abs(float(depths[i].data))
  speed = float(speeds[i].data)

  # find at out what lat, lon we reach our target depth.
  # we only start to consider speed once we reach target depth.
  if reachedTargetDepth == False and depth >= targetDepth:
    startloc = loc
    starttime = lats[i].timestamp
    reachedTargetDepth = True
    
  if reachedTargetDepth:
    # start collecting speeds
    tspeed = tspeed + speed
    tspeedCount = tspeedCount + 1
    lastloc = loc

# make sure our test turned out ok
# did we come up near the target?
if lastloc != None and lastloc.isWithinDistance(target, targetDistance) and tspeedCount > 0:

  vavgspeed = tspeed / tspeedCount # observed by vehicle

  # calc avgspeed from the point where we reached
  # depth to the point where we ended up
  dt = lats[-1].getTimestamp()
  d = startloc.getDistance(lastloc)
  t = utils.timeDelta2Secs(dt - starttime)
  avgspeed = d / t
  if utils.withinDelta(vavgspeed, avgspeed, delta):
    passed = True
  else:
    print "Failed: abs(vavgspeed(%g)-avgspeed(%g)) > delta(%g))" % (vavgspeed, avgspeed, delta)
elif lastloc != None:
  print "Failed: lastloc.getDistance(target)=%g" % lastloc.getDistance(target)
else:
  print "Failed: did not achieve target depth"
TEST_CODE_END
-->

    <Timeout Duration="P5H"/>

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

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

    <Aggregate Id="TestAggregate">

        <Sequence/>

        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.depth/><Units:meter/><Value>20</Value></Setting>
        </Guidance:Pitch>

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

        <Guidance:Waypoint>
            <Sequence/>
            <Setting><Guidance:Waypoint.latitudeDelta/><Units:degree/><Value>0.010</Value></Setting>
        </Guidance:Waypoint>

    </Aggregate>

</Mission>
