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

import sys
import new
import visual
from math import *

#
# 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.white
        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(6)
#
# attach shapes to each links
#
arm.br = 0.17
arm.bh = 0.1215
arm.base.body=visual.cylinder(axis=(0.0, 0.0, arm.bh ), radius=arm.bh, color=(0.5,0.5,0.5))
arm.base.body.frame = arm.base
#
arm.l1r = 0.045
#arm.l1h = 0.1215
arm.l1h = 0.125
arm.links[1].body=visual.cylinder(axis=(0.0, 0.0, arm.l1h ), radius=arm.l1r, color=visual.color.white)
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.045
arm.l2h = 0.250
arm.links[2].body=visual.cylinder(axis=(0.0, 0.0, arm.l2h ), radius=arm.l2r, color=visual.color.white)
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.045
arm.l3h = 0.103
arm.links[3].body=visual.cylinder(axis=(0.0, 0.0, arm.l3h ), radius=arm.l3r, color=visual.color.white)
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.l4r = 0.045
arm.l4h = 0.09
arm.links[4].body=visual.cylinder(axis=(0.0, 0.0, arm.l4h ), radius=arm.l4r, color=visual.color.white)
arm.links[4].body.frame = arm.links[4]
arm.links[4].pos=(0,0, arm.l3h)
arm.links[4].frame=arm.links[3]
arm.links[4].joint="rz"
#
arm.l5r = 0.045
arm.l5h = 0.058
arm.links[5].body=visual.cylinder(axis=(0.0, 0.0, arm.l5h ), radius=arm.l5r, color=visual.color.white)
arm.links[5].body.frame = arm.links[5]
arm.links[5].pos=(0,0, arm.l4h)
arm.links[5].frame=arm.links[4]
arm.links[5].joint="ry"
#
arm.l6r = 0.045
arm.l6h = 0.01
arm.links[6].body=visual.cylinder(axis=(0.0, 0.0, arm.l6h ), radius=arm.l6r, color=visual.color.white)
arm.links[6].body.frame = arm.links[6]
arm.links[6].pos=(0,0, arm.l5h)
arm.links[6].frame=arm.links[5]
arm.links[6].joint="rz"
#
#arm.hand.body=visual.box(length=0.04,height=0.04,width=0.02,color=(0.5,0.5,0.5))
#arm.hand.body.frame=arm.hand
#arm.hand.pos=(0,0, arm.l6h)
#arm.hand.frame=arm.links[6]
arm.hand.ha=visual.box(length=0.0485,height=0.125,width=0.07,color=(0.5,0.5,0.5))
arm.hand.ha0=(0,0,0.045)
arm.hand.ha.pos=arm.hand.ha0
arm.hand.fr=visual.box(length=0.040,height=0.010,width=0.08,color=(0.3,0.3,0.3))
arm.hand.fr0=(0,0.04,0.125)
arm.hand.fr.pos=arm.hand.fr0
arm.hand.fl=visual.box(length=0.040,height=0.010,width=0.08,color=(0.3,0.3,0.3))
arm.hand.fl0=(0,0.05,0.125)
arm.hand.fl.pos=arm.hand.fl0
arm.hand.ha.frame=arm.hand
arm.hand.fr.frame=arm.hand
arm.hand.fl.frame=arm.hand
arm.hand.frame=arm.links[6]
arm.hand.w = 0

def set_hand_width(val):
  """arm.hand.w :[mm] -> 100*arm.hand.w [cm]"""
  try:
  	if ceil(100*arm.hand.w)>= 0 and floor(100*arm.hand.w) <= 8:
    	  print"arm.hand.w = %f" %(100*arm.hand.w)
    	  arm.hand.w = val
    	  arm.hand.fr.pos=(arm.hand.fr0[0], arm.hand.fr0[1]-arm.hand.w, arm.hand.fr0[2])
  	else:
  	  raise ValueError
  except ValueError:
  	print"Inappropriate value as a hand width!"
  	
arm.hand.method = new.instancemethod(set_hand_width,arm.hand,arm.hand.__class__)
