function sysCall_threadmain()
    -- Get some handles first:
    -- local leftMotor=sim.getObjectHandle("remoteApiControlledBubbleRobLeftMotor") -- Handle of the left motor
    -- local rightMotor=sim.getObjectHandle("remoteApiControlledBubbleRobRightMotor") -- Handle of the right motor
    -- local noseSensor=sim.getObjectHandle("remoteApiControlledBubbleRobSensingNose") -- Handle of the proximity sensor

    -- Choose a port that is probably not used (try to always use a similar code):
    --[[
    sim.setThreadAutomaticSwitch(false)
    local portNb=sim.getInt32Parameter(sim.intparam_server_port_next)
    local portStart=sim.getInt32Parameter(sim.intparam_server_port_start)
    local portRange=sim.getInt32Parameter(sim.intparam_server_port_range)
    local newPortNb=portNb+1
    if (newPortNb>=portStart+portRange) then
        newPortNb=portStart
    end
    sim.setInt32Parameter(sim.intparam_server_port_next,newPortNb)
    sim.setThreadAutomaticSwitch(true)
    --]]

    -- Check what OS we are using:
    platf=sim.getInt32Parameter(sim.intparam_platform)
    if (platf==0) then
        pluginFile='simExtRemoteApi.dll'
    end
    if (platf==1) then
        pluginFile='libsimExtRemoteApi.dylib'
    end
    if (platf==2) then
        pluginFile='libsimExtRemoteApi.so'
    end

    -- Check if the required legacy remote Api plugin is there:
    moduleName=0
    moduleVersion=0
    index=0
    pluginNotFound=true
    while moduleName do
        moduleName,moduleVersion=sim.getModuleName(index)
        if (moduleName=='RemoteApi') then
            pluginNotFound=false
        end
        index=index+1
    end

    if (pluginNotFound) then
        -- Plugin was not found
        sim.displayDialog('Error',"Remote Api plugin was not found. ('"..pluginFile.."')&&nSimulation will not run properly",sim.dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
    else
        -- Ok, we found the plugin.
        -- We first start the remote Api server service (this requires the simExtRemoteApi plugin):
        -- simRemoteApi.start(portNb) -- this server function will automatically close again at simulation end
        simRemoteApi.start(19999)
        -- Now we start the client application:
        

        --result=sim.launchExecutable('bubbleRobClient_remoteApi',portNb.." "..leftMotor.." "..rightMotor.." "..noseSensor,0) -- set the last argument to 1 to see the console of the launched client

        -- if (result==-1) then
            -- The executable could not be launched!
            -- sim.displayDialog('Error',"'bubbleRobClient_remoteApi' could not be launched. &&nSimulation will not run properly",sim.dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
        -- end
    end

    -- This thread ends here. The bubbleRob will however still be controlled by
    -- the client application via the legacy remote Api mechanism!
end

function sysCall_cleanup()
    -- Put some clean-up code here
end

-- See the user manual or the available code snippets for additional callback functions and details


function getRobotHandle(inInts,inFloats,inStrings,inBuffer)
    -- inInts, inFloats and inStrings are tables
    -- inBuffer is a string
    robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
    -- Perform any type of operation here.

    -- Always return 3 tables and a string, e.g.:
    return {},{},{},robotHandle
end

-- This is an extremely simple control script for the motorbike. It is meant as a SIMPLE example!
getRobotPos=function(inInts,inFloats,inStrings,inBuffer)
    robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
    position=sim.getObjectPosition(robotHandle,-1)
    return {},position,{},''
end

function setRobotPos(inInts,inFloats,inStrings,inBuffer)
    robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
    position=sim.getObjectPosition(robotHandle,-1)
    return {},position,{},''
end


--[[
    robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
    -- Retrieve some object handles:
    motor=sim.getObjectHandle('Motorbike_motor')
    steering=sim.getObjectHandle('Motorbike_steeringMotor')
    motorbike=sim.getObjectHandle('Motorbike')
    path=sim.getObjectHandle('Motorbike_path')
    pathFollower=sim.getObjectHandle('Motorbike_pathFollower')
    pathFollowerTarget=sim.getObjectHandle('Motorbike_pathFollowerTarget')
    -- Make the path object orphan (because at the beginning of the simulation it is still built on the motorbike model):
    sim.setObjectParent(path,-1,true)
    -- Set a fixed velocity:
    velocity = 30
    cStep = 5
    step = 0.00
    deviation = 0.0

    t = 0
    CInfo = {}
    for i = 1,2,1 do
        CInfo[i] = 0
    end
    
    oldPos = {}
    nowLA = {}
    basicPos = {}
    for i = 1,3,1 do
        oldPos[i] = 0
        nowLA[2*i-1] = 0.0
        nowLA[2*i] = 0.0
        basicPos[1] = {5.35,14.65,0.7575,-0.020503, -1.486533, -1.590551}
        basicPos[2] = {-16.175,-1.875,0.7575,1.485270,-0.178061,-0.010568}
        basicPos[3] = {-9.45,-16.3,0.7575,0.819122, 1.447427, 0.747879}
        basicPos[4] = {8.975,-12.7,0.7575,-1.451466,0.786589,3.057870}
    end
    inclination = 0
    counter = 0
    ------------------------------------------
    -- Check if the required ROS plugin is there:
    moduleName=0
    moduleVersion=0
    index=0
    RosInterface=false
    while moduleName do
        moduleName,moduleVersion=sim.getModuleName(index)
        if (moduleName=='RosInterface') then
            RosInterface=true
        end
        index=index+1
    end

    if (RosInterface) then
        sim.addStatusbarMessage('Ros Connected')
        -- Prepare the sensor publisher and the motor speed subscribers:
        -- Publisher
        The_Pos = simExtRosInterface_advertise('/The_P', 'std_msgs/Float32MultiArray')
        The_Vec = simExtRosInterface_advertise('/The_V', 'std_msgs/Float32MultiArray')
        The_Inf = simExtRosInterface_advertise('/The_I', 'std_msgs/Float32MultiArray')
        The_Loc = simExtRosInterface_advertise('/The_L', 'std_msgs/Float32MultiArray')
        The_Time = simExtRosInterface_advertise('/The_T', 'std_msgs/Float32MultiArray')
        --Subscriber
        Controller = simExtRosInterface_subscribe('/Ctrl_I','std_msgs/Float32MultiArray','setControlInfo')
        ResetRobot=simExtRosInterface_subscribe('/resetRobot','std_msgs/UInt32','resetRobot_cb')
    end
end 
--]]


















    -- do some initialization here
    robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)    

    -- Reset robot subscriber callback
    allModelObjects=sim.getObjectsInTree(robotHandle)
    sim.setThreadAutomaticSwitch(false)
    for i=1,#allModelObjects,1 do
        sim.resetDynamicObject(allModelObjects[i]) -- reset all objects in the model
    end
    --basicEle = sim.getObjectOrientation(robotHandle, -1)
    --basicEle = sim.getObjectQuaternion(robotHandle, -1)
    --sim.addStatusbarMessage(string.format(" -> %f --> msg: %d",basicEle[1],msg.data))
    --sim.addStatusbarMessage(string.format("--> %f --> msg: %d",basicEle[2],msg.data))
    --sim.addStatusbarMessage(string.format("--> %f --> msg: %d",basicEle[3],msg.data))
    --sim.addStatusbarMessage(string.format("--> %f",basicEle[4]))
    --sim.addStatusbarMessage(string.format("--> Start Statement: %d", msg.data))
    oldPos = {}
    nowLA = {}
    basicPos = {}
    for i = 1,3,1 do
        oldPos[i] = 0
        nowLA[2*i-1] = 0.0
        nowLA[2*i] = 0.0
        basicPos[1] = {5.35,14.65,0.7575,-0.020503, -1.486533, -1.590551}
        basicPos[2] = {-16.175,-1.875,0.7575,1.485270,-0.178061,-0.010568}
        basicPos[3] = {-9.45,-16.3,0.7575,0.819122, 1.447427, 0.747879}
        basicPos[4] = {8.975,-12.7,0.7575,-1.451466,0.786589,3.057870}
    end
    
    basicLoc = {}
    basicAng = {}
    nowState = msg.data
    for i = 1,3,1 do
        basicLoc[i] = basicPos[nowState][i]
        basicAng[i] = basicPos[nowState][i+3]
    end
    sim.setObjectPosition(robotHandle,-1,basicLoc)
    sim.setObjectOrientation(robotHandle,-1,basicAng)
    sim.setThreadAutomaticSwitch(true)
    








function sysCall_threadmain()
    -- Check if the required plugin is there:
    moduleName=0
    moduleVersion=0
    index=0
    bubbleRobModuleNotFound=true
    while moduleName do
        moduleName,moduleVersion=sim.getModuleName(index)
        if (moduleName=='BubbleRob') then
            bubbleRobModuleNotFound=false
        end
        index=index+1
    end
    if (bubbleRobModuleNotFound) then
        sim.displayDialog('Error','BubbleRob plugin was not found. (v_repExtBubbleRob.dll)&&nSimulation will not run properly',sim.dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
    else
        local jointHandles={sim.getObjectHandle('leftMotor'),sim.getObjectHandle('rightMotor')}
        local sensorHandle=sim.getObjectHandle('sensingNose')
        local robHandle=simBubble.create(jointHandles,sensorHandle,{0.5,0.25})
        if robHandle>=0 then
            simBubble.start(robHandle,20) -- control happens here
            simBubble.stop(robHandle)
            simBubble.destroy(robHandle)
        end
    end
end






function sysCall_init()
    movingObjects={'bubbleRob'}
    pos={}
    orient={}
    for k, v in pairs(movingObjects) do
        h=sim.getObjectHandle(v)
        movingObjects[k]=h
        pos[h]=sim.getObjectPosition(h,-1)
        orient[h]=sim.getObjectOrientation(h,-1)
    end
    posBase={}
    h=sim.getObjectHandle('bubbleRob')
    posBase=sim.getObjectPosition(h,-1)
    
end

function reset()
    for k, v in pairs(movingObjects) do
        sim.setObjectPosition(v,-1,pos[v])
        sim.setObjectOrientation(v,-1,orient[v])
        sim.resetDynamicObject(v)
    end
end