execfile("Modified_data/realtime.py")
execfile("Modified_data/vehicleState.dr")

trick.TMM_reduced_checkpoint(0)

veh_integloop.getIntegrator(trick.Runge_Kutta_4, 8)

veh.vehicle.position[0] = 0.0
veh.vehicle.position[1] = 0.0
veh.vehicle.heading = 0.0;

veh.vehicle.distanceBetweenWheels = 0.183
veh.vehicle.wheelRadius = 0.045
veh.vehicle.wheelSpeedLimit = 8.880
veh.vehicle.headingRateLimit = 3.14159/3

veh.vehicle.slowDownDistance = 0.5
veh.vehicle.arrivalDistance  = 0.1

#==========================================
# Add the waypoints to the SIM.
#==========================================
waypoints_path = "Modified_data/cross.waypoints"
fp = open(waypoints_path, "r")
for line in fp:
    fields = line.split(r",")
    veh.vehicle.add_waypoint( float(fields[0]), float(fields[1]))

#==========================================
# Start the display VarServer Client
#==========================================
varServerPort = trick.var_server_get_port();
trick_home = os.environ['TRICK_HOME']
EVDisplay_path = trick_home + "/trick_models/Wheelbot/Graphics/dist/EVDisplay.jar"

if (os.path.isfile(EVDisplay_path)) :
    EVDisplay_cmd = "java -jar " \
                  + EVDisplay_path \
                  + " -v images/twoWheelRover.png " \
                  + " -w " + waypoints_path \
                  + " " + str(varServerPort) + " &" ;
    print(EVDisplay_cmd)
    os.system( EVDisplay_cmd);
else :
    print('============================')
    print('EVDisplay needs to be built.')
    print('============================')

trick.stop(100)