#Product of Timothy Richard McJilton
#Sorry for my homework Travis



from visual import *

# Some basics for my robot
k=900
#jet down constant
jy=65
# Set a few things for the screen
floor=box(pos=vector(0,0,0),height=0.1,width=12,length=20,color=color.green)
scene.center=(0,5,0)
scene.autoscale=0


# Create the parts of the robot
parts=[]
struts=[]

# Create the left foot
parts.append(cone(pos=vector(-1,0,0),axis=(0,0.5,0),radius=0.5,color=color.blue))
parts[0].m=2.
parts[0].v=vector(0,0,0)
parts[0].jet=vector(5,jy,0)

# Create the right foot
parts.append(cone(pos=vector(1,0,0),axis=(0,0.5,0),radius=0.5,color=color.blue))
parts[1].m=2.
parts[1].v=vector(0,0,0)
parts[1].jet=vector(-5,jy,0)

# Add the torso
parts.append(box(pos=(0,3,0),color=color.red,width=0.5,length=1.5,height=2))
parts[2].m=20.
parts[2].v=vector(0,0,0)
parts[2].jet=vector(0,jy,0)

#####################################################################################
# Students, you should put in new parts here!

nparts=8

#head
parts.append(sphere(pos=(0,5,0),color=color.yellow, radius=.5))
parts[3].m=5
parts[3].v=vector(0,0,0)
parts[3].jet=vector(0,jy,0)


armjets=.30149


#right elbow
parts.append(sphere(pos=(2,3,0),color=color.red, radius=.2))
parts[4].m=2
parts[4].v=vector(0,0,0)
parts[4].jet=vector(0,armjets*jy,0)
#left elbow
parts.append(sphere(pos=vector(-2,3,0),color=color.red, radius=.2))
parts[5].m=2
parts[5].v=vector(0,0,0)
parts[5].jet=vector(0,armjets*jy,0)
handjets=1
#right hand
parts.append(sphere(pos=vector(3,2,0),color=color.yellow,radius=.1))
parts[6].m=1
parts[6].v=vector(0,0,0)
parts[6].jet=vector(0,handjets*jy,0)


#left hand
parts.append(sphere(pos=vector(-3,2,0),color=color.yellow,radius=.1))
parts[7].m=1
parts[7].v=vector(0,0,0)
parts[7].jet=vector(0,handjets*jy,0)

####################################################################################

#Add all of the struts
#the "side1" and "side2" indicate the parts on either side of the strut
struts.append(helix(radius=0.1))
struts[0].side1=0
struts[0].side2=1
struts[0].k=k

struts.append(helix(radius=0.1))
struts[1].side1=1
struts[1].side2=2
struts[1].k=k

struts.append(helix(radius=0.1))
struts[2].side1=2
struts[2].side2=0
struts[2].k=k


#####################################################################################
# Students, you should put in new struts here!

nstruts=8

struts.append(helix(radius=.1))
struts[3].side1=2
struts[3].side2=3
struts[3].k=k

struts.append(helix(radius=.1))
struts[4].side1=4
struts[4].side2=2
struts[4].k=k

struts.append(helix(radius=.1))
struts[5].side1=5
struts[5].side2=2
struts[5].k=k

struts.append(helix(radius=.1))
struts[6].side1=6
struts[6].side2=4
struts[6].k=k

struts.append(helix(radius=.1))
struts[7].side1=5
struts[7].side2=7
struts[7].k=k
#
####################################################################################


#Initialize the struts
for j in range (nstruts):
	struts[j].pos=parts[struts[j].side1].pos
	struts[j].axis=parts[struts[j].side2].pos-parts[struts[j].side1].pos
	struts[j].x0=mag(struts[j].axis)

# Now the evolution
g=9.8
t=0
dt=0.01

#Note: if all of the parts are connected via a "chain", you shouldn't need to change 
#this part at all!  
while(t < 20):
	rate(100)
	# First, calculate the forces on all of the parts
	for i in range(nparts):
		parts[i].F=parts[i].m*vector(0,-g,0)+parts[i].jet

	for j in range(nstruts):
		# Also, the forces from the springs
		dF=struts[j].k*struts[j].axis*(mag(struts[j].axis)-struts[j].x0)/\
			mag(struts[j].axis)
		parts[struts[j].side1].F=parts[struts[j].side1].F+dF
		parts[struts[j].side2].F=parts[struts[j].side2].F-dF

	# Next, update the positions of the parts

	for i in range(nparts):
		parts[i].v=parts[i].v+parts[i].F/parts[i].m*dt
		# Check to see if anything has gone through the floor
		if ((parts[i].y < 0) and (parts[i].v.y < 0)):
			parts[i].v.y=-parts[i].v.y
			# This part adds a bit of "friction"
			parts[i].v=parts[i].v*0.9
		parts[i].pos=parts[i].pos+parts[i].v*dt

	#Finally, update the struts
	for j in range (nstruts):
		struts[j].pos=parts[struts[j].side1].pos
		struts[j].axis=parts[struts[j].side2].pos-parts[struts[j].side1].pos
	
	t=t+dt