import utime


class Creep:
    speed = 2
    stride = 36
    shift = 12
    shift_steps = 2
    raise_leg = 10
    raise_steps = 2
    next_leg = (
        (2, 0, 3, 1),   # forward
        (1, 3, 0, 2),   # backward
        (1, 2, 3, 0),   # clockwise
        (3, 0, 1, 2),   # counter-clockwise
        (2, 3, 1, 0),   # left
        (3, 2, 0, 1),   # right
    )

    def __init__(self, robot):
        self.robot = robot
        self.shift_x = 0
        self.shift_y = 0
        self.on_ground = list(robot.legs)
        self.dx = 1
        self.dy = 0
        self.rotation = 0
        self.spread = 20
        leg = robot.legs[0]
        self.height = 20
        self.last_leg = 0

    def move(self):
        print("Moving legs backwards")
        for leg in self.on_ground:
            dx = self.dx * leg.x_dir * self.speed
            dy = self.dy * leg.y_dir * self.speed
            leg.move_to(leg.x - dx, leg.y - dy, self.height)
            #leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir / 2)
        utime.sleep_ms(10)

    def step(self, leg):
        dx = (leg.x_dir * self.shift - self.shift_x) / self.shift_steps
        dy = (leg.y_dir * self.shift - self.shift_y) / self.shift_steps
        print("Shifting body by (%d, %d) %d times" % (dx, dy, self.shift_steps))
        for step in range(self.shift_steps):
            self.robot.move_by(dx, dy, 0)
            self.shift_x += dx
            self.shift_y += dy
            self.move()
        self.on_ground.remove(leg)
        print("Raising leg by %d %d times" % (self.raise_leg, self.raise_steps))
        for step in range(self.raise_steps):
            leg.move_by(0, 0, self.raise_leg)
            self.move()
        print("Moving leg forward")
        leg.move_to(
            (self.shift_x + self.dx * self.stride) * leg.x_dir + self.spread,
            (self.shift_y + self.dy * self.stride) * leg.y_dir + self.spread,
            self.raise_leg * self.raise_steps + self.height
        )
        self.move()
        self.move()
        print("Lowering leg")
        for step in range(self.raise_steps - 1):
            leg.move_by(0, 0, -self.raise_leg)
            self.move()
        self.on_ground.append(leg)
        self.move()
        print("Leg done")

    def run(self, steps):
        for step in range(steps):
            print("Moving leg %d" % self.last_leg)
            leg = self.robot.legs[self.last_leg]
            self.step(leg)
            self.last_leg = self.next_leg[0][self.last_leg]