import os
import time
from dynamixel_sdk import * 

 
# Control table address for AX-12A (Protocol 1.0)
ADDR_MX_LED = 25                               # LED address
ADDR_MX_GOAL_POSITION = 30                     # Goal position address
ADDR_MX_PRESENT_POSITION = 36                  # Present position address
ADDR_MX_MOVING_SPEED = 32                      # Moving speed towards goal pos

BAUDRATE = 1000000                             # Baudrate of Dynamixel
DEVICENAME = '/dev/ttyUSB0'                    # Port name (adjust based on your setup, e.g., '/dev/ttyUSB0' for Linux)
PROTOCOL_VERSION = 1.0                         # Version of protocol type to use/search

# Initialize PortHandler for managing port operations, and PacketHandler for managing protocol operations
portHandler = PortHandler(DEVICENAME)
packetHandler = PacketHandler(PROTOCOL_VERSION)

# Open port
if portHandler.openPort():
   print("Port opened successfully")
else:
   print("Failed to open the port")
   quit()


# Set port baudrate 
if portHandler.setBaudRate(BAUDRATE):
   print("Baudrate set successfully")
else:
   print("Failed to set baudrate")
   quit()

def led_blink(idx):
    dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, idx, ADDR_MX_LED, 1)
    if dxl_comm_result != COMM_SUCCESS:
       print(f"Error: {packetHandler.getTxRxResult(dxl_comm_result)}")
    elif dxl_error != 0:
       print(f"Error: {packetHandler.getRxPacketError(dxl_error)}")
    else:
       print(f"LED turned ON for {idx}")

    time.sleep(0.5)  # Wait 0.5 seconds

    # Turn LED OFF
    dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, idx, ADDR_MX_LED, 0)
    if dxl_comm_result != COMM_SUCCESS:
      print(f"Error: {packetHandler.getTxRxResult(dxl_comm_result)}")
    elif dxl_error != 0:
      print(f"Error: {packetHandler.getRxPacketError(dxl_error)}")
    else:
      print(f"LED turned OFF for {idx}")

    time.sleep(0.5)  

def set_speed(idx, speed):
    dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, idx, ADDR_MX_MOVING_SPEED, speed)
    if dxl_comm_result != COMM_SUCCESS:
        print(f"Error: {packetHandler.getTxRxResult(dxl_comm_result)}")
    elif dxl_error != 0:
        print(f"Error: {packetHandler.getRxPacketError(dxl_error)}")
    else:
        print(f"Movement speed for {idx}, set to {speed}")

def set_goal_pos(idx, goal_pos):
    dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, idx, ADDR_MX_GOAL_POSITION, goal_pos)
    if dxl_comm_result != COMM_SUCCESS:
        print(f"Error: {packetHandler.getTxRxResult(dxl_comm_result)}")
    elif dxl_error != 0:
        print(f"Error: {packetHandler.getRxPacketError(dxl_error)}")
    else:
        print(f"Goal position for {idx}, set to {goal_pos}")

# LED BLINK TEST TO CHECK CONNECTION
#for i in range(1,7):
    #led_blink(i)



# Set goal position (range is 0 to 1023 for AX-18)
DEFAULT_POSITION = 512  # (Define a goal position)
DEFAULT_SPEED = 100

if (input("Want to reset positions and speeds? [Y/N]").upper() == "Y"):
    time.sleep(4)
    for i in range(1,7):
        set_speed(i, DEFAULT_SPEED)
        set_goal_pos(i, DEFAULT_POSITION)

input("Ready to continue?")

RIGHT_SUPPORT_HIP = 490
RIGHT_SUPPORT_KNEE = 512
RIGHT_SUPPORT_ANKLE = 495
RIGHT_SWING_HIP = 650
RIGHT_SWING_KNEE = 358
RIGHT_SWING_ANKLE = 546

LEFT_SUPPORT_HIP = 530
LEFT_SUPPORT_KNEE = 512
LEFT_SUPPORT_ANKLE = 529
LEFT_SWING_HIP = 370
LEFT_SWING_KNEE = 666
LEFT_SWING_ANKLE = 478

N_STEPS = 50

for i in range(N_STEPS):


    ####################FIRST STEP#########################

    # LEFT LEG SWING PHASE:
    # 1: MOVE HIP FORWARD
    # 2: MOVE ANKLE DOWN TO PREPARE FOR 'LANDING'
    # 3: BEND KNEE TO ALSO PREPARE FOR LANDING.
    set_goal_pos(2, LEFT_SWING_KNEE) # Knee
    set_goal_pos(3, LEFT_SWING_ANKLE) # Ankle
    time.sleep(.5)
    set_goal_pos(1, LEFT_SWING_HIP) # Hip



    # RIGHT LEG SUPPORT PHASE:
    # 1: ROTATE HIP BACKWARD
    # 2: KNEE STAYS STRAIGT (MIGHT CHANGE TO BEND FOR SUPPORT)
    # 3: ANKLE TILT DOWN(BACK) TO PUSH OFF
    set_goal_pos(4, RIGHT_SUPPORT_HIP)  # Right Hip slightly extended
    set_goal_pos(5, RIGHT_SUPPORT_KNEE)  # Right knee straight
    set_goal_pos(6, RIGHT_SUPPORT_ANKLE)  # Right ankle slightly plantarflexed

    time.sleep(1.7) # WAIT FOR STEP TO FINISH

    ###################SWITCH ROLES#########################

    # RIGHT LEG SWING PHASE:
    # 1: MOVE HIP FORWARD
    # 2: MOVE ANKLE DOWN TO PREPARE FOR 'LANDING'
    # 3: BEND KNEE TO ALSO PREPARE FOR LANDING.
    set_goal_pos(5, RIGHT_SWING_KNEE) # Knee
    set_goal_pos(6, RIGHT_SWING_ANKLE) # Ankle
    time.sleep(.5)
    set_goal_pos(4, RIGHT_SWING_HIP) # Hip
    


    # LEFT LEG SUPPORT PHASE:
    # 1: ROTATE HIP BACKWARD
    # 2: KNEE STAYS STRAIGT (MIGHT CHANGE TO BEND FOR SUPPORT)
    # 3: ANKLE TILT DOWN(BACK) TO PUSH OFF
    set_goal_pos(1, LEFT_SUPPORT_HIP)  # Right Hip slightly extended
    set_goal_pos(2, LEFT_SUPPORT_KNEE)  # Right knee straight
    set_goal_pos(3, LEFT_SUPPORT_ANKLE)  # Right ankle slightly plantarflexed

    time.sleep(1.7) # WAIT FOR STEP TO FINISH




input("Finished?")

# Close port
portHandler.closePort()
print("Port closed")