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

height = 55

shoulder = [height, height, height, height]
knee = [0, 0, 0, 0]
symmetry = [2, 3, 0, 1]

def leg_apply(delay=1, speed1=40, speed2=60):
    for n in range(4):
        robodog.leg(n, shoulder[n], knee[n], speed1, speed2)
    sleep(delay)

def leg_step(what):
    shoulder[what] -= 30
    leg_apply(0.2, 90, 90)
    knee[what] -= 50
    leg_apply(0.2, 90, 90)
    shoulder[what] += 30
    leg_apply(0.2, 90, 90)



def leg_motion(what):
    shoulder[what] += 30
    shoulder[symmetry[what]] -= 20
    leg_apply(0.5)
    shoulder[what] -= 30
    leg_step(what)
    shoulder[symmetry[what]] += 20
    leg_apply(0.5)
    
def body_forward(how):
    for n in range(4):
        knee[n] += how
    leg_apply(0.3)


if __name__ == "__main__":
    robodog = RoboDog()
    robodog.Open()
    jkey = JKeyEvent()  

    leg_apply()
    while not jkey.isKeyEscPressed():
        leg_motion(LEFT_FRONT)
        leg_motion(RIGHT_FRONT)
        body_forward(50)
        leg_motion(LEFT_BACK)
        leg_motion(RIGHT_BACK)

        [_, pitch] = robodog.get_tilt()
        slope = int(math.tan(math.radians(pitch))*55)
        slope = DefLib.constrain(slope, -10, 10)
        shoulder[LEFT_FRONT] = height-slope
        shoulder[RIGHT_FRONT] = height-slope
        shoulder[LEFT_BACK] = height+slope
        shoulder[RIGHT_BACK] = height+slope

    robodog.Close()