from __future__ import division
from visual import *
from physutil import *
from visual.graph import *

#Window setup
scene.center = (0,0,0)
scene.range = 5

#Objects
block = box(pos=(0,0,0), length=5, width=1, height=1, color=color.green,make_trail=True)
ball = sphere(pos=vector(-1,-10,0), radius=0.15, color=color.red,make_trail=True)

#Parameters and Initial Conditions
mball = 400
mblock = 1700
mtot = mball + mblock

vball = vector(0,1600,0)

pball = mball*vball
ptot = pball

Lball = cross(ball.pos,pball)
Ltot = Lball

#Time and time step
t = 0
tf = 1000
dt = 0.00001

#MotionMap/Graph
energyGraph = PhysGraph(numPlots=1)

#Calculation Loop
while t<tf:
	rate(500)

	if ball.pos.y<-0.5 or abs(ball.pos.x)>2.5 or abs(ball.pos.z)>0.5:
		ball.pos = ball.pos + (pball/mball)*dt

		energyGraph.plot(t,(mag(pball)**2)/(2*mball))

	else:
		ball.pos = ball.pos + (ptot/mtot)*dt
		block.pos = block.pos + (ptot/mtot)*dt

		Iball = mball*mag(ball.pos-block.pos)**2
		Iblock = 33
		Itot = Iball+Iblock

		ball.rotate(angle=(mag(Ltot)/Itot)*dt, axis=Ltot/mag(Ltot), origin=block.pos)
		block.rotate(angle=(mag(Ltot)/Itot)*dt, axis=Ltot/mag(Ltot), origin=block.pos)

		energyGraph.plot(t,(1/2)*Itot*((mag(Ltot)/Itot)**2) + (mag(ptot)**2)/(2*mtot))

	t = t + dt