import time
import cv2

def run_task_1(ctrl, msg, image_processor):
    print('Running task 1...')
    
    # 右前方
    msg.mode = 11
    msg.gait_id = 26  # 26 表示快速 trot 步态
    msg.vel_des = [0.5, 0.5, -1.0]
    msg.duration = 1800
    msg.step_height = [0.06, 0.06]
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    time.sleep(1.8)

    msg.mode = 11
    msg.gait_id = 26
    msg.vel_des = [1, 0, 0]
    msg.duration = 200
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    time.sleep(0.2)

    # TAG take photo
    image = image_processor.get_current_image()
    cv2.imwrite('photo.jpg', image)

    # msg.mode = 11
    # msg.gait_id = 26
    # msg.vel_des = [1, 0, 0]
    # msg.duration = 1000
    # msg.life_count += 1
    # ctrl.Send_cmd(msg)
    # time.sleep(1.0)