#!/usr/bin/python2

from vismach import *
import hal
import math
import sys

# VARIABLES TO SET
# Location and radius of workpiece (currently can only be convex)
xc = 0
yc = 0
zc = -10
rho = 20

# Location of goniometer rotation points
# gx represents U stage (Goniometer X), gy represents W stage
gx_x = 15
gx_y = 0
gx_z = 100
gy_x = gx_x
gy_y = gx_y
gy_z = gx_z

# END VARIABLE SETTING (for now)

# create all the hal pins - one for each moving piece
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()

# create stage, move so top is in the xy plane
stage = BoxCentered(200,120,6)
stage = Translate([stage], 0,0,-3)
# not sure what this Capture() thing is I think it tells it where the
# workpiece is
work = Capture()
# create workpiece
workpiece = Sphere(0, 0, zc, rho)
workpiece = Translate([workpiece], xc, yc, 0)
# create W stage: represented by a cylinder
wcyl = CylinderY(-15, 10, 15, 10)
# move below the stage
wcyl = Translate([wcyl], 0,0,-11)
# make a collection so these all move together
wassembly = Collection([stage, work, workpiece, wcyl])

# HalRotate rotates the piece around a line from the origin to its last 3
# arguments; so we need to move the collection so its rotation point is at
# the origin, then rotate it, then translate it back into place
wassembly = Translate([wassembly], -gy_x, -gy_y, -gy_z)
wassembly = HalRotate([wassembly], c, "gonioY", 1, 0, 1, 0)
wassembly = Translate([wassembly], gy_x, gy_y, gy_z)

# same deal: cylinder to represent the U stage, collect it with the others
ucyl = CylinderX(-15, 10, 15, 10)
ucyl = Translate([ucyl], 0,0,-21)
uassembly = Collection([ucyl, wassembly])

# translate, rotate, translate back
uassembly = Translate([uassembly], -gx_x, -gx_y, -gx_z)
uassembly = HalRotate([uassembly], c, "gonioX", 1, 1, 0, 0)
uassembly = Translate([uassembly], gx_x, gx_y, gx_z)

# create the Y stage; just a long rectangle
ymove = Box(-5,-60,-26, 5,60,-31)
yassembly = Collection([ymove, uassembly])
yassembly = HalTranslate([yassembly], c, "yslide", 0, -1, 0)

# same for X
xmove = Box(-100,-5,-31, 100,5,-36)
xassembly = Collection([xmove, yassembly])
xassembly = HalTranslate([xassembly], c, "xslide", -1, 0, 0)

# this Capture() tells it where the tool is, and then I have a cone represent
# it
tooltip = Capture()
toolcyl = CylinderZ(0, 0, 10, 2)
tool = Collection([tooltip, toolcyl])
tool = HalTranslate([tool], c, "zmove", 0, 0, 1)

# and finally this code is necessary for it to run don't ask me what it means
model = Collection([xassembly, tool])
myhud = Hud()
main(model, tooltip, work, 500, hud=myhud)