from time import sleep
from pyjuni.robodog import *
from pyjuni.deflib import *


def 앉기(dogs):
    multi_leg(dogs, ALL_LEG, height=60, step=0, height_speed=100, step_speed=100)
    sleep(0.5)
    multi_motor(dogs, FRONT_LEG, shoulder=0, knee=-90, shld_speed=50, knee_speed=50)
    multi_motor(dogs, BACK_LEG, shoulder=20, knee=-50, shld_speed=50, knee_speed=50)
    sleep(0.5)
    multi_motor(dogs, FRONT_LEG, shoulder=0, knee=-90, shld_speed=100, knee_speed=100)
    multi_motor(dogs, BACK_LEG, shoulder=20, knee=-80, shld_speed=10, knee_speed=10)
    sleep(0.5)
    multi_motor(dogs, FRONT_LEG, shoulder=-75, knee=-45, shld_speed=50, knee_speed=50)
    multi_motor(dogs, BACK_LEG, shoulder=5, knee=-80, shld_speed=10, knee_speed=10)
    sleep(0.5)

def 앉기_되돌리기(dogs):
    multi_motor(dogs, FRONT_LEG, shoulder=5, knee=-90, shld_speed=100, knee_speed=100)
    multi_motor(dogs, BACK_LEG, shoulder=40, knee=-90, shld_speed=20, knee_speed=20)
    sleep(1)

    for n in range(len(dogs)):
        degree = dogs[n].get_rotation()
        degree = 360 if degree > 180 else -360 if degree < -180 else 0 
        dogs[n].rotate(degree)
    sleep(2)

if __name__ == "__main__":
    robodogs = RoboDogMultiOpen()

    앉기(robodogs)

    sleep(0.5)
    for n in range(2):
        multi_motor(robodogs, FRONT_LEG, shoulder=-70, knee=70, shld_speed=90, knee_speed=90)
        sleep(0.5)
        multi_motor(robodogs, FRONT_LEG, shoulder=90, knee=-90, shld_speed=90, knee_speed=90)
        sleep(0.5)

    for n in range(2):
        multi_motor(robodogs, FRONT_LEG, shoulder=90, knee=70, shld_speed=40, knee_speed=90)
        sleep(0.5)
        multi_motor(robodogs, FRONT_LEG, shoulder=0, knee=-90, shld_speed=40, knee_speed=90)
        sleep(0.5)
    
    앉기_되돌리기(robodogs)
    RoboDogMultiClose(robodogs)