#!/usr/bin/python2

from vismach import *
import hal
import math
import sys

c = hal.component("vismach-testing")
c.newpin("gonioX", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("gonioY", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("xslide", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("yslide", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("zmove", hal.HAL_FLOAT, hal.HAL_IN)
c.ready()

stage = BoxCentered(200,120,6)
stage = Translate([stage], 0,0,-3)
work = Capture()
wcyl = CylinderY(-30, 10, 30, 10)
wcyl = Translate([wcyl], 0,0,-11)
wassembly = Collection([stage, work, wcyl])

wassembly = Translate([wassembly], 0,0,111)
wassembly = HalRotate([wassembly], c, "gonioY", 1, 0, 1, 0)
wassembly = Translate([wassembly], 0,0,-111)

ucyl = CylinderX(-30, 10, 30, 10)
ucyl = Translate([ucyl], 0,0,-21)
uassembly = Collection([ucyl, wassembly])

uassembly = Translate([uassembly], 0,0,121)
uassembly = HalRotate([uassembly], c, "gonioX", 1, 1, 0, 0)
uassembly = Translate([uassembly], 0,0,-121)

ymove = Box(-5,-20,-26, 5,20,-31)
yassembly = Collection([ymove, uassembly])
yassembly = HalTranslate([yassembly], c, "yslide", 0, 1, 0)

xmove = Box(-20,-5,-31, 20,5,-36)
xassembly = Collection([xmove, yassembly])
xassembly = HalTranslate([xassembly], c, "xslide", 1, 0, 0)

tooltip = Capture()
toolcyl = CylinderZ(0, 0, 8, 5)
tool = Collection([tooltip, toolcyl])
tool = HalTranslate([tool], c, "zmove", 0, 0, 1)

model = Collection([xassembly, tool])
myhud = Hud()
main(model, tooltip, work, 500, hud=myhud)