#!/usr/bin/python3
#    Derived from a work by John Kasunich and Jeff Epler
#
#    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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA


from vismach import *
import hal
import math
import sys

import linuxcnc
s = linuxcnc.stat()
s.poll()

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

work = Capture()
tool = Capture()
tooltip = Capture()



# kinematic axis
# axis_y -> axis_x -> axis_z  -> tool
# base -> work


# tool

tool = Collection([tooltip, tool])


# axis z

axisz = AsciiOBJ(filename="headz.obj")
axisz = Color([1,1,1,1],[axisz])
axisz = Rotate([axisz],90,1,0,0)
axisz = Translate([axisz],0,0,133)
axisz = Collection([axisz, tool])
axisz = Translate([axisz],0,0,-77)
axisz = HalTranslate([axisz],c,"joint2",0,0,1)

# axis x

head = AsciiOBJ(filename="head.obj")
head = Color([0.5,0.5,1,1],[head])
head = Rotate([head],90,1,0,0)
axisx = Collection([axisz, head])
axisx = HalTranslate([axisx],c,"joint0",1,0,0)

# axis y

rack = AsciiOBJ(filename="gantri.obj")
rack = Color([0.7,0.7,0.4,0.4],[rack])
rack = Rotate([rack],90,1,0,0)
axisy = Collection([axisx, rack])
axisy = HalTranslate([axisy],c,"joint1",0,1,0)

machine = Collection([axisy])



# base

rangka = AsciiOBJ(filename="rangka.obj")
rangka = Color([1,1,1,1],[rangka])
rangka = Rotate([rangka],90,1,0,0)
bed = AsciiOBJ(filename="bed.obj")
bed = Color([1,0.8,0.2,0.2],[bed])
bed = Rotate([bed],90,1,0,0)
atc = AsciiOBJ(filename="atc.obj")
atc = Color([0.5,0.5,0.5,0.5],[atc])
atc = Rotate([atc],90,1,0,0)

work = Capture()

base = Collection([rangka, bed, atc, work])

model = Collection([base, machine])

main(model, tooltip, work, 5000, lat=-60, lon=-45)