import machine
import servo
import time

i2c = machine.I2C(scl=machine.Pin(22), sda=machine.Pin(21))

# Tower pro MG90s
servos = servo.Servos(i2c, address=0x40, freq=50, min_us=600, max_us=2400, degrees=180)

INIT_POSITIONS = [
    (0, 40),
    (1, 90),
    (2, 130),
    (3, 90),
    (4, 60),
    (5, 90),
    (6, 130),
    (7, 90)
]


def init_legs():
    for i, a, in INIT_POSITIONS:
        servos.position(i, a)


def tip_forward(pause=1):
    servos.position(7, 150)
    servos.position(1, 30)
    time.sleep(pause)
    init_legs()


def tip_back(pause=1):
    servos.position(3, 150)
    servos.position(5, 30)
    time.sleep(pause)
    init_legs()


def lean_left(pause=1):
    servos.position(7, 150)
    servos.position(5, 30)
    servos.position(3, 60)
    servos.position(1, 120)
    time.sleep(pause)
    init_legs()


def lean_left(pause=1):
    servos.position(7, 150)
    servos.position(5, 30)
    servos.position(3, 60)
    servos.position(1, 120)
    time.sleep(pause)
    init_legs()


def lean_right(pause=1):
    servos.position(7, 60)
    servos.position(5, 120)
    servos.position(3, 150)
    servos.position(1, 30)
    time.sleep(pause)
    init_legs()


def look_left(pause=1):
    servos.position(0, 10)
    servos.position(2, 100)
    servos.position(4, 30)
    servos.position(6, 100)
    time.sleep(pause)
    init_legs()


def look_right(pause=1):
    servos.position(0, 70)
    servos.position(2, 160)
    servos.position(4, 90)
    servos.position(6, 160)
    time.sleep(pause)
    init_legs()


def all_move(pause=1):
    moves = (
    tip_forward, tip_back, lean_left, lean_right, look_left, look_right, fall, wave, turn_left, turn_right, turn_right,
    turn_left)
    for move in moves:
        move(pause=pause)
        time.sleep(0.5)


def turn_left(pause=None):
    servos.position(0, 10)
    servos.position(2, 100)
    servos.position(4, 30)
    servos.position(6, 100)
    time.sleep(0.1)
    servos.position(7, 120)
    servos.position(6, 160)
    time.sleep(0.1)
    servos.position(7, 90)
    time.sleep(0.1)
    servos.position(3, 120)
    servos.position(2, 160)
    time.sleep(0.1)
    servos.position(3, 90)
    time.sleep(0.1)
    servos.position(5, 30)
    servos.position(4, 90)
    time.sleep(0.1)
    servos.position(5, 90)
    time.sleep(0.1)
    servos.position(1, 30)
    servos.position(0, 90)
    time.sleep(0.1)
    servos.position(1, 90)
    time.sleep(0.1)
    init_legs()


def turn_right(pause=None):
    servos.position(0, 70)
    servos.position(2, 160)
    servos.position(4, 90)
    servos.position(6, 160)
    time.sleep(0.1)
    # front left
    servos.position(1, 30)
    servos.position(0, 10)
    time.sleep(0.1)
    servos.position(1, 90)
    time.sleep(0.1)
    # back right
    servos.position(5, 30)
    servos.position(4, 30)
    time.sleep(0.1)
    servos.position(5, 90)
    time.sleep(0.1)
    # front right
    servos.position(7, 120)
    servos.position(6, 60)
    time.sleep(0.1)
    servos.position(7, 90)
    time.sleep(0.1)
    # back left
    servos.position(3, 120)
    servos.position(2, 60)
    time.sleep(0.1)
    servos.position(3, 90)
    time.sleep(0.1)

    init_legs()


def wave(pause=None, times=3):
    counter = times
    while counter:
        servos.position(7, 180)
        time.sleep(0.1)
        servos.position(7, 160)
        time.sleep(0.1)
        counter -= 1
    servos.position(7, 90)
    init_legs()


def fall(pause=1):
    servos.position(3, 160)
    servos.position(1, 30)
    servos.position(5, 30)
    servos.position(7, 160)
    time.sleep(pause)
    init_legs()


init_legs()