#!/usr/bin/python
#imports
from vismach import *
import hal
import math
import sys
import os
import numpy as np
from stl import mesh

#create the HAL component and pins
c = hal.component("AR2gui")
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("joint6", hal.HAL_FLOAT, hal.HAL_IN)
c.ready()

# "tooltip" for backplot will be the tip of the finger
tooltip = Capture()
work = Capture()

base = AsciiSTL(os.path.dirname(os.path.realpath(__file__))+"/AR2ForVismach/AssemblyJ1_link1_base-ascii.stl");
stlfile = mesh.Mesh.from_file(os.path.dirname(os.path.realpath(__file__))+"/AR2ForVismach/AssemblyJ1_link1_base-ascii.stl")
base = Color([1,0,0,0],[base]);

link1 = AsciiSTL(os.path.dirname(os.path.realpath(__file__))+"/AR2ForVismach/AssemblyJ1_link1_1-ascii.stl");
stlfile1 = mesh.Mesh.from_file(os.path.dirname(os.path.realpath(__file__))+"/AR2ForVismach/AssemblyJ1_link1_1-ascii.stl")
link1 = Color([0,0,1,0],[link1])
link1 = HalRotate([link1],c,"joint1",360,74.8133-140,134.726-150,4.0811+100)

link2 = AsciiSTL(os.path.dirname(os.path.realpath(__file__))+"/AR2ForVismach/AssemblyJ2_link2-ascii.stl");
stlfile2 = mesh.Mesh.from_file(os.path.dirname(os.path.realpath(__file__))+"/AR2ForVismach/AssemblyJ2_link2-ascii.stl")
link2 = Color([0,1,0,0],[link2]);
#link2 = Rotate([link2], 90,1,0,0)
link2 = HalRotate([link2],c,"joint2",360,52.1632-140,197.53-150,73.5975+100)

link3 = AsciiSTL(os.path.dirname(os.path.realpath(__file__))+"/AR2ForVismach/AssemblyJ3_link3-ascii.stl");
link3 = Color([1,1,1,0],[link3]);
#link3 = Translate([link3],-140,-150,100)
link3 = HalRotate([link3],c,"joint3",360,82.0-140,502.5-150,68.2+100)#to be reviewed

link4 = AsciiSTL(os.path.dirname(os.path.realpath(__file__))+"/AR2ForVismach/AssemblyJ4_link4-ascii.stl");
link4 = Color([1,0,0,0],[link4]);
#link4 = Translate([link4],-140,-150,100)
link4 = HalRotate([link4],c,"joint4",360,61.6496-140,503.034-150,109.162+100)

link5 = AsciiSTL(os.path.dirname(os.path.realpath(__file__))+"/AR2ForVismach/AssemblyJ5_link5-ascii.stl");
link5 = Color([1,1,0,0],[link5]);
#link5 = Translate([link5],-140,-150,100)
link5 = HalRotate([link5],c,"joint5",360,100.552-140,504.163-150,294.805+100)

link6 = AsciiSTL(os.path.dirname(os.path.realpath(__file__))+"/AR2ForVismach/AssemblyJGripperMount-ascii.stl");
link6 = Color([1,0,1,0],[link6]);
#link6 = Translate([link6],-140,-150,100)
link6 = HalRotate([link6],c,"joint6",360,82.9568-140,504.71-150,302.0+100)

model = Collection([base,link1,link2,link3,link4,link5,link6,work,tooltip])

main(model,tooltip, work, size=400, hud=0, lat=-45, lon=-45)