#!!! TethysL file automatically generated from RegressionTests/testAltDpthEnvPtchBehavior.xml.

mission TestMission {
  test_code {{{
    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) )
  }}}

  arguments {
    UpAngle = 25 degree

    DownAngle = -25 degree
  }

  timeout duration=P2H

  insert RegressionTests/InsertHighPriority.tl

  insert RegressionTests/InsertSurfaceOps.tl

  aggregate TestAggregate {
    run in sequence

    behavior Guidance:AltitudeEnvelope {
      run in parallel

      set minAltitude = 6 meter

      set upPitch = UpAngle

      set downPitch = DownAngle
    }

    behavior Guidance:DepthEnvelope {
      run in parallel

      set minDepth = 5 meter

      set maxDepth = 270 meter

      set upPitch = UpAngle

      set downPitch = DownAngle
    }

    behavior Guidance:SetSpeed {
      run in parallel

      set speed = 1 meter_per_second
    }

    behavior Guidance:YoYo {
      run in parallel

      set upPitch = UpAngle

      set downPitch = DownAngle
    }

    behavior Guidance:Waypoint {
      run in sequence

      set latitudeDelta = -0.015 degree

      set longitudeDelta = -0.015 degree
    }
  }
}