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


def 물구나무서기(dogs):

    multi_motor(dogs, FRONT_LEG, shoulder=-70, knee=60, shld_speed=50, knee_speed=50)
    multi_motor(dogs, BACK_LEG, shoulder=0, knee=-50, shld_speed=50, knee_speed=50)
    sleep(0.5)
    multi_motor(dogs, FRONT_LEG, shoulder=-90, knee=30, shld_speed=50, knee_speed=50)
    multi_motor(dogs, BACK_LEG, shoulder=0, knee=-90, shld_speed=50, knee_speed=50)
    sleep(0.5)
    multi_motor(dogs, FRONT_LEG, shoulder=0, knee=0, shld_speed=50, knee_speed=50)
    multi_motor(dogs, BACK_LEG, shoulder=-90, knee=-90, shld_speed=50, knee_speed=50)
    sleep(0.5)


def 물구나무_되돌리기(dogs):
    
    multi_motor(dogs, FRONT_LEG, shoulder=-45, knee=-45, shld_speed=50, knee_speed=50)
    multi_motor(dogs, BACK_LEG, shoulder=10, knee=-90, shld_speed=50, knee_speed=50)
    sleep(0.5)
    multi_motor(dogs, FRONT_LEG, shoulder=-70, knee=45, shld_speed=50, knee_speed=50)
    multi_motor(dogs, BACK_LEG, shoulder=10, knee=-90, shld_speed=50, knee_speed=50)
    sleep(0.5)
    multi_motor(dogs, ALL_LEG, shoulder=-45, knee=0, shld_speed=50, knee_speed=50)
    sleep(0.5)

    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(1)

if __name__ == "__main__":
    robodogs = RoboDogMultiOpen()
    물구나무서기(robodogs)
    sleep(0.5)
    for n in range(2):
        multi_motor(robodogs, BACK_LEG, shoulder=70, knee=70, shld_speed=90, knee_speed=90)
        sleep(0.5)
        multi_motor(robodogs, BACK_LEG, shoulder=-90, knee=-90, shld_speed=90, knee_speed=90)
        sleep(0.5)

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

    sleep(0.5)
    물구나무_되돌리기(robodogs)
    RoboDogMultiClose(robodogs)