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 cleanUp(self):
     print "About to stop simulation connected to self.simulID: ", self.simulID
     vrep.simxStopSimulation(self.simulID, vrep.simx_opmode_oneshot)
     vrep.simxSynchronousTrigger(self.simulID)                    
     vrep.simxFinish(self.simulID)
     vrep.simxFinish(-1)
     print "Disconnected from V-REP"

 def close(self, kill=False):
     if self.connected:
         remote_api.simxStopSimulation(self.api_id, remote_api.simx_opmode_oneshot_wait)
         remote_api.simxFinish(self.api_id)
     self.connected = False
     if kill and self.vrep_proc is not None:
         os.killpg(self.vrep_proc.pid, signal.SIGTERM)

def reset_simulation(clientID):
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot)
    time.sleep(1) # um pequeno sleep entre o stop e o start
    vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
    for move in startMoves:
        JointControl(clientID, 0, Body, move)
        time.sleep(0.1)

 def disconnection(self):
     """
     Make disconnection with v-rep simulator
     """
     # stop the simulation:
     vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
     
     # Now close the connection to V-REP:	
     vrep.simxFinish(self.clientID)

def finishSimulation(clientID):
    errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
    errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait)
    error=errorStop or errorClose
    errorFinish=vrep.simxFinish(clientID)
    error=error or errorFinish
    return error

 def restart_simulation(self):
     mode = vrep.simx_opmode_oneshot_wait
     assert vrep.simxStopSimulation(self.clientID, mode) == 0, \
                                    "StopSim Error"
     time.sleep(0.1)
     self.start_simulation()
     self.num_sim_steps = 0

 def stop( self ):
     """
     Stops the simulation
     """
     err = vrep.simxStopSimulation( self.cid, vrep.simx_opmode_oneshot_wait )
     time.sleep(0.01) # Maybe this will prevent V-REP from crashing as often
     return hasattr(self, 'failed') # Returns true if this is a failed run

 def finishSimulation(self, clientID):
     self.getObjectPositionFirstTime = True
     errorStop=vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
     errorClose=vrep.simxCloseScene(clientID,vrep.simx_opmode_oneshot_wait)
     error=errorStop or errorClose
     errorFinish=vrep.simxFinish(clientID)
     error=error or errorFinish
     return error

    def __init__(self, n_robots, base_name):
        self.name = "line follower tester"
        self.n_robots = n_robots
        self.base_name = base_name

        self.robot_names = [self.base_name]
        for i in range(self.n_robots-1):
            self.robot_names.append(self.base_name+'#'+str(i))

        # Establish connection to V-REP
        vrep.simxFinish(-1)  # just in case, close all opened connections

        # Connect to the simulation using V-REP's remote API (configured in V-REP, not scene specific)
        # http://www.coppeliarobotics.com/helpFiles/en/remoteApiServerSide.htm
        self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)

        # Use port 19999 and add simExtRemoteApiStart(19999) to some child-script in your scene for scene specific API
        # (requires the simulation to be running)

        if self.clientID != -1:
            print ('Connected to remote API server')
        else:
            print ('Failed connecting to remote API server')
            sys.exit('Could not connect')

        # Reset running simulation
        vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot)
        time.sleep(0.2)

        # Get initial robots' positions
        self.robot_handles = []
        self.robot_pos0 = []
        for rbt_name in self.robot_names:
            res, rbt_tmp = vrep.simxGetObjectHandle(self.clientID, rbt_name, vrep.simx_opmode_oneshot_wait)
            self.robot_handles.append(rbt_tmp)
            # Initialize data stream
            vrep.simxGetObjectPosition(self.clientID, self.robot_handles[-1], -1, vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_1', vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_2', vrep.simx_opmode_streaming)
            vrep.simxGetFloatSignal(self.clientID, rbt_name+'_3', vrep.simx_opmode_streaming)

        time.sleep(0.2)
        for rbt in self.robot_handles:
            res, pos = vrep.simxGetObjectPosition(self.clientID, rbt, -1, vrep.simx_opmode_buffer)
            self.robot_pos0.append(pos)

 def reset(self):
     """In VREP we reset a simulation by stopping and restarting it"""
     eCode = vrep.simxStopSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)  
     if eCode != 0:
         raise Exception("Could not stop VREP simulation")
     eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait)   
     if eCode != 0:
         raise Exception("Could not start VREP simulation")
     vrep.simxSynchronousTrigger(self._VREP_clientId)

def reset_vrep():
    print 'Start to connect vrep'
    # Close eventual old connections
    vrep.simxFinish(-1)
    # Connect to V-REP remote server
    clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
    opmode = vrep.simx_opmode_oneshot_wait
    # Try to retrieve motors and robot handlers
    # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetObjectHandle
    ret_l, LMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", opmode)
    ret_r, RMotorHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", opmode)
    ret_a, RobotHandle = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx", opmode)
    vrep.simxSetJointTargetVelocity(clientID, LMotorHandle, 0, vrep.simx_opmode_blocking)
    vrep.simxSetJointTargetVelocity(clientID, RMotorHandle, 0, vrep.simx_opmode_blocking)
    time.sleep(1)    
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait)
    vrep.simxFinish(clientID)
    print 'Connection to vrep reset-ed!'

 def reset( self ):
     err = vrep.simxStopSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
     time.sleep(1)
     self.pos_err = [0,0,0]
     self.ori_err = [0,0,0]
     self.lin = [0,0,0]
     self.ang = [0,0,0]
     err = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
     if SYNC:
       vrep.simxSynchronous( self.cid, True )

	def reset_rob(self):
		"""
			Sets the bubbleRob to his starting position; mind the hack, simulation has to be stopped 
		"""

		##### Set absolute position

		#stop simulation
		vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait) 

		#100ms delay, this is a hack because server isn't ready either
		time.sleep(0.3)

		#set on absolute position
		err = vrep.simxSetObjectPosition(self.clientID, self.bubbleRobHandle, -1, self.bubbleRobStartPosition, vrep.simx_opmode_oneshot_wait)

		#start simulation again
		vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)

		time.sleep(0.3)

 def resetSimulation(self):
     returnCode = vrep.simx_return_novalue_flag
     while returnCode!=vrep.simx_return_ok:
         returnCode=vrep.simxStopSimulation(self.__clientID, vrep.simx_opmode_oneshot)
         time.sleep(0.5)
     time.sleep(0.5)
     returnCode = vrep.simx_return_novalue_flag
     while returnCode!=vrep.simx_return_ok:
         returnCode=vrep.simxStartSimulation(self.__clientID, vrep.simx_opmode_oneshot)
         time.sleep(0.5)
     time.sleep(1.0)

 def reset( self ):
     err = vrep.simxStopSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
     time.sleep(1)
     self.pos_err = [0,0,0]
     self.ori_err = [0,0,0]
     self.lin = [0,0,0]
     self.ang = [0,0,0]
     self.vert_prox = 0
     self.left_prox = 0
     self.right_prox = 0
     err = vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot_wait)
     if self.sync:
       vrep.simxSynchronous( self.cid, True )

    def startSim(self, clientID, screen=True):
        #I need the simulator stopped in order to be started
        retSimStop = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
        if retSimStop != simx_return_ok :
            print "simulation couldnt be stopped!"
        else:
            print "simulation stopped!"

        #setting the physics engine
        retSetPhyEngine = vrep.simxSetIntegerParameter(clientID, vrep.sim_intparam_dynamic_engine, bulletEngine, vrep.simx_opmode_oneshot_wait)
        if retSetPhyEngine != simx_return_ok:
            print "unable to set the physical engine"
        else:
            print "physical engine correctly setted"

        if int(self.sysConf.getProperty("physics enable?"))==1:
            vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,True,vrep.simx_opmode_oneshot)
        else:
            vrep.simxSetBooleanParameter(clientID,sim_boolparam_dynamics_handling_enabled,False,vrep.simx_opmode_oneshot)

        #settig simulation speed
        if self.speedmodifier > 0:
            vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_speedmodifier, self.speedmodifier, vrep.simx_opmode_oneshot_wait)

        #settig simulation step
        retSetTimeStep = vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step, self.simulationTimeStepDT, vrep.simx_opmode_oneshot_wait)
        if retSetTimeStep != simx_return_ok :
            print "problems setting time step"
        else:
            print "time step setted!"

        #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_realtime_simulation,1,vrep.simx_opmode_oneshot_wait)

        #sync mode configuration
        if self.synchronous:
            vrep.simxSynchronous(clientID,True)

        #light mode configuration
        if not screen:
            vrep.simxSetIntegerParameter(clientID,vrep.sim_intparam_visible_layers,2,vrep.simx_opmode_oneshot_wait)
            #vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)


        #start simulation
        error=vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
        if int(self.sysConf.getProperty("blank screen?"))==1:
            vrep.simxSetBooleanParameter(clientID,vrep.sim_boolparam_display_enabled,0,vrep.simx_opmode_oneshot_wait)
        return error

def vrepSim(clientID, action):
	vrep.simxFinish(-1)
	localhost = "127.0.0.1"
	clientID=vrep.simxStart(localhost,19997,True,True,1000,5)
	if clientID!=-1:
		print('Connected to remote API server')
		if action=="start":
			err = vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)
			if (not err):
				print('Sim Started')
		elif action=="stop":
			err = vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
			if (not err):
				print('Sim Stopped')		
		print("Disconnected from remote API server")
	else:
		print('Failed connecting to remote API server')
	vrep.simxFinish(clientID)
	return clientID

def JointControl(clientID, motionProxy, postureProxy, i, Body):
    # Head
    # velocity_x = vrep.simxGetObjectFloatParameter(clientID,vrep.simxGetObjectHandle(clientID,'NAO#',vrep.simx_opmode_oneshot_wait)[1],11,vrep.simx_opmode_streaming)
    pos = vrep.simxGetObjectPosition(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1]
    angularPos = vrep.simxGetObjectOrientation(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1]
    alphaPosRegister = []
    betaPosRegister = []
    gamaPosRegister = []
    j = 0
    li = []
    while vrep.simxGetConnectionId(clientID) != -1:
        for x in range(0, 150):
            commandAngles = motionProxy.getAngles('Body', False)
            j+=1
            pos = vrep.simxGetObjectPosition(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1]
            angularPos = vrep.simxGetObjectOrientation(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1]
            alphaPosRegister.append(angularPos[0])
            betaPosRegister.append(angularPos[1])
            gamaPosRegister.append(angularPos[2])

            vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming)
            # Left Leg
            vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming)
            # Right Leg
            vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming)
            # Left Arm
            vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming)
            # Right Arm
            vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming)
            # Left Fingers
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming)
            # Right Fingers
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming)
        motionProxy.stopMove()
        postureProxy.stopMove()
        # postureProxy.goToPosture("Stand",0.5)
        vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)
        break
    print 'End of simulation'
    print 'Final x pos'
    print pos
    calculos = {}
    calcular(alphaPosRegister,calculos)
    desvpadA = calculos['desvio_padrao']
    calculos = {}
    calcular(betaPosRegister,calculos)
    desvpadB = calculos['desvio_padrao']
    calculos = {}
    calcular(gamaPosRegister,calculos)
    desvpadG = calculos['desvio_padrao']
    return [(10*pos[0] - (desvpadA + desvpadB + desvpadG)/3), pos[0]]

import sys

print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to V-REP
if clientID!=-1:
    print ('Connected to remote API server')

    # enable the synchronous mode on the client:
    vrep.simxSynchronous(clientID,True)

    # start the simulation:
    vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait)

    # Now step a few times:
    for i in range(1,10):
        if sys.version_info[0] == 3:
            input('Press <enter> key to step the simulation!')
        else:
            raw_input('Press <enter> key to step the simulation!')
        vrep.simxSynchronousTrigger(clientID);

    # stop the simulation:
    vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait)

    # Now close the connection to V-REP:
    vrep.simxFinish(clientID)
else:
    print ('Failed connecting to remote API server')
print ('Program ended')