import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)

import pybullet as p
import math
import time
import pybullet_data

p.connect(p.GUI)
#p.loadURDF("wheel.urdf",[0,0,3])
p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane=p.loadURDF("plane100.urdf",[0,0,0])
timestep = 1./240.

bike=-1
for i in range (1):

	bike=p.loadURDF("bicycle/bike.urdf",[0,0+3*i,1.5], [0,0,0,1],	useFixedBase=False)
	p.setJointMotorControl2(bike,0,p.VELOCITY_CONTROL,targetVelocity=0,force=0.05)
	#p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=1000)
	p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=0)
	p.setJointMotorControl2(bike,2,p.VELOCITY_CONTROL,targetVelocity=15, force=20)

	p.changeDynamics(plane,-1, mass=0,lateralFriction=1, linearDamping=0, angularDamping=0)
	p.changeDynamics(bike,1,lateralFriction=1,linearDamping=0, angularDamping=0)
	p.changeDynamics(bike,2,lateralFriction=1,linearDamping=0, angularDamping=0)
	#p.resetJointState(bike,1,0,100)
	#p.resetJointState(bike,2,0,100)
	#p.resetBaseVelocity(bike,[0,0,0],[0,0,0])
#p.setPhysicsEngineParameter(numSubSteps=0)
#bike=p.loadURDF("frame.urdf",useFixedBase=True)
#bike = p.loadURDF("handlebar.urdf", useFixedBase=True)
#p.loadURDF("handlebar.urdf",[0,2,1])
#coord	=	p.loadURDF("handlebar.urdf", [0.7700000000000005,	0, 0.22000000000000006],useFixedBase=True)#	p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
#coord	=	p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(0)
#coordPos	=	[0,0,0]
#coordOrnEuler = [0,0,0]

#coordPos= [0.7000000000000004, 0, 0.22000000000000006]
#coordOrnEuler= [0, -0.2617993877991496, 0]

coordPos= [0.07, 0, -0.6900000000000004]
coordOrnEuler= [0, 0, 0]

coordPos2= [0, 0, 0 ]
coordOrnEuler2= [0, 0, 0]

invPos,invOrn=p.invertTransform(coordPos,p.getQuaternionFromEuler(coordOrnEuler))
mPos,mOrn = p.multiplyTransforms(invPos,invOrn, coordPos2,p.getQuaternionFromEuler(coordOrnEuler2))
eul = p.getEulerFromQuaternion(mOrn)
print("rpy=\"",eul[0],eul[1],eul[2],"\" xyz=\"", mPos[0],mPos[1], mPos[2])


shift=0
gui = 1


frame=0
states=[]
states.append(p.saveState())
#observations=[]
#observations.append(obs)
				
running = True
reverting = False
p.getCameraImage(320,200)#,renderer=p.ER_BULLET_HARDWARE_OPENGL )
				
while	(1):

	updateCam = 0
	keys = p.getKeyboardEvents()
	for k,v in keys.items():
		if (reverting or (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED))):
			reverting=True
			stateIndex = len(states)-1
			#print("prestateIndex=",stateIndex)
			time.sleep(timestep)
			updateCam=1
			if (stateIndex>0):
				stateIndex-=1
				states=states[:stateIndex+1]
				#observations=observations[:stateIndex+1]
				
				
				#print("states=",states)
			#print("stateIndex =",stateIndex )
			p.restoreState(states[stateIndex])
			#obs=observations[stateIndex]
			
		
			#obs, r, done, _ = env.step(a)
		if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_RELEASED)):
			reverting = False

		if (k == ord('1') and (v&p.KEY_WAS_TRIGGERED)):
			gui = not gui
		
		
		
		if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_RELEASED)):
			running=False
			
		if (running or (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_TRIGGERED))):
			running = True
				
	if (running):
		
		p.stepSimulation()
		
		updateCam=1
		time.sleep(timestep)
		pts = p.getContactPoints()
		#print("numPoints=",len(pts))
		#for point in pts:
		#	print("Point:bodyA=", point[1],"bodyB=",point[2],"linkA=",point[3],"linkB=",point[4],"dist=",point[8],"force=",point[9])
		
		states.append(p.saveState())
		#observations.append(obs)
		stateIndex = len(states)
		#print("stateIndex =",stateIndex )
		frame += 1
	if (updateCam):
		distance=5
		yaw = 0
		humanPos, humanOrn = p.getBasePositionAndOrientation(bike)
		humanBaseVel = p.getBaseVelocity(bike)
		#print("frame",frame, "humanPos=",humanPos, "humanVel=",humanBaseVel)
		if (gui):
			
			camInfo = p.getDebugVisualizerCamera()
			curTargetPos = camInfo[11]
			distance=camInfo[10]
			yaw = camInfo[8]
			pitch=camInfo[9]
			targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]]
			p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos);