#!/usr/bin/env python3
#    Copyright 2009 Alex Joni
#
#    This program is free software; you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation; either version 2 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program; if not, write to the Free Software
#    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

# graphic model of a Unimate Puma 560
# according to the dimensions from: http://www.juve.ro/blog/puma/
# the obj files can be downloaded there, and may be included in emc2 later

from vismach import *  # Ensure all necessary components are imported

import hal

c = hal.component("puma560gui")
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.ready()



floor = Collection([Box(-500,-500, 0, 500,500, 10)])
floor = Color([0,1,0,0],[floor])


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



try: # Expect files in working directory
    link4 = AsciiOBJ(filename="puma_link4.obj")
    link3 = AsciiOBJ(filename="puma_link3.obj")
    link2 = AsciiOBJ(filename="puma_link2.obj")
    link1 = AsciiOBJ(filename="puma_link1.obj");
except Exception as detail:
    print(detail)
    raise SystemExit("puma560gui requires files: puma_link[1-4].obj")


link4 = Translate([link4],  0, 12, 130)
link4 = Collection([link4, tool])
link4 = Color([1,0.6,0.3,1],[link4])
link4 = Translate([link4],  -64, 30, -250)
link4 = Rotate([link4], 180, 0, 0, 1)
link4 = Rotate([link4], 180, 1, 0, 0)


link3 = Rotate([link3], 180, 0, 1, 0)
link3 = Rotate([link3], 180, 1, 0, 0)
link3 = Color([1,0.6,0.3,1],[link3])
link3 = Collection([link3, link4])
link3 = HalRotate([link3], c, "joint3", 1, 0, 1, 0)
link3 = Translate([link3],  0, 73, -151)

link2 = Color([0.5,0.8,1,1],[link2])
link2 = Collection([link2, link3])
link2 = Translate([link2],  0, -45, 0)  
link2 = HalRotate([link2], c, "joint2", 1, 0, 1, 0)
link2 = Rotate([link2], -90, 1, 0, 0)
link2 = Rotate([link2], -90, 0, 1, 0)
link2 = Translate([link2],  -60, 75, 127)  

link1 = Color([0.98,0.92,0.84,1],[link1])
link1 = Translate([link1],  0, 190, 20)
link1 = Color([0.5,0.5,0.5,1],[link1])

link1 = Collection([link1, link2])  

link1 = HalRotate([link1], c, "joint1", 1, 0, 0, 1)




model = Collection([tooltip, link1, floor, work])




main(model, tooltip, work, size=1000, lat=-65, lon=-45)