import rs274.OpenGLTk, Tkinter, hal
from minigl import *

class Collection:
    def __init__(self, parts):
	self.parts = parts

    def traverse(self):
	for p in self.parts:
	    if hasattr(p, "apply"):
		p.apply()
	    if hasattr(p, "draw"):
		p.draw()
	    if hasattr(p, "traverse"):
		p.traverse()
	    if hasattr(p, "unapply"):
		p.unapply()

class Translate(Collection):
    def __init__(self, parts, x, y, z):
	self.parts = parts
	self.where = x, y, z

    def apply(self):
	glPushMatrix()
	glTranslatef(*self.where)

    def unapply(self):
	glPopMatrix()

class HalTranslate(Collection):
    def __init__(self, parts, comp, var, x, y, z):
	self.parts = parts
	self.where = x, y, z
	self.comp = comp
	self.var = var

    def apply(self):
	x, y, z = self.where
	v = self.comp[self.var]
	
	glPushMatrix()
	glTranslatef(x*v, y*v, z*v)

    def unapply(self):
	glPopMatrix()

class HalRotate(Collection):
    def __init__(self, parts, comp, var, th, x, y, z):
	self.parts = parts
	self.where = th, x, y, z
	self.comp = comp
	self.var = var

    def apply(self):
	th, x, y, z = self.where
	glPushMatrix()
	glRotatef(th * self.comp[self.var], x, y, z)

    def unapply(self):
	glPopMatrix()


class Rotate(Collection):
    def __init__(self, parts, th, x, y, z):
	self.parts = parts
	self.where = th, x, y, z

    def apply(self):
	th, x, y, z = self.where
	glPushMatrix()
	glRotatef(th, x, y, z)

    def unapply(self):
	glPopMatrix()

# give endpoint X values and radii
# resulting cylinder is on the X axis
class CylinderX:
    def __init__(self, x1, r1, x2, r2):
	self.coords = x1, r1, x2, r2
	self.q = gluNewQuadric()

    def draw(self):
	x1, r1, x2, r2 = self.coords
	if x1 > x2:
	    x1, x2 = x2, x1
	    r1, r2 = r2, r1
	glPushMatrix()
	# GL creates cylinders along Z, so need to rotate
	z1 = x1
	z2 = x2
	glRotatef(90,0,1,0)
	# need to translate the whole thing to z1
	glTranslatef(0,0,z1)
	# the cylinder starts out at Z=0
	gluCylinder(self.q, r1, r2, z2-z1, 32, 1)
	# bottom cap
	gluDisk(self.q, 0, r1, 32, 1)
	# the top cap needs flipped and translated
	glPushMatrix()
	glTranslatef(0,0,z2-z1)
	glRotatef(180,1,0,0)
	gluDisk(self.q, 0, r2, 32, 1)
	glPopMatrix()
	glPopMatrix()

# give endpoint Y values and radii
# resulting cylinder is on the Y axis
class CylinderY:
    def __init__(self, y1, r1, y2, r2):
	self.coords = y1, r1, y2, r2
	self.q = gluNewQuadric()

    def draw(self):
	y1, r1, y2, r2 = self.coords
	if y1 > y2:
	    y1, y2 = y2, y1
	    r1, r2 = r2, r1
	glPushMatrix()
	# GL creates cylinders along Z, so need to rotate
	z1 = y1
	z2 = y2
	glRotatef(90,1,0,0)
	# need to translate the whole thing to z1
	glTranslatef(0,0,z1)
	# the cylinder starts out at Z=0
	gluCylinder(self.q, r1, r2, z2-z1, 32, 1)
	# bottom cap
	gluDisk(self.q, 0, r1, 32, 1)
	# the top cap needs flipped and translated
	glPushMatrix()
	glTranslatef(0,0,z2-z1)
	glRotatef(180,1,0,0)
	gluDisk(self.q, 0, r2, 32, 1)
	glPopMatrix()
	glPopMatrix()

# give endpoint Z values and radii
# resulting cylinder is on the Z axis
class CylinderZ:
    def __init__(self, z1, r1, z2, r2):
	self.coords = z1, r1, z2, r2
	self.q = gluNewQuadric()

    def draw(self):
	z1, r1, z2, r2 = self.coords
	if z1 > z2:
	    z1, z2 = z2, z1
	    r1, r2 = r2, r1
	# need to translate the whole thing to z1
	glPushMatrix()
	glTranslatef(0,0,z1)
	# the cylinder starts out at Z=0
	gluCylinder(self.q, r1, r2, z2-z1, 32, 1)
	# bottom cap
	gluDisk(self.q, 0, r1, 32, 1)
	# the top cap needs flipped and translated
	glPushMatrix()
	glTranslatef(0,0,z2-z1)
	glRotatef(180,1,0,0)
	gluDisk(self.q, 0, r2, 32, 1)
	glPopMatrix()
	glPopMatrix()

# six coordinate version - specify each side of the box
class Box:
    def __init__(self, x1, y1, z1, x2, y2, z2):
        self.coords = x1, y1, z1, x2, y2, z2

    def draw(self):
        x1, y1, z1, x2, y2, z2 = self.coords
        if x1 > x2:
	    x1, x2 = x2, x1
        if y1 > y2:
	    y1, y2 = y2, y1
        if z1 > z2:
	    z1, z2 = z2, z1

        glBegin(GL_QUADS)
	# bottom face
        glNormal3f(0,0,1)
        glVertex3f(x2, y2, z1)
        glVertex3f(x1, y2, z1)
        glVertex3f(x1, y1, z1)
        glVertex3f(x2, y1, z1)
	# positive X face
        glNormal3f(1,0,0)
        glVertex3f(x2, y1, z1)
        glVertex3f(x2, y2, z1)
        glVertex3f(x2, y2, z2)
        glVertex3f(x2, y1, z2)
	# positive Y face
        glNormal3f(0,1,0)
        glVertex3f(x2, y2, z1)
        glVertex3f(x2, y2, z2)
        glVertex3f(x1, y2, z2)
        glVertex3f(x1, y2, z1)
	# negative Y face
        glNormal3f(0,-1,0)
        glVertex3f(x2, y1, z1)
        glVertex3f(x1, y1, z1)
        glVertex3f(x1, y1, z2)
        glVertex3f(x2, y1, z2)
	# negative X face
        glNormal3f(-1,0,0)
        glVertex3f(x1, y2, z1)
        glVertex3f(x1, y2, z2)
        glVertex3f(x1, y1, z2)
        glVertex3f(x1, y1, z1)
	# top face
        glNormal3f(0,0,-1)
        glVertex3f(x2, y2, z2)
        glVertex3f(x2, y1, z2)
        glVertex3f(x1, y1, z2)
        glVertex3f(x1, y2, z2)
        glEnd()

# specify the width in X and Y, and the height in Z
# the box is centered on the origin
class BoxCentered(Box):
    def __init__(self, xw, yw, zw):
        self.coords = -xw/2.0, -yw/2.0, -zw/2.0, xw/2.0, yw/2.0, zw/2.0

# specify the width in X and Y, and the height in Z
# the box is centered in X and Y, and runs from Z=0 up
# (or down) to the specified Z value
class BoxCenteredXY(Box):
    def __init__(self, xw, yw, zw):
        self.coords = -xw/2.0, -yw/2.0, 0, xw/2.0, yw/2.0, zw


class O(rs274.OpenGLTk.Opengl):
    def __init__(self, *args, **kw):
        rs274.OpenGLTk.Opengl.__init__(self, *args, **kw)
        self.r_back = self.g_back = self.b_back = 0

    def basic_lighting(self):
        self.activate()
        glLightfv(GL_LIGHT0, GL_POSITION, (1, -1, .5, 0))
        glLightfv(GL_LIGHT0, GL_AMBIENT, (.2,.2,.2,0))
        glLightfv(GL_LIGHT0, GL_DIFFUSE, (.6,.6,.4,0))
        glLightfv(GL_LIGHT0+1, GL_POSITION, (-1, -1, .5, 0))
        glLightfv(GL_LIGHT0+1, GL_AMBIENT, (.0,.0,.0,0))
        glLightfv(GL_LIGHT0+1, GL_DIFFUSE, (.0,.0,.4,0))
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, (1,1,1,0))
        glEnable(GL_LIGHTING)
        glEnable(GL_LIGHT0)
        glEnable(GL_LIGHT0+1)
        glDepthFunc(GL_LESS)
        glEnable(GL_DEPTH_TEST)
        glMatrixMode(GL_MODELVIEW)
        glLoadIdentity()

    def redraw(self, *args):
        if self.winfo_width() == 1: return
	self.model.traverse() 

c = hal.component("r")
c.newpin("joint1", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint2", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint3", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint4", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint5", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("joint6", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("grip", hal.HAL_FLOAT, hal.HAL_IN)
c.ready()

app = Tkinter.Tk()
t = O(app, double=1, depth=1)


###################
# this stuff is the actual definition of the machine
# ideally it would be in a separate file from the code above
#

# gripper fingers
finger1 = CylinderZ(-0.02, 0.012, 0.1, 0.010)
finger2 = CylinderZ(-0.02, 0.012, 0.1, 0.010)
finger1 = HalRotate([finger1],c,"grip", 40,0,1,0)
finger2 = HalRotate([finger2],c,"grip",-40,0,1,0)
finger1 = Translate([finger1], 0.025,0.0,0.1)
finger2 = Translate([finger2],-0.025,0.0,0.1)
# "hand" - the part the fingers are attached to
link6 = Collection([
	Box(-0.060, -0.015, 0.02, 0.060, 0.015, 0.1),
	Box(-0.05, -0.05, 0.0, 0.05, 0.05, 0.02)])
# assembly fingers, and make it rotate
link6 = HalRotate([finger1,finger2,link6],c,"joint6",360,0,0,1)

# moving part of wrist joint
link5 = Collection([
	CylinderZ( 0.055, 0.060, 0.070, 0.060),
	CylinderX(-0.026, 0.050, 0.026, 0.050),
	Box(-0.022, -0.050, 0.0, 0.022, 0.050, 0.055)])
# move gripper to end of wrist and attach
link5 = Collection([
	link5,
	Translate([link6],0,0,0.070)])
# make wrist bend
link5 = HalRotate([link5],c,"joint5",90,1,0,0)

# fixed part of wrist joint (rotates on end of arm)
link4 = Collection([
	CylinderX(-0.027, 0.045, -0.055, 0.045),
	CylinderX( 0.027, 0.045,  0.055, 0.045),
	Box(-0.030, -0.045, -0.060, -0.050, 0.045, 0.0),
	Box( 0.030, -0.045, -0.060,  0.050, 0.045, 0.0),
	Box(-0.050, -0.050, -0.090,  0.050, 0.050, -0.060)])
# attach wrist, move whole assembly forward so joint 4 is at origin
link4 = Translate([link4,link5], 0, 0, 0.090)
# make joint 4 rotate
link4 = HalRotate([link4],c,"joint4",360,0,0,1)

# next chunk
link3 = Collection([
	CylinderX(-0.08, 0.10, 0.08, 0.12),
	CylinderZ(0.0, 0.07, 0.7, 0.05)])
# move link4 forward and attach
link3 = Collection([
	link3,
	Translate([link4],0.0, 0.0, 0.7)])
# move whole assembly over so joint 3 is at origin
link3 = Translate([link3],-0.08, 0.0, 0.0)
# make joint 3 rotate
link3 = HalRotate([link3],c,"joint3",360,1,0,0)

# elbow stuff
link2 = Collection([
	CylinderX(-0.1,0.1,-0.09,0.1),
	CylinderX(-0.09,0.13,0.09,0.12),
	CylinderX(0.09,0.10,0.12,0.08)])
# move elbow to end of upper arm
link2 = Translate([link2],0.0,0.0,1.2)
# rest of upper arm
link2 = Collection([
	link2,
	CylinderZ(1.2,0.08, 0.0, 0.1),
	CylinderX(-0.14,0.17,0.14,0.15)])
# move link 3 into place and attach
link2 = Collection([
	link2,
	Translate([link3],-0.1,0.0,1.2)])
# move whole assembly over so joint 2 is at origin
link2 = Translate([link2],0.14, 0.0, 0.0)
# make joint 2 rotate
link2 = HalRotate([link2],c,"joint2",360,1,0,0)

# shoulder stuff
link1 = Collection([
	CylinderX(0.18,0.14,0.20,0.14),
	CylinderX(-0.23,0.18,0.18,0.18),
	CylinderX(-0.23,0.17,-0.29,0.13),
	Box(-0.15,-0.15,0.0,0.15,0.15,-0.20)])
# move link2 to end and attach
link1 = Collection([
	link1,
	Translate([link2],0.20,0.0,0.0)])
# move whole assembly up so joint 1 is at origin
link1 = Translate([link1],0.0, 0.0, 0.2)
# make joint 1 rotate
link1 = HalRotate([link1],c,"joint1",360,0,0,1)

# stationary base
link0 = Collection([
	CylinderZ(1.9, 0.15, 2.0, 0.15),
	CylinderZ(0.05, 0.25, 1.9, 0.13),
	CylinderZ(0.00, 0.4, 0.07, 0.4)])
# move link1 to top and attach
link0 = Collection([
	link0,
	Translate([link1],0.0,0.0,2.0)])

# add a floor
floor = Box(-1.5,-1.5,-0.02,1.5,1.5,0.0)

t.model = Collection([link0, floor])

t.distance = 10

t.pack(fill="both", expand=1)

def update():
    t.tkRedraw()
    t.after(50, update)
update()
app.mainloop()