#!/usr/bin/python

from vismach import *
import hal
import math
import sys


c = hal.component("irbgui")
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)
#c.newpin("tool_length", hal.HAL_FLOAT, hal.HAL_IN)
c.ready()


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

work = Capture()

#tool goes here.. maybe later
tool = Capture()

# "tooltip" for backplot will be the tip of the tool, for now link7
tooltip = Capture()
tool = Collection([tooltip, tool])

# link 5
link5 = AsciiOBJ(filename="/home/asea/linuxcnc/configs/irb/5.obj")
link5 = Color([0.5,0.5,0.5,1],[link5])
#link5 = Rotate([link5],90,0,1,0)
#link5 = Translate([link5],745,0,1260)

link5 = Translate([link5],0,0,-20)
link5 = Collection([link5, tool])
link5 = Translate([link5],0,0,20)

link5 = HalRotate([link5],c,"joint4",1,0,0,1)


# link 4
link4 = AsciiOBJ(filename="/home/asea/linuxcnc/configs/irb/4.obj")
link4 = Color([0.5,0.5,0.5,1],[link4])
link4 = Rotate([link4],-90,0,1,0)
#link4 = Translate([link4],670,20,1260) 
link4 = Translate([link4],20,0,-75) 

link4 = Collection([link5, link4])
#apply HAL DOF
link4 = Translate([link4],0,0,75) 

link4 = HalRotate([link4],c,"joint3",1,1,0,0)


# link 3
link3 = AsciiOBJ(filename="/home/asea/linuxcnc/configs/irb/3.obj")
link3 = Color([0.5,0.5,0.5,1],[link3])
link3 = Rotate([link3],90,0,1,0)
#link3 = Translate([link3],50,50,1210)
link3 = Translate([link3],-50,-50,-50)
link3 = Collection([link4, link3])
#apply HAL DOF
link3 = Translate([link3],0,0,670)
link3 = Rotate([link3],-90,1,0,0)

link3 = HalRotate([link3],c,"joint2",1,1,0,0)


# link 2
link2 = AsciiOBJ(filename="/home/asea/linuxcnc/configs/irb/2.obj")
link2 = Color([0.5,0.5,0.5,1],[link2])
#link2 = Rotate([link2],90,0,0,1)
#link2 = Translate([link2],50,-95,750)
link2 = Translate([link2],-95,-50,-450)

link2 = Collection([link3, link2])

link2 = Translate([link2],0,0,450)

#apply HAL DOF
link2 = HalRotate([link2],c,"joint1",1,1,0,0)


# link 1
link1 = AsciiOBJ(filename="/home/asea/linuxcnc/configs/irb/1.obj")
link1 = Color([0.5,0.5,0.5,1],[link1])
#link1 = Translate([link1],0,0,110)
link1 = Rotate([link1],90,0,0,1)
link1 = Translate([link1],0,0,-590)

#assemble
link1 = Collection([link2, link1])
#apply 
link1 = Translate([link1],0,0,590)
link1 = HalRotate([link1],c,"joint0",1,0,0,1)



# base
base = AsciiOBJ(filename="/home/asea/linuxcnc/configs/irb/base.obj")
base = Color([0.5,0.5,0.5,1],[base])
base = Rotate([base],90,1,0,0)
base = Translate([base],-205,205,-110)
base = Collection([link1, base])
base = Translate([base],0,0,110)

#apply HAL DOF
#link2 = HalRotate([link2],c,"joint1",1,0,0,1)


# stationary base
#irb6 = Collection([link1, base])
model = Collection([tooltip, base, floor, work])


main(model, tooltip, work,1000)