from vcScript import *
import vcVector as v
import math

def OnRun():
  #define vehicle speed, etc.
  #clear path of vehicle, including at start of simulation
  comp = getComponent()
  vehicle = comp.findBehaviour("VehicleController")
  vehicle.MaxSpeed = 1000.0
  vehicle.Acceleration = 300.0
  vehicle.Deceleration = 300.0
  vehicle.clearMove()
  
  #get robot attached to vehicle, must be child component
  #disable executor from automatically running its program
  robot = None
  for c in comp.ChildComponents:
    if c.findBehavioursByType("rRobotExecutor") != []:
      robot = c
      break
  rx = robot.findBehaviour("Executor")
  rx.IsEnabled = False
  
  #each path point is a vector
  pointA = v.new(5000,0,0)
  pointB = v.new(5000,5000,0)
  angle = pointA.angle(pointB-pointA)
  angle = angle*180.0/math.pi
  
  #first call to go to point A and executor Main routine
  #event listeners of vcSimVehicle still seem like they don't work/implemented
  #so assume vehicle is where it needs to be then stop it
  #callRoutine delays Python execution until routine is finished by default
  vehicle.addControlPoint(pointA)
  delay(vehicle.TotalTime)
  vehicle.clearMove()
  rx.callRoutine(rx.Program.MainRoutine)
  
  #rotate to face new target
  vehicle.clearMove()
  vehicle.rotateInPlace(angle,100,100,100)
  delay(vehicle.TotalTime)
  vehicle.clearMove()
  
  #next call
  vehicle.addControlPoint(pointB)
  delay(vehicle.TotalTime)
  vehicle.clearMove()
  rx.callRoutine(rx.Program.Routines[0])