#!/usr/bin/env python
from vismach import *
import hal
import math
import sys



# CREATING HAL PINS

c = hal.component("abbirbgui")
c.newpin("joint0", hal.HAL_FLOAT, hal.HAL_IN)
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.ready()



# CREATE THE FLOOR, TOOL AND WORK

floor = Collection([Box(-205, -205, -30, 205, 205, 0)])
floor = Color([1, 1, 1, 1],[floor]) #Color([R, G, B, Opacity], [part])

work = Capture()

tool = Capture()
tooltip = Capture()
tool = Collection([tooltip, tool])



# CREATING/ MOVING/ ANIMATING PARTS


# LINK 6
link6 = AsciiSTL(filename="~/linuxcnc/configs/abbirb/junta6")
link6 = Color([1,1,1,1],[link6])
link6 = Collection([link6, tool])
	# link6 = Translate([link6], X, Y, Z)
	# link6 = Rotate([link6],Theta, X, Y, Z)
link6 = HalRotate([link6],c, "joint5", 1,0,1,0)


# LINK 5
link5 = AsciiSTL(filename="~/linuxcnc/configs/abbirb/junta5")
link5 = Color([1,0,1,1],[link5])
link5 = Collection([link6, link5])
	# link5 = Translate([link5], X, Y, Z)
	# link5 = Rotate([link5],Theta, X, Y, Z)
link5 = HalRotate([link5],c, "joint4", 1,1,0,0)



# LINK 4
link4 = AsciiSTL(filename="~/linuxcnc/configs/abbirb/junta4")
link4 = Color([1,1,0,1],[link4])
link4 = Collection([link5, link4])
	# link4 = Translate([link4], X, Y, Z)
	# link4 = Rotate([link4],Theta, X, Y, Z)
link4 = HalRotate([link4],c, "joint3", 1,0,1,0)



# LINK 3
link3 = AsciiSTL(filename="~/linuxcnc/configs/abbirb/junta3")
link3 = Color([0,1,1,1],[link3])
link3 = Collection([link4, link3])
	# link3 = Translate([link3], X, Y, Z)
	# link3 = Rotate([link3],Theta, X, Y, Z)
link3 = HalRotate([link3],c, "joint2", 1,1,0,0)



# LINK 2
link2 = AsciiSTL(filename="~/linuxcnc/configs/abbirb/junta2")
link2 = Color([1,0,0,1],[link2])
link2 = Collection([link3, link2])
	# link2 = Translate([link2], X, Y, Z)
	# link2 = Rotate([link2],Theta, X, Y, Z)
link2 = HalRotate([link2],c, "joint1", 1,1,0,0)



# LINK 1
link1 = AsciiSTL(filename="~/linuxcnc/configs/abbirb/junta1")
link1 = Color([0,1,0,1],[link1])
link1 = Collection([link2, link1])
	# link1 = Translate([link1], X, Y, Z)
	# link1 = Rotate([link1],Theta, X, Y, Z)
link1 = HalRotate([link1],c, "joint0", 1,0,0,1)



# BASE
base = AsciiSTL(filename="~/linuxcnc/configs/abbirb/base")
base = Color([0,0,1,1],[base])
base = Collection([link1, base])






# ASSEMBLING THE MODEL

model = Collection([tooltip, base, floor, work])
main(model, tooltip, work, 1000)