"""The Python implementation of the PyBullet GRPC client."""

from __future__ import print_function

import grpc

import pybullet_pb2
import pybullet_pb2_grpc

#todo: how to add this?
MJCF_COLORS_FROM_FILE = 512

def run():
	print("grpc.insecure_channel")
	channel = grpc.insecure_channel('localhost:6667')
	print("pybullet_pb2_grpc.PyBulletAPIStub")
	stub = pybullet_pb2_grpc.PyBulletAPIStub(channel)
	response=0

	print("submit CheckVersionCommand")
	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(checkVersionCommand=pybullet_pb2.CheckVersionCommand(clientVersion=123)))
	print("PyBullet client received: " , response)


        print("submit_ResetSimulationCommand")
        response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(resetSimulationCommand=pybullet_pb2.ResetSimulationCommand()))
        print("PyBullet client received: ", response)


	print("submit LoadUrdfCommand ")
	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadUrdfCommand=pybullet_pb2.LoadUrdfCommand(fileName="door.urdf", initialPosition=pybullet_pb2.vec3(x=0,y=0,z=0), useMultiBody=True, useFixedBase=True, globalScaling=2, flags = 1)))
	print("PyBullet client received: " , response)
	bodyUniqueId = response.urdfStatus.bodyUniqueId
	
	

	print("submit LoadSdfCommand")
	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadSdfCommand=pybullet_pb2.LoadSdfCommand(fileName="two_cubes.sdf", useMultiBody=True, globalScaling=2)))
	print("PyBullet client received: " , response)
	

	print("submit LoadMjcfCommand")
	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadMjcfCommand=pybullet_pb2.LoadMjcfCommand(fileName="mjcf/humanoid.xml",flags=MJCF_COLORS_FROM_FILE)))
	print("PyBullet client received: " , response)

	
	
	print("submit ChangeDynamicsCommand ")
	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(changeDynamicsCommand=pybullet_pb2.ChangeDynamicsCommand(bodyUniqueId=bodyUniqueId, linkIndex=-1, mass=10)))
	print("PyBullet client received: " , response)
	
	print("submit GetDynamicsCommand ")
	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(getDynamicsCommand=pybullet_pb2.GetDynamicsCommand(bodyUniqueId=bodyUniqueId, linkIndex=-1)))
	print("PyBullet client received: " , response)

	print("submit InitPoseCommand")
	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(initPoseCommand=pybullet_pb2.InitPoseCommand(bodyUniqueId=bodyUniqueId, initialStateQ=[1,2,3],hasInitialStateQ=[1,1,1])))
	print("PyBullet client received: " , response)
	
	print("submit RequestActualStateCommand")
	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(requestActualStateCommand=pybullet_pb2.RequestActualStateCommand(bodyUniqueId=bodyUniqueId, computeForwardKinematics=True, computeLinkVelocities=True )))
	print("PyBullet client received: " , response)
	
	
        i=0
        while(True):
                        i=i+1
			print("submit StepSimulationCommand: ", i)
			response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(stepSimulationCommand=pybullet_pb2.StepSimulationCommand()))
			print("PyBullet client received: " , response.statusType)
  
  #print("TerminateServerCommand")
	#response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(terminateServerCommand=pybullet_pb2.TerminateServerCommand()))
	#print("PyBullet client received: " , response.statusType)


if __name__ == '__main__':
	run()