import pybullet as p
import time
import math

pi = 3.14159264
limitVal = 2*pi
legpos = 3./4.*pi
legposS = 0
legposSright = 0#-0.3
legposSleft = 0#0.3

defaultERP=0.4
maxMotorForce = 5000
maxGearForce = 10000
jointFriction = 0.1


p.connect(p.GUI)


amplitudeId= p.addUserDebugParameter("amplitude",0,3.14,0.5)
timeScaleId = p.addUserDebugParameter("timeScale",0,10,1)


jointTypeNames={}
jointTypeNames[p.JOINT_REVOLUTE]="JOINT_REVOLUTE"
jointTypeNames[p.JOINT_FIXED]="JOINT_FIXED"
p.setPhysicsEngineParameter(numSolverIterations=100)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)


jointNamesToIndex={}


p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)


for j in range(p.getNumJoints(vision)):
	jointInfo = p.getJointInfo(vision,j)
	jointInfoName = jointInfo[1].decode("utf-8") 
	print("joint ",j," = ",jointInfoName, "type=",jointTypeNames[jointInfo[2]])
	jointNamesToIndex[jointInfoName ]=j
	#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
	p.setJointMotorControl2(vision,j,p.VELOCITY_CONTROL,targetVelocity=0, force=jointFriction)


chassis_right_center = jointNamesToIndex['chassis_right_center']
motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint']
motor_front_rightS_joint = jointNamesToIndex['motor_front_rightS_joint']
hip_front_rightR_joint = jointNamesToIndex['hip_front_rightR_joint']
knee_front_rightR_joint = jointNamesToIndex['knee_front_rightR_joint']
motor_front_rightL_joint = jointNamesToIndex['motor_front_rightL_joint']
motor_back_rightR_joint = jointNamesToIndex['motor_back_rightR_joint']
motor_back_rightS_joint = jointNamesToIndex['motor_back_rightS_joint']
hip_back_rightR_joint = jointNamesToIndex['hip_back_rightR_joint']
knee_back_rightR_joint = jointNamesToIndex['knee_back_rightR_joint']
motor_back_rightL_joint = jointNamesToIndex['motor_back_rightL_joint']
chassis_left_center = jointNamesToIndex['chassis_left_center']
motor_front_leftL_joint = jointNamesToIndex['motor_front_leftL_joint']
motor_front_leftS_joint = jointNamesToIndex['motor_front_leftS_joint']
hip_front_leftL_joint = jointNamesToIndex['hip_front_leftL_joint']
knee_front_leftL_joint = jointNamesToIndex['knee_front_leftL_joint']
motor_front_leftR_joint = jointNamesToIndex['motor_front_leftR_joint']
motor_back_leftL_joint = jointNamesToIndex['motor_back_leftL_joint']
hip_back_leftL_joint = jointNamesToIndex['hip_back_leftL_joint']
knee_back_leftL_joint = jointNamesToIndex['knee_back_leftL_joint']
motor_back_leftR_joint = jointNamesToIndex['motor_back_leftR_joint']
motor_back_leftS_joint = jointNamesToIndex['motor_back_leftS_joint']

motA_rf_Id= p.addUserDebugParameter("motA_rf",-limitVal,limitVal,legpos)
motB_rf_Id= p.addUserDebugParameter("motB_rf",-limitVal,limitVal,legpos)
motC_rf_Id= p.addUserDebugParameter("motC_rf",-limitVal,limitVal,legposSright)
erp_rf_Id= p.addUserDebugParameter("erp_rf",0,1,defaultERP)
relPosTarget_rf_Id= p.addUserDebugParameter("relPosTarget_rf",-limitVal,limitVal,0)


motA_lf_Id= p.addUserDebugParameter("motA_lf",-limitVal,limitVal,-legpos)
motB_lf_Id= p.addUserDebugParameter("motB_lf",-limitVal,limitVal,-legpos)
motC_lf_Id= p.addUserDebugParameter("motC_lf",-limitVal,limitVal,legposSleft)

erp_lf_Id= p.addUserDebugParameter("erp_lf",0,1,defaultERP)
relPosTarget_lf_Id= p.addUserDebugParameter("relPosTarget_lf",-limitVal,limitVal,0)

motA_rb_Id= p.addUserDebugParameter("motA_rb",-limitVal,limitVal,legpos)
motB_rb_Id= p.addUserDebugParameter("motB_rb",-limitVal,limitVal,legpos)
motC_rb_Id= p.addUserDebugParameter("motC_rb",-limitVal,limitVal,legposSright)

erp_rb_Id= p.addUserDebugParameter("erp_rb",0,1,defaultERP)
relPosTarget_rb_Id= p.addUserDebugParameter("relPosTarget_rb",-limitVal,limitVal,0)


motA_lb_Id= p.addUserDebugParameter("motA_lb",-limitVal,limitVal,-legpos)
motB_lb_Id= p.addUserDebugParameter("motB_lb",-limitVal,limitVal,-legpos)
motC_lb_Id= p.addUserDebugParameter("motC_lb",-limitVal,limitVal,legposSleft)

erp_lb_Id= p.addUserDebugParameter("erp_lb",0,1,defaultERP)
relPosTarget_lb_Id= p.addUserDebugParameter("relPosTarget_lb",-limitVal,limitVal,0)

camTargetPos=[0.25,0.62,-0.15]
camDist = 2
camYaw = -2
camPitch=-16
p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos)





c_rf = p.createConstraint(vision,knee_front_rightR_joint,vision,motor_front_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,maxForce=maxGearForce)

c_lf = p.createConstraint(vision,knee_front_leftL_joint,vision,motor_front_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,maxForce=maxGearForce)

c_rb = p.createConstraint(vision,knee_back_rightR_joint,vision,motor_back_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,maxForce=maxGearForce)

c_lb = p.createConstraint(vision,knee_back_leftL_joint,vision,motor_back_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,maxForce=maxGearForce)




p.setRealTimeSimulation(1)
for i in range (1):
#while (1):
	motA_rf = p.readUserDebugParameter(motA_rf_Id)
	motB_rf = p.readUserDebugParameter(motB_rf_Id)
	motC_rf = p.readUserDebugParameter(motC_rf_Id)
	erp_rf = p.readUserDebugParameter(erp_rf_Id)
	relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id)
	#motC_rf
	p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce)
	p.setJointMotorControl2(vision,motor_front_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rf,force=maxMotorForce)
	p.setJointMotorControl2(vision,motor_front_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rf,force=maxMotorForce)
	p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,erp=erp_rf, relativePositionTarget=relPosTarget_rf,maxForce=maxGearForce)

	motA_lf = p.readUserDebugParameter(motA_lf_Id)
	motB_lf = p.readUserDebugParameter(motB_lf_Id)
	motC_lf = p.readUserDebugParameter(motC_lf_Id)
	erp_lf = p.readUserDebugParameter(erp_lf_Id)
	relPosTarget_lf = p.readUserDebugParameter(relPosTarget_lf_Id)
	p.setJointMotorControl2(vision,motor_front_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lf,force=maxMotorForce)
	p.setJointMotorControl2(vision,motor_front_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lf,force=maxMotorForce)
	p.setJointMotorControl2(vision,motor_front_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lf,force=maxMotorForce)
	p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,erp=erp_lf, relativePositionTarget=relPosTarget_lf,maxForce=maxGearForce)


	motA_rb = p.readUserDebugParameter(motA_rb_Id)
	motB_rb = p.readUserDebugParameter(motB_rb_Id)
	motC_rb = p.readUserDebugParameter(motC_rb_Id)
	erp_rb = p.readUserDebugParameter(erp_rb_Id)
	relPosTarget_rb = p.readUserDebugParameter(relPosTarget_rb_Id)
	p.setJointMotorControl2(vision,motor_back_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rb,force=maxMotorForce)
	p.setJointMotorControl2(vision,motor_back_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rb,force=maxMotorForce)
	p.setJointMotorControl2(vision,motor_back_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rb,force=maxMotorForce)
	p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,erp=erp_rb, relativePositionTarget=relPosTarget_rb,maxForce=maxGearForce)

	motA_lb = p.readUserDebugParameter(motA_lb_Id)
	motB_lb = p.readUserDebugParameter(motB_lb_Id)
	motC_lb = p.readUserDebugParameter(motC_lb_Id)
	erp_lb = p.readUserDebugParameter(erp_lb_Id)
	relPosTarget_lb = p.readUserDebugParameter(relPosTarget_lb_Id)
	p.setJointMotorControl2(vision,motor_back_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lb,force=maxMotorForce)
	p.setJointMotorControl2(vision,motor_back_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lb,force=maxMotorForce)
	p.setJointMotorControl2(vision,motor_back_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lb,force=maxMotorForce)
	p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,erp=erp_lb, relativePositionTarget=relPosTarget_lb,maxForce=maxGearForce)



	p.setGravity(0,0,-10)
	time.sleep(1./240.)
t = 0
prevTime = time.time()
while (1):
        timeScale = p.readUserDebugParameter(timeScaleId)
        amplitude = p.readUserDebugParameter(amplitudeId)
        newTime = time.time()
        dt = (newTime-prevTime)*timeScale
        t = t+dt
        prevTime = newTime

        amp=amplitude
        motA_rf = math.sin(t)*amp+legpos
        motA_rb = math.sin(t)*amp+legpos
        motA_lf = -(math.sin(t)*amp+legpos)
        motA_lb = -(math.sin(t)*amp+legpos)

        motB_rf = math.sin(t)*amp+legpos
        motB_rb = math.sin(t)*amp+legpos
        motB_lf = -(math.sin(t)*amp+legpos)
        motB_lb = -(math.sin(t)*amp+legpos)
	
        p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce)
        p.setJointMotorControl2(vision,motor_front_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rf,force=maxMotorForce)
        p.setJointMotorControl2(vision,motor_front_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rf,force=maxMotorForce)
      

        p.setJointMotorControl2(vision,motor_front_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lf,force=maxMotorForce)
        p.setJointMotorControl2(vision,motor_front_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lf,force=maxMotorForce)
        p.setJointMotorControl2(vision,motor_front_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lf,force=maxMotorForce)


        p.setJointMotorControl2(vision,motor_back_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rb,force=maxMotorForce)
        p.setJointMotorControl2(vision,motor_back_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rb,force=maxMotorForce)
        p.setJointMotorControl2(vision,motor_back_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rb,force=maxMotorForce)


        p.setJointMotorControl2(vision,motor_back_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lb,force=maxMotorForce)
        p.setJointMotorControl2(vision,motor_back_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lb,force=maxMotorForce)
        p.setJointMotorControl2(vision,motor_back_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lb,force=maxMotorForce)

        p.setGravity(0,0,-10)
        time.sleep(1./240.)

