import logging

import numpy as np
from math import *
from logs import *
from dynamixel_sdk import *
from xbox import *
import time

# DEBUG
LEGS_LOG_LEVEL = logging.WARNING
CONTROLLER_LOG_LEVEL = logging.CRITICAL
# Variables configurations

l1h = 0.049
l1v = 0.032
l1 = l1h  # this is not the real distance as it's not the one needed to calculate position.

# length between motor 2 and motor 3.
l2h = 0.0605
l2v = 0.02215
l2 = sqrt(l2h ** 2 + l2v ** 2)

# length between motor 3 and end of the leg.
l3h = 0.012
l3v = 0.093
l3 = sqrt(l3h ** 2 + l3v ** 2)

# offset of the 'head', the legs isolated at each end.
tete_x = 0.095

# offset of the legs at the side.
patte_y = 0.032
patte_x = 0.079
num_patte = 6

# Logs functions
legsLogger = setup_logger("legs", LEGS_LOG_LEVEL)
controllerLogger = setup_logger("Controller", CONTROLLER_LOG_LEVEL)

# Initialize controller
xbox = Xbox(controllerLogger)
CONTROLLER_MODE = xbox.initialized
xbox.mode_count = 5

neutral_position = np.array([
    [0.1, 0.16, -0.15],
    [-0.1, 0.16, -0.15],
    [-0.2, -0.00, -0.15],
    [-0.1, -0.16, -0.15],
    [0.1, -0.16, -0.15],
    [0.2, 0, -0.15]
])

REAL = 0

# Robot setup
if REAL == 1:
    portHandler = PortHandler("/dev/ttyUSB0")
    packetHandler = PacketHandler(1.0)

    portHandler.openPort()
    x = portHandler.setBaudRate(1000000)
    print(x)
    ADDR_GOAL_POSITION = 30

    ids = []
    for id in range(0, 100):
        model_number, result, error = packetHandler.ping(portHandler, id)
        if model_number == 12:
            print(f"Found AX-12 with id: {id}")
            ids.append(id)
        time.sleep(0.01)


# Done

def interpol2(point2, point1, t):
    x1, y1, z1 = point1
    x2, y2, z2 = point2
    return t * x1 + (1 - t) * x2, t * y1 + (1 - t) * y2, t * z1 + (1 - t) * z2


def get_current_step(t, step_duration, movement_duration):
    time_passed = 0
    for i in range(len(step_duration)):
        time_passed += step_duration[i]
        if t % movement_duration < time_passed:
            return i


def get_current_step_advancement(t, movement_duration, step_duration, current_step):
    current_step = get_current_step(t, step_duration, movement_duration)
    t = t % movement_duration
    for i in range(0, current_step):
        t -= step_duration[i]
    return t / step_duration[current_step]


def inverse(x, y, z):
    """
    """
    # Dimensions (m)
    z += l1v

    theta0 = atan2(y, x)

    l = sqrt((sqrt(x ** 2 + y ** 2) - l1) ** 2 + z ** 2)
    # l = sqrt((x - l1h*cos(theta0)) ** 2 + (y - l1h*sin(theta0)) ** 2 + (z + l1v) ** 2)

    param2 = -1 * (-(l ** 2) + l2 ** 2 + l3 ** 2) / (2 * l2 * l3)

    if param2 > 1 or param2 < -1:
        legsLogger.warning("fTentative d'acces a une position impossible (param2) ({x}, {y}, {z})")
        param2 = 1 if param2 > 1 else -1

    theta2 = acos(param2)

    param1 = (-l3 ** 2 + l2 ** 2 + l ** 2) / (2 * l2 * l)
    if param1 > 1 or param1 < -1:
        legsLogger.warning(f"Tentative d'acces a une position impossible (param1) ({x}, {y}, {z})")
        param1 = 1 if param1 > 1 else -1

    theta1 = acos(param1) + asin(z / l)

    # return [-theta0, theta1, theta2]
    angle1 = atan(l2v / l2h)
    return [-theta0, theta1 + angle1, theta2 + angle1 - pi / 2 + atan(l3h / l3v)]
    # return [0, angle1 , angle1 -pi/2 + atan(l3h/l3v)]


def legs(targets_robot):
    """
    takes a list of target and offsets it to be in the legs referential
    """

    targets = [0] * 18

    cos_val = [0, 0, -1, 0, 0, 1]
    sin_val = [-1, -1, 0, 1, 1, 0]

    offset_x = [-patte_x, -patte_x, -tete_x, -patte_x, -patte_x, -tete_x]
    offset_y = [patte_y, -patte_y, 0, patte_y, -patte_y, 0]

    for i in range(6):
        target_x, target_y, target_z = targets_robot[i]

        target_x_tmp = cos_val[i] * target_x - sin_val[i] * target_y
        target_y = sin_val[i] * target_x + cos_val[i] * target_y
        target_x = target_x_tmp

        target_x += offset_x[i]
        target_y += offset_y[i]

        alpha, beta, gamma = inverse(target_x, target_y, target_z)
        targets[3 * i] = alpha
        targets[3 * i + 1] = beta
        targets[3 * i + 2] = gamma

    return targets


def naive_walk(t, speed_x, speed_y):
    slider_max = 0.200

    real_position = np.copy(neutral_position)

    movement_x = np.array([
        [0.00, 0, 0],
        [0.04, 0, 0],
        [-0.04, 0, 0],
    ])

    movement_y = np.array([
        [0.0, 0, 0],
        [0, 0.04, 0],
        [0, -0.04, 0],
    ])

    movement_z = np.array([
        [0, 0, 0.08],
        [0, 0, -0.02],
        [0, 0, -0.02]
    ])

    # duration of each step of the movement
    step_duration = np.array([0.05, 0.3, 0.05])

    step_count = len(movement_z)
    movement_duration = np.sum(step_duration)

    assert len(
        step_duration) == step_count, f"all movements steps must have a length, currently, {len(step_duration)}/{step_count} have them"

    def get_next_step(t):
        return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)

    offsets = np.array([0, 1 / 3, 2 / 3, 0, 1 / 3, 2 / 3]) * movement_duration  # offset between each leg
    assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them"

    for patte in range(num_patte):
        time = t + offsets[patte]
        mov_index_start = get_current_step(time, step_duration, movement_duration)
        mov_index_end = get_next_step(time)

        mov_start_x = normalize(movement_x[mov_index_start], slider_max, speed_x)
        mov_end_x = normalize(movement_x[mov_index_end], slider_max, speed_x)

        mov_start_y = normalize(movement_y[mov_index_start], slider_max, speed_y)
        mov_end_y = normalize(movement_y[mov_index_end], slider_max, speed_y)

        mov_start_z = movement_z[mov_index_start]
        mov_end_z = movement_z[mov_index_end]

        mov_start = neutral_position[patte] + mov_start_z + mov_start_x + mov_start_y
        mov_end = neutral_position[patte] + mov_end_z + mov_end_x + mov_end_y

        (real_position[patte][0],
         real_position[patte][1],
         real_position[patte][2]) = interpol2(mov_start, mov_end,
                                              get_current_step_advancement(time, movement_duration, step_duration,
                                                                           mov_index_start))
        legsLogger.debug(
            f"[{patte}] [{mov_index_start}->{mov_index_end}], start: {mov_start}, end: {mov_end}, current ({real_position[patte][0]}, {real_position[patte][1]}, {real_position[patte][2]})")

    return legs(real_position)


def normalize(matrix, slider_max, speed):
    return (matrix / slider_max) * speed


counter = 0


def convert_to_robot(positions):
    global counter
    counter += 1
    if counter % 2 != 0:
        return
    print(positions)
    index = [
        11, 12, 13,
        41, 42, 43,
        21, 22, 23,
        51, 52, 53,
        61, 62, 63,
        31, 32, 33]
    dir = [-1] * 18

    for i in range(len(positions)):
        value = int(512 + positions[i] * 150 * dir[i])
        packetHandler.write2ByteTxOnly(portHandler, index[i], ADDR_GOAL_POSITION, value)
        # packetHandler.writeTxOnly(portHandler, index[i], ADDR_GOAL_POSITION, 2,
        #                          bytes(value))


def stand(x1, y1, x2, y2):
    return [
        0, 0, 0,
        0, 0, 0,
        x1, 0, 0,
        0, -0.5, 0,
        0, -0.5, 0,
        y1, 0, 0
    ]


def walk(t, sx, sy, sr):
    xboxdata = xbox.get_data()
    val = xboxdata["x1"]

    max_slider = 0.200
    controllerLogger.debug(xboxdata)
    if xbox.mode == 0:
        positions = static()
    elif xbox.mode == 1:
        positions = jump(xboxdata["y2"])
    elif xbox.mode == 2:
        positions = dino_naive(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"], max_slider * xboxdata["y2"],
                               max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
    elif xbox.mode == 3:
        positions = naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
    elif xbox.mode == 4:
        positions = dev(t, max_slider * xboxdata["y1"], -1 * max_slider * xboxdata["x1"], xboxdata["x2"] * max_slider,
                        max_slider * xboxdata["y2"])
    elif xbox.mode == 5:
        positions = stand(xboxdata["x1"], xboxdata["y1"], xboxdata["x2"], xboxdata["y2"])

    print(positions)
    if REAL == 1:
        convert_to_robot(positions)

    return positions


def static():
    return legs(neutral_position)


# Walk V2
def dev(t, speed_x, speed_y, speed_r, y2):
    """
    x1: speed along the x-axis
    y1: speed along the y-axis
    x2: rotational speed along the z-axis
    """
    max_dist = 0.3
    print(speed_r)

    def get_rotation_center(speed_x, speed_y, theta_point):
        norm = sqrt(speed_x ** 2 + speed_y ** 2)

        r = 1000 if theta_point == 0 else norm / theta_point
        if norm != 0:
            return np.array([speed_y, -speed_x]) / norm * r
        else:
            return 0, 0

    center_x, center_y = get_rotation_center(speed_x, speed_y, speed_r)

    legsLogger.debug(f"rotation center: center_x: {center_x}, center_y: {center_y}")

    real_position = np.copy(neutral_position)

    movement_z = np.array([
        [0, 0, 0.02],
        [0, 0, -0.01],
        [0, 0, -0.01]
    ])

    step_duration = np.array([0.15, 0.3, 0.15])

    step_count = len(movement_z)
    movement_duration = np.sum(step_duration)

    assert len(
        step_duration) == step_count, f"all movements steps must have a length, currently, {len(step_duration)}/{step_count} have them"

    def get_next_step(t):
        return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)

    offsets = np.array([0, 1 / 3, 2 / 3, 0, 1 / 3, 2 / 3]) * movement_duration  # offset between each leg
    assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them"

    # used to cap the speed to a max
    max_radius = 0
    for patte in range(num_patte):
        x1, y1 = real_position[patte][0], real_position[patte][1]
        circle_radius = sqrt((center_x - x1) ** 2 + (center_y - y1) ** 2)
        max_radius = max(max_radius, circle_radius)

    for patte in range(num_patte):
        time = t + offsets[patte]
        mov_index_start = get_current_step(time, step_duration, movement_duration)
        mov_index_end = get_next_step(time)

        step_adv = get_current_step_advancement(time, movement_duration, step_duration, mov_index_start)

        mov_start_z = movement_z[mov_index_start]
        mov_end_z = movement_z[mov_index_end]

        mov_start = neutral_position[patte] + mov_start_z
        mov_end = neutral_position[patte] + mov_end_z

        (real_position[patte][0],
         real_position[patte][1],
         real_position[patte][2]) = interpol2(mov_start, mov_end, step_adv)

        x1, y1 = real_position[patte][0], real_position[patte][1]

        circle_radius = sqrt((center_x - x1) ** 2 + (center_y - y1) ** 2)
        cur_theta = acos((x1 - center_x) / circle_radius)

        if y1 < center_y:
            cur_theta *= -1
        if speed_r != 0:
            speed = sqrt(speed_x ** 2 + speed_y ** 2) * (speed_r / abs(speed_r))
        else:
            speed = sqrt(speed_x ** 2 + speed_y ** 2)

        if speed == 0:
            speed = speed_r
        dthteta = speed * (1 / max_radius * max_dist)
        legsLogger.info(f"speed: {speed}, dthteta: {dthteta}")
        if mov_index_start == 0:
            # midair part 1
            theta_offset = dthteta * (- step_adv)
        elif mov_index_start == 1:
            # moving part
            theta_offset = dthteta * (step_adv - 0.5) * 2
        else:
            # midair part 1
            theta_offset = dthteta * (1 - step_adv)
        wanted_theta = cur_theta + theta_offset

        wanted_x = center_x + circle_radius * cos(wanted_theta)
        wanted_y = center_y + circle_radius * sin(wanted_theta)

        real_position[patte][0] = wanted_x
        real_position[patte][1] = wanted_y

    if REAL:
        return legs(real_position)
    return legs(real_position), [center_x, center_y]


def jump(sy):
    offset = np.array([
        [0, 0, -0.15],
        [0, 0, -0.15],
        [0, 0, -0.15],
        [0, 0, -0.15],
        [0, 0, -0.15],
        [0, 0, -0.15]
    ])
    offset *= sy
    return legs(neutral_position + offset)


# based on walk V1
def dino_naive(t, speed_x, speed_y, hz, hy, hx):
    slider_max = 0.200

    real_position = np.copy(neutral_position)

    movement_x = np.array([
        [0.00, 0, 0],
        [0.04, 0, 0],
        [-0.04, 0, 0],
    ])

    movement_y = np.array([
        [0.0, 0, 0],
        [0, 0.04, 0],
        [0, -0.04, 0],
    ])

    movement_z = np.array([
        [0, 0, 0.08],
        [0, 0, -0.02],
        [0, 0, -0.02]
    ])

    # duration of each step of the movement
    step_duration = np.array([0.05, 0.3, 0.05])

    step_count = len(movement_z)
    movement_duration = np.sum(step_duration)

    assert len(
        step_duration) == step_count, f"all movements steps must have a length, currently, {len(step_duration)}/{step_count} have them"

    def get_next_step(t):
        return floor((get_current_step(t, step_duration, movement_duration) + 1) % step_count)

    offsets = np.array([0, 1 / 3, 2 / 3, 0, 1 / 3, 2 / 3]) * movement_duration  # offset between each leg
    assert len(offsets) == num_patte, f"all offsets must be set, currently, {len(offsets)}/{num_patte} have them"

    for patte in range(num_patte):
        if patte in [2, 5]:
            continue
        time = t + offsets[patte]
        mov_index_start = get_current_step(time, step_duration, movement_duration)
        mov_index_end = get_next_step(time)

        mov_start_x = normalize(movement_x[mov_index_start], slider_max, speed_x)
        mov_end_x = normalize(movement_x[mov_index_end], slider_max, speed_x)

        mov_start_y = normalize(movement_y[mov_index_start], slider_max, speed_y)
        mov_end_y = normalize(movement_y[mov_index_end], slider_max, speed_y)

        mov_start_z = movement_z[mov_index_start]
        mov_end_z = movement_z[mov_index_end]

        mov_start = neutral_position[patte] + mov_start_z + mov_start_x + mov_start_y
        mov_end = neutral_position[patte] + mov_end_z + mov_end_x + mov_end_y

        (real_position[patte][0],
         real_position[patte][1],
         real_position[patte][2]) = interpol2(mov_start, mov_end,
                                              get_current_step_advancement(time, movement_duration, step_duration,
                                                                           mov_index_start))

    real_position[2][2] = -0.1
    real_position[2][0] = -0.28

    real_position[5][2] = 0.05 + hz
    real_position[5][0] = 0.25 + hx
    real_position[5][1] = hy

    return legs(real_position)


def robot_input():
    print("Send motors wave (press enter)")
    input()
    for id in ids:
        packetHandler.write2ByteTxRx(portHandler, id, ADDR_GOAL_POSITION, 512)
    input()
    angle = [512, 624, 400, 512, 624, 400]
    for id in range(1, 7):
        packetHandler.write2ByteTxRx(portHandler, id * 10 + 1, ADDR_GOAL_POSITION, angle[id - 1])
    input()

    # Dino mode
    packetHandler.write2ByteTxRx(portHandler, 12, ADDR_GOAL_POSITION, 650)
    packetHandler.write2ByteTxRx(portHandler, 42, ADDR_GOAL_POSITION, 650)
    packetHandler.write2ByteTxRx(portHandler, 43, ADDR_GOAL_POSITION, 400)

    input()
    for id in ids:
        packetHandler.write2ByteTxRx(portHandler, id, ADDR_GOAL_POSITION, 512)
    # t = 0.0
    # while True:
    #     print("Send motors wave (press enter)")
    #     angle = 512 + int(50 * np.sin(t))
    #     print(angle)
    #     packetHandler.write2ByteTxRx(portHandler, 53, ADDR_GOAL_POSITION, angle)
    #     time.sleep(0.001)
    #     t += 0.01


if __name__ == "__main__":
    print("N'exécutez pas ce fichier, mais simulator.py")