#!/usr/bin/python2.4
# Copyright 2007 Ben Lipkowitz
# You may distribute this software under the GNU GPL v2 or later
#
# Hexapod visualization.

from vismach import *
import hal
from math import *
import sys
import random
import emc

for setting in sys.argv[1:]: exec setting

#compname must be the same as given in 'loadusr -W' or the comp
#will never be ready
compname = "hexagui"
#if(randomize): compname += str(random.randrange(0,10000))
c = hal.component(compname)
#declare hal pins here
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)
#get the tool position in cartesian coordinates from emc
#so we dont have to do kinematics
c.newpin("axis0", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis1", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis2", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis3", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis4", hal.HAL_FLOAT, hal.HAL_IN)
c.newpin("axis5", hal.HAL_FLOAT, hal.HAL_IN)

c.ready()


#################################
#draw it!

#provide some reference frames
#tool_coords starts out at the origin
tool_coords = Capture()

blob = CylinderZ(-3, 0.3, 0, 0)
tool = Collection([tool_coords, blob])

workpiece = Box(2,2,0,3,4,1)
work_coords = Capture()
workpiece = Collection([work_coords, workpiece])

struts = []
spheres = []

platform = CylinderZ(0, 5, 1, 5)
platform = Translate([platform],0,0,-3)
platform = Collection([tool, platform])
platform = HalTranslate([platform],c, "axis0",1,0,0)
platform = HalTranslate([platform],c, "axis1",0,1,0)
platform = HalTranslate([platform],c, "axis2",0,0,1)
platform = HalRotate([platform], c, "axis3",1,1,0,0)
platform = HalRotate([platform], c, "axis4",1,0,1,0)
platform = HalRotate([platform], c, "axis5",1,0,0,1)
platform = Translate([platform],0,0,15)

for i in range(6):
  #the end-cap is so we can always see where the cylinder is
  inner = CylinderZ(0, 0.8, 10, 0.8)
  endcap = CylinderZ(9,1.2,10,1.2)
  inner = Collection([inner, endcap])
  outer = CylinderZ(0, 1, 10, 1)
  #account for joint offset
  inner = Translate([inner],0,0,-30)
  # make strut extend and retract
  hal_pin = "joint" + str(i)
  inner = HalTranslate([inner],c,hal_pin,0,0,1)
  # HalTranslate fiddles with the position directly, so provide another layer
  faux_inner = Collection([inner])
  faux_inner = Translate([faux_inner],0,0,10)
  strut = Collection([faux_inner, outer])
  hex_angle = 2*pi/6
  radius = 5
  x = radius * cos(i * hex_angle)
  y = radius * sin(i * hex_angle)
  strut = Translate([strut],x, y,0)
  #just for looks
  strut = Rotate([strut],10,y, -x,0)
  struts = Collection([struts, strut])
  
  sweep = Sphere(0, 0, 0, 2)
  sweep = HalScale([sweep], c, hal_pin, 0.1, 0.1, 0.1)
  sweep = Translate([sweep],x,y,0)
  spheres = Collection([spheres, sweep])

# add a floor
size=20
floor = Box(-0.5*size,-0.5*size,-0.02*size,0.5*size,0.5*size,0.0)


model = Collection([struts, platform, floor, workpiece])
#model = Collection([spheres, tool, floor, workpiece])
main(model, tool_coords, work_coords, size)