def __init__(self,clientID):
     self.desiredSteeringAngle=0
     self.desiredWheelRotSpeed=0
     
     self.steeringAngleDx=2*math.pi/180
     self.wheelRotSpeedDx=1
     
     self.d=0.755
     self.l=2.5772
     
     self.clientID=clientID
     
     self.absSpeed={-1:-3,0:0,1:3}
     self.absAngle={-1:-8*math.pi/180,0:0,1:8*math.pi/180}
     
     
     errorCodeML,ml = vrep.simxGetObjectHandle(clientID,'nakedCar_motorLeft',vrep.simx_opmode_oneshot_wait)
     errorCodeMR,mr = vrep.simxGetObjectHandle(clientID,'nakedCar_motorRight',vrep.simx_opmode_oneshot_wait)
     errorCodeSL,sl = vrep.simxGetObjectHandle(clientID,'nakedCar_steeringLeft',vrep.simx_opmode_oneshot_wait)
     errorCodeSR,sr = vrep.simxGetObjectHandle(clientID,'nakedCar_steeringRight',vrep.simx_opmode_oneshot_wait)
     
     self.ml = ml
     self.mr = mr
     self.sl = sl
     self.sr = sr

	def __init__(self, clientID):
		self.clientID = clientID
		
		err,leftMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobLeftMotor",vrep.simx_opmode_oneshot_wait)
		if err==vrep.simx_error_noerror:
			self.leftMotorHandle = leftMotorHandle
			print('Get handle for left motor: {}'.format(leftMotorHandle))
		else:
			print('Error by getting handle for left motor: {}'.format(err))
			
		err,rightMotorHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRobRightMotor",vrep.simx_opmode_oneshot_wait)
		if err==vrep.simx_error_noerror:
			self.rightMotorHandle = rightMotorHandle
			print('Get handle for right motor: {}'.format(rightMotorHandle))
		else:
			print('Error by getting handle for right motor: {}'.format(err))

		err,visionSensorHandle=vrep.simxGetObjectHandle(clientID,"Vision_sensor",vrep.simx_opmode_oneshot_wait)
		if err==vrep.simx_error_noerror:
			self.visionSensorHandle = visionSensorHandle
			print('Get handle for vision sensor: {}'.format(visionSensorHandle))
		else:
			print('Error by getting handle for vision sensor: {}'.format(err))

		err,bubbleRobHandle=vrep.simxGetObjectHandle(clientID,"remoteApiControlledBubbleRob",vrep.simx_opmode_oneshot_wait) #getHandle
		if err==vrep.simx_error_noerror:
			self.bubbleRobHandle = bubbleRobHandle
			print('Get handle for bubbleRob: {}'.format(bubbleRobHandle))
		else:
			print('Error by getting handle for bubbleRob: {}'.format(err))

		err,self.bubbleRobStartPosition = vrep.simxGetObjectPosition(clientID, bubbleRobHandle, -1, vrep.simx_opmode_oneshot_wait) #getStartPosition

 def __init__(self, **kwargs):
     self.id = str(kwargs.get("id", ""))
     self.generation = kwargs.get("generation", 0)
     self.model = kwargs.get("model")
     self.DNA = kwargs.get("DNA")
     self.opmode = kwargs.get("opmode", vrep.simx_opmode_oneshot_wait)
     print "bot number ", self.id, " args : ", self.DNA
     self.name = "2W1A"
     if self.id != "0":
         self.name += "#" + str(self.id)
     else:
         self.id = ""
     print "Bot [" + self.name + "] created"
     print "Internal caracteristique : A DEFINIR ASAP!"
     self.clientID = kwargs.get("clientID")
     vrep.simxCopyPasteObjects(self.clientID, self.model, self.opmode)
     ret1, self.wristHandle = vrep.simxGetObjectHandle(self.clientID, "WristMotor" + self.id, kwargs.get("opmode"))
     ret2, self.elbowHandle = vrep.simxGetObjectHandle(self.clientID, "ElbowMotor" + self.id, kwargs.get("opmode"))
     ret3, self.shoulderHandle = vrep.simxGetObjectHandle(
         self.clientID, "ShoulderMotor" + self.id, kwargs.get("opmode")
     )
     ret4, self.robotHandle = vrep.simxGetObjectHandle(self.clientID, self.id, kwargs.get("opmode"))
     print ret1, ret2, ret3
     if ret1 != 0 or ret2 != 0 or ret3 != 0:
         sys.exit("handlers problem")

 def __init__(self, clientID, name):
     self.clientID = clientID
     self.name = name
     
     self.noDetectionDist = 0.5
     self.maxDetectionDist = 0.2
     self.detect = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
     self.braitenbergL = [-0.2,-0.4,-0.6,-0.8,-1,-1.2,-1.4,-1.6, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
     self.braitenbergR = [-1.6,-1.4,-1.2,-1,-0.8,-0.6,-0.4,-0.2, 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
     self.v0 = 2
     
     errorCode, self.left_motor_handle = vrep.simxGetObjectHandle(clientID,
                                       'Pioneer_p3dx_leftMotor'+name,
                                       vrep.simx_opmode_oneshot_wait)
                                       
     errorCode, self.right_motor_handle = vrep.simxGetObjectHandle(clientID,
                                       'Pioneer_p3dx_rightMotor'+name,
                                       vrep.simx_opmode_oneshot_wait)
         
     self.ultra_sensors = []
     for i in range(1,16):
         errorCode, sensor_handle = vrep.simxGetObjectHandle(clientID,
                             'Pioneer_p3dx_ultrasonicSensor'+str(i)+name,
                             vrep.simx_opmode_oneshot_wait)
         if not errorCode:
             self.ultra_sensors.append(sensor_handle)
             
             returnCode, detectionState, \
             detectedPoint, detectedObjectHandle, \
             detectedSurfaceNormalVector = \
                 vrep.simxReadProximitySensor(clientID, 
                                             self.ultra_sensors[i-1],
                                             vrep.simx_opmode_streaming)
         else:
             print 'Failed to connect to ultrasonic sensor '+str(i)

    def restart(self,earlyStop = False):
        if (self.cid != None):
            vrep.simxStopSimulation(self.cid, self.mode())
            vrep.simxSynchronousTrigger(self.cid)
        vrep.simxFinish(-1)  # just in case, close all opened connections
        time.sleep(1)
        self.connect()
        time.sleep(1)

        vrep.simxLoadScene(self.cid, '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, self.mode())
        if earlyStop:
            vrep.simxStopSimulation(self.cid, self.mode())
            vrep.simxSynchronousTrigger(self.cid)
            vrep.simxFinish(-1)  # just in case, close all opened connections
            return
        vrep.simxStartSimulation(self.cid, self.mode())
        self.runtime = 0
        err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
                                               vrep.simx_opmode_oneshot_wait)
        err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
                                               vrep.simx_opmode_oneshot_wait)

        err, self.front_camera = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)

        err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming)
        self.getTargetStart()
        for i in range(4):
            self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid,
                                                            'Quadricopter_propeller_respondable' + str(i) + str(1),
                                                            self.mode())

    def __init__(self):

        self.ip = '127.0.0.1'
        self.port = 19997
        self.scene = './simu.ttt'
        self.gain = 2
        self.initial_position = [3,3,to_rad(45)]

        self.r = 0.096 # wheel radius
        self.R = 0.267 # demi-distance entre les r

        print('New pioneer simulation started')
        vrep.simxFinish(-1)
        self.client_id = vrep.simxStart(self.ip, self.port, True, True, 5000, 5)

        if self.client_id!=-1:
            print ('Connected to remote API server on %s:%s' % (self.ip, self.port))
            res = vrep.simxLoadScene(self.client_id, self.scene, 1, vrep.simx_opmode_oneshot_wait)
            res, self.pioneer = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
            res, self.left_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
            res, self.right_motor = vrep.simxGetObjectHandle(self.client_id, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)

            self.set_position(self.initial_position)
            vrep.simxStartSimulation(self.client_id, vrep.simx_opmode_oneshot_wait)

        else:
            print('Unable to connect to %s:%s' % (self.ip, self.port))

    def __init__( self, sim_dt=0.01, max_target_distance=3, noise=False,
                  noise_std=[0,0,0,0,0,0],
                  target_func=None,
                ):

        super(Quadcopter, self).__init__(sim_dt)

        err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
                                                vrep.simx_opmode_oneshot_wait )
        err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
                                                vrep.simx_opmode_oneshot_wait )

        # Reset the motor commands to zero
        packedData=vrep.simxPackFloats([0,0,0,0])
        raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData) 

        err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
                                        raw_bytes,
                                        vrep_mode)

        self.pos = [0,0,0]
        self.pos_err = [0,0,0]
        self.t_pos = [0,0,0]
        self.lin = [0,0,0]
        self.ori = [0,0,0]
        self.ori_err = [0,0,0]
        self.t_ori = [0,0,0]
        self.ang = [0,0,0]

        self.vert_prox_dist = 0
        self.left_prox_dist = 0
        self.right_prox_dist = 0
        
        # Distance reading recorded when nothing is in range
        self.max_vert_dist = 1.5
        self.max_left_dist = 1.0
        self.max_right_dist = 1.0
        
        # Maximum target distance error that can be returned
        self.max_target_distance = max_target_distance
 
        # If noise is being modelled
        self.noise = noise

        # Standard Deviation of the noise for the 4 state variables
        self.noise_std = noise_std
        
        # Overwrite the get_target method if the target is to be controlled by a
        # function instead of by V-REP
        if target_func is not None:
          
          self.step = 0
          self.target_func = target_func

          def get_target():
            self.t_pos, self.t_ori = self.target_func( self.step )
            self.step += 1

          self.get_target = get_target

def getSensorHandles_dr20_(clientID):
    err_h_infra6, infra6 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor6_', vrep.simx_opmode_oneshot_wait)
    err_h_infra2, infra2 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor2_', vrep.simx_opmode_oneshot_wait)
    err_h_infra5, infra5 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor5_', vrep.simx_opmode_oneshot_wait)
    err_h_infra1, infra1 = vrep.simxGetObjectHandle(clientID,'dr20_infraredSensor1_', vrep.simx_opmode_oneshot_wait)
    err_h_sonic, sonic = vrep.simxGetObjectHandle(clientID,'dr20_ultrasonicSensor_', vrep.simx_opmode_oneshot_wait)
    return (err_h_infra6, err_h_infra2, err_h_infra5, err_h_infra1, err_h_sonic,
           infra6, infra2, infra5, infra1, sonic)

    def __init__(self, simulatorModel=None):
        self.robotOrientationFirstTime = True
        self.getDistanceToGoalFirstTime = True
        self.getUpDistanceFirstTime = True
        self.getObjectPositionFirstTime = True
        self.sysConf = LoadSystemConfiguration()
        #this data structure is like a cache for the joint handles
        self.jointHandleMapping = {} 
        robotConf = LoadRobotConfiguration()
        self.model = simulatorModel
        self.LucyJoints = robotConf.getJointsName()            
        for joint in self.LucyJoints:
            self.jointHandleMapping[joint]=0
        self.clientId = self.connectVREP()
        RETRY_ATTEMPTS = 50
        if simulatorModel:
            for i in range(RETRY_ATTEMPTS):
                #TODO try to reutilize the same scene for the sake of performance
                error = self.loadscn(self.clientId, simulatorModel)
                if not error:
                    break
                print "retrying connection to vrep"

            if error:
                raise VrepException("error loading Vrep robot model", -1)
       
        if int(self.sysConf.getProperty("synchronous mode?"))==1:
            self.synchronous = True
            vrep.simxSynchronousTrigger(self.clientId)
        else:
            self.synchronous = False
        
        self.speedmodifier = int(self.sysConf.getProperty("speedmodifier"))

        #setting the simulation time step                           
        self.simulationTimeStepDT = float(self.sysConf.getProperty("simulation time step"))

        '''#testing printing in vrep
        error, consoleHandler = vrep.simxAuxiliaryConsoleOpen(self.clientId, "lucy debugging", 8, 22, None, None, None, None, vrep.simx_opmode_oneshot_wait)
        print "console handler: ", consoleHandler
        vrep.simxAuxiliaryConsoleShow(self.clientId, consoleHandler, 1, vrep.simx_opmode_oneshot_wait)
        error = vrep.simxAuxiliaryConsolePrint(self.clientId, consoleHandler, "Hello World", vrep.simx_opmode_oneshot_wait)'''

        error, self.upDistanceHandle = vrep.simxGetDistanceHandle(self.clientId, "upDistance#", vrep.simx_opmode_blocking)
        error, self.distLFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceLFootGoal#", vrep.simx_opmode_blocking)
        error, self.distRFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceRFootGoal#", vrep.simx_opmode_blocking)
        error, self.bioloidHandle = vrep.simxGetObjectHandle(self.clientId,"Bioloid", vrep.simx_opmode_oneshot_wait)
        error, self.cuboidHandle = vrep.simxGetObjectHandle(self.clientId,"Cuboid", vrep.simx_opmode_oneshot_wait)
        error, self.armHandler = vrep.simxGetObjectHandle(self.clientId, "Pivot", vrep.simx_opmode_oneshot_wait)

        self.populateJointHandleCache(self.clientId)

        self.isRobotUpFirstCall = True
        self.getDistanceToSceneGoal() #to fix the first invocation
        self.getUpDistance()
        self.isRobotUp(self.clientId)
        self.armPositionXAxis = -9.0000e-02

 def config_handles(self):
     # Object-handle-configuration                                  
     errorFlag, self.Quadbase = vrep.simxGetObjectHandle(self.clientID,
                                                         'Quadricopter_base',
                                                         vrep.simx_opmode_oneshot_wait)
     errorFlag, self.Quadobj = vrep.simxGetObjectHandle(self.clientID,
                                                        'Quadricopter',
                                                        vrep.simx_opmode_oneshot_wait)
     self.getState(initial=True)
     time.sleep(0.05)

 def start(self):
     print ('Program started')
     vrep.simxFinish(-1) # just in case, close all opened connections
     self.clientID = vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
     if self.clientID != -1:
         print ('Connected to remote API server')
         self.LWMotor_hdl = vrep.simxGetObjectHandle(self.clientID,'LeftWheel_Motor', vrep.simx_opmode_oneshot_wait) # LeftWheel Motor handle
         self.RWMotor_hdl = vrep .simxGetObjectHandle(self.clientID,'RightWheel_Motor', vrep.simx_opmode_oneshot_wait) # RightWheel Motor handle
         self.Robot_hdl = vrep.simxGetObjectHandle(self.clientID, 'Cubot', vrep.simx_opmode_oneshot_wait)
         print ('Handle acquired !!')
     else:
         print ('Error : connection to vrep failed')

 def __init__(self, address, port):
     print ('Program started')
     vrep.simxFinish(-1) 
     self.clientID=vrep.simxStart(address,port,True,True,5000,5)
     if self.clientID!=-1:
         print "Conexion establecida, todo en orden"
     else:
         print "Conexion con el simulador fallida, reinicia el simulador"
     error,self.motorFL=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
     error,self.motorFR=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_oneshot_wait)
     error,self.camera=vrep.simxGetObjectHandle(self.clientID,'Vision_sensor',vrep.simx_opmode_oneshot_wait)
     error,self.robot=vrep.simxGetObjectHandle(self.clientID,'Pioneer_p3dx',vrep.simx_opmode_oneshot_wait)

 def getDistanceBetwObjects(self, objectNameA, objectNameB):
     """Get the distance between two named objects in V-REP.
        Raise exception if either does not exist"""
        
     eCode, handleA = vrep.simxGetObjectHandle(self._VREP_clientId, objectNameA, vrep.simx_opmode_oneshot_wait)
     if eCode != 0:
         raise Exception("Could not get handle of object", objectNameA)
     eCode, poseA =  vrep.simxGetObjectPosition(self._VREP_clientId, handleA, -1, vrep.simx_opmode_oneshot_wait)
     eCode, handleB = vrep.simxGetObjectHandle(self._VREP_clientId, objectNameB, vrep.simx_opmode_oneshot_wait)
     if eCode != 0:
         raise Exception("Could not get handle of object", objectNameB)
     eCode, poseB =  vrep.simxGetObjectPosition(self._VREP_clientId, handleB, -1, vrep.simx_opmode_oneshot_wait)
     return distance(poseA,poseB)

def fetchKinect(depthSTR,rgbSTR):
        errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,rgbSTR,vrep.simx_opmode_oneshot_wait)
        errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_oneshot_wait)

        
        errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_oneshot_wait)
        img,imgArr=Raftaar.ProcessImage(image,resolution)        
        rgbArr=np.array(imgArr)

        errorHere,resol,depth=vrep.simxGetVisionSensorDepthBuffer(clientID,kinectDepth,vrep.simx_opmode_oneshot_wait)
        depthArr=np.array(depth)


        return depthArr,rgbArr

def setJointPosition(clientID, joint, angle):
    if (jointHandleMapping[joint] > 0):
        jhandle=jointHandleMapping[joint]
    else:
        error, jhandle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait)
        jointHandleMapping[joint]=jhandle
    vrep.simxSetJointPosition(clientID,jhandle,angle,vrep.simx_opmode_oneshot_wait)

def streamVisionSensor(visionSensorName,clientID,pause=0.0001):
    #Get the handle of the vision sensor
    res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
    #Get the image
    res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
    #Allow the display to be refreshed
    plt.ion()
    #Initialiazation of the figure
    time.sleep(0.5)
    res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
    im = I.new("RGB", (resolution[0], resolution[1]), "white")
    #Give a title to the figure
    fig = plt.figure(1)    
    fig.canvas.set_window_title(visionSensorName)
    #inverse the picture
    plotimg = plt.imshow(im,origin='lower')
    #Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
    time.sleep(1)
    while (vrep.simxGetConnectionId(clientID)!=-1): 
        #Get the image of the vision sensor
        res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
        #Transform the image so it can be displayed using pyplot
        image_byte_array = array.array('b',image)
        im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
        #Update the image
        plotimg.set_data(im)
        #Refresh the display
        plt.draw()
        #The mandatory pause ! (or it'll not work)
        plt.pause(pause)
    print 'End of Simulation'

 def setJointPositionNonBlock(self, clientID, joint, angle):
     handle = self.jointHandleMapping[joint]
     error = False
     if (handle == 0):
         error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot)
         self.jointHandleMapping[joint]=handle
     vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_streaming)

def prox_sens_initialize(clientID):
    """ Initialize proximity sensor. Maybe helpful later """
    proxSens=[]
    for i in range(8):
        _, oneProxSens = vrep.simxGetObjectHandle(clientID, 'ePuck_proxSensor%d' % (i+1), vrep.simx_opmode_streaming)
        proxSens.append(oneProxSens)
    return proxSens

def get_object_handle(object_name,all_switch):
	#if all_switch == 1:
	#		res,objs = vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshow_wait)
	#else:
	res,objs = vrep.simxGetObjectHandle(clientID,object_name,vrep.simx_opmode_oneshot_wait)

	return objs

def getVisionSensor(visionSensorName,clientID,sample_time):
    #Get the handle of the vision sensor
    res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
    #Get the image
    res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
    time.sleep(0.5)
    res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
    print resolution
    img = I.new("RGB", (resolution[0], resolution[1]))
    while (vrep.simxGetConnectionId(clientID)!=-1): 
        start = time.time()
        #Get the image of the vision sensor
        res,resolution,image = vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)

        image_byte_array = array.array('b',image)
        im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)

        # #Set the image for naoqi
        # imagepixel = []#*(resolution[0]*resolution[1])
        # for i in xrange(resolution[0] * resolution[1]):
        #     r = i*3
        #     g = r+1
        #     b = r+2
        #     imagepixel.append((image[b], image[g], image[r]))
        # img.putdata(imagepixel)
        # print image
        # print imagepixel
        videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0], resolution[1], im.rotate(180).tostring())
        end = time.time()
        dt = end-start
        # if dt < sample_time:
        #     time.sleep(sample_time - dt)
        # else:
        #     print "Can't keep a period (%gs>%gs)" % (dt,sample_time)
    print 'End of Simulation'