import time
import cv2

def turn_90(ctrl, msg, direction="right"):
    msg.mode = 11
    msg.gait_id = 10
    msg.vel_des = [1.1, 0, -1.5 if direction == "right" else 1.5]
    msg.duration = 1500
    msg.step_height = [0.06, 0.06]
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    time.sleep(1.5)

def turn_right_180(ctrl, msg):
    msg.mode = 11
    msg.gait_id = 3
    msg.vel_des = [0.55, 0, -1.0]
    msg.duration = 3500
    msg.step_height = [0.06, 0.06]
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    time.sleep(3.5)

def go_straight(ctrl, msg, time_ms, fb=True):
    msg.mode = 11
    msg.gait_id = 26
    msg.vel_des = [1 if fb else -1, 0, 0]
    msg.duration = time_ms
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    time.sleep(time_ms / 1000)

def stand_up(ctrl, msg):
    msg.mode = 12  # Recovery stand
    msg.gait_id = 0
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    time.sleep(1.5)

def down_and_back(ctrl, msg):
    # # TAG 趴下
    msg.mode = 7
    msg.gait_id = 1
    msg.duration = 2000
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    ctrl.Wait_finish(7, 1)

    stand_up(ctrl, msg)

    go_straight(ctrl, msg, 1500, False)

def circle_90(ctrl, msg, direction="right"):
    msg.mode = 11
    msg.gait_id = 10
    msg.duration = 1000
    msg.vel_des = [0, 0, -2 if direction == "right" else 2]
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    time.sleep(1)

def run_task_1(ctrl, msg, image_processor):
    print('Running task 1...')
    
    # 右前方
    turn_90(ctrl, msg)

    # TAG take photo
    qrcode_info = image_processor.decode_qrcode()
    print(qrcode_info)

    # TODO 不同的走向
    turn_right_180(ctrl, msg)

    go_straight(ctrl, msg, 200)

    down_and_back(ctrl, msg)

    circle_90(ctrl, msg)

    # turn_90(ctrl, msg, 'left')