#!/usr/bin/python
# -*- coding: utf-8 -*-

from i611_MCS import *
from teachdata import *
from i611_extend import *
from rbsys import *
from i611_common import *
from i611_io import *
from i611shm import * 
import thread
import time
start=0
flag=0
def thread_func(rb):
    global flag,start
    cnt=0
    
    while True:
        # if flag==1:
        #     cnt+=1

        # if cnt>=35:
        #     # print 'cnt 310'
        #     dout(20,'0001')
        #     flag=0
        #     cnt=0
        timer = time.time()-start 
        if flag == 1 and timer >= 0.275 and timer < 1:
            dout(20,'0001')
            # rb.join()
            print time.time()-start
            flag=0
        time.sleep(0.001)



def main():
    global flag, start

    rb = i611Robot()

    _BASE = Base()

    rb.open()

    IOinit( rb )

    data = Teachdata( "teach_data" )

    th = threading.Thread(target=thread_func, args= (rb,))
    th.setDaemon(True) 
    th.start()

    rb.set_mdo( mdoid=1, portno=20, value=1, kind=1, distance=50 )
    rb.set_mdo( mdoid=2, portno=21, value=0, kind=1, distance=50 )
    
    p1 = data.get_position( "pos11", 1 )
    print(p1.pos2list())
    j2 = data.get_joint( "joint11", 0 )
    print(j2.jnt2list())
    j1 = rb.Position2Joint(p1)
    # j1 = j1.offset(dj1=-90)
    j2 = j2.offset(dj1=-90)
    p1 = rb.Joint2Position(j1)
    before_pick = p1.offset(dx=-4.5,dy=9.5, dz=50)      # 물병 잡기전 위치
    pick_bottle = p1.offset(dx=-4.5,dy=9.5, dz=-12)     # 물병 잡는 위치  (X,Y는 before_pick이랑 동일하게)
    before_flip = j2
    flip_1 = j2.offset(dj2=20, dj3=-28, dj5 = 0)
    flip_2 = flip_1.offset(dj2=10, dj3=-20, dj5=20)
    flip_3 = flip_2.offset(dj2=-20,dj3=20)
    flip_3_jnt = Joint(-89.59,11.76,-118.01,-0.05,-69.37,271.60)
    flip_3_point = rb.Joint2Position(flip_3_jnt)
    flip_3_point = flip_3_point.offset(dz=110)           # 물병 던지기 전 높이 (dz만 수정)
    flip_3_jnt = rb.Position2Joint(flip_3_point)
    flip_3_jnt = flip_3_jnt.offset(dj2=-10, dj3=10)
    flip_4 = flip_3_jnt.offset(dj2=30, dj3=-20, dj5=140)
    after_flip = flip_4.offset(dj2=10, dj5=10)
    # m = MotionParam( jnt_speed=5, lin_speed=100, pose_speed=10, overlap = 30 )
    # m_fast = MotionParam( jnt_speed=10, lin_speed=100, pose_speed=10, overlap = 30)
    m = MotionParam( jnt_speed=20, lin_speed=100, pose_speed=10, overlap = 0, acctime=0.4, dacctime=0.4)
    m_slow = MotionParam( jnt_speed=10, lin_speed=50, pose_speed=10, overlap = 0, acctime=0.4, dacctime=0.4)
    m_fast = MotionParam( jnt_speed=100, lin_speed=100, pose_speed=100, overlap = 0, acctime=0.1, dacctime=0.1)
    rb.motionparam( m )
    try:
        dout(20, '0001')
        rb.sleep(0.5)
        dout(20, '0000')
        while True:
            rb.asyncm(2)

            rb.motionparam( m )
            rb.override(100)
            # dout(20, '0001')
            rb.sleep(1)
            rb.move(before_pick)
            while din ( 9 ) == '1':
                rb.sleep(0.2)
            rb.line(pick_bottle)
            dout(20, '0010')
            rb.sleep(1)
            rb.motionparam( m_slow )
            rb.line(before_pick)
            # rb.move(before_flip)
            # rb.move(flip_1)
            # rb.move(flip_2)
            rb.move(flip_3_jnt)
            # break
            while din ( 9 ) == '0' and flag==0:
                rb.sleep(0.2)
            rb.sleep(0.5)
            # rb.enable_mdo(3)
            rb.motionparam( m_fast )
            rb.override(120)
            flag=1
            start = time.time()
            rb.asyncm(1)
            rb.move(flip_4)
            rb.motionparam( m )
            rb.move(after_flip)
            # rb.disable_mdo(3)
    except Exception as e:
        print e
    except KeyboardInterrupt:
        print 'Key Interrupt'
    finally:
        dout(20, '0000')
        rb.close()

    # # try:
    # print("start")
    
    # dout(16, '01000000')

    # print("send 7")

    # rb.sleep( sec = 2 )

    # dout(16, '10000000')

    # print("send 8")

    # rb.sleep( sec= 2 )

    # dout(16, '00010000')

    # print("send 5")

    # rb.sleep( sec= 2 )

    # dout(16, '00100000')

    # print("send 6")

    # rb.sleep( sec= 2 )

    # dout(16, '00000000')

    # print("reset")

    # except robot_emo:
    #     rb.exit(1)
    
    # finally:
    #     rb.close()
    #     rb.exit(1)


if __name__ == '__main__':
    main()