#!/usr/bin/env python
# -*- Python -*-

# MyRtcDir = "/home/takashi/openrtm/myRTC"

import sys
import new
import visual
from math import *

#save_path = sys.path[:]
#sys.path.append(MyRtcDir+'/tools/geo')
# from geo import *
#sys.path = save_path
#
# axis
#
class AxesXYZ(visual.frame) :
    def __init__(self, size=0.1) :
        visual.frame.__init__(self)
        self.x_axis=visual.arrow(axis=(size,0.0,0.0))
        self.x_axis.color=visual.color.red
        self.x_axis.frame=self
        self.y_axis=visual.arrow(axis=(0.0,size,0.0))
        self.y_axis.color=visual.color.green
        self.y_axis.frame=self
        self.z_axis=visual.arrow(axis=(0.0,0.0,size))
        self.z_axis.color=visual.color.yellow
        self.z_axis.frame=self
    def visible(self,vv=1) :
        self.x_axis.visible=vv
        self.y_axis.visible=vv
        self.z_axis.visible=vv
    def resize(self,size=0.1) :
        self.x_axis.length=size
        self.y_axis.length=size
        self.z_axis.length=size

#
# link
# 
class Link(visual.frame) :
    def __init__(self) :
        visual.frame.__init__(self)
        self.joint = None
        self.sc =1.0
        self.angle=0.0
#
        self.axes=AxesXYZ(0.1)
        self.axes.frame=self
    def set_joint(self, val) :
        if self.joint=="tx" :
            self.pos=(self.sc*val,0,0)
        elif self.joint=="ty" :
            self.pos=(0,self.sc*val,0)
        elif self.joint=="tz" :
            self.pos=(0,0,self.sc*val)
        elif self.joint=="rx" :
            diff = self.sc*val - self.angle
            self.angle += diff
            self.rotate(axis=(1,0,0),angle=diff)
        elif self.joint=="ry" :
            diff = self.sc*val - self.angle
            self.angle += diff
            self.rotate(axis=(0,1,0),angle=diff)
        elif self.joint=="rz" :
            diff = self.sc*val - self.angle
            self.angle += diff
            self.rotate(axis=(0,0,1),angle=diff)
#
# serial arm
#
class Arm :
    def __init__(self,dof) :
        self.dof = dof
        self.links=[]
        self.base = Link()           # base
        self.links.append(self.base)
        for i in range(dof) :
            self.links.append(Link())
        self.hand = Link()           # hand
        self.links.append(self.hand)
# 
# instanciate 3dof arm
#
arm = Arm(3)
#
# attach shapes to each links
#
arm.bh = 0.1
arm.br = 0.2
arm.base.body=visual.cylinder(axis=(0.0, 0.0, arm.bh ), radius=arm.bh)
arm.base.body.frame = arm.base
#
arm.l1r = 0.1
arm.l1h = 0.05
arm.links[1].body=visual.cylinder(axis=(0.0, 0.0, arm.l1h ), radius=arm.l1r)
arm.links[1].body.frame = arm.links[1]
arm.links[1].pos=(0,0,arm.bh)
arm.links[1].frame=arm.base
arm.links[1].joint="rz"
#
arm.l2r = 0.04
arm.l2h = 0.4
arm.links[2].body=visual.cylinder(axis=(0.0, 0.0, arm.l2h ), radius=arm.l2r)
arm.links[2].body.frame = arm.links[2]
arm.links[2].pos=(0,0, arm.l1h)
arm.links[2].frame=arm.links[1]
arm.links[2].joint="ry"
#
arm.l3r = 0.02
arm.l3h = 0.4
arm.links[3].body=visual.cylinder(axis=(0.0, 0.0, arm.l3h ), radius=arm.l3r)
arm.links[3].body.frame = arm.links[3]
arm.links[3].pos=(0,0, arm.l2h)
arm.links[3].frame=arm.links[2]
arm.links[3].joint="ry"
#
arm.hand.body=visual.box(length=0.04,height=0.04,width=0.02)
arm.hand.body.frame=arm.hand
arm.hand.pos=(0,0, arm.l3h)
arm.hand.frame=arm.links[3]

