#!/usr/bin/python
# -*- coding: utf-8 -*-
## 1. 초기 설정① #######################################
# 라이브러리 가져오기
## 1．초기 설정 ①　모듈 가져오기 ######################
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

def thread(rb):
    try:
        while True:
            Joint_list = shm_read( 0x3050, 6 ).split( ',' ) 
            Joint_list[0] = math.degrees(float(Joint_list[0])) # Joint 1 위치
            Joint_list[1] = math.degrees(float(Joint_list[1])) # Joint 2 위치
            Joint_list[2] = math.degrees(float(Joint_list[2])) # Joint 3 위치
            Joint_list[3] = math.degrees(float(Joint_list[3])) # Joint 4 위치
            Joint_list[4] = math.degrees(float(Joint_list[4])) # Joint 5 위치
            Joint_list[5] = math.degrees(float(Joint_list[5])) # Joint 6 위치

            # print 'Joint',Joint_list[0],Joint_list[1],Joint_list[2],Joint_list[3],Joint_list[4],Joint_list[5]
            rb.sleep(0.1) 
    except Robot_emo:
        rb.exit(1)

def main():
    ## 2. 초기 설정② ####################################
    # ZERO 로봇 생성자
    rb = i611Robot()
    # 좌표계의 정의
    _BASE = Base()
    # 로봇과 연결 시작 초기화
    rb.open()
    # I/O 입출력 기능의 초기화 
    IOinit( rb )
    # 교시 데이터 파일 읽기
    data = Teachdata( "teach_data" )
    # Thread 시작
    th = threading.Thread(target=thread, args= (rb,))
    th.setDaemon(True) 
    th.start()
    rb.home()
    ## 1. 교시 포인트 설정 ######################
    # p1 = data.get_position( "pos1", 0 )
    # p2 = data.get_position( "pos1", 1 )
    ## 2. 동작 조건 설정 ######################## 
    m = MotionParam( jnt_speed=23, lin_speed=23, pose_speed=23, overlap = 30 ,posture = 2)
    #MotionParam 형으로 동작 조건 설정
    rb.motionparam( m )
    
    #소켓 설정 
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 1)
    sock.connect(('192.168.0.25',5000))
    Send_Data = 'OK'
    send_data_fail = 'fail'
    ## 3. 로봇 동작을 정의 ##############################
    # 작업 시작
    
    try:
        while True:
            
            Socket_data = sock.recv(65535) 
            Socket_data = Socket_data.decode()
            print Socket_data.decode()
            
            try:
                pos = map(float, Socket_data.decode().split(','))
                p1 = Position(pos[0], pos[1], 300,-90.17,0.15,179.85)
                rb.move(p1)
                
                Send_Data = Send_Data.encode()
                sock.send(Send_Data)
            except e:
                print(e)
                
                
            # Socket_data = sock.recv(65535) 
            # Socket_data = Socket_data.decode()
            # print Socket_data  
            
            
            # try:
            #     pos = Socket_data.split('.')

            # if Socket_data == 'A':
            #     rb.move(p1)
            #     Send_Data = Send_Data.encode()
            #     sock.send(Send_Data) 

            # elif Socket_data == 'B':   
            #     rb.move(p2)
            #     Send_Data = Send_Data.encode()
            #     sock.send(Send_Data) 
            # elif Socket_data == 'H':
            #     rb.home()
            #     Send_Data = Send_Data.encode()
            #     sock.send(Send_Data) 
            # else:
            #     sock.send(send_data_fail.encode()) 
                
                
    except Robot_emo:
        rb.close() 
        rb.exit(1)

    finally:
        rb.close()
        rb.exit(1)

 
    ## 4. 종료 ######################################
    # 로봇과의 연결을 종료
    rb.close()
if __name__ == '__main__':
    main()