import GlobalVariables as gv
import config as cfg
import DataTypes as dt
import kinematics as kin
import numpy as np
import math
import time

# Leg_Pair 1 {leg[num] = 0,2,4}
# Leg Pair 2 {leg[num] = 1,3,5}

def emitCalculation(target_rad: dt.RadArray):
    if gv.shared_sim != None:
        gv.shared_sim.updatePos(target_rad)
        gv.shared_sim.step()
    if gv.robotCommunication != None:
        gv.robotCommunication.send_motion(target_rad)
    gv.current_rad = target_rad

def initPos():
    # Init Position and gv.current_rad
    gv.current_rad = gv.init_deg.to_rad()
    gv.current_pos = gv.center_points
    target_rad: dt.RadArray = kin.ikpyInverse(gv.center_points)
    emitCalculation(target_rad)


def walking(duration=cfg.standard_duration, tickpersec=cfg.standard_tickpersec, curve_height=cfg.step_height):
    # init Values
    ticks = int(duration * tickpersec)
    tick_duration = 1 / tickpersec
    tick_pos: dt.PosArray  # aktuelle XYZ-Positionen
    current_pos_copy: dt.PosArray = gv.current_pos
    vx, vy, omega = gv.vector_dirmov

    # Apply config scaling
    vx *= cfg.translation_gain
    vy *= cfg.translation_gain
    omega *= cfg.rotation_gain

    target_pos_temp = []

    for i in range(6):
        cx, cy, cz = gv.center_points[i]

        # relative position (assuming body center = 0,0)
        rx = cx
        ry = cy

        # rotation component
        v_rot_x = -omega * ry
        v_rot_y =  omega * rx

        # combine translation + rotation
        v_x = vx + v_rot_x
        v_y = vy + v_rot_y

        # normalize combined vector (important!)
        length = (v_x**2 + v_y**2) ** 0.5
        if length > 1.0:
            v_x /= length
            v_y /= length

        target_pos_temp.append([
            cx + v_x * cfg.step_length,
            cy + v_y * cfg.step_length,
            cz
        ])
    target_pos: dt.PosArray = target_pos_temp

    def interpolate(t, p0, p1, p2):
        return (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2

    # Smooth Step
    for tick in range(ticks + 1):
        loop_start = time.perf_counter()
        t = tick / ticks
        tick_pos_temp = []

        for leg_id in range(6):
            if gv.leg_state[leg_id] == "drag":
                # lineares Gleiten in die Center-Position
                tick_pos_temp.append(current_pos_copy[leg_id] + (gv.center_points[leg_id] - current_pos_copy[leg_id]) * t)

            elif gv.leg_state[leg_id] == "step":
                # Mittlerer Kontrollpunkt für Bezier-Kurve
                mid_point = [
                    (current_pos_copy[leg_id][0] + target_pos[leg_id][0]) / 2,
                    (current_pos_copy[leg_id][1] + target_pos[leg_id][1]) / 2,
                    max(current_pos_copy[leg_id][2], target_pos[leg_id][2])
                    + cfg.step_height,
                ]
                x = interpolate(
                    t, current_pos_copy[leg_id][0], mid_point[0], target_pos[leg_id][0]
                )
                y = interpolate(
                    t, current_pos_copy[leg_id][1], mid_point[1], target_pos[leg_id][1]
                )
                z = interpolate(
                    t, current_pos_copy[leg_id][2], mid_point[2], target_pos[leg_id][2]
                )
                tick_pos_temp.append([x, y, z])

        tick_pos = dt.PosArray(tick_pos_temp)
        # IK mit letzter Winkelstellung als Startpunkt
        emitCalculation(kin.ikpyInverse(tick_pos))
        print("Tick Position: \n", tick_pos)
        if (cfg.sim == True):
            gv.shared_sim.step()

        # Timing anpassen, damit Loop gleichmäßig bleibt
        elapsed = time.perf_counter() - loop_start
        sleep_time = tick_duration - elapsed
        if sleep_time > 0:
            time.sleep(sleep_time)

    # Endposition sichern
    #emitCalculation(kin.ikpyInverse(target_pos))
    #print("END TargetPos:\n", target_pos, "\n\n\n")

    # Update current pos
    gv.current_pos = tick_pos
    gv.current_rad = kin.ikpyInverse(tick_pos)
    #emitCalculation(kin.ikpyInverse(tick_pos))

    # Gait-Zustand wechseln
    if gv.leg_state[0] == "step":
        gv.leg_state = np.array(["drag", "step", "drag", "step", "drag", "step"])
    else:
        gv.leg_state = np.array(["step", "drag", "step", "drag", "step", "drag"])

def walking_four(duration=cfg.standard_duration, tickpersec=cfg.standard_tickpersec, curve_height=cfg.step_height):
    # init Values
    ticks = duration * tickpersec
    tick_duration = 1 / tickpersec
    tick_pos: dt.PosArray  # aktuelle XYZ-Positionen
    current_rad_copy: dt.RadArray = gv.current_rad
    current_pos_copy: dt.PosArray = gv.current_pos
    dirmov_copy = gv.vector_dirmov

    # Zielpunkte für jede Beinspitze berechnen
    target_pos_temp = []
    for i in range(6):
        target_pos_temp.append(
            [
                gv.center_points[i][0] + dirmov_copy[0] * cfg.step_length,
                gv.center_points[i][1] + dirmov_copy[1] * cfg.step_length,
                gv.center_points[i][2]
            ]
        )
    target_pos: dt.PosArray = target_pos_temp

    def interpolate(t, p0, p1, p2):
        return (1 - t) ** 2 * p0 + 2 * (1 - t) * t * p1 + t**2 * p2

    # Smooth Step
    for tick in range(int(ticks) + 1):
        loop_start = time.perf_counter()
        t = tick / ticks
        tick_pos_temp = []

        for leg_id in range(6):
            if gv.leg_state[leg_id] == "drag":
                # lineares Gleiten in die Center-Position
                tick_pos_temp.append(current_pos_copy[leg_id] + (gv.center_points[leg_id] - current_pos_copy[leg_id]) * t)

            elif gv.leg_state[leg_id] == "step":
                # Mittlerer Kontrollpunkt für Bezier-Kurve
                mid_point = [
                    (current_pos_copy[leg_id][0] + target_pos[leg_id][0]) / 2,
                    (current_pos_copy[leg_id][1] + target_pos[leg_id][1]) / 2,
                    max(current_pos_copy[leg_id][2], target_pos[leg_id][2])
                    + cfg.step_height,
                ]
                x = interpolate(
                    t, current_pos_copy[leg_id][0], mid_point[0], target_pos[leg_id][0]
                )
                y = interpolate(
                    t, current_pos_copy[leg_id][1], mid_point[1], target_pos[leg_id][1]
                )
                z = interpolate(
                    t, current_pos_copy[leg_id][2], mid_point[2], target_pos[leg_id][2]
                )
                tick_pos_temp.append([x, y, z])

        tick_pos = dt.PosArray(tick_pos_temp)
        # IK mit letzter Winkelstellung als Startpunkt
        emitCalculation(kin.ikpyInverse(tick_pos))
        print("Tick Position: \n", tick_pos)

        # Timing anpassen, damit Loop gleichmäßig bleibt
        elapsed = time.perf_counter() - loop_start
        sleep_time = tick_duration - elapsed
        if sleep_time > 0:
            time.sleep(sleep_time)

    # Endposition sichern
    #emitCalculation(kin.ikpyInverse(target_pos))
    print("END TargetPos:\n", target_pos, "\n\n\n")

    # Gait-Zustand wechseln
    if gv.leg_state[0] == "step":
        gv.leg_state = np.array(["drag", "drag", "step", "drag", "drag", "drag"])
    elif gv.leg_state[2] == "step":
        gv.leg_state = np.array(["drag", "drag", "drag", "step", "drag", "drag"])
    elif gv.leg_state[3] == "step":
        gv.leg_state = np.array(["drag", "drag", "drag", "drag", "drag", "step"])
    elif gv.leg_state[5] == "step":
        gv.leg_state = np.array(["step", "drag", "drag", "drag", "drag", "drag"])

def wave_emote(cycles=3, duration=1.5, tickpersec=20,
                   height=40, amplitude=25, inward_offset=10):
    """
    Greeting wave using front leg (leg 0)

    Motion:
    - Z: lifts leg up
    - Y: waves left/right
    - X: slightly pulled inward to avoid IK limits
    """

    ticks = int(duration * tickpersec)
    tick_duration = 1 / tickpersec

    # Safe copy
    base_pos = dt.PosArray(np.copy(gv.current_pos.data))

    leg_id = 0  # front leg

    for cycle in range(cycles):
        for tick in range(ticks):
            loop_start = time.perf_counter()

            t = tick / ticks
            tick_pos = np.copy(base_pos.data)

            cx, cy, cz = base_pos[leg_id]

            # smooth outward-only wave
            y_wave = math.sin(4 * math.pi * t)
            y_offset = amplitude * (0.5 * (y_wave + 1))

            # vertical lift
            z_offset = height * math.sin(math.pi * t)

            # clamp sideways motion
            max_y_dev = 30
            new_y = cy + y_offset
            new_y = max(cy - max_y_dev, min(cy + max_y_dev, new_y))

            tick_pos[leg_id] = [
                cx + 10,     # small forward bias (IMPORTANT)
                new_y,
                cz + z_offset
            ]

            tick_pos = dt.PosArray(tick_pos)

            emitCalculation(kin.ikpyInverse(tick_pos))

            # Timing
            elapsed = time.perf_counter() - loop_start
            sleep_time = tick_duration - elapsed
            if sleep_time > 0:
                time.sleep(sleep_time)

    # Return to base pose
    emitCalculation(kin.ikpyInverse(base_pos))
    gv.current_pos = base_pos

def laola_wave_emote(cycles=3, duration=1.5, tickpersec=cfg.standard_tickpersec, height=10, amplitude=30):
    ticks = int(duration * tickpersec)
    tick_duration = 1 / tickpersec

    # Safe copy of base position
    base_pos = dt.PosArray(np.copy(gv.current_pos.data))

    # Legs that perform the wave
    wave_legs = [1, 4]

    for cycle in range(cycles):
        for tick in range(ticks):
            loop_start = time.perf_counter()
            t = tick / ticks  # normalized 0 → 1
            tick_pos = np.copy(base_pos.data)

            for leg_id in wave_legs:
                cx, cy, cz = base_pos[leg_id]

                # Phase shift for Laola wave
                phase = 0 if leg_id == 1 else math.pi

                # Sideways motion (Y) — reduced amplitude to avoid overextension
                y_offset = amplitude * math.sin(2 * math.pi * t + phase)

                # Vertical motion (Z) — full up/down oscillation
                z_offset = height * math.sin(2 * math.pi * t + phase)

                # Apply movement in YZ plane, X stays fixed
                tick_pos[leg_id] = [
                    cx,
                    cy + y_offset,
                    cz + z_offset
                ]

            tick_pos = dt.PosArray(tick_pos)

            # IK + send command
            emitCalculation(kin.ikpyInverse(tick_pos))

            # Timing control
            elapsed = time.perf_counter() - loop_start
            sleep_time = tick_duration - elapsed
            if sleep_time > 0:
                time.sleep(sleep_time)

    # Return to original stance
    emitCalculation(kin.ikpyInverse(base_pos))
    gv.current_pos = base_pos