import math
import ik
import time


class Leg:
    def __init__(self, joints, direction, hardware):
        self.hardware = hardware
        self.joints = joints
        self.x_dir, self.y_dir = direction
        self.x = 0
        self.y = 0
        self.z = 0

    def move_to(self, x, y, z):
        angles = ik.ik(x + ik.HOME, y + ik.HOME, z - ik.TIBIA)
        for joint, angle in zip(self.joints, angles):
            radians = ik.PI2 - angle * self.x_dir * self.y_dir
            self.hardware.servo(joint, radians)
        self.x = x
        self.y = y
        self.z = z

    def move_by(self, dx=0, dy=0, dz=0):
        self.move_to(self.x + dx, self.y + dy, self.z + dz)

    def rotate_by(self, radians):
        radians *= self.x_dir * self.y_dir
        x = self.x + ik.BASE + ik.HOME
        y = self.y + ik.BASE + ik.HOME
        nx = x * math.cos(radians) - y * math.sin(radians) - ik.BASE - ik.HOME
        ny = x * math.sin(radians) + y * math.cos(radians) - ik.BASE - ik.HOME
        self.move_to(nx, ny, self.z)


class Robot:
    def __init__(self, hardware):
        self.legs = (
            Leg((0, 1, 2), (1,  -1), hardware),
            Leg((7, 6, 5), (-1,  -1), hardware),
            Leg((8, 9, 10), (-1,  1), hardware),
            Leg((15, 14, 13), (1,  1), hardware),
        )
        self.hardware = hardware

    def move_by(self, dx=0, dy=0, dz=0):
        for leg in self.legs:
            leg.move_by(dx * leg.x_dir, dy * leg.y_dir, dz)

    def rotate_by(self, radians):
        for leg in self.legs:
            leg.rotate_by(radians)

    def home(self):
        for leg in self.legs:
            leg.move_to(0, 0, 0)

    def unfold(self):
        for servo in (2, 5, 10, 13, 0, 7, 8, 15, 1, 6, 9, 14):
            self.hardware.servo(servo, ik.PI2)
            time.sleep(0.25)

    def fold(self):
        for servo, angle in (
            (2, 3 * ik.PI4),
            (5, ik.PI4),
            (10, 3 * ik.PI4),
            (13, ik.PI4),

            (1, 3 * ik.PI4),
            (6, ik.PI4),
            (9, 3 * ik.PI4),
            (14, ik.PI4),

            (7, 3 * ik.PI4),
            (0, ik.PI4),
            (8, ik.PI4),
            (15, 3 * ik.PI4),

        ):
            self.hardware.servo(servo, angle)
            time.sleep(0.25)
            self.hardware.servo(servo, None)