# -*- coding: utf-8 -*-

import math

from servos import Servos
from machine import I2C, Pin


PI2 = math.pi / 2
PI4 = math.pi / 4
SQRT2 = math.sqrt(2)


def _solve_triangle(a, b, c):
    a, b, c = abs(a), abs(b), abs(c)
    if a + b < c or a + c < b or b + c < a:
        raise ValueError("Impossible triangle: %r, %r, %r" % (a, b, c))
    cos = float(a ** 2 + b ** 2 - c ** 2) / (2 * a * b)
    return math.acos(cos)


def _norm(a, b):
    return math.sqrt(a ** 2 + b ** 2)


class Leg:
    # Lengths of leg segments, in mm
    coxa = 12.3     # between the hip and the knee
    femur = 39.0    # between the knee and the ankle
    tibia = 55.0    # between the ankle and the foot
    base = 21.0     # from the center of body

    def __init__(self, joints, direction, servos):
        self.servos = servos
        self.joints = joints
        self.x_dir, self.y_dir = direction
        self.home = (self.coxa + self.femur) / math.sqrt(2)
        self.x = 0
        self.y = 0
        self.z = 0

    def move_to(self, x, y, z):
        print("Moving leg %d, %d to %d, %d, %d" %
              (self.x_dir, self.y_dir, x, y, z))
        x += self.home
        y += self.home
        z -= self.tibia
        f = _norm(x, y) - self.coxa
        d = _norm(f, z)
        if d > self.femur + self.tibia:
            raise ValueError("Out of reach")
        hip = PI4 - math.atan2(y, x)
        knee = _solve_triangle(self.femur, d, self.tibia) - math.atan2(-z, f)
        ankle = PI2 - _solve_triangle(self.femur, self.tibia, d)
        for joint, angle in zip(self.joints, (hip, knee, ankle)):
            self.servos.position(
                joint,
                radians=PI2 - angle * self.x_dir * self.y_dir
            )
        self.x = x - self.home
        self.y = y - self.home
        self.z = z + self.tibia

    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):
        x = self.x + self.base
        y = self.y + self.base
        nx = x * math.cos(radians) - y * math.sin(radians) - self.base
        ny = x * math.sin(radians) + y * math.cos(radians) - self.base
        self.move_to(nx, ny, self.z)


class Robot:
    def __init__(self):
        i2c = I2C(-1, Pin(5, Pin.OUT, Pin.PULL_UP),
                  Pin(4, Pin.OUT, Pin.PULL_UP), freq=100000)
        self.servos = Servos(i2c)
        self.legs = (
            Leg((0, 1, 2), (1,  -1), self.servos),
            Leg((7, 6, 5), (-1,  -1), self.servos),
            Leg((8, 9, 10), (-1,  1), self.servos),
            Leg((15, 14, 13), (1,  1), self.servos),
        )

    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 * leg.x_dir * leg.y_dir)

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