#Controlling Drawing Arm with mouse
import math
import time
import math
import threading
import sys
import numpy
import os
import time
import ctypes

if os.name == 'nt':
    import msvcrt
    def getch():
        return msvcrt.getch().decode()
else:
    import sys, tty, termios
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    def getch():
        try:
            tty.setraw(sys.stdin.fileno())
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch

import dynamixel_functions as dynamixel                     # Uses Dynamixel SDK library

# DynamixelSDK parameter
PROTOCOL_VERSION            = 1                             # See which protocol version is used in the Dynamixel
BAUDRATE                    = 1000000    #57600
DEVICENAME                  = "/dev/ttyUSB0".encode('utf-8')
TORQUE_ENABLE               = 1                             # Value for enabling the torque
TORQUE_DISABLE              = 0                             # Value for disabling the torque position value is out of movable
ESC_ASCII_VALUE             = 0x1b
COMM_SUCCESS                = 0                             # Communication Success result value
COMM_TX_FAIL                = -1001                         # Communication Tx Failed
ADDR_GOAL_POS               = 30
ADDR_TORQUE_ENABLE          = 24
ADDR_MOVING_SPEED           = 32
ADDR_PRESENT_POS            = 36
ADDR_FSR_1                  = 26
ADDR_FSR_2                  = 28
ADDR_FSR_3                  = 30
ADDR_FSR_4                  = 32
ADDR_FSR_X                  = 34
ADDR_FSR_Y                  = 35
LEN_GOAL_POSITION           = 2

# Initialize ids
motor_ids = range(1,4)
fsr_id = 112
# Initialize PortHandler and PacketHandler Structs
port_num = dynamixel.portHandler(DEVICENAME)
dynamixel.packetHandler()

# Initialize Groupsyncwrite and groupBulkRead instance
group_num = dynamixel.groupBulkRead(port_num, PROTOCOL_VERSION)
groupwrite = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_GOAL_POS, LEN_GOAL_POSITION)

dxl_comm_result = COMM_TX_FAIL                              # Communication result
dxl_error = 0                                               # Dynamixel error
dxl_present_position = 0                                    # Present position

# Open port
if dynamixel.openPort(port_num):
    print("Succeeded to open the port!")
else:
    print("Failed to open the port!")
    print("Press any key to terminate...")
    getch()
    quit()

# Set port baudrate
if dynamixel.setBaudRate(port_num, BAUDRATE):
    print("Succeeded to change the baudrate!")
else:
    print("Failed to change the baudrate!")
    print("Press any key to terminate...")
    getch()
    quit()

def enable_torque(ids):
    for id in ids:
        dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
        dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
        dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
        if dxl_comm_result != COMM_SUCCESS:
            print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
        elif dxl_error != 0:
            print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))


atos= 4096/360
angle1=int(22.5*atos)+2048
angle2=int(45*atos)+2048
angle3=int(22.5*atos)+2048
angle4=int(60*atos)+2048
angle5=int(120*atos)+2048
angle6=int(60*atos)+2048

# Add parameter storage for Dynamixel present position value
for id in motor_ids:
    dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupBulkReadAddParam(group_num, id, ADDR_PRESENT_POS, LEN_GOAL_POSITION)).value
    if dxl_addparam_result != 1:
        print("[ID:%03d] groupBulkRead addparam failed" % (id))
        quit()

dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupBulkReadAddParam(group_num, fsr_id, ADDR_FSR_Y, LEN_GOAL_POSITION)).value
if dxl_addparam_result != 1:
    print("[ID:%03d] groupBulkRead addparam failed" % (fsr_id))
    quit()

def set_moving_speed(ids, speeds):
    for id, speed in zip(ids,speeds):
        dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MOVING_SPEED, speed)
        dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
        dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
        if dxl_comm_result != COMM_SUCCESS:
            print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
        elif dxl_error != 0:
            print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))


def sync_write(ids, positions):
    for id, pos in zip(ids, positions):
        # Add dxl goal position value to the Syncwrite storage
        addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite, id, pos, LEN_GOAL_POSITION)).value
        if addparam_result != 1:
            print("[ID:%03d] groupSyncWrite addparam failed" % (id))
            quit()
    # Syncwrite goal position
    dynamixel.groupSyncWriteTxPacket(groupwrite)
    # Clear syncwrite parameter storage
    dynamixel.groupSyncWriteClearParam(groupwrite)


def slideloop():
    file = open('y_zmp.txt', 'w')
    set_moving_speed(motor_ids, [150, 300, 150])
    sync_write(motor_ids, [2048, 2048, 2048])
    time.sleep(1)
    while True:
        pos = []
        # Bulkread present position and moving status
        dynamixel.groupBulkReadTxRxPacket(group_num)
        dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
        for id in motor_ids:
            dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(group_num, id, ADDR_PRESENT_POS, LEN_GOAL_POSITION)).value
            pos.append(dynamixel.groupBulkReadGetData(group_num, id, ADDR_PRESENT_POS, LEN_GOAL_POSITION))
        dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(group_num, fsr_id, ADDR_FSR_Y, LEN_GOAL_POSITION)).value
        pos.append(dynamixel.groupBulkReadGetData(group_num, fsr_id, ADDR_FSR_Y, LEN_GOAL_POSITION))
        print pos
        y_zmp = pos[-1]
        file.write(str(y_zmp)+ "\n")
        if y_zmp <= 65:
            angles = [2730, 3413, 2730]
            sync_write(motor_ids, angles)

slide = threading.Thread(name='slideloop', target=slideloop)
slide.start()